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