diff --git a/toid_behaviors/include/toid_behaviors/simple_move.hpp b/toid_behaviors/include/toid_behaviors/simple_move.hpp index 135e6bf..673dd39 100644 --- a/toid_behaviors/include/toid_behaviors/simple_move.hpp +++ b/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::SharedPtr rivals_sub_; diff --git a/toid_bot_description/src/toid_bot_control.xacro b/toid_bot_description/src/toid_bot_control.xacro index 4117934..7ab6048 100644 --- a/toid_bot_description/src/toid_bot_control.xacro +++ b/toid_bot_description/src/toid_bot_control.xacro @@ -1,12 +1,13 @@ - + toid_control/StepperInterface + ${serial_port} diff --git a/toid_bot_description/src/toid_bot_description.urdf b/toid_bot_description/src/toid_bot_description.urdf index 55efc49..7c24d29 100644 --- a/toid_bot_description/src/toid_bot_description.urdf +++ b/toid_bot_description/src/toid_bot_description.urdf @@ -125,6 +125,6 @@ - + \ No newline at end of file diff --git a/toid_control/launch/toid.launch.py b/toid_control/launch/toid.launch.py index 93fa682..b2f52d5 100644 --- a/toid_control/launch/toid.launch.py +++ b/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') - params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml') - default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'rviz.rviz') + pkg_share = FindPackageShare("").find("toid_control") + params = os.path.join(pkg_share, "params", "toid_general_params.yaml") + 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, - 'src', - 'toid_bot_description.urdf' + description_pkg_share, "src", "toid_bot_description.urdf" ) visualize = LaunchConfiguration("visualize") visualize_arg = DeclareLaunchArgument( - 'visualize', - default_value='False', - description="Whether to launch rviz2" + "visualize", default_value="False", description="Whether to launch rviz2" ) use_mock = LaunchConfiguration("use_mock") use_mock_arg = DeclareLaunchArgument( - 'use_mock', - default_value='True', - description="Whether to use mock controller" + "use_mock", default_value="True", description="Whether to use mock controller" ) odom_broadcast = Node( - package='toid_odometry', - executable='toid_odometry', - name='map_to_odom_broadcaster', + package="toid_odometry", + executable="toid_odometry", + 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', - executable='robot_state_publisher', - name='robot_state_publisher', - output='screen', + package="robot_state_publisher", + 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])}] + 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', - executable='ros2_control_node', - output='screen', + package="controller_manager", + executable="ros2_control_node", + 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', - executable='spawner', - output='screen', + package="controller_manager", + executable="spawner", + output="screen", emulate_tty=True, - arguments=[ - "joint_state_broadcaster", - '--ros-args', '--log-level', 'warn' - ] + arguments=["joint_state_broadcaster", "--ros-args", "--log-level", "warn"], ) velocity_controller = Node( - package='controller_manager', - executable='spawner', - output='screen', + package="controller_manager", + executable="spawner", + 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', - executable='spawner', - output='both', + package="controller_manager", + executable="spawner", + 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, - "--param odom_frame_id:=odom", - "--param odom_frame_id:=odom_expected" + IfElseSubstitution( + use_mock, + "--param odom_frame_id:=odom", + "--param odom_frame_id:=odom_expected", ), "--controller-ros-args", - IfElseSubstitution(use_mock, - "--param enable_odom_tf:=true", - "--param enable_odom_tf:=false" + IfElseSubstitution( + use_mock, + "--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', - executable='rviz2', - name='rviz2', - output='screen', + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", emulate_tty=True, - arguments=['-d', default_rviz_config_path, - '--ros-args', '--log-level', 'warn' - ], - condition=IfCondition(visualize) + arguments=["-d", default_rviz_config_path, "--ros-args", "--log-level", "warn"], + condition=IfCondition(visualize), ) - return LaunchDescription([ - visualize_arg, - use_mock_arg, - odom_broadcast, - robot_state_publisher, - controller_manager, - joint_state_broadcaster, - diffbot_base_controller, - velocity_controller, - rviz_node - ]) + return LaunchDescription( + [ + visualize_arg, + use_mock_arg, + odom_broadcast, + robot_state_publisher, + controller_manager, + joint_state_broadcaster, + diffbot_base_controller, + velocity_controller, + rviz_node, + ] + )