EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

achieved d435 rgbd camera simulation

This commit is contained in:
Huang Zhenbiao 2025-03-01 12:03:54 +08:00
parent dc32d8c281
commit 5e6a9d0b9c
20 changed files with 2087 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

123
controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
View File

@ -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 &interface_type: command_interface_types_) {
if (!command_prefix_.empty()) {
for (const auto& joint_name : joint_names_)
{
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 &interface_type: state_interface_types_) {
for (const auto& joint_name : joint_names_)
{
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) {
current_state_->run();
if (mode_ == FSMMode::NORMAL)
{
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() {
try {
joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_);
controller_interface::CallbackReturn UnitreeGuideController::on_init()
{
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_);
stand_pos_ = auto_declare<std::vector<double> >("stand_pos", stand_pos_);
down_pos_ = auto_declare<std::vector<double>>("down_pos", down_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_) {
if (interface.get_prefix_name() == imu_name_) {
for (auto& interface : state_interfaces_)
{
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,28 +218,34 @@ 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 {
switch (stateName) {
std::shared_ptr<FSMState> UnitreeGuideController::getNextState(const FSMStateName stateName) const
{
switch (stateName)
{
case FSMStateName::INVALID:
return state_list_.invalid;
case FSMStateName::PASSIVE:

8
descriptions/unitree/go2_description/xacro/gazebo.xacro
View File

@ -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>

2
descriptions/unitree/go2_description/xacro/robot.xacro
View File

@ -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"/>

2
hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro
View File

@ -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">

2
libraries/gz_quadruped_playground/CMakeLists.txt
View File

@ -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}/
)

1
libraries/gz_quadruped_playground/README.md
View File

@ -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)

3
libraries/gz_quadruped_playground/launch/gazebo.launch.py
View File

@ -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=[

14
libraries/gz_quadruped_playground/models/D435/materials/scripts/model.material Normal file
View File

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

BIN
libraries/gz_quadruped_playground/models/D435/materials/textures/RealSense_Albedo.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 317 KiB

BIN
libraries/gz_quadruped_playground/models/D435/materials/textures/RealSense_Metalness.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 197 KiB

BIN
libraries/gz_quadruped_playground/models/D435/materials/textures/RealSense_Normal.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 453 KiB

BIN
libraries/gz_quadruped_playground/models/D435/materials/textures/RealSense_Roughness.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.4 MiB

1750
libraries/gz_quadruped_playground/models/D435/meshes/realsense.dae Normal file
View File

File diff suppressed because one or more lines are too long

16
libraries/gz_quadruped_playground/models/D435/model.config Normal file
View File

@ -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>

103
libraries/gz_quadruped_playground/models/D435/model.sdf Normal file
View File

@ -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>

90
libraries/gz_quadruped_playground/models/D435/model.xacro Normal file
View File

@ -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>

BIN
libraries/gz_quadruped_playground/models/D435/thumbnails/1.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 46 KiB

BIN
libraries/gz_quadruped_playground/models/D435/thumbnails/2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 28 KiB

BIN
libraries/gz_quadruped_playground/models/D435/thumbnails/3.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 50 KiB

BIN
libraries/gz_quadruped_playground/models/D435/thumbnails/4.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 24 KiB