Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wip-behaviors #3

Merged
Pimpest merged 86 commits from wip-behaviors into main 2026-05-28 06:21:26 +00:00
Conversation 0 Commits 86 Files Changed 129 +91 -75
4 changed files with 91 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 733a774c37 - Show all commits

6
toid_behaviors/include/toid_behaviors/simple_move.hpp
View File

@@ -140,6 +140,8 @@ public:
local_rival.x = dx * cosp - dy * sinp; local_rival.x = dx * cosp - dy * sinp;
local_rival.y = dx * sinp + dy * cosp; 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 qx = std::abs(local_rival.x) - robot_length_ / 2.0;
const double qy = std::abs(local_rival.y) - robot_width_ / 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 length = std::sqrt(mqx * mqx + mqy * mqy);
double sdf = length + std::min(std::max(qx, qy), 0.0); 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_) { if (sdf < rival_radius_) {
return true; return true;
} }
@@ -168,7 +170,7 @@ protected:
double robot_width_ = 0.30; double robot_width_ = 0.30;
double robot_length_ = 0.30; double robot_length_ = 0.30;
double rival_radius_ = 0.30; double rival_radius_ = 0.22;
Rival::SharedPtr rivals_; Rival::SharedPtr rivals_;
rclcpp::Subscription<Rival>::SharedPtr rivals_sub_; rclcpp::Subscription<Rival>::SharedPtr rivals_sub_;

3
toid_bot_description/src/toid_bot_control.xacro
View File

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

2
toid_bot_description/src/toid_bot_description.urdf
View File

@@ -125,6 +125,6 @@
<xacro:cstr prefix="right" y_reflect="-1" /> <xacro:cstr prefix="right" y_reflect="-1" />
<xacro:include filename="$(find toid_bot_description)/src/toid_bot_control.xacro"/> <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> </robot>

149
toid_control/launch/toid.launch.py
View File

@@ -6,92 +6,101 @@ from launch_ros.actions import Node, LifecycleNode
from launch_ros.substitutions import FindPackageShare from launch_ros.substitutions import FindPackageShare
import os import os
def generate_launch_description(): 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( 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 = LaunchConfiguration("visualize")
visualize_arg = DeclareLaunchArgument( 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 = LaunchConfiguration("use_mock")
use_mock_arg = DeclareLaunchArgument( 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( 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, 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( 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, 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( controller_manager = Node(
package='controller_manager', package="controller_manager",
executable='ros2_control_node', executable="ros2_control_node",
output='screen', output="screen",
emulate_tty=True, emulate_tty=True,
parameters=[params], parameters=[params],
arguments=['--ros-args', '--log-level', 'warn'] arguments=["--ros-args", "--log-level", "warn"],
) )
joint_state_broadcaster = Node( joint_state_broadcaster = Node(
package='controller_manager', package="controller_manager",
executable='spawner', executable="spawner",
output='screen', output="screen",
emulate_tty=True, emulate_tty=True,
arguments=[ arguments=["joint_state_broadcaster", "--ros-args", "--log-level", "warn"],
"joint_state_broadcaster",
'--ros-args', '--log-level', 'warn'
]
) )
velocity_controller = Node( velocity_controller = Node(
package='controller_manager', package="controller_manager",
executable='spawner', executable="spawner",
output='screen', output="screen",
emulate_tty=True, emulate_tty=True,
arguments=[ arguments=[
"velocity_controller", "velocity_controller",
"--inactive", "--inactive",
"-p", "-p",
params, params,
'--ros-args', '--log-level', 'warn' "--ros-args",
], "--log-level",
"warn",
],
) )
diffbot_base_controller = Node( diffbot_base_controller = Node(
package='controller_manager', package="controller_manager",
executable='spawner', executable="spawner",
output='both', output="both",
emulate_tty=True, emulate_tty=True,
arguments=[ arguments=[
"diffdrive_controller", "diffdrive_controller",
@@ -102,39 +111,43 @@ def generate_launch_description():
"--controller-ros-args", "--controller-ros-args",
"-r diffdrive_controller/odom:=/odom", "-r diffdrive_controller/odom:=/odom",
"--controller-ros-args", "--controller-ros-args",
IfElseSubstitution(use_mock, IfElseSubstitution(
use_mock,
"--param odom_frame_id:=odom", "--param odom_frame_id:=odom",
"--param odom_frame_id:=odom_expected" "--param odom_frame_id:=odom_expected",
), ),
"--controller-ros-args", "--controller-ros-args",
IfElseSubstitution(use_mock, IfElseSubstitution(
use_mock,
"--param enable_odom_tf:=true", "--param enable_odom_tf:=true",
"--param enable_odom_tf:=false" "--param enable_odom_tf:=false",
), ),
'--ros-args', '--log-level', 'warn' "--ros-args",
] "--log-level",
"warn",
],
) )
rviz_node = Node( rviz_node = Node(
package='rviz2', package="rviz2",
executable='rviz2', executable="rviz2",
name='rviz2', name="rviz2",
output='screen', output="screen",
emulate_tty=True, 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,
]
)