diff --git a/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro b/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro
index d2bc6ae31..8cdcee0ef 100644
--- a/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro
+++ b/example_11/description/ros2_control/carlikebot_ackermann.ros2_control.xacro
@@ -8,19 +8,26 @@
ros2_control_demo_example_11/CarlikeBotSystemHardware
0
3.0
+ 1
-
-
+
+
+
+
+
+
+
+
+
-
+
-
-
+
\ No newline at end of file
diff --git a/example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro b/example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro
index d2bc6ae31..3cc0de60b 100644
--- a/example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro
+++ b/example_11/description/ros2_control/carlikebot_bicycle.ros2_control.xacro
@@ -6,17 +6,14 @@
ros2_control_demo_example_11/CarlikeBotSystemHardware
- 0
- 3.0
+ 0
-
-
+
+
-
-
+
-
diff --git a/example_11/description/urdf/carlikebot.urdf.xacro b/example_11/description/urdf/carlikebot.urdf.xacro
index cd6901543..4a0454025 100644
--- a/example_11/description/urdf/carlikebot.urdf.xacro
+++ b/example_11/description/urdf/carlikebot.urdf.xacro
@@ -4,16 +4,18 @@
-
-
+
+
+
+
diff --git a/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro b/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro
new file mode 100644
index 000000000..0657851b6
--- /dev/null
+++ b/example_11/description/urdf/carlikebot_ackermann_description.urdf.xacro
@@ -0,0 +1,214 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/example_11/description/urdf/carlikebot_description.urdf.xacro b/example_11/description/urdf/carlikebot_bicycle_description.urdf.xacro
similarity index 100%
rename from example_11/description/urdf/carlikebot_description.urdf.xacro
rename to example_11/description/urdf/carlikebot_bicycle_description.urdf.xacro
diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp
index 108fa886a..98ad780c5 100644
--- a/example_11/hardware/carlikebot_system.cpp
+++ b/example_11/hardware/carlikebot_system.cpp
@@ -36,17 +36,35 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
return hardware_interface::CallbackReturn::ERROR;
}
- // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
- hw_start_sec_ = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
- 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());
+ m_running_simulation = std::stod(info_.hardware_parameters["is_simulation"]);
+ if (m_running_simulation && info_.joints.size() != 4)
+ {
+ RCLCPP_ERROR(
+ rclcpp::get_logger("CarlikeBotSystemHardware"),
+ "CarlikeBotSystemHardware::on_init() - Failed to initialize, "
+ "because the number of joints %ld is not 4 for simulation.",
+ info_.joints.size());
+ return hardware_interface::CallbackReturn::ERROR;
+ }
+
+ if (!m_running_simulation && info_.joints.size() != 2)
+ {
+ RCLCPP_ERROR(
+ rclcpp::get_logger("CarlikeBotSystemHardware"),
+ "CarlikeBotSystemHardware::on_init() - Failed to initialize, "
+ "because the number of joints %ld is not 2 for physical.",
+ info_.joints.size());
+ return hardware_interface::CallbackReturn::ERROR;
+ }
+
+ RCLCPP_INFO(
+ rclcpp::get_logger("CarlikeBotSystemHardware"),
+ "CarlikeBotSystemHardware is %s.", m_running_simulation ? "running in simulation" : "running on hardware");
+
+ // All joints should have one state interface and one command interface
for (const hardware_interface::ComponentInfo & joint : info_.joints)
{
- // CarlikeBotSystem has exactly two states and one command interface on each joint
if (joint.command_interfaces.size() != 1)
{
RCLCPP_FATAL(
@@ -56,41 +74,35 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
return hardware_interface::CallbackReturn::ERROR;
}
- if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY)
- {
- RCLCPP_FATAL(
- rclcpp::get_logger("CarlikeBotSystemHardware"),
- "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
- joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
- return hardware_interface::CallbackReturn::ERROR;
- }
-
- if (joint.state_interfaces.size() != 2)
+ if (joint.state_interfaces.size() != 1)
{
RCLCPP_FATAL(
rclcpp::get_logger("CarlikeBotSystemHardware"),
- "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(),
+ "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(),
joint.state_interfaces.size());
return hardware_interface::CallbackReturn::ERROR;
}
+ }
- if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
- {
- RCLCPP_FATAL(
- rclcpp::get_logger("CarlikeBotSystemHardware"),
- "Joint '%s' have '%s' as first 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)
- {
- RCLCPP_FATAL(
- rclcpp::get_logger("CarlikeBotSystemHardware"),
- "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(),
- joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY);
- return hardware_interface::CallbackReturn::ERROR;
- }
+ // The steering joints are controlled by position and the drive joints by velocity
+ // Running in simulation which means we have individual control of two front steering wheels and two rear drive wheels
+ if (m_running_simulation)
+ {
+ // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
+ hw_start_sec_ = std::stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
+ 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(2, std::numeric_limits::quiet_NaN());
+ // hw_velocities_.resize(2, std::numeric_limits::quiet_NaN());
+ // hw_commands_.resize(4, std::numeric_limits::quiet_NaN());
+ }
+ // Running on hardware which means we have a single front steering and a single rear drive motor
+ else if (!m_running_simulation)
+ {
+ // hw_positions_.resize(1, std::numeric_limits::quiet_NaN());
+ // hw_velocities_.resize(1, std::numeric_limits::quiet_NaN());
+ // hw_commands_.resize(2, std::numeric_limits::quiet_NaN());
}
return hardware_interface::CallbackReturn::SUCCESS;
@@ -101,10 +113,27 @@ std::vector CarlikeBotSystemHardware::export
std::vector state_interfaces;
for (auto i = 0u; i < info_.joints.size(); i++)
{
- state_interfaces.emplace_back(hardware_interface::StateInterface(
- info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i]));
- state_interfaces.emplace_back(hardware_interface::StateInterface(
- info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i]));
+ bool is_steering_joint = info_.joints[i].name.find("steering") != std::string::npos;
+
+ RCLCPP_DEBUG(
+ rclcpp::get_logger("CarlikeBotSystemHardware"),
+ "Joint '%s' has state interfaces: '%s', '%s'.", info_.joints[i].name.c_str(),
+ info_.joints[i].state_interfaces[0].name.c_str(),
+ info_.joints[i].state_interfaces[1].name.c_str()
+ );
+
+ if (is_steering_joint)
+ {
+ hw_positions_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits::quiet_NaN()));
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_.at(info_.joints[i].name)));
+ }
+ else
+ {
+ hw_velocities_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits::quiet_NaN()));
+ state_interfaces.emplace_back(hardware_interface::StateInterface(
+ info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_.at(info_.joints[i].name)));
+ }
}
return state_interfaces;
@@ -116,8 +145,13 @@ CarlikeBotSystemHardware::export_command_interfaces()
std::vector command_interfaces;
for (auto i = 0u; i < info_.joints.size(); i++)
{
+ bool is_steering_joint = info_.joints[i].name.find("steering") != std::string::npos;
+
+ auto hw_if = is_steering_joint ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY;
+
+ hw_commands_.insert(std::make_pair(info_.joints[i].name, std::numeric_limits::quiet_NaN()));
command_interfaces.emplace_back(hardware_interface::CommandInterface(
- info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i]));
+ info_.joints[i].name, hw_if, &hw_commands_.at(info_.joints[i].name)));
}
return command_interfaces;
@@ -126,25 +160,31 @@ CarlikeBotSystemHardware::export_command_interfaces()
hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/)
{
- // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
- RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Activating ...please wait...");
-
- for (auto i = 0; i < hw_start_sec_; i++)
+ if (m_running_simulation)
{
- rclcpp::sleep_for(std::chrono::seconds(1));
- RCLCPP_INFO(
+ RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Activating ...please wait...");
+
+ for (auto i = 0; i < hw_start_sec_; i++)
+ {
+ rclcpp::sleep_for(std::chrono::seconds(1));
+ RCLCPP_INFO(
rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i);
- }
- // END: This part here is for exemplary purposes - Please do not copy to your production code
+ }
- // set some default values
- for (auto i = 0u; i < hw_positions_.size(); i++)
- {
- if (std::isnan(hw_positions_[i]))
+ // iterate through hw_positions_ map and set all second values to 0
+ for (auto it = hw_positions_.begin(); it != hw_positions_.end(); ++it)
+ {
+ it->second = 0;
+ }
+
+ for (auto it = hw_velocities_.begin(); it != hw_velocities_.end(); ++it)
{
- hw_positions_[i] = 0;
- hw_velocities_[i] = 0;
- hw_commands_[i] = 0;
+ it->second = 0;
+ }
+
+ for (auto it = hw_commands_.begin(); it != hw_commands_.end(); ++it)
+ {
+ it->second = 0;
}
}
@@ -156,17 +196,19 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate(
hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_deactivate(
const rclcpp_lifecycle::State & /*previous_state*/)
{
- // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
- RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Deactivating ...please wait...");
-
- for (auto i = 0; i < hw_stop_sec_; i++)
+ if (m_running_simulation)
{
- rclcpp::sleep_for(std::chrono::seconds(1));
- RCLCPP_INFO(
- rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i);
- }
- // END: This part here is for exemplary purposes - Please do not copy to your production code
+ // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
+ RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Deactivating ...please wait...");
+ for (auto i = 0; i < hw_stop_sec_; i++)
+ {
+ rclcpp::sleep_for(std::chrono::seconds(1));
+ RCLCPP_INFO(
+ rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_stop_sec_ - i);
+ }
+ // END: This part here is for exemplary purposes - Please do not copy to your production code
+ }
RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully deactivated!");
return hardware_interface::CallbackReturn::SUCCESS;
@@ -175,41 +217,43 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_deactivate(
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++)
+ if (m_running_simulation)
{
- // Simulate DiffBot wheels's movement as a first-order system
- // Update the joint status: this is a revolute joint without any limit.
- // Simply integrates
- hw_positions_[i] = hw_positions_[1] + period.seconds() * hw_velocities_[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());
+ // Inside the write method both position and velocity states for the simulation are updated
+ }
+ else
+ {
+ // Robot specific code to read data from the hardware
}
- // END: This part here is for exemplary purposes - Please do not copy to your production code
-
return hardware_interface::return_type::OK;
}
hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemHardware::write(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
- // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing...");
- for (auto i = 0u; i < hw_commands_.size(); i++)
+ if (m_running_simulation)
{
- // Simulate sending commands to the hardware
- RCLCPP_INFO(
- rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", hw_commands_[i],
- info_.joints[i].name.c_str());
+ for (auto it = hw_commands_.begin(); it != hw_commands_.end(); ++it)
+ {
+ RCLCPP_INFO(
+ rclcpp::get_logger("CarlikeBotSystemHardware"), "Got command %.5f for '%s'!", it->second,
+ it->first.c_str());
+
+ if (it->first.find("steering") != std::string::npos)
+ {
+ hw_positions_.at(it->first) = it->second;
+ }
+ else
+ {
+ hw_velocities_.at(it->first) = it->second;
+ }
- hw_velocities_[i] = hw_commands_[i];
+ }
}
+
RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Joints successfully written!");
- // END: This part here is for exemplary purposes - Please do not copy to your production code
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 4772eb1de..6be3332ae 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
@@ -18,6 +18,7 @@
#include
#include
#include
+#include