ROSLink Bridge for Ar Drone¶

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

To build a ROSLinkBridge for a drone, we identify the following actions - while other could also be defined - including: (1) take-off, (2) land, (3) move. 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. The take-off action is mapped to the /ardrone/takeoff topic in ROS. The landing action is mapped to the /ardrone/land topic. A ROS message of type std\_msgs must be published on both take-off and landing topics to execute the action. To command the robot to move or fly, a Twist message must be published on cmd\_vel topic.

Step 1: Identify required topics:

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

/ardrone/takeoff, std_msgs message

/ardrone/land, std_msgs message

/ardrone/reset, std_msgs message

/cmd_vel, Twist message

And we need to subscribe in following topics:

/ground_truth/state, Odometry message

/ardrone/navdata, Navdata

/ardrone/front/image_raw/compressed, CompressedImage 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
    ROSLinkBridgeARDrone.takeoff_publisher = rospy.Publisher('/ardrone/takeoff', Empty, queue_size=10)
    ROSLinkBridgeARDrone.land_publisher    = rospy.Publisher('/ardrone/land', Empty, queue_size=10)
    ROSLinkBridgeARDrone.reset_publisher   = rospy.Publisher('/ardrone/reset',Empty, queue_size=10)
    ROSLinkBridgeARDrone.move_publisher    = rospy.Publisher('/cmd_vel',Twist, queue_size=10)

#start ROS subscribers
@staticmethod
def start_ros_subscribers():
    rospy.Subscriber("/ground_truth/state", Odometry, ROSLinkBridgeARDrone.odometryCallback)
    rospy.Subscriber("/ardrone/navdata", Navdata, ROSLinkBridgeARDrone.navdataCallback)
    rospy.Subscriber("/ardrone/front/image_raw/compressed", CompressedImage, ROSLinkBridgeARDrone.frontCompressedImageCallback)

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", "ARDRONE")
    # get robot type
    ROSLinkStateVariables.type = rospy.get_param("/type", ROBOT_TYPE.ROBOT_TYPE_PARROT)
    # 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\n\n'
           ROSLinkBridgeARDrone.takeoff_publisher.publish(Empty())

       elif command['header']['message_id'] == MESSAGE_TYPE.ROSLINK_MESSAGE_COMMAND_LAND:
           print 'I received Land command'
           print '\n\nThe robot is landing\n\n'
           ROSLinkBridgeARDrone.land_publisher.publish(Empty())

       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
           ROSLinkBridgeARDrone.move_publisher.publish (TwistCommand)