achieved d435 rgbd camera simulation

controllers/unitree_guide_controller/src/UnitreeGuideController.cpp

@@ -7,18 +7,25 @@
 #include <unitree_guide_controller/gait/WaveGenerator.h>
 #include "unitree_guide_controller/robot/QuadrupedRobot.h"
 
-namespace unitree_guide_controller {
+namespace unitree_guide_controller
+{
     using config_type = controller_interface::interface_configuration_type;
 
-    controller_interface::InterfaceConfiguration UnitreeGuideController::command_interface_configuration() const {
+    controller_interface::InterfaceConfiguration UnitreeGuideController::command_interface_configuration() const
+    {
         controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
 
         conf.names.reserve(joint_names_.size() * command_interface_types_.size());
-        for (const auto &joint_name: joint_names_) {
+        for (const auto& joint_name : joint_names_)
-            for (const auto &interface_type: command_interface_types_) {
+        {
-                if (!command_prefix_.empty()) {
+            for (const auto& interface_type : command_interface_types_)
+            {
+                if (!command_prefix_.empty())
+                {
                     conf.names.push_back(command_prefix_ + "/" + joint_name + "/" += interface_type);
-                } else {
+                }
+                else
+                {
                     conf.names.push_back(joint_name + "/" += interface_type);
                 }
             }
@@ -27,17 +34,21 @@ namespace unitree_guide_controller {
         return conf;
     }
 
-    controller_interface::InterfaceConfiguration UnitreeGuideController::state_interface_configuration() const {
+    controller_interface::InterfaceConfiguration UnitreeGuideController::state_interface_configuration() const
+    {
         controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
 
         conf.names.reserve(joint_names_.size() * state_interface_types_.size());
-        for (const auto &joint_name: joint_names_) {
+        for (const auto& joint_name : joint_names_)
-            for (const auto &interface_type: state_interface_types_) {
+        {
+            for (const auto& interface_type : state_interface_types_)
+            {
                 conf.names.push_back(joint_name + "/" += interface_type);
             }
         }
 
-        for (const auto &interface_type: imu_interface_types_) {
+        for (const auto& interface_type : imu_interface_types_)
+        {
             conf.names.push_back(imu_name_ + "/" += interface_type);
         }
 
@@ -45,7 +56,8 @@ namespace unitree_guide_controller {
     }
 
     controller_interface::return_type UnitreeGuideController::
-    update(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
+    update(const rclcpp::Time& time, const rclcpp::Duration& period)
+    {
         // auto now = std::chrono::steady_clock::now();
         // std::chrono::duration<double> time_diff = now - last_update_time_;
         // last_update_time_ = now;
@@ -54,7 +66,8 @@ namespace unitree_guide_controller {
         // update_frequency_ = 1.0 / time_diff.count();
         // RCLCPP_INFO(get_node()->get_logger(), "Update frequency: %f Hz", update_frequency_);
 
-        if (ctrl_component_.robot_model_ == nullptr) {
+        if (ctrl_component_.robot_model_ == nullptr)
+        {
             return controller_interface::return_type::OK;
         }
 
@@ -62,16 +75,20 @@ namespace unitree_guide_controller {
         ctrl_component_.wave_generator_->update();
         ctrl_component_.estimator_->update();
 
-        if (mode_ == FSMMode::NORMAL) {
+        if (mode_ == FSMMode::NORMAL)
-            current_state_->run();
+        {
+            current_state_->run(time, period);
             next_state_name_ = current_state_->checkChange();
-            if (next_state_name_ != current_state_->state_name) {
+            if (next_state_name_ != current_state_->state_name)
+            {
                 mode_ = FSMMode::CHANGE;
                 next_state_ = getNextState(next_state_name_);
                 RCLCPP_INFO(get_node()->get_logger(), "Switched from %s to %s",
                             current_state_->state_name_string.c_str(), next_state_->state_name_string.c_str());
             }
-        } else if (mode_ == FSMMode::CHANGE) {
+        }
+        else if (mode_ == FSMMode::CHANGE)
+        {
             current_state_->exit();
             current_state_ = next_state_;
 
@@ -82,25 +99,27 @@ namespace unitree_guide_controller {
         return controller_interface::return_type::OK;
     }
 
-    controller_interface::CallbackReturn UnitreeGuideController::on_init() {
+    controller_interface::CallbackReturn UnitreeGuideController::on_init()
-        try {
+    {
-            joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_);
+        try
+        {
+            joint_names_ = auto_declare<std::vector<std::string>>("joints", joint_names_);
             command_interface_types_ =
-                    auto_declare<std::vector<std::string> >("command_interfaces", command_interface_types_);
+                auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
             state_interface_types_ =
-                    auto_declare<std::vector<std::string> >("state_interfaces", state_interface_types_);
+                auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);
 
             // imu sensor
             imu_name_ = auto_declare<std::string>("imu_name", imu_name_);
             base_name_ = auto_declare<std::string>("base_name", base_name_);
-            imu_interface_types_ = auto_declare<std::vector<std::string> >("imu_interfaces", state_interface_types_);
+            imu_interface_types_ = auto_declare<std::vector<std::string>>("imu_interfaces", state_interface_types_);
             command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
             feet_names_ =
-                    auto_declare<std::vector<std::string> >("feet_names", feet_names_);
+                auto_declare<std::vector<std::string>>("feet_names", feet_names_);
 
             // pose parameters
-            down_pos_ = auto_declare<std::vector<double> >("down_pos", down_pos_);
+            down_pos_ = auto_declare<std::vector<double>>("down_pos", down_pos_);
-            stand_pos_ = auto_declare<std::vector<double> >("stand_pos", stand_pos_);
+            stand_pos_ = auto_declare<std::vector<double>>("stand_pos", stand_pos_);
             stand_kp_ = auto_declare<double>("stand_kp", stand_kp_);
             stand_kd_ = auto_declare<double>("stand_kd", stand_kd_);
 
@@ -108,7 +127,9 @@ namespace unitree_guide_controller {
             RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_interfaces_.frequency_);
 
             ctrl_component_.estimator_ = std::make_shared<Estimator>(ctrl_interfaces_, ctrl_component_);
-        } catch (const std::exception &e) {
+        }
+        catch (const std::exception& e)
+        {
             fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
             return controller_interface::CallbackReturn::ERROR;
         }
@@ -117,9 +138,11 @@ namespace unitree_guide_controller {
     }
 
     controller_interface::CallbackReturn UnitreeGuideController::on_configure(
-        const rclcpp_lifecycle::State & /*previous_state*/) {
+        const rclcpp_lifecycle::State& /*previous_state*/)
+    {
         control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
-            "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
+            "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg)
+            {
                 // Handle message
                 ctrl_interfaces_.control_inputs_.command = msg->command;
                 ctrl_interfaces_.control_inputs_.lx = msg->lx;
@@ -130,7 +153,8 @@ namespace unitree_guide_controller {
 
         robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(
             "/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
-            [this](const std_msgs::msg::String::SharedPtr msg) {
+            [this](const std_msgs::msg::String::SharedPtr msg)
+            {
                 ctrl_component_.robot_model_ = std::make_shared<QuadrupedRobot>(
                     ctrl_interfaces_, msg->data, feet_names_, base_name_);
                 ctrl_component_.balance_ctrl_ = std::make_shared<BalanceCtrl>(ctrl_component_.robot_model_);
@@ -142,25 +166,34 @@ namespace unitree_guide_controller {
     }
 
     controller_interface::CallbackReturn
-    UnitreeGuideController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
+    UnitreeGuideController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
+    {
         // clear out vectors in case of restart
         ctrl_interfaces_.clear();
 
         // assign command interfaces
-        for (auto &interface: command_interfaces_) {
+        for (auto& interface : command_interfaces_)
+        {
             std::string interface_name = interface.get_interface_name();
-            if (const size_t pos = interface_name.find('/'); pos != std::string::npos) {
+            if (const size_t pos = interface_name.find('/'); pos != std::string::npos)
+            {
                 command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface);
-            } else {
+            }
+            else
+            {
                 command_interface_map_[interface_name]->push_back(interface);
             }
         }
 
         // assign state interfaces
-        for (auto &interface: state_interfaces_) {
+        for (auto& interface : state_interfaces_)
-            if (interface.get_prefix_name() == imu_name_) {
+        {
+            if (interface.get_prefix_name() == imu_name_)
+            {
                 ctrl_interfaces_.imu_state_interface_.emplace_back(interface);
-            } else {
+            }
+            else
+            {
                 state_interface_map_[interface.get_interface_name()]->push_back(interface);
             }
         }
@@ -185,46 +218,52 @@ namespace unitree_guide_controller {
     }
 
     controller_interface::CallbackReturn UnitreeGuideController::on_deactivate(
-        const rclcpp_lifecycle::State & /*previous_state*/) {
+        const rclcpp_lifecycle::State& /*previous_state*/)
+    {
         release_interfaces();
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn
-    UnitreeGuideController::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) {
+    UnitreeGuideController::on_cleanup(const rclcpp_lifecycle::State& /*previous_state*/)
+    {
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn
-    UnitreeGuideController::on_error(const rclcpp_lifecycle::State & /*previous_state*/) {
+    UnitreeGuideController::on_error(const rclcpp_lifecycle::State& /*previous_state*/)
+    {
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn
-    UnitreeGuideController::on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) {
+    UnitreeGuideController::on_shutdown(const rclcpp_lifecycle::State& /*previous_state*/)
+    {
         return CallbackReturn::SUCCESS;
     }
 
-    std::shared_ptr<FSMState> UnitreeGuideController::getNextState(const FSMStateName stateName) const {
+    std::shared_ptr<FSMState> UnitreeGuideController::getNextState(const FSMStateName stateName) const
-        switch (stateName) {
+    {
-            case FSMStateName::INVALID:
+        switch (stateName)
-                return state_list_.invalid;
+        {
-            case FSMStateName::PASSIVE:
+        case FSMStateName::INVALID:
-                return state_list_.passive;
+            return state_list_.invalid;
-            case FSMStateName::FIXEDDOWN:
+        case FSMStateName::PASSIVE:
-                return state_list_.fixedDown;
+            return state_list_.passive;
-            case FSMStateName::FIXEDSTAND:
+        case FSMStateName::FIXEDDOWN:
-                return state_list_.fixedStand;
+            return state_list_.fixedDown;
-            case FSMStateName::FREESTAND:
+        case FSMStateName::FIXEDSTAND:
-                return state_list_.freeStand;
+            return state_list_.fixedStand;
-            case FSMStateName::TROTTING:
+        case FSMStateName::FREESTAND:
-                return state_list_.trotting;
+            return state_list_.freeStand;
-            case FSMStateName::SWINGTEST:
+        case FSMStateName::TROTTING:
-                return state_list_.swingTest;
+            return state_list_.trotting;
-            case FSMStateName::BALANCETEST:
+        case FSMStateName::SWINGTEST:
-                return state_list_.balanceTest;
+            return state_list_.swingTest;
-            default:
+        case FSMStateName::BALANCETEST:
-                return state_list_.invalid;
+            return state_list_.balanceTest;
+        default:
+            return state_list_.invalid;
         }
     }
 }

descriptions/unitree/go2_description/xacro/gazebo.xacro

@@ -261,6 +261,12 @@
 
   <xacro:arg name="EXTERNAL_SENSORS" default="false"/>
   <xacro:if value="$(arg EXTERNAL_SENSORS)">
+    <xacro:include filename="$(find gz_quadruped_playground)/models/D435/model.xacro"/>
+
+    <xacro:d435 camID="1" name="d435">
+      <origin rpy="0 0.1 0" xyz="0.28 0 0.093"/>
+    </xacro:d435>
+
     <gazebo reference="front_camera">
       <sensor name="front_camera" type="camera">
         <camera>
@@ -316,7 +322,7 @@
             <resolution>0.01</resolution>
           </range>
         </lidar>
-        <alwaysOn>1</alwaysOn>
+        <always_on>true</always_on>
         <visualize>true</visualize>
       </sensor>
     </gazebo>

descriptions/unitree/go2_description/xacro/robot.xacro

@@ -122,7 +122,7 @@
     </collision>
   </link>
 
-  <joint name="front_camera_joint" type="fixed">
+  <joint name="camera_joint_front" type="fixed">
     <parent link="trunk"/>
     <child link="front_camera"/>
     <origin rpy="0 0 0" xyz="0.32715 0 0.04297"/>

hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro

@@ -4,7 +4,7 @@
   <xacro:macro name="foot_force_sensor" params="name">
     <gazebo reference="${name}_foot_fixed">
       <!-- Enable feedback for this joint -->
-      <provideFeedback>true</provideFeedback>
+      <provide_feedback>true</provide_feedback>
       <!-- Prevent ros2_control from lumping this fixed joint with others -->
       <disableFixedJointLumping>true</disableFixedJointLumping>
       <sensor name="${name}_foot_force" type="force_torque">

libraries/gz_quadruped_playground/CMakeLists.txt

@@ -4,7 +4,7 @@ project(gz_quadruped_playground)
 find_package(ament_cmake REQUIRED)
 
 install(
-        DIRECTORY worlds launch config
+        DIRECTORY worlds launch config models
         DESTINATION share/${PROJECT_NAME}/
 )
 

libraries/gz_quadruped_playground/README.md

@@ -37,3 +37,4 @@ colcon build --packages-up-to gz_quadruped_playground --symlink-install
 ## Related Materials
 
 * [Gazebo OdometryPublisher Plugin](https://gazebosim.org/api/sim/8/classgz_1_1sim_1_1systems_1_1OdometryPublisher.html#details)
+* [Gazebo Intel Realsense D435 RGBD camera](https://app.gazebosim.org/OpenRobotics/fuel/models/Intel%20RealSense%20D435)

libraries/gz_quadruped_playground/launch/gazebo.launch.py

@@ -122,6 +122,7 @@ def generate_launch_description():
             "/camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo",
             "/scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan",
             "/scan/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked",
+            "/rgbd_d435/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked"
             # "/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry",
             # "/odom_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance",
             # "/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V"
@@ -137,6 +138,8 @@ def generate_launch_description():
         executable="image_bridge",
         arguments=[
             "/camera/image",
+            '/rgbd_d435/depth_image',
+            '/rgbd_d435/image',
         ],
         output="screen",
         parameters=[

libraries/gz_quadruped_playground/models/D435/materials/scripts/model.material

@@ -0,0 +1,14 @@
+
+material UrbanTile/RealSense_Diffuse
+{
+  technique
+  {
+    pass
+    {
+      texture_unit
+      {
+        texture RealSense_Albedo.png
+        }
+      }
+    }
+  }

libraries/gz_quadruped_playground/models/D435/model.config

@@ -0,0 +1,16 @@
+<?xml version='1.0'?>
+<model>
+  <name>Intel RealSense D435</name>
+  <version>1.0</version>
+  <sdf version='1.6'>model.sdf</sdf>
+
+  <author>
+    <name>Cole Biesemeyer</name>
+    <email>cole@openrobotics.org</email>
+  </author>
+
+  <description>
+    Intel RealSense D435.
+  </description>
+</model>
+    

libraries/gz_quadruped_playground/models/D435/model.sdf

@@ -0,0 +1,103 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <model name="realsense_d435">
+    <link name="link">
+      <inertial>
+        <mass>0.0615752</mass>
+        <inertia>
+          <ixx>9.108e-05</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>2.51e-06</iyy>
+          <iyz>0</iyz>
+          <izz>8.931e-05</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <collision name="collision">
+        <geometry>
+          <mesh>
+            <uri>model://RealSense_D435/meshes/realsense.dae</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name= "visual">
+        <geometry>
+          <mesh>
+            <uri>model://RealSense_D435/meshes/realsense.dae</uri>
+            <submesh>
+              <name>RealSense</name>
+              <center>false</center>
+            </submesh>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>1.0 1.0 1.0</diffuse>
+          <specular>1.0 1.0 1.0</specular>
+          <pbr>
+            <metal>
+              <albedo_map>model://RealSense_D435/materials/textures/RealSense_Albedo.png</albedo_map>
+              <normal_map>model://RealSense_D435/materials/textures/RealSense_Normal.png</normal_map>
+              <metalness_map>model://RealSense_D435/materials/textures/RealSense_Metalness.png</metalness_map>
+              <roughness_map>model://RealSense_D435/materials/textures/RealSense_Roughness.png</roughness_map>
+            </metal>
+          </pbr>
+          <!-- fallback to script if no PBR support-->
+          <script>
+            <uri>model://RealSense_D435/materials/scripts/</uri>
+            <uri>model://RealSense_D435/materials/textures/</uri>
+            <name>UrbanTile/RealSense_Diffuse</name>
+          </script>
+        </material>
+      </visual>
+
+      <sensor name="realsense_d435" type="rgbd_camera">
+        <update_rate>60</update_rate>
+        <camera name="camera">
+          <horizontal_fov>1.0472</horizontal_fov>
+          <lens>
+            <intrinsics>
+              <!-- fx = fy = width / ( 2 * tan (hfov / 2 ) ) -->
+              <fx>554.25469</fx>
+              <fy>554.25469</fy>
+              <!-- cx = ( width + 1 ) / 2 -->
+              <cx>320.5</cx>
+              <!-- cy = ( height + 1 ) / 2 -->
+              <cy>240.5</cy>
+              <s>0</s>
+            </intrinsics>
+          </lens>
+          <distortion>
+            <k1>0.0</k1>
+            <k2>0.0</k2>
+            <k3>0.0</k3>
+            <p1>0.0</p1>
+            <p2>0.0</p2>
+            <center>0.5 0.5</center>
+          </distortion>
+          <image>
+            <width>640</width>
+            <height>480</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.01</near>
+            <far>300</far>
+          </clip>
+          <depth_camera>
+            <clip>
+              <near>0.1</near>
+              <far>10</far>
+            </clip>
+          </depth_camera>
+          <noise>
+            <type>gaussian</type>
+            <mean>0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+      </sensor>
+
+    </link>
+  </model>
+</sdf>

libraries/gz_quadruped_playground/models/D435/model.xacro

@@ -0,0 +1,90 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="d435" params="camID name *origin">
+    <joint name="camera_joint_${name}" type="fixed">
+      <xacro:insert_block name="origin"/>
+      <parent link="trunk"/>
+      <child link="camera_${name}"/>
+    </joint>
+
+    <link name="camera_${name}">
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <box size=".001 .001 .001"/>
+        </geometry>
+      </collision>
+      <inertial>
+        <mass value="0.0615752"/>
+        <inertia ixx="9.108e-05" ixy="0" ixz="0" iyy="2.51e-06" iyz="0" izz="8.931e-05"/>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+      </inertial>
+
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 1.57"/>
+        <geometry>
+          <mesh filename="file://$(find gz_quadruped_playground)/models/D435/meshes/realsense.dae" scale="1 1 1"/>
+        </geometry>
+      </visual>
+      <material>
+        <diffuse>1.0 1.0 1.0</diffuse>
+        <specular>1.0 1.0 1.0</specular>
+      </material>
+    </link>
+
+    <gazebo reference="camera_${name}">
+      <sensor name="realsense_d435" type="rgbd_camera">
+        <camera name="d435">
+          <horizontal_fov>1.0472</horizontal_fov>
+          <lens>
+            <intrinsics>
+              <fx>554.25469</fx>
+              <fy>554.25469</fy>
+              <cx>320.5</cx>
+              <cy>240.5</cy>
+              <s>0</s>
+            </intrinsics>
+          </lens>
+          <distortion>
+            <k1>0.0</k1>
+            <k2>0.0</k2>
+            <k3>0.0</k3>
+            <p1>0.0</p1>
+            <p2>0.0</p2>
+            <center>0.5 0.5</center>
+          </distortion>
+          <image>
+            <width>640</width>
+            <height>480</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.01</near>
+            <far>300</far>
+          </clip>
+          <depth_camera>
+            <clip>
+              <near>0.1</near>
+              <far>10</far>
+            </clip>
+          </depth_camera>
+          <noise>
+            <type>gaussian</type>
+            <mean>0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+          <optical_frame_id>camera_${name}</optical_frame_id>
+        </camera>
+        <always_on>true</always_on>
+        <update_rate>15</update_rate>
+        <visualize>true</visualize>
+        <topic>rgbd_${name}</topic>
+        <plugin
+            filename="RosGzPointCloud"
+            name="ros_gz_point_cloud::PointCloud">
+        </plugin>
+      </sensor>
+    </gazebo>
+  </xacro:macro>
+</robot>