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.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_;

3
toid_bot_description/src/toid_bot_control.xacro
View File

@@ -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}">

2
toid_bot_description/src/toid_bot_description.urdf
View File

@@ -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>

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
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,
IfElseSubstitution(
use_mock,
"--param odom_frame_id:=odom",
"--param odom_frame_id:=odom_expected"
"--param odom_frame_id:=odom_expected",
),
"--controller-ros-args",
IfElseSubstitution(use_mock,
IfElseSubstitution(
use_mock,
"--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(
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,
]
)