diff --git a/example_11/bringup/config/carlikebot_controllers.yaml b/example_11/bringup/config/carlikebot_controllers.yaml
index bd2c9d9e0..cc9bc55ba 100644
--- a/example_11/bringup/config/carlikebot_controllers.yaml
+++ b/example_11/bringup/config/carlikebot_controllers.yaml
@@ -5,28 +5,25 @@ controller_manager:
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
- ackermann_steering_controller:
- type: ackermann_steering_controller/AckermannSteeringController
+ bicycle_steering_controller:
+ type: bicycle_steering_controller/BicycleSteeringController
-ackermann_steering_controller:
+bicycle_steering_controller:
ros__parameters:
-
- reference_timeout: 2.0
+ wheelbase: 0.325
+ front_wheel_radius: 0.05
+ rear_wheel_radius: 0.05
front_steering: true
+ reference_timeout: 2.0
+ rear_wheels_names: ['virtual_rear_wheel_joint']
+ front_wheels_names: ['virtual_front_wheel_joint']
open_loop: false
velocity_rolling_window_size: 10
- position_feedback: true
- use_stamped_vel: false
- front_wheels_names: [right_steering_joint, left_steering_joint]
- rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
- odom_frame_id: odom
base_frame_id: base_link
+ odom_frame_id: odom
enable_odom_tf: true
- in_chained_mode: false
-
- wheelbase: 0.325
- front_wheel_track: 0.2
- rear_wheel_track: 0.2
- front_wheels_radius: 0.05
- rear_wheels_radius: 0.05
+ twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
+ pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
+ position_feedback: false
+ use_stamped_vel: true
diff --git a/example_11/bringup/launch/carlikebot.launch.py b/example_11/bringup/launch/carlikebot.launch.py
index 40b3285a5..cedfe33e8 100644
--- a/example_11/bringup/launch/carlikebot.launch.py
+++ b/example_11/bringup/launch/carlikebot.launch.py
@@ -56,16 +56,22 @@ def generate_launch_description():
]
)
rviz_config_file = PathJoinSubstitution(
- [FindPackageShare("ros2_control_demo_description"), "carlikebot/rviz", "carlikebot.rviz"]
+ [
+ FindPackageShare("ros2_control_demo_description"),
+ "carlikebot/rviz",
+ "carlikebot.rviz",
+ ]
)
# the steering controller libraries by default publish odometry on a separate topic than /tf
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
- parameters=[robot_description, robot_controllers],
+ parameters=[robot_controllers],
output="both",
- remappings=[("/ackermann_steering_controller/tf_odometry", "/tf")],
+ remappings=[
+ ("~/robot_description", "/robot_description"),
+ ],
)
robot_state_pub_ackermann_node = Node(
package="robot_state_publisher",
@@ -73,7 +79,8 @@ def generate_launch_description():
output="both",
parameters=[robot_description],
remappings=[
- ("/ackermann_steering_controller/reference_unstamped", "/cmd_vel"),
+ ("/bicycle_steering_controller/reference", "/reference"),
+ ("~/robot_description", "/robot_description"),
],
)
rviz_node = Node(
@@ -94,7 +101,7 @@ def generate_launch_description():
robot_ackermann_controller_spawner = Node(
package="controller_manager",
executable="spawner",
- arguments=["ackermann_steering_controller", "--controller-manager", "/controller_manager"],
+ arguments=["bicycle_steering_controller", "--controller-manager", "/controller_manager"],
)
# Delay rviz start after `joint_state_broadcaster`
diff --git a/example_11/description/ros2_control/carlikebot.ros2_control.xacro b/example_11/description/ros2_control/carlikebot.ros2_control.xacro
index 4106601a6..f1c547068 100644
--- a/example_11/description/ros2_control/carlikebot.ros2_control.xacro
+++ b/example_11/description/ros2_control/carlikebot.ros2_control.xacro
@@ -10,29 +10,14 @@
3.0
1
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp
index 5f16ebd3c..4d5f8b747 100644
--- a/example_11/hardware/carlikebot_system.cpp
+++ b/example_11/hardware/carlikebot_system.cpp
@@ -37,21 +37,21 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
}
// Check if the number of joints is correct based on the mode of operation
- if (info_.joints.size() != 6)
+ if (info_.joints.size() != 2)
{
RCLCPP_ERROR(
rclcpp::get_logger("CarlikeBotSystemHardware"),
"CarlikeBotSystemHardware::on_init() - Failed to initialize, "
- "because the number of joints %ld is not 6.",
+ "because the number of joints %ld is not 2.",
info_.joints.size());
return hardware_interface::CallbackReturn::ERROR;
}
for (const hardware_interface::ComponentInfo & joint : info_.joints)
{
- bool joint_is_steering = joint.name.find("steering") != std::string::npos;
+ bool joint_is_steering = joint.name.find("front") != std::string::npos;
- // Steering joints have a position command and state interface
+ // Steering joints have a position command interface and a position state interface
if (joint_is_steering)
{
RCLCPP_INFO(
@@ -100,7 +100,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
rclcpp::get_logger("CarlikeBotSystemHardware"), "Joint '%s' is a drive joint.",
joint.name.c_str());
- // Drive joints have a velocity command interface and velocity and position state interface
+ // Drive joints have a velocity command interface and a velocity state interface
if (joint.command_interfaces.size() != 1)
{
RCLCPP_FATAL(
@@ -119,7 +119,7 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
return hardware_interface::CallbackReturn::ERROR;
}
- if (joint.state_interfaces.size() != 1)
+ if (joint.state_interfaces.size() != 2)
{
RCLCPP_FATAL(
rclcpp::get_logger("CarlikeBotSystemHardware"),
@@ -136,6 +136,15 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_init(
joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY);
return hardware_interface::CallbackReturn::ERROR;
}
+
+ if (joint.state_interfaces[1].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[1].name.c_str(), hardware_interface::HW_IF_POSITION);
+ return hardware_interface::CallbackReturn::ERROR;
+ }
}
}
@@ -145,11 +154,14 @@ 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
- // joint name, state, command
- hw_interfaces_.resize(
- 6, std::make_tuple(
- std::string(), std::numeric_limits::quiet_NaN(),
- std::numeric_limits::quiet_NaN()));
+ hw_interfaces_["steering"] = Joint(
+ "virtual_front_wheel_joint"
+ );
+
+ hw_interfaces_["traction"] = Joint(
+ "virtual_rear_wheel_joint"
+ );
+
return hardware_interface::CallbackReturn::SUCCESS;
}
@@ -158,34 +170,23 @@ std::vector CarlikeBotSystemHardware::export
{
std::vector state_interfaces;
- for (auto i = 0u; i < info_.joints.size(); i++)
+ for (auto & joint : hw_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; });
+ state_interfaces.emplace_back(
+ hardware_interface::StateInterface(
+ joint.second.joint_name, hardware_interface::HW_IF_POSITION, &joint.second.state.position));
- 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
+ if (joint.first == "traction")
{
- state_interfaces.emplace_back(hardware_interface::StateInterface(
- info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &std::get<1>(*it)));
+ state_interfaces.emplace_back(
+ hardware_interface::StateInterface(
+ joint.second.joint_name, hardware_interface::HW_IF_VELOCITY,
+ &joint.second.state.velocity));
}
}
+
+
RCLCPP_INFO(
rclcpp::get_logger("CarlikeBotSystemHardware"), "Exported %zu state interfaces.",
state_interfaces.size());
@@ -205,31 +206,21 @@ CarlikeBotSystemHardware::export_command_interfaces()
{
std::vector command_interfaces;
- for (uint i = 0; i < info_.joints.size(); i++)
+ for (auto & joint : hw_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())
+ if (joint.first == "steering")
{
- std::get<0>(hw_interfaces_[i]) = info_.joints[i].name;
+ command_interfaces.emplace_back(
+ hardware_interface::CommandInterface(
+ joint.second.joint_name, hardware_interface::HW_IF_POSITION,
+ &joint.second.command.position));
}
-
- // 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)
+ else if (joint.first == "traction")
{
- command_interfaces.emplace_back(hardware_interface::CommandInterface(
- 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, &std::get<2>(*it)));
+ command_interfaces.emplace_back(
+ hardware_interface::CommandInterface(
+ joint.second.joint_name, hardware_interface::HW_IF_VELOCITY,
+ &joint.second.command.velocity));
}
}
@@ -259,10 +250,20 @@ hardware_interface::CallbackReturn CarlikeBotSystemHardware::on_activate(
rclcpp::get_logger("CarlikeBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i);
}
- for (uint i = 0; i < hw_interfaces_.size(); i++)
+ for (auto & joint : hw_interfaces_)
{
- std::get<1>(hw_interfaces_[i]) = 0.0;
- std::get<2>(hw_interfaces_[i]) = 0.0;
+ joint.second.state.position = 0.0;
+
+ if (joint.first == "traction")
+ {
+ joint.second.state.velocity = 0.0;
+ joint.second.command.velocity = 0.0;
+ }
+
+ else if (joint.first == "steering")
+ {
+ joint.second.command.position = 0.0;
+ }
}
RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Successfully activated!");
@@ -292,26 +293,17 @@ 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_interfaces_.size(); ++i)
+ for (auto & joint : hw_interfaces_)
{
- // Simulate CarlikeBot wheels' movement as a first-order system
- // Update the joint status: this is a revolute joint without any limit.
- // Simply integrates
- bool is_steering_joint = std::get<0>(hw_interfaces_[i]).find("steering") != std::string::npos;
-
- if (is_steering_joint)
+ if (joint.first == "steering")
{
- std::get<1>(hw_interfaces_[i]) = std::get<2>(hw_interfaces_[i]);
+ joint.second.state.position = joint.second.command.position;
}
- else
+ else if (joint.first == "traction")
{
- std::get<1>(hw_interfaces_[i]) += period.seconds() * std::get<2>(hw_interfaces_[i]);
+ joint.second.state.velocity = joint.second.command.velocity;
+ joint.second.state.position += joint.second.state.velocity * period.seconds();
}
-
- // 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
@@ -323,15 +315,30 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH
{
RCLCPP_INFO(rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing...");
- for (auto i = 0u; i < hw_interfaces_.size(); ++i)
+ // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
+ for (auto & joint : hw_interfaces_)
{
- // 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());
+ if (joint.first == "steering")
+ {
+ RCLCPP_INFO(
+ rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing to steering joint '%s'.",
+ joint.second.joint_name.c_str());
+ RCLCPP_INFO(
+ rclcpp::get_logger("CarlikeBotSystemHardware"), "Position command: %.2f.",
+ joint.second.command.position);
+ }
+ else if (joint.first == "traction")
+ {
+ RCLCPP_INFO(
+ rclcpp::get_logger("CarlikeBotSystemHardware"), "Writing to traction joint '%s'.",
+ joint.second.joint_name.c_str());
+ RCLCPP_INFO(
+ rclcpp::get_logger("CarlikeBotSystemHardware"), "Velocity command: %.2f.",
+ joint.second.command.velocity);
+ }
}
+ // 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 f1556a8cc..6de2c646e 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
@@ -36,6 +36,30 @@
namespace ros2_control_demo_example_11
{
+struct JointValue
+{
+ double position{0.0};
+ double velocity{0.0};
+ double effort{0.0};
+};
+
+struct Joint
+{
+ Joint(
+ const std::string & name
+ ) :
+ joint_name(name)
+ {
+ state = JointValue();
+ command = JointValue();
+ }
+
+ Joint() = default;
+
+ std::string joint_name;
+ JointValue state;
+ JointValue command;
+};
class CarlikeBotSystemHardware : public hardware_interface::SystemInterface
{
public:
@@ -72,9 +96,9 @@ class CarlikeBotSystemHardware : public hardware_interface::SystemInterface
double hw_start_sec_;
double hw_stop_sec_;
- std::vector>
- hw_interfaces_; // name of joint, state, command
-
+ // std::vector>
+ // hw_interfaces_; // name of joint, state, command
+ std::map hw_interfaces_;
};
} // namespace ros2_control_demo_example_11
diff --git a/example_11/package.xml b/example_11/package.xml
index 634813127..c690fb943 100644
--- a/example_11/package.xml
+++ b/example_11/package.xml
@@ -21,7 +21,7 @@
rclcpp_lifecycle
topic_tools
- ackermann_steering_controller
+ bicycle_steering_controller
controller_manager
joint_state_broadcaster
joint_state_publisher_gui
diff --git a/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro b/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro
index 6af69eff0..3f2024c17 100644
--- a/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro
+++ b/ros2_control_demo_description/carlikebot/urdf/carlikebot_description.urdf.xacro
@@ -39,10 +39,10 @@
-
+
@@ -73,7 +73,7 @@
-
+
@@ -100,9 +100,9 @@
+ ixx="0.000270" ixy="0.0" ixz="0.0"
+ iyy="0.000270" iyz="0.0"
+ izz="0.000426"/>
@@ -111,7 +111,7 @@
-
+
@@ -138,9 +138,9 @@
+ ixx="0.000270" ixy="0.0" ixz="0.0"
+ iyy="0.000270" iyz="0.0"
+ izz="0.000426"/>
@@ -149,7 +149,7 @@
-
+
@@ -157,7 +157,7 @@
-
+
@@ -178,7 +178,7 @@
-
+
@@ -202,9 +202,9 @@
+ ixx="0.000270" ixy="0.0" ixz="0.0"
+ iyy="0.000270" iyz="0.0"
+ izz="0.000426"/>
@@ -241,9 +241,9 @@
+ ixx="0.000270" ixy="0.0" ixz="0.0"
+ iyy="0.000270" iyz="0.0"
+ izz="0.000426"/>