make diff drive load correctly (#89)

* make diff drive load correctly

Signed-off-by: Karsten Knese <[email protected]>

* use spawner script to launch controller

Signed-off-by: Karsten Knese <[email protected]>

* cpplint + precommit fixes

Co-authored-by: Bence Magyar <[email protected]>

Author: Karsten1987

ros2_control_demo_hardware/include/ros2_control_demo_hardware/rrbot_system_multi_interface.hpp

@@ -50,9 +50,9 @@ class RRBotSystemMultiInterfaceHardware
   std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
 
   ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
-  virtual return_type prepare_command_mode_switch(
+  return_type prepare_command_mode_switch(
     const std::vector<std::string> & start_interfaces,
-    const std::vector<std::string> & stop_interfaces);
+    const std::vector<std::string> & stop_interfaces) override;
 
   ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
   return_type start() override;

ros2_control_demo_hardware/src/diffbot_system.cpp

@@ -95,7 +95,7 @@ hardware_interface::return_type DiffBotSystemHardware::configure(
 std::vector<hardware_interface::StateInterface> DiffBotSystemHardware::export_state_interfaces()
 {
   std::vector<hardware_interface::StateInterface> state_interfaces;
-  for (uint i = 0; i < info_.joints.size(); i++)
+  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_states_[i]));
@@ -109,7 +109,7 @@ std::vector<hardware_interface::StateInterface> DiffBotSystemHardware::export_st
 std::vector<hardware_interface::CommandInterface> DiffBotSystemHardware::export_command_interfaces()
 {
   std::vector<hardware_interface::CommandInterface> command_interfaces;
-  for (uint i = 0; i < info_.joints.size(); i++)
+  for (auto i = 0u; i < info_.joints.size(); i++)
   {
     command_interfaces.emplace_back(hardware_interface::CommandInterface(
       info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i]));
@@ -122,15 +122,15 @@ hardware_interface::return_type DiffBotSystemHardware::start()
 {
   RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Starting ...please wait...");
 
-  for (int i = 0; i <= hw_start_sec_; i++)
+  for (auto i = 0; i <= hw_start_sec_; i++)
   {
     rclcpp::sleep_for(std::chrono::seconds(1));
     RCLCPP_INFO(
       rclcpp::get_logger("DiffBotSystemHardware"), "%.1f seconds left...", hw_start_sec_ - i);
   }
 
   // set some default values
-  for (uint i = 0; i < hw_states_.size(); i++)
+  for (auto i = 0u; i < hw_states_.size(); i++)
   {
     if (std::isnan(hw_states_[i]))
     {
@@ -150,7 +150,7 @@ hardware_interface::return_type DiffBotSystemHardware::stop()
 {
   RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Stopping ...please wait...");
 
-  for (int i = 0; i <= hw_stop_sec_; i++)
+  for (auto i = 0; i <= hw_stop_sec_; i++)
   {
     rclcpp::sleep_for(std::chrono::seconds(1));
     RCLCPP_INFO(
@@ -190,7 +190,7 @@ hardware_interface::return_type ros2_control_demo_hardware::DiffBotSystemHardwar
 {
   RCLCPP_INFO(rclcpp::get_logger("DiffBotSystemHardware"), "Writing...");
 
-  for (uint i = 0; i < hw_commands_.size(); i++)
+  for (auto i = 0u; i < hw_commands_.size(); i++)
   {
     // Simulate sending commands to the hardware
     RCLCPP_INFO(

ros2_control_demo_robot/controllers/diffbot_diff_drive_controller.yaml renamed to ros2_control_demo_robot/config/diffbot_diff_drive_controller.yaml

@@ -35,7 +35,7 @@ def generate_launch_description():
 
     diffbot_diff_drive_controller = os.path.join(
         get_package_share_directory("ros2_control_demo_robot"),
-        "controllers",
+        "config",
         "diffbot_diff_drive_controller.yaml",
     )
 
@@ -48,12 +48,6 @@ def generate_launch_description():
                 output="screen",
                 parameters=[robot_description],
             ),
-            Node(
-                package="joint_state_publisher",
-                executable="joint_state_publisher",
-                name="joint_state_publisher",
-                output="screen",
-            ),
             Node(
                 package="controller_manager",
                 executable="ros2_control_node",
@@ -63,5 +57,11 @@ def generate_launch_description():
                     "stderr": "screen",
                 },
             ),
+            Node(
+                package="controller_manager",
+                executable="spawner.py",
+                parameter=["joint_state_controller"],
+                output="screen",
+            ),
         ]
     )