toid_behaviors/include/toid_behaviors/simple_move.hpp

@@ -140,6 +140,8 @@ public:
       local_rival.x = dx * cosp - dy * sinp;
       local_rival.y = dx * sinp + dy * cosp;
 
+      local_rival.x -= 0.105;
+
       const double qx = std::abs(local_rival.x) - robot_length_ / 2.0;
       const double qy = std::abs(local_rival.y) - robot_width_ / 2.0;
 
@@ -149,7 +151,7 @@ public:
       double length = std::sqrt(mqx * mqx + mqy * mqy);
 
       double sdf = length + std::min(std::max(qx, qy), 0.0);
-      RCLCPP_INFO(this->logger_, "Distance to rival %d: %lf", i++, sdf-rival_radius_);
+      RCLCPP_DEBUG(this->logger_, "Distance to rival %d: %lf", i++, sdf-rival_radius_);
       if (sdf < rival_radius_) {
         return true;
       }
@@ -168,7 +170,7 @@ protected:
 
   double robot_width_ = 0.30;
   double robot_length_ = 0.30;
-  double rival_radius_ = 0.30;
+  double rival_radius_ = 0.22;
 
   Rival::SharedPtr rivals_;
   rclcpp::Subscription<Rival>::SharedPtr rivals_sub_;

toid_bot_description/src/toid_bot_control.xacro

@@ -1,12 +1,13 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:macro name="toid_control" params="name:=toid_control use_mock_hardware:=^|true">
+  <xacro:macro name="toid_control" params="name:=toid_control use_mock_hardware:=^|true serial_port:=/dev/ttyACM0">
 
     <ros2_control name="${name}" type="system">
       <xacro:unless value="${use_mock_hardware}">
         <hardware>
             <plugin>toid_control/StepperInterface</plugin>
+            <param name="serial_port">${serial_port}</param>
         </hardware>
       </xacro:unless>
       <xacro:if value="${use_mock_hardware}">

toid_bot_description/src/toid_bot_description.urdf

@@ -125,6 +125,6 @@
 <xacro:cstr prefix="right" y_reflect="-1" />
 
 <xacro:include filename="$(find toid_bot_description)/src/toid_bot_control.xacro"/>
-<xacro:toid_control name="toid_bot_control" use_mock_hardware="$(arg use_mock)"/>
+<xacro:toid_control name="toid_bot_control" use_mock_hardware="$(arg use_mock)" serial_port="$(arg serial_port)"/>
 
 </robot>

toid_control/launch/toid.launch.py

@@ -1,97 +1,106 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition, UnlessCondition 
+from launch.conditions import IfCondition, UnlessCondition
 from launch.substitutions import Command, LaunchConfiguration, IfElseSubstitution
 from launch_ros.actions import Node, LifecycleNode
 from launch_ros.substitutions import FindPackageShare
 import os
 
+
 def generate_launch_description():
 
-    pkg_share = FindPackageShare("").find('toid_control')
+    pkg_share = FindPackageShare("").find("toid_control")
-    params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml')
+    params = os.path.join(pkg_share, "params", "toid_general_params.yaml")
-    default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'rviz.rviz')
+    default_rviz_config_path = os.path.join(pkg_share, "rviz", "rviz.rviz")
 
-    description_pkg_share = FindPackageShare("").find('toid_bot_description')
+    description_pkg_share = FindPackageShare("").find("toid_bot_description")
     default_model_path = os.path.join(
-        description_pkg_share, 
+        description_pkg_share, "src", "toid_bot_description.urdf"
-        'src', 
-        'toid_bot_description.urdf'
     )
 
     visualize = LaunchConfiguration("visualize")
 
     visualize_arg = DeclareLaunchArgument(
-        'visualize',
+        "visualize", default_value="False", description="Whether to launch rviz2"
-        default_value='False',
-        description="Whether to launch rviz2" 
     )
 
     use_mock = LaunchConfiguration("use_mock")
 
     use_mock_arg = DeclareLaunchArgument(
-        'use_mock',
+        "use_mock", default_value="True", description="Whether to use mock controller"
-        default_value='True',
-        description="Whether to use mock controller" 
     )
 
     odom_broadcast = Node(
-        package='toid_odometry',
+        package="toid_odometry",
-        executable='toid_odometry',
+        executable="toid_odometry",
-        name='map_to_odom_broadcaster',
+        name="map_to_odom_broadcaster",
         emulate_tty=True,
-        parameters=[{'mock_odom': use_mock}],
+        parameters=[
+            {
+                "mock_odom": use_mock,
+                "serial_port": "/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if02",
+            }
+        ],
     )
 
