254 lines
9.0 KiB
Python
254 lines
9.0 KiB
Python
# Copyright 2020 ros2_control Development Team
|
|
#
|
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
# you may not use this file except in compliance with the License.
|
|
# You may obtain a copy of the License at
|
|
#
|
|
# http://www.apache.org/licenses/LICENSE-2.0
|
|
#
|
|
# Unless required by applicable law or agreed to in writing, software
|
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
# See the License for the specific language governing permissions and
|
|
# limitations under the License.
|
|
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, RegisterEventHandler
|
|
from launch.conditions import IfCondition
|
|
from launch.event_handlers import OnProcessExit
|
|
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
|
|
|
|
from launch_ros.actions import Node
|
|
from launch_ros.substitutions import FindPackageShare
|
|
|
|
def generate_launch_description():
|
|
# Declare arguments
|
|
declared_arguments = []
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"gui",
|
|
default_value="false",
|
|
description="Start RViz2 automatically with this launch file.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"joy",
|
|
default_value="true",
|
|
description="Start joy node with this launch file.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"use_mock_hardware",
|
|
default_value="false",
|
|
description="Start robot with mock hardware mirroring command to its states.",
|
|
)
|
|
)
|
|
lidar_serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200')
|
|
lidar_channel_type = LaunchConfiguration('channel_type', default='serial')
|
|
lidar_frame_id = LaunchConfiguration('frame_id', default='laser')
|
|
lidar_serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0')
|
|
lidar_inverted = LaunchConfiguration('inverted', default='false')
|
|
lidar_angle_compensate = LaunchConfiguration('angle_compensate', default='true')
|
|
lidar_scan_mode = LaunchConfiguration('scan_mode', default='Standard')
|
|
joy_dev = LaunchConfiguration('joy_dev', default='/dev/input/js0')
|
|
|
|
# Initialize Arguments
|
|
gui = LaunchConfiguration("gui")
|
|
joy = LaunchConfiguration("joy")
|
|
use_mock_hardware = LaunchConfiguration("use_mock_hardware")
|
|
|
|
# Get URDF via xacro
|
|
robot_description_content = Command(
|
|
[
|
|
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
|
" ",
|
|
PathJoinSubstitution(
|
|
[FindPackageShare("cyphal_hardware_interface"), "urdf", "staubi.urdf.xacro"]
|
|
),
|
|
" ",
|
|
"use_mock_hardware:=",
|
|
use_mock_hardware,
|
|
]
|
|
)
|
|
robot_description = {"robot_description": robot_description_content}
|
|
|
|
robot_controllers = PathJoinSubstitution(
|
|
[
|
|
FindPackageShare("cyphal_hardware_interface"),
|
|
"config",
|
|
"staubi_controllers.yaml",
|
|
]
|
|
)
|
|
rviz_config_file = PathJoinSubstitution(
|
|
[FindPackageShare("cyphal_hardware_interface"), "staubi/rviz", "staubi.rviz"]
|
|
)
|
|
|
|
|
|
|
|
lidar_ld = [
|
|
DeclareLaunchArgument(
|
|
'channel_type',
|
|
default_value=lidar_channel_type,
|
|
description='Specifying channel type of lidar'),
|
|
|
|
DeclareLaunchArgument(
|
|
'serial_port',
|
|
default_value=lidar_serial_port,
|
|
description='Specifying usb port to connected lidar'),
|
|
|
|
DeclareLaunchArgument(
|
|
'serial_baudrate',
|
|
default_value=lidar_serial_baudrate,
|
|
description='Specifying usb port baudrate to connected lidar'),
|
|
|
|
DeclareLaunchArgument(
|
|
'frame_id',
|
|
default_value=lidar_frame_id,
|
|
description='Specifying frame_id of lidar'),
|
|
|
|
DeclareLaunchArgument(
|
|
'inverted',
|
|
default_value=lidar_inverted,
|
|
description='Specifying whether or not to invert scan data'),
|
|
|
|
DeclareLaunchArgument(
|
|
'angle_compensate',
|
|
default_value=lidar_angle_compensate,
|
|
description='Specifying whether or not to enable angle_compensate of scan data'),
|
|
DeclareLaunchArgument(
|
|
'scan_mode',
|
|
default_value=lidar_scan_mode,
|
|
description='Specifying scan mode of lidar'),
|
|
|
|
Node(
|
|
package='rplidar_ros',
|
|
executable='rplidar_node',
|
|
name='rplidar_node',
|
|
parameters=[{'channel_type':lidar_channel_type,
|
|
'serial_port': lidar_serial_port,
|
|
'serial_baudrate': lidar_serial_baudrate,
|
|
'frame_id': lidar_frame_id,
|
|
'inverted': lidar_inverted,
|
|
'angle_compensate': lidar_angle_compensate,
|
|
'scan_mode': lidar_scan_mode
|
|
}],
|
|
output='screen'),]
|
|
|
|
control_node = Node(
|
|
package="controller_manager",
|
|
executable="ros2_control_node",
|
|
parameters=[robot_description, robot_controllers],
|
|
output="both",
|
|
remappings=[
|
|
("/vacuum_controller/joy", "/joy"),
|
|
("/staubi_base_controller/cmd_vel", "/cmd_vel_nav"),
|
|
],
|
|
)
|
|
robot_state_pub_node = Node(
|
|
package="robot_state_publisher",
|
|
executable="robot_state_publisher",
|
|
output="both",
|
|
parameters=[robot_description],
|
|
# remappings=[
|
|
# ("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"),
|
|
# ],
|
|
)
|
|
rviz_node = Node(
|
|
package="rviz2",
|
|
executable="rviz2",
|
|
name="rviz2",
|
|
output="log",
|
|
arguments=["-d", rviz_config_file],
|
|
condition=IfCondition(gui),
|
|
)
|
|
|
|
joint_state_broadcaster_spawner = Node(
|
|
package="controller_manager",
|
|
executable="spawner",
|
|
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
|
|
)
|
|
|
|
robot_controller_spawner = Node(
|
|
package="controller_manager",
|
|
executable="spawner",
|
|
arguments=["staubi_base_controller", "--controller-manager", "/controller_manager"],
|
|
)
|
|
|
|
vacuum_controller_spawner = Node(
|
|
package="controller_manager",
|
|
executable="spawner",
|
|
arguments=["vacuum_controller", "--controller-manager", "/controller_manager"],
|
|
)
|
|
|
|
# Delay rviz start after `joint_state_broadcaster`
|
|
delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
|
|
event_handler=OnProcessExit(
|
|
target_action=joint_state_broadcaster_spawner,
|
|
on_exit=[rviz_node],
|
|
)
|
|
)
|
|
|
|
# Delay start of robot_controller after `joint_state_broadcaster`
|
|
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
|
|
event_handler=OnProcessExit(
|
|
target_action=joint_state_broadcaster_spawner,
|
|
on_exit=[robot_controller_spawner],
|
|
)
|
|
)
|
|
|
|
delay_vacuum_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
|
|
event_handler=OnProcessExit(
|
|
target_action=joint_state_broadcaster_spawner,
|
|
on_exit=[vacuum_controller_spawner],
|
|
)
|
|
)
|
|
|
|
|
|
|
|
joy_ld = [
|
|
DeclareLaunchArgument('joy_vel', default_value='/cmd_vel_nav'),
|
|
DeclareLaunchArgument('joy_config', default_value='ps3'),
|
|
DeclareLaunchArgument('joy_dev', default_value=joy_dev),
|
|
Node(
|
|
package='joy_linux', executable='joy_linux_node', name='joy_linux_node',
|
|
parameters=[{
|
|
'dev': joy_dev,
|
|
# 'deadzone': 0.3,
|
|
# 'autorepeat_rate': 20.0,
|
|
}],
|
|
condition=IfCondition(joy),
|
|
),
|
|
Node(
|
|
package='teleop_twist_joy', executable='teleop_node',
|
|
name='teleop_twist_joy_node', parameters=[ {
|
|
"publish_stamped_twist": True,
|
|
# "require_enable_button": False,
|
|
"enable_button": 10,
|
|
# "scale_linear.x":3.0,
|
|
"axis_angular.yaw": 2,
|
|
"axis_linear.x": 3,
|
|
"scale_linear.x": 0.5,
|
|
"scale_angular.yaw": 1.6,
|
|
"scale_linear_turbo.x": 1.8,
|
|
"scale_angular_turbo.yaw": 2.6,
|
|
"enable_turbo_button": 7,
|
|
"frame": "base_link",
|
|
}],
|
|
# condition=IfCondition(joystick),
|
|
remappings={('/cmd_vel', LaunchConfiguration('joy_vel'))},
|
|
),
|
|
]
|
|
|
|
nodes = [
|
|
control_node,
|
|
robot_state_pub_node,
|
|
joint_state_broadcaster_spawner,
|
|
delay_rviz_after_joint_state_broadcaster_spawner,
|
|
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
|
|
delay_vacuum_controller_spawner_after_joint_state_broadcaster_spawner,
|
|
] + joy_ld + lidar_ld
|
|
|
|
return LaunchDescription(declared_arguments + nodes)
|