def testPing(self): """Ping between client and server""" c = Connection(self.address) c.ping() c.close() c.ping() c.close()
import rospy, socket from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus from std_msgs.msg import Float32 rospy.init_node("spencer_robot_battery_monitor") rate = rospy.Rate(1.0 / 60.0) publisher = rospy.Publisher("/diagnostics", DiagnosticArray, latch=True, queue_size=1) batteryVoltagePublisher = rospy.Publisher("/spencer/sensors/battery/voltage", Float32, latch=True, queue_size=1) batteryPercentagePublisher = rospy.Publisher("/spencer/sensors/battery/percentage", Float32, latch=True, queue_size=1) rospy.loginfo("Starting robot battery monitor...") while not rospy.is_shutdown(): proxy = Connection(("ant", 1234)) proxy.login("Master", "robox") data = proxy.inspect(StringArray(["Safety:batteryPower,batteryVoltage"])) diagnosticArray = DiagnosticArray() diagnosticArray.header.stamp = rospy.Time.now() batteryStatus = DiagnosticStatus() batteryStatus.name = "battery" batteryStatus.hardware_id = socket.gethostname() batteryStatus.message="%.3fV %.1f%%" % (data.Safety.batteryVoltage, data.Safety.batteryPower) if data.Safety.batteryPower < 10: batteryStatus.message += "; Critically low power, recharge NOW!" batteryStatus.level = DiagnosticStatus.ERROR elif data.Safety.batteryPower < 20:
def bblos_main(address): rospy.loginfo ('bblos_main') """Read scans from the platform.""" global getIntensities global getMeasuredSpeed global testPlatform global timeout global lastreqtime global velreq global omegareq global pubodo global publaser # pre-initialise the constant part of the messages odomsg = Odometry() odomsg.header.frame_id = globalFrameName odomsg.child_frame_id = "biba_base"; odomsg.pose.pose.position.z = 0 for i in range(0,36): odomsg.pose.covariance[i] = 0 for i in range(0,36): odomsg.twist.covariance[i] = 0 odomsg.twist.twist.linear.y = 0 odomsg.twist.twist.linear.z = 0 odomsg.twist.twist.angular.x = 0 odomsg.twist.twist.angular.y = 0 laser = PointCloud() laser.header.frame_id = "laser" laser.channels.append(ChannelFloat32()) laser.points = [] if getIntensities: laser.channels[0].name = "intensities" laser.channels[0].values = [] # Use a Rate object to try to have a constant sampling time (which LOS # cannnot provide anyway) rate = rospy.Rate(10) proxy = Connection(address) proxy.login("User", "none") while not rospy.is_shutdown(): # First acquire all the data tstamp = rospy.rostime.get_rostime() try: proxy.Watchdog.reset(3.0) # We can de-activate intensities if requested if getIntensities: (tlaser, pose, maxAge, indices, coordinates, intensities) = proxy.Scan.get(Int32(gfCoordinates | gfSync | gfIntensities)) else: (tlaser, pose, maxAge, indices, coordinates) = proxy.Scan.get(Int32(gfCoordinates | gfSync )) #rospy.logdebug("info") #rospy.logdebug(tlaser) #rospy.logdebug(pose) #rospy.logdebug(maxAge) #rospy.logdebug(indices) #rospy.logdebug("coordinates ") #rospy.logdebug(coordinates) #if getIntensities: # rospy.logdebug(intensities) (todo, lospose) = proxy.Odometry.getPose() if getMeasuredSpeed: # Activate this part if you really need the robot speed speeds = proxy.Motion.getSpeed() else: speeds = [0, velreq, omegareq] # Implement a timeout if the last requested control message is too # old if (rospy.rostime.get_time() - lastreqtime) > timeout: velreq = 0.0 omerareq = 0.0 # And finally send the latest command if testPlatform: proxy.Motion.setSpeed(0.1*math.sin(rospy.rostime.get_time()), 0.1*math.cos(rospy.rostime.get_time())) else: proxy.Motion.setSpeed(velreq,omegareq) # rospy.loginfo("Speed sent %f and %f" % (velreq,omegareq)) except SyntaxError: traceback.print_exc() print "Exception while accessing los proxy" break except: print "Interrupted" break # Now convert the LOS type to ROS messages n = len(coordinates)/2 laser.header.stamp = tstamp if not (n == len(laser.points)): laser.points = [Point32()]*n if getIntensities: laser.channels[0].values = [0]*n for i in range(0,n): laser.points[i] = Point32(coordinates[2*i+0], coordinates[2*i+1], 0) #laser.points[i].x = Point32(coordinates[2*i+0], coordinates[2*i+1], 0) #laser.points[i].y = Point32(coordinates[2*i+1]) #laser.points[i].z = 0 #rospy.logdebug("points: %f, %f", laser.points[i].x, laser.points[i].y) if getIntensities: laser.channels[0].values[i] = intensities[i] publaser.publish(laser) # Directly send requested values odomsg.header.stamp = tstamp odomsg.twist.twist.linear.x = speeds[1] odomsg.twist.twist.angular.z = speeds[2] odomsg.pose.pose.position.x = lospose[0]; odomsg.pose.pose.position.y = lospose[1]; odomsg.pose.pose.position.z = wheelRadius; q = tf.transformations.quaternion_from_euler(0,0,lospose[2]) odomsg.pose.pose.orientation.x = q[0] odomsg.pose.pose.orientation.y = q[1] odomsg.pose.pose.orientation.z = q[2] odomsg.pose.pose.orientation.w = q[3] # Assuming the covariance is Cov[x y 0 . . theta] odomsg.pose.covariance[6*0+0] = lospose[3]; # x * x odomsg.pose.covariance[6*0+1] = lospose[6]; # x * y odomsg.pose.covariance[6*1+0] = lospose[6]; # y * x odomsg.pose.covariance[6*1+1] = lospose[4]; # y * y odomsg.pose.covariance[6*0+5] = lospose[7]; # x * theta odomsg.pose.covariance[6*5+0] = lospose[7]; # theta * x odomsg.pose.covariance[6*1+5] = lospose[8]; # y * theta odomsg.pose.covariance[6*5+1] = lospose[8]; # theta * y odomsg.pose.covariance[6*5+5] = lospose[5]; # theta * theta pubodo.publish(odomsg) # Publish transformation frame br = tf.TransformBroadcaster() br.sendTransform( (odomsg.pose.pose.position.x, odomsg.pose.pose.position.y, odomsg.pose.pose.position.z), (odomsg.pose.pose.orientation.x, odomsg.pose.pose.orientation.y, odomsg.pose.pose.orientation.z, odomsg.pose.pose.orientation.w), rospy.Time.now(), odomsg.child_frame_id, globalFrameName ) #rospy.loginfo("published laser and odo") rate.sleep()
rate = rospy.Rate(1.0 / 60.0) publisher = rospy.Publisher("/diagnostics", DiagnosticArray, latch=True, queue_size=1) batteryVoltagePublisher = rospy.Publisher("/spencer/sensors/battery/voltage", Float32, latch=True, queue_size=1) batteryPercentagePublisher = rospy.Publisher( "/spencer/sensors/battery/percentage", Float32, latch=True, queue_size=1) rospy.loginfo("Starting robot battery monitor...") while not rospy.is_shutdown(): proxy = Connection(("ant", 1234)) proxy.login("Master", "robox") data = proxy.inspect(StringArray(["Safety:batteryPower,batteryVoltage"])) diagnosticArray = DiagnosticArray() diagnosticArray.header.stamp = rospy.Time.now() batteryStatus = DiagnosticStatus() batteryStatus.name = "battery" batteryStatus.hardware_id = socket.gethostname() batteryStatus.message = "%.3fV %.1f%%" % (data.Safety.batteryVoltage, data.Safety.batteryPower) if data.Safety.batteryPower < 10: batteryStatus.message += "; Critically low power, recharge NOW!" batteryStatus.level = DiagnosticStatus.ERROR
def moveTo(argv): global lat_rad,lon_rad,h,yaw global fakeGpsOn,targetLat,targetLon global distance, flagMotion,x,y,theta,bearing,bearing0,sat # #************************check the arguments********************* if len(argv) < 4: sys.stderr.write("Usage: donkeyMoveToPose %s {mode} [0 = local cartesian frame;1 = fake GPS] {Localization.active} [bool] {X} [m] {Y} [m] {Theta} [rad] \n ex.:\n rosrun donkeyMoveToPose %s False False 0.5 0.0 0.5 \n\n rosrun donkeyMoveToPose %s True False 49.56789 11.45677 \n\n" % (os.path.basename(argv[0]),os.path.basename(argv[0]),os.path.basename(argv[0]))) return 2 # TODO: da perfezionare la logica di errore riportata sotto e quella sopra # if(argv[1]!="True" and argv[1]!="False"): # sys.stderr.write("The 1th argument must be False or True \n\n") # return 3 # if(argv[2]!="True" and argv[2]!="False"): # sys.stderr.write("The 2th argument must be False or True \n\n") # return 3 # #*****************connection to ANT************************ rospy.init_node('donkeyGPSListener', anonymous=True) # rospy.Subscriber("mti/filter/orientation",orientationEstimate,callbackEstimate) proxy = Connection(("192.168.8.149", 1234), timeout=2.5) proxy.open() proxy.ping() proxy.login("User", "none") print "ANT is connencted." if(argv[1]=="0") : # #********************MOVE TO (WP in the map)************************* rospy.init_node('donkeyGPSListener', anonymous=True) client = actionlib.SimpleActionClient('move_base', MoveBaseAction) goal = MoveBaseGoal() wp_num = (len(argv)-3)/3 print "goal: x=%0.2f y=%0.2f" % (float(argv[3]),float(argv[4])) goal.target_pose.pose.position.x = float(argv[3]) goal.target_pose.pose.position.y = float(argv[4]) goal.target_pose.pose.orientation.w=1.0 goal.target_pose.header.frame_id= "map" print "frame_id= map" goal.target_pose.header.stamp=rospy.Time.now() client.wait_for_server() client.send_goal(goal) client.wait_for_result() print "result:" print client.get_result() print "state:" print client.get_state() wp_num=wp_num -1 while(wp_num!=0) : if(client.get_state()==3): goal.target_pose.pose.position.x = float(argv[3+3*wp_num]) goal.target_pose.pose.position.y = float(argv[4+3*wp_num]) goal.target_pose.pose.orientation.w=1.0 goal.target_pose.header.frame_id= "map" goal.target_pose.header.stamp=rospy.Time.now() client.wait_for_server() client.send_goal(goal) client.wait_for_result() print client.get_result() wp_num=wp_num -1 else : print client.get_state() elif (argv[1]=="1") : rospy.init_node('donkeyGPSListener', anonymous=True) #rospy.Subscriber("mti/filter/orientation",orientationEstimate,callbackEstimate) #Thread fakeGpsOn=True fakeGpsUpdate = threading.Thread(target=updateFakeGps, args=()) fakeGpsUpdate.setDaemon(True) fakeGpsUpdate.start() targetUpdateFlag=True targetUpdate = threading.Thread(target=updateTargetFake, args=()) targetUpdate.setDaemon(True) time.sleep(1) print "moving from latitude %.6f and longitude %.6f" % (lat,lon) h=0 yaw=float(argv[6])*math.pi/180 me=earth_radius*(1-earth_ecc*earth_ecc)/((1-earth_ecc*earth_ecc*math.sin(lat_rad)*math.sin(lat_rad))**1.5) print "me= %0.6f" % me ne=earth_radius/((1-earth_ecc*earth_ecc*math.sin(lat_rad)*math.sin(lat_rad))**0.5) print "ne= %0.6f" % ne targetLat=float(argv[3]) targetLon=float(argv[4]) # print "%.6f"%(targetLon*math.pi/180.0-lon_rad) # print "%.6f"%(targetLat*math.pi/180.0-lat_rad) y1=(targetLon*math.pi/180.0-lon_rad)*(ne+h)*math.cos(lat_rad) x1=(targetLat*math.pi/180.0-lat_rad)*(me+h) print "x1=%0.3f"%x1 print "y1=%0.3f"%y1 print "yaw=%.2f"%yaw x=x1*math.cos(-yaw) - y1*math.sin(-yaw) y=x1*math.sin(-yaw) + y1*math.cos(-yaw) theta=math.atan2(y,x) bearing0=math.atan2(y,x) distance = math.sqrt(x*x + y*y) print "distance %0.3f" % distance print "bearing %0.3f" % bearing0 if(distance>sat): distanceSat = distance/abs(distance) * sat x= distanceSat*math.cos(bearing0) y= distanceSat*math.sin(bearing0) else: targetUpdateFlag=False print "CALCOLO ESEGUITO x: %f y: %f \n" % (x,y) # print "Drive here in simulated GPS mode" client = actionlib.SimpleActionClient('move_base', MoveBaseAction) goal = MoveBaseGoal() print "RIDOTTO x: %f y: %f" % (x,y) goal.target_pose.pose.position.x = x goal.target_pose.pose.position.y = - y goal.target_pose.pose.orientation.w=1.0 goal.target_pose.header.frame_id= "base_link" goal.target_pose.header.stamp=rospy.Time.now() client.wait_for_server() client.send_goal(goal) time.sleep(1) #targetUpdate.start() #client.wait_for_result() #print client.get_result() #print client.get_state() #time.sleep(10) #targetUpdateFlag=True while (distance>1): print "target recalculation" me=earth_radius*(1-earth_ecc*earth_ecc)/((1-earth_ecc*earth_ecc*math.sin(lat_rad)*math.sin(lat_rad))**1.5) ne=earth_radius/((1-earth_ecc*earth_ecc*math.sin(lat_rad)*math.sin(lat_rad))**0.5) y1=(targetLon*math.pi/180.0-lon_rad)*(ne+h)*math.cos(lat_rad) x1=(targetLat*math.pi/180.0-lat_rad)*(me+h) print "x1=%0.3f"%x1 print "y1=%0.3f"%y1 print "yaw=%.2f"%yaw x=x1*math.cos(-yaw) - y1*math.sin(-yaw) y=x1*math.sin(-yaw) + y1*math.cos(-yaw) theta=math.atan2(y,x) bearing0=math.atan2(y,x) distance = math.sqrt(x*x + y*y) print "distance %0.3f" % distance print "bearing %0.3f" % bearing0 if(distance>sat): distanceSat = distance/abs(distance) * sat x= distanceSat*math.cos(bearing0) y= distanceSat*math.sin(bearing0) # else: # targetUpdateFlag=False print "CALCOLO ESEGUITO x: %f y: %f \n" % (x,y) # print "Drive here in simulated GPS mode" client = actionlib.SimpleActionClient('move_base', MoveBaseAction) goal = MoveBaseGoal() print "RIDOTTO x: %f y: %f" % (x,y) goal.target_pose.pose.position.x = x goal.target_pose.pose.position.y = - y goal.target_pose.pose.orientation.w=1.0 goal.target_pose.header.frame_id= "base_link" goal.target_pose.header.stamp=rospy.Time.now() #client.wait_for_server() client.send_goal(goal) #client.wait_for_result() #print client.get_result() time.sleep(3) fakeGpsOn=False elif (argv[1]=="2") : #******************************MOVE TO (GPS)***************************************** rospy.init_node('donkeyGPSListener', anonymous=True) #rospy.Subscriber("mti/sensor/gnssPvt", gnssSample, callback) rospy.Subscriber("mti/filter/orientation",orientationEstimate,callbackEstimate) rospy.Subscriber("mti/filter/position",positionEstimate,callback) time.sleep(4) #calcolo le coordinate locali: print "\nprima posizione:\nlat=%0.7f lon=%0.7f h=%0.3f\n" % (lat,lon,h) me=(6378137 * (1-math.exp(2)))/(1-(math.exp(2)*(math.sin(lat* math.pi/180.0)*math.sin(lat* math.pi/180.0))))**(1.5) print "me= %0.6f\n" % me ne=(6378137)/((abs(1-(math.exp(2)*(math.sin(lat*math.pi/180.0))*math.sin(lat* math.pi/180.0))))**(0.5)) print "ne= %0.6f\n" % ne targetLat=float(argv[3]) targetLon=float(argv[4]) y1=((float(argv[4])* math.pi/180.0)-(lon* math.pi/180.0))*(ne+h)*math.cos(lat* math.pi/180.0) x1=((float(argv[3])* math.pi/180.0)-(lat* math.pi/180.0))*(me+h) print "x1=%0.3f"%x1 print "y1=%0.3f"%y1 print "yaw=%.2f"%yaw x=x1*math.cos(-yaw) - y1*math.sin(-yaw) y=x1*math.sin(-yaw) + y1*math.cos(-yaw) theta=math.atan2(y,x) bearing0=math.atan2(y,x) distance = math.sqrt(x*x + y*y) print "distance %0.3f" % distance print "bearing %0.3f" % bearing0 if(distance>sat): distanceSat = distance/abs(distance) * sat x= distanceSat*math.cos(bearing0) y= distanceSat*math.sin(bearing0) print "CALCOLO ESEGUITO x: %f y: %f \n" % (x,y) try : rospy.init_node('donkeyGPSListener', anonymous=True) #Thread targetUpdate = threading.Thread(target=updateTarget, args=()) targetUpdate.setDaemon(True) client = actionlib.SimpleActionClient('move_base', MoveBaseAction) goal = MoveBaseGoal() print "RIDOTTO x: %f y: %f" % (x,y) goal.target_pose.pose.position.x = x goal.target_pose.pose.position.y =y goal.target_pose.pose.orientation.w=1.0 goal.target_pose.header.frame_id= "base_link" goal.target_pose.header.stamp=rospy.Time.now() client.wait_for_server() client.send_goal(goal) flagMotion=True targetUpdate.start() print "thread started" client.wait_for_result() print client.get_result() print client.get_state() flagMotion= False # proxy.configure(Struct({"ObstacleAvoidance.syncActive":True})) # if(argv[2]=="False"): # proxy.configure(Struct({"Localization.active": False})) # elif(argv[2]=='True'): # proxy.configure(Struct({"Localization.active":True})) # proxy.Localization.snapToPose(0.0,0.0,yaw) # proxy.Motion.moveToPose(Float64(x),Float64(y),Float64(theta)) except Exception, e: print str(e) finally :
def testCall(self): """Calling a function""" c = Connection(self.address) assertEqual(6, c.object.add(Int32(1), Int32(2), Int32(3))) assertEqual(["", ""], c.object.identity("", "")) c.close()