ROSLink Bridge for TurtlebotΒΆ
To build a ROSLinkBridge for a turtlebot, we identify the following actions - while other could also be defined - including: (1) command TurtleBot to move in some direction with a certain speed.
In addition, we would like to monitor the internal status of the drone, namely (1) motion parameters, (2) current position.
First, we need to identify the topics related to the actions and status considering in this context.
To send TurtleBot to a specific location using ROS, a topic named /move\_base\_simple/goal is used to setup the goal location. This topic hold PoseStamped message.
To command TurtleBot to move in some direction with a certain speed, a topic named /cmd\_vel\_mux/input/teleop must be published with Twist message.
Step 1: Identify required topics:
For Turtlebot we need to publish in following topics for executing ROSLink commands:
/cmd_vel_mux/input/teleop, Twist message
And we need to subscribe in following topics:
/odom, Odometry message
Step 2: Declare a set of ROS publishers and subscribers:
#start ROS publishers
@staticmethod
def start_ros_publishers():
# ROS publishers: for executing ROSLink commands
ROSLinkBridgeTurtlebot.move_publisher = rospy.Publisher('/cmd_vel_mux/input/teleop',Twist, queue_size=10)
#start ROS subscribers
@staticmethod
def start_ros_subscribers():
rospy.Subscriber("/odom", Odometry, ROSLinkBridgeTurtlebot.odometryCallback)
Step 3: Declare a set of ROS parameters:
@staticmethod
def init_params():
rospy.loginfo('[ROSLink Bridge] reading initialization parameters')
# get roslink version
ROSLinkStateVariables.roslink_version = rospy.get_param("/roslink_version", ROSLINK_VERSION.ABUBAKR)
# get ROS version
ROSLinkStateVariables.ros_version = rospy.get_param("/ros_version", ROS_VERSION.INDIGO)
# get system id
ROSLinkStateVariables.system_id = rospy.get_param("/system_id", 15)
# get robot name
ROSLinkStateVariables.robot_name = rospy.get_param("/robot_name", "TURTLEBOT")
# get robot type
ROSLinkStateVariables.type = rospy.get_param("/type", ROBOT_TYPE.ROBOT_TYPE_TURTLEBOT)
# get owner id
ROSLinkStateVariables.owner_id = rospy.get_param("/owner_id", 3)
# get key
ROSLinkStateVariables.key = rospy.get_param("/key", "1243-0000-0000-FGFG")
# define periods of updates
ROSLinkBridgeTurtlebot.heartbeat_msg_rate = rospy.get_param("/heartbeat_msg_period", ROSLINK_MESSAGE_PERIOD.ROSLINK_HEARTBEAT_MESSAGE_RATE)
ROSLinkBridgeTurtlebot.robot_status_msg_rate = rospy.get_param("/robot_status_msg_period", ROSLINK_MESSAGE_PERIOD.ROSLINK_ROBOT_STATUS_MESSAGE_RATE)
ROSLinkBridgeTurtlebot.global_motion_msg_rate = rospy.get_param("/global_motion_msg_period", ROSLINK_MESSAGE_PERIOD.ROSLINK_GLOBAL_MOTION_MESSAGE_RATE)
ROSLinkBridgeTurtlebot.gps_raw_info_msg_rate = rospy.get_param("/gps_raw_info_msg_period", ROSLINK_MESSAGE_PERIOD.ROSLINK_GPS_RAW_INFO_MESSAGE_RATE)
ROSLinkBridgeTurtlebot.range_finder_data_msg_rate = rospy.get_param("/range_finder_data_msg_period", ROSLINK_MESSAGE_PERIOD.ROSLINK_RANGE_FINDER_DATA_MESSAGE_RATE)
Step 4: Declare ROSLink messages:
@staticmethod
def static_build_roslink_header_message(message_id):
header = ROSLinkHeader(ROSLinkStateVariables.roslink_version, ROSLinkStateVariables.ros_version, ROSLinkStateVariables.system_id, message_id, ROSLinkStateVariables.sequence_number,ROSLinkStateVariables.key)
ROSLinkStateVariables.sequence_number = ROSLinkStateVariables.sequence_number + 1
return header.__dict__
@staticmethod
def static_build_heartbeat_message():
message_header = ROSLinkBridgeTurtlebot.static_build_roslink_header_message(MESSAGE_TYPE.ROSLINK_MESSAGE_HEARTBEAT)
heartbeat_message = HeartBeat(message_header, ROBOT_TYPE.ROBOT_TYPE_TURTLEBOT, ROSLinkStateVariables.robot_name, ROBOT_STATE.ROBOT_STATE_ACTIVE, ROSLinkStateVariables.owner_id ,ROBOT_MODE.ROBOT_STATE_UNKNOWN)
return heartbeat_message.__dict__
@staticmethod
def static_build_robot_status_message():
message_header = ROSLinkBridgeTurtlebot.static_build_roslink_header_message(MESSAGE_TYPE.ROSLINK_MESSAGE_ROBOT_STATUS)
robot_status_message = HeartBeat(message_header, 0, ROSLinkStateVariables.robot_name, 0, 0 ,0)
return robot_status_message.__dict__
@staticmethod
def static_build_global_motion_message():
message_header = ROSLinkBridgeTurtlebot.static_build_roslink_header_message(MESSAGE_TYPE.ROSLINK_MESSAGE_GLOBAL_MOTION)
global_motion_message = GlobalMotion(message_header, ROSLinkStateVariables.time_boot_ms , ROSLinkStateVariables.x, ROSLinkStateVariables.y, ROSLinkStateVariables.yaw, ROSLinkStateVariables.vx, ROSLinkStateVariables.vy, ROSLinkStateVariables.vz, ROSLinkStateVariables.wx, ROSLinkStateVariables.wy, ROSLinkStateVariables.wz, ROSLinkStateVariables.pitch, ROSLinkStateVariables.roll, ROSLinkStateVariables.yaw)
return global_motion_message.__dict__
@staticmethod
def static_build_gps_raw_info_message():
message_header = ROSLinkBridgeTurtlebot.static_build_roslink_header_message(MESSAGE_TYPE.ROSLINK_MESSAGE_GPS_RAW_INFO)
global_motion_message = GPSRawInfo(message_header, ROSLinkStateVariables.time_boot_ms , ROSLinkStateVariables.fix_type, ROSLinkStateVariables.lat, ROSLinkStateVariables.lon, ROSLinkStateVariables.alt, ROSLinkStateVariables.eph, ROSLinkStateVariables.epv, ROSLinkStateVariables.vel, ROSLinkStateVariables.cog, ROSLinkStateVariables.satellites_visible)
return global_motion_message.__dict__
Step 5: Send ROSLink message: After subscribing to topics and getting their information, the ROSLink messages will be updated with these information and sent to the ROSLink proxy.
def run ( self ):
while True:
self.count=self.count+1
time.sleep(1.0/self.data_rate)
print 'thread %s %d\n'%(self.name, self.count)
#self.send(self.socket, json.dumps(self.roslink_message.__dict__))
if (self.roslink_message_type == MESSAGE_TYPE.ROSLINK_MESSAGE_HEARTBEAT):
self.send(self.socket, json.dumps(ROSLinkBridgeTurtlebot.static_build_heartbeat_message()))
elif (self.roslink_message_type == MESSAGE_TYPE.ROSLINK_MESSAGE_ROBOT_STATUS):
self.send(self.socket, json.dumps(ROSLinkBridgeTurtlebot.static_build_robot_status_message()))
elif (self.roslink_message_type == MESSAGE_TYPE.ROSLINK_MESSAGE_GLOBAL_MOTION):
self.send(self.socket, json.dumps(ROSLinkBridgeTurtlebot.static_build_global_motion_message()))
elif (self.roslink_message_type == MESSAGE_TYPE.ROSLINK_MESSAGE_GPS_RAW_INFO):
self.send(self.socket, json.dumps(ROSLinkBridgeTurtlebot.static_build_gps_raw_info_message()))
Step 6: Receive ROSLink message: The ROSLink Bridge will receive command messages from ROSLink proxy. After parsing the incoming ROSLink Message and extracting the command, the ROSLink Bridge publishes the command to the appropriate ROS topic or request the appropriate ROS service to execute the action. The ROSLink Bridge will know what topic to publish with message content from message_id field from ROSLink message.
class ROSLinkCommandProcessingThread ( ):
def __init__(self, sock,thread_name='noname'):
self.name = thread_name
self.socket = sock
t = threading.Thread(target=self.run)
t.setName(thread_name)
t.start()
def run ( self):
print "Start ROSLINK Command Processing Thread"
while True:
try:
msg, address = self.socket.recvfrom(MESSAGE_MAX_LENGTH)
ROSLinkBridgeTurtlebot.process_roslink_command_message(msg)
except socket.timeout:
continue
@staticmethod
def process_roslink_command_message(msg):
#print 'msg is ', msg
command = json.loads(msg)
print 'ROSLink command received ..'
print msg
if command['header']['message_id'] == MESSAGE_TYPE.ROSLINK_MESSAGE_COMMAND_TWIST:
print 'I received Twist command successfully'
TwistCommand = Twist()
TwistCommand.linear.x = command['vx']
TwistCommand.linear.y = command['vy']
TwistCommand.linear.z = command['vz']
TwistCommand.angular.x = command['wx']
TwistCommand.angular.y = command['wy']
TwistCommand.angular.z = command['wz']
print TwistCommand
ROSLinkBridgeTurtlebot.move_publisher.publish (TwistCommand)