New to ros2 here. I am using ros2 humble. I am taking to use ros2_control for the keyestudio ks0223 smart car (it uses gpio pins). I keep getting Waiting for /controller_manager to be available when I run my launch file. I also notice that I get duplicate robot_state_publisher nodes.
Command: ros2 launch my_robot my_robot.launch.py Output:
[INFO] [launch]: All log files can be found below /home/pi/.ros/log/2024-06-23-13-26-08-959242-raspberrypi-9127
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [9138]
[robot_state_publisher-1] [INFO] [1719145570.016500002] [robot_state_publisher]: got segment base_footprint
[robot_state_publisher-1] [INFO] [1719145570.016821944] [robot_state_publisher]: got segment base_link
[robot_state_publisher-1] [INFO] [1719145570.016872185] [robot_state_publisher]: got segment left_back_wheel_link
[robot_state_publisher-1] [INFO] [1719145570.016903000] [robot_state_publisher]: got segment left_front_wheel_link
[robot_state_publisher-1] [INFO] [1719145570.016929462] [robot_state_publisher]: got segment right_back_wheel_link
[robot_state_publisher-1] [INFO] [1719145570.016954129] [robot_state_publisher]: got segment right_front_wheel_link
[INFO] [ros2_control_node-2]: process started with pid [9152]
[INFO] [spawner-3]: process started with pid [9154]
[INFO] [spawner-4]: process started with pid [9156]
[ros2_control_node-2] [INFO] [1719145571.088513307] [controller_manager]: Subscribing to '~/robot_description' topic for robot description file.
[ros2_control_node-2] [INFO] [1719145571.094744619] [controller_manager]: update rate is 10 Hz
[ros2_control_node-2] [INFO] [1719145571.099397238] [controller_manager]: RT kernel is recommended for better performance
[spawner-3] [INFO] [1719145576.181104083] [spawner_joint_broad]: Waiting for '/controller_manager' services to be available
[spawner-4] [INFO] [1719145576.218799121] [spawner_diff_cont]: Waiting for '/controller_manager' services to be available
[spawner-3] [INFO] [1719145578.244257259] [spawner_joint_broad]: Waiting for '/controller_manager' services to be available
[spawner-4] [INFO] [1719145578.279637757] [spawner_diff_cont]: Waiting for '/controller_manager' services to be available
[spawner-3] [INFO] [1719145580.303168443] [spawner_joint_broad]: Waiting for '/controller_manager' services to be available
[spawner-4] [INFO] [1719145580.333648138] [spawner_diff_cont]: Waiting for '/controller_manager' services to be available
[spawner-3] [INFO] [1719145582.366774022] [spawner_joint_broad]: Waiting for '/controller_manager' services to be available
[spawner-4] [INFO] [1719145582.381937462] [spawner_diff_cont]: Waiting for '/controller_manager' services to be available
[spawner-3] [ERROR] [1719145584.007675581] [spawner_joint_broad]: Controller manager not available
[spawner-4] [ERROR] [1719145584.015068275] [spawner_diff_cont]: Controller manager not available
[ERROR] [spawner-3]: process has died [pid 9154, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner joint_broad --controller-manager /controller_manager --ros-args'].
[ERROR] [spawner-4]: process has died [pid 9156, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner diff_cont --controller-manager /controller_manager --ros-args'].
my_robot.launch.py:
def generate_launch_description():
# Include rsp.launch.py
rsp = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('my_robot'), 'bringup', 'launch', 'rsp.launch.py'
)]), launch_arguments={'use_sim_time': 'false', 'use_ros2_control': 'true'}.items()
)
robot_controllers = os.path.join(get_package_share_directory('my_robot'), 'bringup', 'config', 'my_robot_controllers.yaml')
# Control node
control_node = Node(
package='controller_manager',
executable='ros2_control_node',
parameters=[robot_controllers],
output='both'
)
# TimerAction to delay the controller manager
delayed_controller_manager = TimerAction(period=1.0, actions=[control_node])
# Spawner nodes
diff_drive_spawner = Node(
package='controller_manager',
executable='spawner',
arguments=['diff_cont', '--controller-manager', '/controller_manager'],
)
joint_broad_spawner = Node(
package='controller_manager',
executable='spawner',
arguments=['joint_broad', '--controller-manager', '/controller_manager'],
)
# Delayed spawner handlers
delayed_diff_drive_spawner = RegisterEventHandler(
event_handler=OnProcessStart(
target_action=control_node,
on_start=[diff_drive_spawner],
)
)
delayed_joint_broad_spawner = RegisterEventHandler(
event_handler=OnProcessStart(
target_action=control_node,
on_start=[joint_broad_spawner],
)
)
return LaunchDescription([
rsp,
delayed_controller_manager,
delayed_diff_drive_spawner,
delayed_joint_broad_spawner
])
rsp.launch.py:
def generate_launch_description():
# Check if we're told to use sim time
use_sim_time = LaunchConfiguration('use_sim_time')
use_ros2_control = LaunchConfiguration('use_ros2_control')
# Process the URDF file
pkg_path = os.path.join(get_package_share_directory('my_robot'))
xacro_file = os.path.join(pkg_path,'description','urdf','my_robot.urdf.xacro')
# robot_description_config = xacro.process_file(xacro_file).toxml()
robot_description_config = Command(['xacro ', xacro_file, ' use_ros2_control:=', use_ros2_control, ' sim_mode:=', use_sim_time])
# Create a robot_state_publisher node
params = {'robot_description': robot_description_config, 'use_sim_time': use_sim_time}
node_robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
parameters=[params]
)
# Launch!
return LaunchDescription([
DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use sim time if true'),
DeclareLaunchArgument(
'use_ros2_control',
default_value='true',
description='Use ros2_control if true'),
node_robot_state_publisher
])
my_robot_controllers.yaml:
controller_manager:
ros__parameters:
update_rate: 10
diff_cont:
type: diff_drive_controller/DiffDriveController
joint_broad:
type: joint_state_broadcaster/JointStateBroadcaster
diff_cont:
ros__parameters:
publish_rate: 10.0
base_frame_id: base_footprint
wheels_per_side: 2
left_wheel_names: ["base_left_front_wheel_joint", "base_left_back_wheel_joint"]
right_wheel_names: ["base_right_front_wheel_joint", "base_right_back_wheel_joint"]
odom_frame_id: odom
wheel_separation: 0.145
wheel_radius: 0.03
open_loop: true
enable_odom_tf: true
# cmd_vel_timeout: 0.5
#publish_limited_velocity: true
#velocity_rolling_window_size: 10
# Velocity and acceleration limits
# Whenever a min_* is unspecified, default to -max_*
linear.x.has_velocity_limits: true
linear.x.has_acceleration_limits: true
linear.x.has_jerk_limits: false
linear.x.max_velocity: 1.0
linear.x.min_velocity: -1.0
linear.x.max_acceleration: 1.0
linear.x.max_jerk: 0.0
linear.x.min_jerk: 0.0
angular.z.has_velocity_limits: true
angular.z.has_acceleration_limits: true
angular.z.has_jerk_limits: false
angular.z.max_velocity: 1.0
angular.z.min_velocity: -1.0
angular.z.max_acceleration: 1.0
angular.z.min_acceleration: -1.0
angular.z.max_jerk: 0.0
angular.z.min_jerk: 0.0
use_stamped_vel: false
// because I also believe I might have messed up my hardware interface: my_robot_hardware.hpp:
#ifndef MY_ROBOT_HARDWARE_HPP
#define MY_ROBOT_HARDWARE_HPP
#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/handle.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>
#include <rclcpp/rclcpp.hpp>
#include <pigpiod_if2.h> // pigpio library header
#include "pluginlib/class_list_macros.hpp"
namespace my_robot
{
class MyRobotHardware : public hardware_interface::SystemInterface
{
public:
RCLCPP_SHARED_PTR_DEFINITIONS(MyRobotHardware);
hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;
hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override;
hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
hardware_interface::return_type read(const rclcpp::Time &time, const rclcpp::Duration &period) override;
hardware_interface::return_type write(const rclcpp::Time &time, const rclcpp::Duration &period) override;
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
private:
// Member variables to hold the state and command values
std::vector<double> hw_states_;
std::vector<double> hw_commands_;
std::vector<std::string> joint_names_;
size_t number_of_joints_;
// Motor control pins
int pi_;
int L_IN1, L_IN2, L_PWM1, L_IN3, L_IN4, L_PWM2;
int R_IN1, R_IN2, R_PWM1, R_IN3, R_IN4, R_PWM2;
};
} // namespace my_robot
#endif // MY_ROBOT_HARDWARE_HPP_
my_robot_hardware.cpp:
#include "my_robot/my_robot_hardware.hpp"
namespace my_robot
{
// Define the hardware interface constants if they are not defined
const std::string HW_IF_POSITION = "position";
const std::string HW_IF_VELOCITY = "velocity";
hardware_interface::CallbackReturn MyRobotHardware::on_init(const hardware_interface::HardwareInfo &info)
{
if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS)
{
return hardware_interface::CallbackReturn::ERROR;
}
number_of_joints_ = info.joints.size();
hw_states_.resize(number_of_joints_, 0.0);
hw_commands_.resize(number_of_joints_, 0.0);
joint_names_.resize(number_of_joints_);
for (size_t i = 0; i < number_of_joints_; ++i)
{
joint_names_[i] = info.joints[i].name;
}
// Initialize pigpio
pi_ = pigpio_start(nullptr, nullptr);
if (pi_ < 0)
{
RCLCPP_ERROR(rclcpp::get_logger("MyRobotHardware"), "Failed to initialize pigpio");
return hardware_interface::CallbackReturn::ERROR;
}
// Define motor control pins
L_IN1 = 20;
L_IN2 = 21;
L_PWM1 = 0;
L_IN3 = 22;
L_IN4 = 23;
L_PWM2 = 1;
R_IN1 = 24;
R_IN2 = 25;
R_PWM1 = 12;
R_IN3 = 26;
R_IN4 = 27;
R_PWM2 = 13;
// Set up motor control pins
set_mode(pi_, L_IN1, PI_OUTPUT);
set_mode(pi_, L_IN2, PI_OUTPUT);
set_mode(pi_, L_PWM1, PI_OUTPUT);
set_mode(pi_, L_IN3, PI_OUTPUT);
set_mode(pi_, L_IN4, PI_OUTPUT);
set_mode(pi_, L_PWM2, PI_OUTPUT);
set_mode(pi_, R_IN1, PI_OUTPUT);
set_mode(pi_, R_IN2, PI_OUTPUT);
set_mode(pi_, R_PWM1, PI_OUTPUT);
set_mode(pi_, R_IN3, PI_OUTPUT);
set_mode(pi_, R_IN4, PI_OUTPUT);
set_mode(pi_, R_PWM2, PI_OUTPUT);
RCLCPP_INFO(rclcpp::get_logger("MyRobotHardware"), "Hardware interface initialized successfully.");
return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::CallbackReturn MyRobotHardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/)
{
std::fill(hw_commands_.begin(), hw_commands_.end(), 0.0);
RCLCPP_INFO(rclcpp::get_logger("MyRobotHardware"), "Hardware interface activated successfully.");
return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::CallbackReturn MyRobotHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
{
RCLCPP_INFO(rclcpp::get_logger("MyRobotHardware"), "Hardware interface deactivated successfully.");
return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::return_type MyRobotHardware::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
// Read hardware state here (e.g., encoders)
// For now, we are just simulating with dummy values
hw_states_[0] = 1.0; // Left front wheel position
hw_states_[1] = 1.0; // Left rear wheel position
hw_states_[2] = 1.0; // Right front wheel position
hw_states_[3] = 1.0; // Right rear wheel position
return hardware_interface::return_type::OK;
}
hardware_interface::return_type MyRobotHardware::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
// Write commands to hardware here using pigpio
int left_front_speed = static_cast<int>(hw_commands_[0] * 255); // Scale command to PWM range
int left_rear_speed = static_cast<int>(hw_commands_[1] * 255); // Scale command to PWM range
int right_front_speed = static_cast<int>(hw_commands_[2] * 255); // Scale command to PWM range
int right_rear_speed = static_cast<int>(hw_commands_[3] * 255); // Scale command to PWM range
// Left front motor control
if (left_front_speed >= 0)
{
gpio_write(pi_, L_IN1, 0);
gpio_write(pi_, L_IN2, 1);
}
else
{
gpio_write(pi_, L_IN1, 1);
gpio_write(pi_, L_IN2, 0);
}
set_PWM_dutycycle(pi_, L_PWM1, abs(left_front_speed));
// Left rear motor control
if (left_rear_speed >= 0)
{
gpio_write(pi_, L_IN3, 0);
gpio_write(pi_, L_IN4, 1);
}
else
{
gpio_write(pi_, L_IN3, 1);
gpio_write(pi_, L_IN4, 0);
}
set_PWM_dutycycle(pi_, L_PWM2, abs(left_rear_speed));
// Right front motor control
if (right_front_speed >= 0)
{
gpio_write(pi_, R_IN1, 1);
gpio_write(pi_, R_IN2, 0);
}
else
{
gpio_write(pi_, R_IN1, 0);
gpio_write(pi_, R_IN2, 1);
}
set_PWM_dutycycle(pi_, R_PWM1, abs(right_front_speed));
// Right rear motor control
if (right_rear_speed >= 0)
{
gpio_write(pi_, R_IN3, 1);
gpio_write(pi_, R_IN4, 0);
}
else
{
gpio_write(pi_, R_IN3, 0);
gpio_write(pi_, R_IN4, 1);
}
set_PWM_dutycycle(pi_, R_PWM2, abs(right_rear_speed));
return hardware_interface::return_type::OK;
}
std::vector<hardware_interface::StateInterface> MyRobotHardware::export_state_interfaces()
{
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.emplace_back(hardware_interface::StateInterface(
joint_names_[0], HW_IF_POSITION, &hw_states_[0]));
state_interfaces.emplace_back(hardware_interface::StateInterface(
joint_names_[1], HW_IF_POSITION, &hw_states_[1]));
state_interfaces.emplace_back(hardware_interface::StateInterface(
joint_names_[2], HW_IF_POSITION, &hw_states_[2]));
state_interfaces.emplace_back(hardware_interface::StateInterface(
joint_names_[3], HW_IF_POSITION, &hw_states_[3]));
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface> MyRobotHardware::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.emplace_back(hardware_interface::CommandInterface(
joint_names_[0], HW_IF_VELOCITY, &hw_commands_[0]));
command_interfaces.emplace_back(hardware_interface::CommandInterface(
joint_names_[1], HW_IF_VELOCITY, &hw_commands_[1]));
command_interfaces.emplace_back(hardware_interface::CommandInterface(
joint_names_[2], HW_IF_VELOCITY, &hw_commands_[2]));
command_interfaces.emplace_back(hardware_interface::CommandInterface(
joint_names_[3], HW_IF_VELOCITY, &hw_commands_[3]));
return command_interfaces;
}
} // namespace my_robot
// Register the hardware interface as a plugin
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(my_robot::MyRobotHardware, hardware_interface::SystemInterface)
Things I tried:
- I tried changing the name of the node in rsp.launch.py to /robot_state_publisher_2 but it did not work.
- I went back to modify my yaml config file.
Sorry, I am used to the formatting here yet. I know this is quite long, but I am so confused.
