ROSLink Bridge for Gapter¶

Note

Learn ROS and get your ROS certificate by enrolling in the Udemy course (Highest Rated course): ROS for Beginners: Basics, Motion and OpenCV.

Learn ROS-Navigation and get your ROS-Navigation certificate by enrolling in the Udemy course (Highest Rated course): ROS for Beginners II: Localization, Navigation and SLAM.

Learn about ROS2: ROS Next Generation by enrolling in the Udemy course ROS2 How To: Discover Next Generation ROS

Step 1: Identify required topics:

For AR Drone we need to publish in following topics for executing ROSLink commands:

/mavros/setpoint_velocity/cmd_vel, TwistStamped message

And we need to subscribe in following topics:

/mavros/global_position/raw/fix, NavSatFix 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
    ROSLinkBridgeGapter.move_publisher = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)

#start ROS subscribers
@staticmethod
def start_ros_subscribers():
    rospy.Subscriber("/mavros/global_position/raw/fix", NavSatFix, ROSLinkBridgeGapter.globalPositionCallback)

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", "Gapter")
    # get robot type
    ROSLinkStateVariables.type = rospy.get_param("/type", ROBOT_TYPE.ROBOT_TYPE_GENERIC)
    # 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
    ROSLinkBridgeARDrone.heartbeat_msg_rate = rospy.get_param("/heartbeat_msg_period", ROSLINK_MESSAGE_PERIOD.ROSLINK_HEARTBEAT_MESSAGE_RATE)
    ROSLinkBridgeARDrone.robot_status_msg_rate = rospy.get_param("/robot_status_msg_period", ROSLINK_MESSAGE_PERIOD.ROSLINK_ROBOT_STATUS_MESSAGE_RATE)
    ROSLinkBridgeARDrone.global_motion_msg_rate = rospy.get_param("/global_motion_msg_period", ROSLINK_MESSAGE_PERIOD.ROSLINK_GLOBAL_MOTION_MESSAGE_RATE)
    ROSLinkBridgeARDrone.gps_raw_info_msg_rate = rospy.get_param("/gps_raw_info_msg_period", ROSLINK_MESSAGE_PERIOD.ROSLINK_GPS_RAW_INFO_MESSAGE_RATE)
    ROSLinkBridgeARDrone.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 = ROSLinkBridgeARDrone.static_build_roslink_header_message(MESSAGE_TYPE.ROSLINK_MESSAGE_HEARTBEAT)
    heartbeat_message = HeartBeat(message_header, ROBOT_TYPE.ROBOT_TYPE_QUADROTOR, 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 = ROSLinkBridgeARDrone.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 = ROSLinkBridgeARDrone.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 = ROSLinkBridgeARDrone.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(ROSLinkBridgeARDrone.static_build_heartbeat_message()))
        elif (self.roslink_message_type == MESSAGE_TYPE.ROSLINK_MESSAGE_ROBOT_STATUS):
            self.send(self.socket, json.dumps(ROSLinkBridgeARDrone.static_build_robot_status_message()))
        elif (self.roslink_message_type == MESSAGE_TYPE.ROSLINK_MESSAGE_GLOBAL_MOTION):
            self.send(self.socket, json.dumps(ROSLinkBridgeARDrone.static_build_global_motion_message()))
        elif (self.roslink_message_type == MESSAGE_TYPE.ROSLINK_MESSAGE_GPS_RAW_INFO):
            self.send(self.socket, json.dumps(ROSLinkBridgeARDrone.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)
               ROSLinkBridgeARDrone.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_TAKEOFF:
           print 'I received Takeoff command'
           print '\n\nThe robot is Taking off with altitude',command['altitude']  , '\n\n'

           ROSLinkBridgeGapter.setTakeoffMode(command['altitude'])

       elif command['header']['message_id'] == MESSAGE_TYPE.ROSLINK_MESSAGE_COMMAND_LAND:
           print 'I received Land command'
           print '\n\nThe robot is landing\n\n'

           ROSLinkBridgeGapter.setLandMode()