154 lines
4.5 KiB
Python
154 lines
4.5 KiB
Python
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument
|
|
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")
|
|
|
|
description_pkg_share = FindPackageShare("").find("toid_bot_description")
|
|
default_model_path = os.path.join(
|
|
description_pkg_share, "src", "toid_bot_description.urdf"
|
|
)
|
|
|
|
visualize = LaunchConfiguration("visualize")
|
|
|
|
visualize_arg = DeclareLaunchArgument(
|
|
"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"
|
|
)
|
|
|
|
odom_broadcast = Node(
|
|
package="toid_odometry",
|
|
executable="toid_odometry",
|
|
name="map_to_odom_broadcaster",
|
|
emulate_tty=True,
|
|
parameters=[
|
|
{
|
|
"mock_odom": use_mock,
|
|
"serial_port": "/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_504430456075AF1C-if02",
|
|
}
|
|
],
|
|
)
|
|
|
|
robot_state_publisher = Node(
|
|
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,
|
|
" serial_port:=/dev/serial/by-id/usb-Mg_Robotics_Magrob_Odometry_MCU_504430456075AF1C-if00",
|
|
]
|
|
)
|
|
}
|
|
],
|
|
)
|
|
|
|
controller_manager = Node(
|
|
package="controller_manager",
|
|
executable="ros2_control_node",
|
|
output="screen",
|
|
emulate_tty=True,
|
|
parameters=[params],
|
|
arguments=["--ros-args", "--log-level", "warn"],
|
|
)
|
|
|
|
joint_state_broadcaster = Node(
|
|
package="controller_manager",
|
|
executable="spawner",
|
|
output="screen",
|
|
emulate_tty=True,
|
|
arguments=["joint_state_broadcaster", "--ros-args", "--log-level", "warn"],
|
|
)
|
|
|
|
velocity_controller = Node(
|
|
package="controller_manager",
|
|
executable="spawner",
|
|
output="screen",
|
|
emulate_tty=True,
|
|
arguments=[
|
|
"velocity_controller",
|
|
"--inactive",
|
|
"-p",
|
|
params,
|
|
"--ros-args",
|
|
"--log-level",
|
|
"warn",
|
|
],
|
|
)
|
|
|
|
diffbot_base_controller = Node(
|
|
package="controller_manager",
|
|
executable="spawner",
|
|
output="both",
|
|
emulate_tty=True,
|
|
arguments=[
|
|
"diffdrive_controller",
|
|
"-p",
|
|
params,
|
|
"--controller-ros-args",
|
|
"-r diffdrive_controller/cmd_vel:=/cmd_vel",
|
|
"--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",
|
|
),
|
|
"--controller-ros-args",
|
|
IfElseSubstitution(
|
|
use_mock,
|
|
"--param enable_odom_tf:=true",
|
|
"--param enable_odom_tf:=false",
|
|
),
|
|
"--ros-args",
|
|
"--log-level",
|
|
"warn",
|
|
],
|
|
)
|
|
|
|
rviz_node = Node(
|
|
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),
|
|
)
|
|
|
|
return LaunchDescription(
|
|
[
|
|
visualize_arg,
|
|
use_mock_arg,
|
|
odom_broadcast,
|
|
robot_state_publisher,
|
|
controller_manager,
|
|
joint_state_broadcaster,
|
|
diffbot_base_controller,
|
|
velocity_controller,
|
|
rviz_node,
|
|
]
|
|
)
|