Add robot_description support [cpp,cflib,sim] (#412)

* add robot_description support in cpp, cflib and sim backends

crazyflie/CMakeLists.txt

@@ -112,10 +112,11 @@ install(PROGRAMS
   DESTINATION lib/${PROJECT_NAME}
 )
 
-# Install launch and config files.
+# Install launch, config, and urdf files.
 install(DIRECTORY
   launch
   config
+  urdf
   DESTINATION share/${PROJECT_NAME}/
 )
 

crazyflie/config/config.rviz

@@ -52,21 +52,52 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: true
-        test:
+        cf231:
           Value: true
         world:
           Value: true
       Marker Scale: 1
       Name: TF
       Show Arrows: true
       Show Axes: true
-      Show Names: false
+      Show Names: true
       Tree:
         world:
-          test:
+          cf231:
             {}
       Update Interval: 0
       Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cf231/robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        cf231:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: CF231
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -113,25 +144,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 6.814720153808594
+      Distance: 3.1647839546203613
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
+        X: -0.028163839131593704
+        Y: -0.049007341265678406
+        Z: -0.025782346725463867
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.665398120880127
+      Pitch: 0.4847976565361023
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.8785719871520996
+      Yaw: 2.9185757637023926
     Saved: ~
 Window Geometry:
   Displays:

crazyflie/launch/launch.py

@@ -30,6 +30,15 @@ def generate_launch_description():
 
     server_params = [crazyflies] + [server_yaml_contents["/crazyflie_server"]["ros__parameters"]]
 
+    # robot description
+    urdf = os.path.join(
+        get_package_share_directory('crazyflie'),
+        'urdf',
+        'crazyflie_description.urdf')
+    with open(urdf, 'r') as f:
+        robot_desc = f.read()
+    server_params[1]["robot_description"] = robot_desc
+
     # construct motion_capture_configuration
     motion_capture_yaml = os.path.join(
         get_package_share_directory('crazyflie'),

crazyflie/scripts/crazyflie_server.py

@@ -30,6 +30,7 @@
 from motion_capture_tracking_interfaces.msg import NamedPoseArray
 
 from std_srvs.srv import Empty
+from std_msgs.msg import String
 from geometry_msgs.msg import Twist
 from geometry_msgs.msg import PoseStamped, TransformStamped
 from sensor_msgs.msg import LaserScan
@@ -220,6 +221,16 @@ def __init__(self):
 
         for uri in self.cf_dict:
             name = self.cf_dict[uri]
+
+            pub = self.create_publisher(String, name + '/robot_description',
+            rclpy.qos.QoSProfile(
+                depth=1,
+                durability=rclpy.qos.QoSDurabilityPolicy.TRANSIENT_LOCAL))
+
+            msg = String()
+            msg.data = self._ros_parameters['robot_description'].replace("$NAME", name)
+            pub.publish(msg)
+
             self.create_service(
                 Empty, name +
                 "/emergency", partial(self._emergency_callback, uri=uri)

crazyflie/src/crazyflie_server.cpp

@@ -12,6 +12,7 @@
 #include "crazyflie_interfaces/srv/land.hpp"
 #include "crazyflie_interfaces/srv/go_to.hpp"
 #include "crazyflie_interfaces/srv/notify_setpoints_stop.hpp"
+#include "std_msgs/msg/string.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
@@ -160,6 +161,15 @@ class CrazyflieROS
     subscription_cmd_full_state_ = node->create_subscription<crazyflie_interfaces::msg::FullState>(name + "/cmd_full_state", rclcpp::SystemDefaultsQoS(), std::bind(&CrazyflieROS::cmd_full_state_changed, this, _1), sub_opt_cf_cmd);
     subscription_cmd_position_ = node->create_subscription<crazyflie_interfaces::msg::Position>(name + "/cmd_position", rclcpp::SystemDefaultsQoS(), std::bind(&CrazyflieROS::cmd_position_changed, this, _1), sub_opt_cf_cmd);
 
+    publisher_robot_description_ = node->create_publisher<std_msgs::msg::String>(name + "/robot_description",
+      rclcpp::QoS(1).transient_local());
+    {
+      auto msg = std::make_unique<std_msgs::msg::String>();
+      auto robot_desc = node->get_parameter("robot_description").get_parameter_value().get<std::string>();
+      msg->data = std::regex_replace(robot_desc, std::regex("\\$NAME"), name);
+      publisher_robot_description_->publish(std::move(msg));
+    }
+
     // spinning timer
     // used to process all incoming radio messages
     spin_timer_ =
@@ -871,6 +881,8 @@ class CrazyflieROS
   rclcpp::Subscription<crazyflie_interfaces::msg::FullState>::SharedPtr subscription_cmd_full_state_;
   rclcpp::Subscription<crazyflie_interfaces::msg::Position>::SharedPtr subscription_cmd_position_;
 
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_robot_description_;
+
   // logging
   std::unique_ptr<LogBlock<logPose>> log_block_pose_;
   rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr publisher_pose_;
@@ -953,6 +965,8 @@ class CrazyflieServer : public rclcpp::Node
     broadcasts_delay_between_repeats_ms_ = this->get_parameter("all.broadcasts.delay_between_repeats_ms").get_parameter_value().get<int>();
     mocap_enabled_ = false;
 
+    this->declare_parameter("robot_description", "");
+
     // Warnings
     this->declare_parameter("warnings.frequency", 1.0);
     float freq = this->get_parameter("warnings.frequency").get_parameter_value().get<float>();