-
     robot_state_publisher = Node(
-        package='robot_state_publisher',
+        package="robot_state_publisher",
-        executable='robot_state_publisher',
+        executable="robot_state_publisher",
-        name='robot_state_publisher',
+        name="robot_state_publisher",
-        output='screen',
+        output="screen",
         emulate_tty=True,
-        parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_mock:=', use_mock])}]
+        parameters=[
+            {
+                "robot_description": Command(
+                    [
+                        "xacro ",
+                        default_model_path,
+                        " use_mock:=",
+                        use_mock,
+                        " serial_port:=/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_E66368254F4F4036-if02",
+                    ]
+                )
+            }
+        ],
     )
 
     controller_manager = Node(
-        package='controller_manager',
+        package="controller_manager",
-        executable='ros2_control_node',
+        executable="ros2_control_node",
-        output='screen',
+        output="screen",
         emulate_tty=True,
         parameters=[params],
-        arguments=['--ros-args', '--log-level', 'warn']
+        arguments=["--ros-args", "--log-level", "warn"],
     )
 
     joint_state_broadcaster = Node(
-        package='controller_manager',
+        package="controller_manager",
-        executable='spawner',
+        executable="spawner",
-        output='screen',
+        output="screen",
         emulate_tty=True,
-        arguments=[
+        arguments=["joint_state_broadcaster", "--ros-args", "--log-level", "warn"],
-            "joint_state_broadcaster",
-            '--ros-args', '--log-level', 'warn'
-            ]
     )
 
     velocity_controller = Node(
-        package='controller_manager',
+        package="controller_manager",
-        executable='spawner',
+        executable="spawner",
-        output='screen',
+        output="screen",
         emulate_tty=True,
         arguments=[
             "velocity_controller",
             "--inactive",
             "-p",
             params,
-            '--ros-args', '--log-level', 'warn'
+            "--ros-args",
-            ],
+            "--log-level",
+            "warn",
+        ],
     )
 
-
     diffbot_base_controller = Node(
-        package='controller_manager',
+        package="controller_manager",
-        executable='spawner',
+        executable="spawner",
-        output='both',
+        output="both",
         emulate_tty=True,
         arguments=[
             "diffdrive_controller",
@@ -102,39 +111,43 @@ def generate_launch_description():
             "--controller-ros-args",
             "-r diffdrive_controller/odom:=/odom",
             "--controller-ros-args",
-            IfElseSubstitution(use_mock, 
+            IfElseSubstitution(
-                "--param odom_frame_id:=odom", 
+                use_mock,
-                "--param odom_frame_id:=odom_expected"
+                "--param odom_frame_id:=odom",
+                "--param odom_frame_id:=odom_expected",
             ),
             "--controller-ros-args",
-            IfElseSubstitution(use_mock, 
+            IfElseSubstitution(
-                "--param enable_odom_tf:=true", 
+                use_mock,
-                "--param enable_odom_tf:=false"
+                "--param enable_odom_tf:=true",
+                "--param enable_odom_tf:=false",
             ),
-            '--ros-args', '--log-level', 'warn'
+            "--ros-args",
-        ]
+            "--log-level",
+            "warn",
+        ],
     )
 
     rviz_node = Node(
-        package='rviz2',
+        package="rviz2",
-        executable='rviz2',
+        executable="rviz2",
-        name='rviz2',
+        name="rviz2",
-        output='screen',
+        output="screen",
         emulate_tty=True,
-        arguments=['-d', default_rviz_config_path, 
+        arguments=["-d", default_rviz_config_path, "--ros-args", "--log-level", "warn"],
-                   '--ros-args', '--log-level', 'warn'
+        condition=IfCondition(visualize),
-                   ],
-        condition=IfCondition(visualize)
     )
 
-    return LaunchDescription([
+    return LaunchDescription(
-        visualize_arg,
+        [
-        use_mock_arg,
+            visualize_arg,
-        odom_broadcast,
+            use_mock_arg,
-        robot_state_publisher,
+            odom_broadcast,
-        controller_manager,
+            robot_state_publisher,
-        joint_state_broadcaster,
+            controller_manager,
-        diffbot_base_controller,
+            joint_state_broadcaster,
-        velocity_controller,
+            diffbot_base_controller,
-        rviz_node
+            velocity_controller,
-    ])
+            rviz_node,
+        ]
+    )