00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 import rospy
00035
00036 import math
00037 import sys
00038 import tf
00039 import PyKDL
00040
00041 from sphero_driver import sphero_driver
00042 import dynamic_reconfigure.server
00043
00044 from sensor_msgs.msg import Imu
00045 from nav_msgs.msg import Odometry
00046 from geometry_msgs.msg import Point, Pose, Quaternion, Twist, TwistWithCovariance, Vector3
00047 from sphero_node.msg import SpheroCollision
00048 from std_msgs.msg import ColorRGBA, Float32, Bool
00049 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00050 from sphero_node.cfg import ReconfigConfig
00051
00052
00053 class SpheroNode(object):
00054 battery_state = {1:"Battery Charging",
00055 2:"Battery OK",
00056 3:"Battery Low",
00057 4:"Battery Critical"}
00058
00059
00060 ODOM_POSE_COVARIANCE = [1e-3, 0, 0, 0, 0, 0,
00061 0, 1e-3, 0, 0, 0, 0,
00062 0, 0, 1e6, 0, 0, 0,
00063 0, 0, 0, 1e6, 0, 0,
00064 0, 0, 0, 0, 1e6, 0,
00065 0, 0, 0, 0, 0, 1e3]
00066
00067
00068 ODOM_TWIST_COVARIANCE = [1e-3, 0, 0, 0, 0, 0,
00069 0, 1e-3, 0, 0, 0, 0,
00070 0, 0, 1e6, 0, 0, 0,
00071 0, 0, 0, 1e6, 0, 0,
00072 0, 0, 0, 0, 1e6, 0,
00073 0, 0, 0, 0, 0, 1e3]
00074
00075 def __init__(self, default_update_rate=50.0):
00076 rospy.init_node('sphero')
00077 self.update_rate = default_update_rate
00078 self.sampling_divisor = int(400/self.update_rate)
00079
00080 self.is_connected = False
00081 self._init_pubsub()
00082 self._init_params()
00083 self.robot = sphero_driver.Sphero()
00084 self.imu = Imu()
00085 self.imu.orientation_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6]
00086 self.imu.angular_velocity_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6]
00087 self.imu.linear_acceleration_covariance = [1e-6, 0, 0, 0, 1e-6, 0, 0, 0, 1e-6]
00088 self.last_cmd_vel_time = rospy.Time.now()
00089 self.last_diagnostics_time = rospy.Time.now()
00090 self.cmd_heading = 0
00091 self.cmd_speed = 0
00092 self.power_state_msg = "No Battery Info"
00093 self.power_state = 0
00094
00095 def _init_pubsub(self):
00096 self.odom_pub = rospy.Publisher('odom', Odometry)
00097 self.imu_pub = rospy.Publisher('imu', Imu)
00098 self.collision_pub = rospy.Publisher('collision', SpheroCollision)
00099 self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
00100 self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Twist, self.cmd_vel, queue_size = 1)
00101 self.color_sub = rospy.Subscriber('set_color', ColorRGBA, self.set_color, queue_size = 1)
00102 self.back_led_sub = rospy.Subscriber('set_back_led', Float32, self.set_back_led, queue_size = 1)
00103 self.stabilization_sub = rospy.Subscriber('disable_stabilization', Bool, self.set_stabilization, queue_size = 1)
00104 self.heading_sub = rospy.Subscriber('set_heading', Float32, self.set_heading, queue_size = 1)
00105 self.angular_velocity_sub = rospy.Subscriber('set_angular_velocity', Float32, self.set_angular_velocity, queue_size = 1)
00106 self.reconfigure_srv = dynamic_reconfigure.server.Server(ReconfigConfig, self.reconfigure)
00107 self.transform_broadcaster = tf.TransformBroadcaster()
00108
00109 def _init_params(self):
00110 self.connect_color_red = rospy.get_param('~connect_red',0)
00111 self.connect_color_blue = rospy.get_param('~connect_blue',0)
00112 self.connect_color_green = rospy.get_param('~connect_green',255)
00113 self.cmd_vel_timeout = rospy.Duration(rospy.get_param('~cmd_vel_timeout', 0.6))
00114 self.diag_update_rate = rospy.Duration(rospy.get_param('~diag_update_rate', 1.0))
00115
00116 def normalize_angle_positive(self, angle):
00117 return math.fmod(math.fmod(angle, 2.0*math.pi) + 2.0*math.pi, 2.0*math.pi);
00118
00119 def start(self):
00120 try:
00121 self.is_connected = self.robot.connect()
00122 rospy.loginfo("Connect to Sphero with address: %s" % self.robot.bt.target_address)
00123 except:
00124 rospy.logerr("Failed to connect to Sphero.")
00125 sys.exit(1)
00126
00127 self.robot.set_filtered_data_strm(self.sampling_divisor, 1 , 0, True)
00128 self.robot.add_async_callback(sphero_driver.IDCODE['DATA_STRM'], self.parse_data_strm)
00129
00130 self.robot.set_power_notify(True, False)
00131 self.robot.add_async_callback(sphero_driver.IDCODE['PWR_NOTIFY'], self.parse_power_notify)
00132
00133 self.robot.config_collision_detect(1, 45, 110, 45, 110, 100, False)
00134 self.robot.add_async_callback(sphero_driver.IDCODE['COLLISION'], self.parse_collision)
00135
00136 self.robot.set_rgb_led(self.connect_color_red,self.connect_color_green,self.connect_color_blue,0,False)
00137
00138 self.robot.start()
00139
00140 def spin(self):
00141 r = rospy.Rate(10.0)
00142 while not rospy.is_shutdown():
00143 now = rospy.Time.now()
00144 if (now - self.last_cmd_vel_time) > self.cmd_vel_timeout:
00145 if self.cmd_heading != 0 or self.cmd_speed != 0:
00146 self.cmd_heading = 0
00147 self.cmd_speed = 0
00148 self.robot.roll(int(self.cmd_speed), int(self.cmd_heading), 1, False)
00149 if (now - self.last_diagnostics_time) > self.diag_update_rate:
00150 self.last_diagnostics_time = now
00151 self.publish_diagnostics(now)
00152 r.sleep()
00153
00154 def stop(self):
00155
00156 self.robot.roll(int(0), int(0), 1, False)
00157 self.robot.shutdown = True
00158 rospy.sleep(1.0)
00159 self.is_connected = self.robot.disconnect()
00160 self.robot.join()
00161
00162 def publish_diagnostics(self, time):
00163 diag = DiagnosticArray()
00164 diag.header.stamp = time
00165
00166 stat = DiagnosticStatus(name="Battery Status", level=DiagnosticStatus.OK, message=self.power_state_msg)
00167 if self.power_state == 3:
00168 stat.level=DiagnosticStatus.WARN
00169 if self.power_state == 4:
00170 stat.level=DiagnosticStatus.ERROR
00171 diag.status.append(stat)
00172
00173 self.diag_pub.publish(diag)
00174
00175
00176 def parse_collision(self, data):
00177 if self.is_connected:
00178 now = rospy.Time.now()
00179 collision = SpheroCollision()
00180 collision.header.stamp = now
00181 collision.x = data["X"]
00182 collision.y = data["Y"]
00183 collision.z = data["Z"]
00184 collision.axis = int(data["Axis"])
00185 collision.x_magnitude = data["xMagnitude"]
00186 collision.y_magnitude = data["yMagnitude"]
00187 collision.speed = data["Speed"]
00188 collision.timestamp = data["Timestamp"]
00189
00190 self.collision = collision
00191 self.collision_pub.publish(self.collision)
00192
00193
00194 def parse_power_notify(self, data):
00195 if self.is_connected:
00196 self.power_state = data
00197 self.power_state_msg = self.battery_state[data]
00198
00199 def parse_data_strm(self, data):
00200 if self.is_connected:
00201 now = rospy.Time.now()
00202 imu = Imu(header=rospy.Header(frame_id="imu_link"))
00203 imu.header.stamp = now
00204 imu.orientation.x = data["QUATERNION_Q0"]
00205 imu.orientation.y = data["QUATERNION_Q1"]
00206 imu.orientation.z = data["QUATERNION_Q2"]
00207 imu.orientation.w = data["QUATERNION_Q3"]
00208 imu.linear_acceleration.x = data["ACCEL_X_FILTERED"]/4096.0*9.8
00209 imu.linear_acceleration.y = data["ACCEL_Y_FILTERED"]/4096.0*9.8
00210 imu.linear_acceleration.z = data["ACCEL_Z_FILTERED"]/4096.0*9.8
00211 imu.angular_velocity.x = data["GYRO_X_FILTERED"]*10*math.pi/180
00212 imu.angular_velocity.y = data["GYRO_Y_FILTERED"]*10*math.pi/180
00213 imu.angular_velocity.z = data["GYRO_Z_FILTERED"]*10*math.pi/180
00214
00215 self.imu = imu
00216 self.imu_pub.publish(self.imu)
00217
00218 odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_footprint')
00219 odom.header.stamp = now
00220 odom.pose.pose = Pose(Point(data["ODOM_X"]/100.0,data["ODOM_Y"]/100.0,0.0), Quaternion(0.0,0.0,0.0,1.0))
00221 odom.twist.twist = Twist(Vector3(data["VELOCITY_X"]/1000.0, 0, 0), Vector3(0, 0, data["GYRO_Z_FILTERED"]*10.0*math.pi/180.0))
00222 odom.pose.covariance =self.ODOM_POSE_COVARIANCE
00223 odom.twist.covariance =self.ODOM_TWIST_COVARIANCE
00224 self.odom_pub.publish(odom)
00225
00226
00227 self.transform_broadcaster.sendTransform((0.0, 0.0, 0.038 ),
00228 (data["QUATERNION_Q0"], data["QUATERNION_Q1"], data["QUATERNION_Q2"], data["QUATERNION_Q3"]),
00229 odom.header.stamp, "base_link", "base_footprint")
00230
00231 def cmd_vel(self, msg):
00232 if self.is_connected:
00233 self.last_cmd_vel_time = rospy.Time.now()
00234 self.cmd_heading = self.normalize_angle_positive(math.atan2(msg.linear.x,msg.linear.y))*180/math.pi
00235 self.cmd_speed = math.sqrt(math.pow(msg.linear.x,2)+math.pow(msg.linear.y,2))
00236 self.robot.roll(int(self.cmd_speed), int(self.cmd_heading), 1, False)
00237
00238 def set_color(self, msg):
00239 if self.is_connected:
00240 self.robot.set_rgb_led(int(msg.r*255),int(msg.g*255),int(msg.b*255),0,False)
00241
00242 def set_back_led(self, msg):
00243 if self.is_connected:
00244 self.robot.set_back(msg.data, False)
00245
00246 def set_stabilization(self, msg):
00247 if self.is_connected:
00248 if not msg.data:
00249 self.robot.set_stablization(1, False)
00250 else:
00251 self.robot.set_stablization(0, False)
00252
00253 def set_heading(self, msg):
00254 if self.is_connected:
00255 heading_deg = int(self.normalize_angle_positive(msg.data)*180.0/math.pi)
00256 self.robot.set_heading(heading_deg, False)
00257
00258 def set_angular_velocity(self, msg):
00259 if self.is_connected:
00260 rate = int((msg.data*180/math.pi)/0.784)
00261 self.robot.set_rotation_rate(rate, False)
00262
00263 def configure_collision_detect(self, msg):
00264 pass
00265
00266 def reconfigure(self, config, level):
00267 if self.is_connected:
00268 self.robot.set_rgb_led(int(config['red']*255),int(config['green']*255),int(config['blue']*255),0,False)
00269 return config
00270
00271
00272 if __name__ == '__main__':
00273 s = SpheroNode()
00274 s.start()
00275 s.spin()
00276 s.stop()
00277