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()
