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

Switched to using serial ports by id for control

Browse Source
This commit is contained in:
Pimpest
2026-04-16 04:30:21 +02:00
parent 12a83e876a
commit 733a774c37
4 changed files with 91 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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,
]
)