.. _roslink-parrot: =========================== 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:** .. code-block:: Python #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:** .. code-block:: Python @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:** .. code-block:: Python @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``. .. code-block:: Python 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. .. code-block:: Python 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)