m5p3nc3r
ec554bcf2c
Uses the mecanum controller properly across physical and virtual There is a timing issue with i2c which is why the control update is limited to 10hz The sonar and LED's arent yet working, this will come soon.
120 lines
4.5 KiB
Python
120 lines
4.5 KiB
Python
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, RegisterEventHandler,IncludeLaunchDescription
|
|
from launch.conditions import IfCondition,UnlessCondition
|
|
from launch.event_handlers import OnProcessExit
|
|
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
|
|
|
|
from launch_ros.actions import Node, PushRosNamespace
|
|
from launch_ros.parameter_descriptions import ParameterValue
|
|
from launch_ros.substitutions import FindPackageShare
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
|
|
def generate_launch_description():
|
|
use_sim_time = LaunchConfiguration('use_sim_time')
|
|
use_hardware = LaunchConfiguration('use_hardware')
|
|
namespace = LaunchConfiguration('namespace')
|
|
enable_camera = LaunchConfiguration('enable_camera')
|
|
|
|
declared_args = [
|
|
DeclareLaunchArgument(
|
|
'namespace', default_value='',
|
|
description='ROS namespace for all robot nodes (e.g. "robot1").'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'use_sim_time', default_value='false',
|
|
description='Use simulation time.'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'use_hardware', default_value='false',
|
|
description='Load the real I2C hardware interface plugin (set true on physical robot).'
|
|
),
|
|
DeclareLaunchArgument(
|
|
'enable_camera', default_value='false',
|
|
description='Enable the camera node.'
|
|
),
|
|
]
|
|
|
|
|
|
robot_controllers = PathJoinSubstitution([
|
|
FindPackageShare('raspbot_v2_control'), 'config', 'controllers.yaml'
|
|
])
|
|
|
|
robot_description_content = Command([
|
|
PathJoinSubstitution([FindExecutable(name='xacro')]),
|
|
' ',
|
|
PathJoinSubstitution([
|
|
FindPackageShare('raspbot_v2_description'), 'urdf', 'raspbot_v2.urdf.xacro'
|
|
]),
|
|
' use_sim_time:=', use_sim_time,
|
|
' use_hardware:=', use_hardware,
|
|
' controller_config:=', robot_controllers,
|
|
])
|
|
robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)}
|
|
|
|
control_node = Node(
|
|
package='controller_manager',
|
|
executable='ros2_control_node',
|
|
parameters=[robot_description, robot_controllers],
|
|
output='both',
|
|
remappings=[
|
|
# ("~/robot_description", "robot_description"),
|
|
("mecanum_drive_controller/reference", "/cmd_vel"),
|
|
("mecanum_drive_controller/odometry", "/odom"),
|
|
("mecanum_drive_controller/tf_odometry", "/tf"),
|
|
],
|
|
condition=UnlessCondition(use_sim_time),
|
|
)
|
|
|
|
robot_state_publisher_node = Node(
|
|
package='robot_state_publisher',
|
|
executable='robot_state_publisher',
|
|
output='both',
|
|
parameters=[robot_description],
|
|
)
|
|
|
|
joint_state_broadcaster_spawner = Node(
|
|
package='controller_manager',
|
|
executable='spawner',
|
|
arguments=['joint_state_broadcaster', '--controller-manager',
|
|
PathJoinSubstitution(["/", namespace, "controller_manager"]),
|
|
'--controller-manager-timeout', '30',
|
|
],
|
|
)
|
|
|
|
robot_controller_spawner = Node(
|
|
package='controller_manager',
|
|
executable='spawner',
|
|
arguments=['mecanum_drive_controller', '--controller-manager',
|
|
PathJoinSubstitution(["/", namespace, "controller_manager"]),
|
|
'--controller-manager-timeout', '30',
|
|
]
|
|
)
|
|
|
|
# Delay starting the robot controller until the joint state broadcaster has finished starting up.
|
|
delay_robot_controller_after_joint_state_broadcaster = RegisterEventHandler(
|
|
event_handler=OnProcessExit(
|
|
target_action=joint_state_broadcaster_spawner,
|
|
on_exit=[robot_controller_spawner],
|
|
),
|
|
)
|
|
|
|
pan_tilt_controller_spawner = Node(
|
|
package='controller_manager',
|
|
executable='spawner',
|
|
arguments=['pan_tilt_controller', '--controller-manager',
|
|
PathJoinSubstitution(["/", namespace, "controller_manager"]),
|
|
'--controller-manager-timeout', '30',
|
|
],
|
|
condition=IfCondition(enable_camera),
|
|
)
|
|
|
|
return LaunchDescription([
|
|
*declared_args,
|
|
PushRosNamespace(namespace),
|
|
control_node,
|
|
robot_state_publisher_node,
|
|
joint_state_broadcaster_spawner,
|
|
delay_robot_controller_after_joint_state_broadcaster,
|
|
pan_tilt_controller_spawner,
|
|
])
|