diff --git a/example_11/bringup/config/carlikebot_controllers.yaml b/example_11/bringup/config/carlikebot_controllers.yaml
index d0bca8c56..bd2c9d9e0 100644
--- a/example_11/bringup/config/carlikebot_controllers.yaml
+++ b/example_11/bringup/config/carlikebot_controllers.yaml
@@ -23,6 +23,7 @@ ackermann_steering_controller:
odom_frame_id: odom
base_frame_id: base_link
enable_odom_tf: true
+ in_chained_mode: false
wheelbase: 0.325
front_wheel_track: 0.2
diff --git a/example_11/description/ros2_control/carlikebot.ros2_control.xacro b/example_11/description/ros2_control/carlikebot.ros2_control.xacro
index fcbb59014..5ef7801a4 100644
--- a/example_11/description/ros2_control/carlikebot.ros2_control.xacro
+++ b/example_11/description/ros2_control/carlikebot.ros2_control.xacro
@@ -20,12 +20,10 @@
-
-
diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp
index 7303e1390..3c3e6aab4 100644
--- a/example_11/hardware/carlikebot_system.cpp
+++ b/example_11/hardware/carlikebot_system.cpp
@@ -119,7 +119,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
return hardware_interface::CallbackReturn::ERROR;
}
- if (joint.state_interfaces.size() != 2)
+ if (joint.state_interfaces.size() != 1)
{
RCLCPP_FATAL(
rclcpp::get_logger("CarlikeBotSystemHardware"),
@@ -128,16 +128,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
return hardware_interface::CallbackReturn::ERROR;
}
- if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
- {
- RCLCPP_FATAL(
- rclcpp::get_logger("CarlikeBotSystemHardware"),
- "Joint '%s' has %s state interface. '%s' expected.", joint.name.c_str(),
- joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
- return hardware_interface::CallbackReturn::ERROR;
- }
-
- if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY)
+ if (joint.state_interfaces[0].name != hardware_interface::HW_IF_VELOCITY)
{
RCLCPP_FATAL(
rclcpp::get_logger("CarlikeBotSystemHardware"),
@@ -154,9 +145,26 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
hw_stop_sec_ = std::stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
// // END: This part here is for exemplary purposes - Please do not copy to your production code
- hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN());
- hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN());
- hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN());
+ // joint name, state, command
+ hw_interfaces_.resize(4, std::make_tuple(
+ std::string(),
+ std::numeric_limits::quiet_NaN(),
+ std::numeric_limits::quiet_NaN()
+ )
+ );
+
+
+
+ // hw_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN());
+ // hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN());
+ // hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN());
+
+ // hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN());
+ // hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN());
+
+ // hw_position_states_.resize(2, std::numeric_limits::queit_NaN());
+ // hw_velocity_states_.resize(2, std::numeric_limits::queit_NaN());
+
return hardware_interface::CallbackReturn::SUCCESS;
}
@@ -169,13 +177,28 @@ std::vector CarlikeBotSystemHardware::export
{
bool joint_is_steering = info_.joints[i].name.find("steering") != std::string::npos;
- state_interfaces.emplace_back(hardware_interface::StateInterface(
- info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i]));
+ // if joint name not in hw_interfaces_, add it
+ if (std::get<0>(hw_interfaces_[i]).empty())
+ {
+ std::get<0>(hw_interfaces_[i]) = info_.joints[i].name;
+ }
+
+ // get the index of the joint name in hw_interfaces_
+ auto it = std::find_if(
+ hw_interfaces_.begin(), hw_interfaces_.end(),
+ [this, i](const std::tuple & element) {
+ return std::get<0>(element) == info_.joints[i].name;
+ });
- if (!joint_is_steering)
+ if (joint_is_steering)
+ {
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ info_.joints[i].name, hardware_interface::HW_IF_POSITION, &std::get<1>(*it)));
+ }
+ else
{
state_interfaces.emplace_back(hardware_interface::StateInterface(
- info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i]));
+ info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &std::get<1>(*it)));
}
}
@@ -202,15 +225,28 @@ CarlikeBotSystemHardware::export_command_interfaces()
{
bool joint_is_steering = info_.joints[i].name.find("steering") != std::string::npos;
+ // if joint name not in hw_interfaces_, add it
+ if (std::get<0>(hw_interfaces_[i]).empty())
+ {
+ std::get<0>(hw_interfaces_[i]) = info_.joints[i].name;
+ }
+
+ // get the index of the joint name in hw_interfaces_
+ auto it = std::find_if(
+ hw_interfaces_.begin(), hw_interfaces_.end(),
+ [this, i](const std::tuple & element) {
+ return std::get<0>(element) == info_.joints[i].name;
+ });
+
if (joint_is_steering)
{
command_interfaces.emplace_back(hardware_interface::CommandInterface(
- info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
+ info_.joints[i].name, hardware_interface::HW_IF_POSITION, &std::get<2>(*it)));
}
else
{
command_interfaces.emplace_back(hardware_interface::CommandInterface(
- info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i]));
+ info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &std::get<2>(*it)));
}
}
@@ -240,11 +276,10 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate(
rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i);
}
- for (uint i = 0; i < hw_positions_.size(); i++)
+ for (uint i = 0; i < hw_interfaces_.size(); i++)
{
- hw_positions_[i] = 0.0;
- hw_velocities_[i] = 0.0;
- hw_commands_[i] = 0.0;
+ std::get<1>(hw_interfaces_[i]) = 0.0;
+ std::get<2>(hw_interfaces_[i]) = 0.0;
}
RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully activated!");
@@ -274,17 +309,26 @@ hardware_interface::return_type CarlikeBotSystemHardware::read(
const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
{
// BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
- for (std::size_t i = 0; i < hw_velocities_.size(); ++i)
+ for (std::size_t i = 0; i < hw_interfaces_.size(); ++i)
{
- // Simulate DiffBot wheels's movement as a first-order system
+ // Simulate CarlikeBot wheels' movement as a first-order system
// Update the joint status: this is a revolute joint without any limit.
// Simply integrates
- hw_positions_[i] += hw_velocities_[i] * period.seconds();
+ bool is_steering_joint = std::get<0>(hw_interfaces_[i]).find("steering") != std::string::npos;
- RCLCPP_INFO(
- rclcpp::get_logger("CarlikeBotSystemHardware"),
- "Got position state %.5f and velocity state %.5f for '%s'", hw_positions_[i],
- hw_velocities_[i], info_.joints[i].name.c_str());
+ if (is_steering_joint)
+ {
+ std::get<1>(hw_interfaces_[i]) = std::get<2>(hw_interfaces_[i]);
+ }
+ else
+ {
+ std::get<1>(hw_interfaces_[i]) += period.seconds() * std::get<2>(hw_interfaces_[i]);
+ }
+
+ // RCLCPP_INFO(
+ // rclcpp::get_logger("CarlikeBotSystemHardware"),
+ // "Got position state %.5f and velocity state %.5f for '%s'", hw_positions_[i],
+ // hw_velocities_[i], info_.joints[i].name.c_str());
}
// END: This part here is for exemplary purposes - Please do not copy to your production code
@@ -296,13 +340,14 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH
{
RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing...");
- for (auto i = 0u; i < hw_commands_.size(); ++i)
+ for (auto i = 0u; i < hw_interfaces_.size(); ++i)
{
- hw_velocities_[i] = hw_commands_[i];
+ // hw_velocities_[i] = hw_commands_[i];
+ std::get<2>(hw_interfaces_[i]) = std::get<2>(hw_interfaces_[i]);
- RCLCPP_INFO(
- rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'", hw_commands_[i],
- info_.joints[i].name.c_str());
+ // RCLCPP_INFO(
+ // rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'", hw_commands_[i],
+ // info_.joints[i].name.c_str());
}
return hardware_interface::return_type::OK;
diff --git a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp
index 55739cf6c..76ad73afc 100644
--- a/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp
+++ b/example_11/hardware/include/ros2_control_demo_example_11/carlikebot_system.hpp
@@ -73,11 +73,22 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface
double hw_stop_sec_;
// Store the command for the CarlikeBot robot
- std::vector hw_commands_;
- std::vector hw_positions_;
- std::vector hw_velocities_;
+ // std::vector hw_position_commands_;
+ // std::vector hw_velocity_commands_;
+ // std::vector hw_position_states_;
+ // std::vector hw_velocity_states_;
- std::vector> commands_counterpart_;
+ // std::vector hw_interfaces_;
+ // std::vector hw_commands_;
+ // std::vector hw_states_;
+
+ std::vector> hw_interfaces_; // name of joint, state, command
+
+ // std::vector> commands_counterpart_;
+
+ // std::vector hw_commands_;
+ // std::vector hw_positions_;
+ // std::vector hw_velocities_;
// std::map hw_commands_;
// std::map hw_positions_;