Added the ability to switch between cmd_vel control and induvidual motor control

scripts/test_wheel.sh

@@ -17,8 +17,6 @@ fi
 
 source install/setup.bash
 
-trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT
-
 case $TARGET in
     "forward") 
         ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
@@ -75,8 +73,19 @@ case $TARGET in
     "zero")
         ros2 service call /zero std_srvs/srv/Empty
         ;;
+    "vel")
+        ros2 topic pub --rate 10 /velocity_controller/commands std_msgs/msg/Float64MultiArray "
+          data: [$2, $3]"
+        ;;
+    "switch")
+        controllers=$(ros2 control list_controllers | grep -E "velocity|diffdrive")
+        to_deactivate=$(echo "$controllers" | awk '$3 == "active" { print $1 }')
+        to_activate=$(echo "$controllers" | awk '$3 == "inactive" { print $1 }')
+        ros2 control switch_controllers --deactivate $to_deactivate --activate $to_activate --best-effort
+        ;;
     *)
         echo "Target not defined"
         exit 1
         ;;
 esac
+exit 0

toid_control/launch/toid.launch.py

@@ -35,18 +35,11 @@ def generate_launch_description():
         description="Whether to use mock controller" 
     )
 
-    # odom_broadcast = Node(
-    #     package='tf2_ros',
-    #     executable='static_transform_publisher',
-    #     name='map_to_odom_broadcaster',
-    #     arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
-    #     condition=IfCondition(LaunchConfiguration('visualize'))
-    # )
-
     odom_broadcast = Node(
         package='toid_odometry',
         executable='toid_odometry',
         name='map_to_odom_broadcaster',
+        emulate_tty=True,
         parameters=[{'mock_odom': use_mock}],
         condition=IfCondition(LaunchConfiguration('visualize'))
     )
@@ -57,6 +50,7 @@ def generate_launch_description():
         executable='robot_state_publisher',
         name='robot_state_publisher',
         output='screen',
+        emulate_tty=True,
         parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_mock:=', use_mock])}]
     )
 
@@ -64,20 +58,42 @@ def generate_launch_description():
         package='controller_manager',
         executable='ros2_control_node',
         output='screen',
-        parameters=[params]
+        emulate_tty=True,
+        parameters=[params],
+        arguments=['--ros-args', '--log-level', 'warn']
     )
 
     joint_state_broadcaster = Node(
         package='controller_manager',
         executable='spawner',
         output='screen',
-        arguments=["joint_state_broadcaster"]
+        emulate_tty=True,
+        arguments=[
+            "joint_state_broadcaster",
+            '--ros-args', '--log-level', 'warn'
+            ]
     )
 
+    velocity_controller = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='screen',
+        emulate_tty=True,
+        arguments=[
+            "velocity_controller",
+            "--inactive",
+            "-p",
+            params,
+            '--ros-args', '--log-level', 'warn'
+            ],
+    )
+
+
     diffbot_base_controller = Node(
         package='controller_manager',
         executable='spawner',
         output='both',
+        emulate_tty=True,
         arguments=[
             "diffdrive_controller",
             "-p",
@@ -96,6 +112,7 @@ def generate_launch_description():
                 "--param enable_odom_tf:=true", 
                 "--param enable_odom_tf:=false"
             ),
+            '--ros-args', '--log-level', 'warn'
         ]
     )
 
@@ -104,7 +121,10 @@ def generate_launch_description():
         executable='rviz2',
         name='rviz2',
         output='screen',
-        arguments=['-d', default_rviz_config_path],
+        emulate_tty=True,
+        arguments=['-d', default_rviz_config_path, 
+                   '--ros-args', '--log-level', 'warn'
+                   ],
         condition=IfCondition(visualize)
     )
 
@@ -116,5 +136,6 @@ def generate_launch_description():
         controller_manager,
         joint_state_broadcaster,
         diffbot_base_controller,
+        velocity_controller,
         rviz_node
     ])

toid_control/package.xml

@@ -21,6 +21,7 @@
 
   <depend>controller_manager</depend>
   <depend>diff_drive_controller</depend>
+  <depend>forward_command_controller</depend>
   <depend>robot_state_publisher</depend>
   <depend>joint_state_broadcaster</depend>
 

toid_control/params/toid_general_params.yaml

@@ -6,6 +6,15 @@ controller_manager:
       type: diff_drive_controller/DiffDriveController
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
+    velocity_controller:
+      type: forward_command_controller/ForwardCommandController
+
+velocity_controller:
+  ros__parameters:
+    joints:
+     - drivewhl_l_joint
+     - drivewhl_r_joint
+    interface_name: velocity
 
 diffdrive_controller:
   ros__parameters:

toid_odometry/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED)
 set(PACKAGE_DEPS
   rclcpp
   Boost
+  nav_msgs
   std_srvs
   tf2
   tf2_ros

toid_odometry/package.xml

@@ -11,6 +11,7 @@
 
   <depend>rclcpp</depend>
   <depend>boost</depend>
+  <depend>nav_msgs</depend>
   <depend>std_srvs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>

toid_odometry/src/odometry.cpp

@@ -23,9 +23,20 @@
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2/convert.hpp"
 
+#include "nav_msgs/msg/odometry.hpp"
+
 
 namespace asio = boost::asio;
 
+const std::array<double, 36> COVARIENCE_MATRIX{
+    0.0001, .0, 0, 0, 0, 0,
+    0, 0.0001, 0, 0, 0, 0,
+    0, .0, 0.0001, 0, 0, 0,
+    0, .0, 0, 0.0001, 0, 0,
+    0, .0, 0, 0, 0.0001, 0,
+    0, .0, 0, 0, 0, 0.0001,
+}; 
+
 class ToidOdomNode : public rclcpp::Node {
     using SendDoubleSrv = toid_msgs::srv::SendDouble;
     using ZeroSrv       = std_srvs::srv::Empty;
@@ -78,6 +89,9 @@ public:
 
             zero_service_ = create_service<ZeroSrv> (
                 "/zero", std::bind(&ToidOdomNode::zero, this, _1));
+
+            odometry_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>(
+                "/odom", 1);
         }
 
     }
@@ -179,13 +193,30 @@ private:
 
     void send_transform(tRespState &state) {
         geometry_msgs::msg::TransformStamped t;
+        nav_msgs::msg::Odometry o;
 
         t.header.stamp = this->get_clock()->now();
         t.header.frame_id = this->odometry_frame_id_;
         t.child_frame_id = this->base_frame_id_;
 
+        o.header = t.header;
+        o.child_frame_id = t.child_frame_id;
+
         t.transform = make_transform(state.x, state.y, state.theta);
 
+        const auto& [x,y,z] = t.transform.translation;
+        o.pose.pose.position.x = x;
+        o.pose.pose.position.x = y;
+        o.pose.pose.position.x = z;
+        o.pose.pose.orientation = t.transform.rotation;
+        o.pose.covariance = COVARIENCE_MATRIX;
+
+        o.twist.twist.linear.x = state.vl;
+        o.twist.twist.angular.z = state.vr;
+        o.twist.covariance = COVARIENCE_MATRIX;
+
+        odometry_publisher_->publish(o);
+
         tf_broadcaster_->sendTransform(t);
     }
 
@@ -237,6 +268,7 @@ private:
     rclcpp::Service<ZeroSrv>::SharedPtr zero_service_;
 
     rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr startpose_listener_;
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_publisher_;
 };
 
 int main(const int argc,const char** argv) {