Migrate to new ros_control
This commit is contained in:
Bastian Hofmann
@@ -3,7 +3,7 @@ import subprocess
|
||||
|
||||
import yaml
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.actions import Node,SetParameter
|
||||
from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, RegisterEventHandler, UnsetEnvironmentVariable
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
|
||||
@@ -51,7 +51,7 @@ def load_yaml(package_name, file_path):
|
||||
|
||||
def generate_launch_description():
|
||||
robot_name = "iisy"
|
||||
# pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
|
||||
pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
|
||||
world_path = os.path.join(FindPackageShare(package="gaz_simulation").find("gaz_simulation"), "world/test2.xml")
|
||||
urdf_path = os.path.join(FindPackageShare(package="iisy_config").find("iisy_config"), "urdf/iisy.urdf")
|
||||
|
||||
@@ -64,10 +64,21 @@ def generate_launch_description():
|
||||
kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yml')
|
||||
robot_description_kinematics = {'robot_description_kinematics': kinematics_yaml}
|
||||
|
||||
ompl_planning_pipeline_config = {'move_group': {
|
||||
'planning_plugin': 'ompl_interface/OMPLPlanner',
|
||||
'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
|
||||
'start_state_max_bounds_error': 0.1}}
|
||||
robot_controllers = PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("iisy_config"),
|
||||
"config",
|
||||
"iisy_controllers.yaml",
|
||||
]
|
||||
)
|
||||
|
||||
ompl_planning_pipeline_config = {
|
||||
'move_group': {
|
||||
'planning_plugin': 'ompl_interface/OMPLPlanner',
|
||||
'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
|
||||
'start_state_max_bounds_error': 0.1
|
||||
}
|
||||
}
|
||||
ompl_planning_yaml = load_yaml('iisy_config', 'config/ompl_planning.yml')
|
||||
ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
|
||||
|
||||
@@ -81,18 +92,25 @@ def generate_launch_description():
|
||||
}
|
||||
|
||||
controllers_yaml = load_yaml('iisy_config', 'config/iisy_moveit_controllers.yml')
|
||||
moveit_controllers = {'moveit_simple_controller_manager': controllers_yaml,
|
||||
'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager'}
|
||||
|
||||
trajectory_execution = {'moveit_manage_controllers': True,
|
||||
'trajectory_execution.allowed_execution_duration_scaling': 1.2,
|
||||
'trajectory_execution.allowed_goal_duration_margin': 0.5,
|
||||
'trajectory_execution.allowed_start_tolerance': 0.01}
|
||||
moveit_controllers = {
|
||||
'moveit_simple_controller_manager': controllers_yaml,
|
||||
'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager'
|
||||
}
|
||||
|
||||
planning_scene_monitor_parameters = {"publish_planning_scene": True,
|
||||
"publish_geometry_updates": True,
|
||||
"publish_state_updates": True,
|
||||
"publish_transforms_updates": True}
|
||||
trajectory_execution = {
|
||||
'moveit_manage_controllers': True,
|
||||
'trajectory_execution.allowed_execution_duration_scaling': 1.2,
|
||||
'trajectory_execution.allowed_goal_duration_margin': 0.5,
|
||||
'trajectory_execution.allowed_start_tolerance': 0.01
|
||||
}
|
||||
|
||||
planning_scene_monitor_parameters = {
|
||||
"publish_planning_scene": True,
|
||||
"publish_geometry_updates": True,
|
||||
"publish_state_updates": True,
|
||||
"publish_transforms_updates": True
|
||||
}
|
||||
|
||||
planning = {
|
||||
"move_group": {
|
||||
@@ -105,13 +123,14 @@ def generate_launch_description():
|
||||
spawn_robot = Node(
|
||||
package='ros_gz_sim',
|
||||
executable='create',
|
||||
arguments=['-world', 'empty', '-string', robot_description_config],
|
||||
arguments=['-world', 'default', '-string', robot_description_config],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
robot_state_publisher = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
output='screen',
|
||||
parameters=[robot_description]
|
||||
)
|
||||
|
||||
@@ -124,7 +143,7 @@ def generate_launch_description():
|
||||
robot_description_semantic,
|
||||
robot_description_planning,
|
||||
ompl_planning_pipeline_config,
|
||||
kinematics_yaml,
|
||||
robot_description_kinematics,
|
||||
planning,
|
||||
trajectory_execution,
|
||||
moveit_controllers,
|
||||
@@ -138,28 +157,48 @@ def generate_launch_description():
|
||||
|
||||
# octomap_updater_config = load_yaml('moveit_tutorials', 'config/sensors_3d.yaml')
|
||||
|
||||
return LaunchDescription([
|
||||
# SetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE', value='true'),
|
||||
pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
|
||||
|
||||
# IncludeLaunchDescription(
|
||||
gz_sim = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
|
||||
launch_arguments={'gz_args': '-r /home/ros/workspace/src/ign_world/world/gaz_new_test.sdf'}.items(),
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
SetParameter(name="use_sim_time",value=True),
|
||||
|
||||
#SetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE', value='true'),
|
||||
#SetEnvironmentVariable(name='DISPLAY', value=':0'),
|
||||
|
||||
#IncludeLaunchDescription(
|
||||
# PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
|
||||
# launch_arguments={'world': world_path}.items()
|
||||
# ),
|
||||
#),
|
||||
|
||||
# UnsetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE'),
|
||||
gz_sim,
|
||||
|
||||
# Start Gazebo client
|
||||
# IncludeLaunchDescription(
|
||||
#Node(package='ros_gz_bridge',
|
||||
# executable='parameter_bridge',
|
||||
# name='jointstate_bridge',
|
||||
# arguments=["/world/default/model/iisy/joint_state"]
|
||||
#),
|
||||
|
||||
#UnsetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE'),
|
||||
|
||||
#Start Gazebo client
|
||||
#IncludeLaunchDescription(
|
||||
# PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py'))
|
||||
# ),
|
||||
|
||||
RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=spawn_robot,
|
||||
on_exit=[robot_state_publisher],
|
||||
)
|
||||
),
|
||||
#),
|
||||
|
||||
#RegisterEventHandler(
|
||||
# event_handler=OnProcessExit(
|
||||
# target_action=spawn_robot,
|
||||
# on_exit=gzclient,
|
||||
# )
|
||||
#),
|
||||
|
||||
|
||||
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
@@ -173,7 +212,7 @@ def generate_launch_description():
|
||||
"--roll", "1.5707963267949",
|
||||
"--pitch", "0",
|
||||
"--yaw", "0",
|
||||
"--frame-id", "map",
|
||||
"--frame-id", "world",
|
||||
"--child-frame-id", "lidar_1_link"]
|
||||
),
|
||||
|
||||
@@ -189,49 +228,38 @@ def generate_launch_description():
|
||||
"--roll", "0",
|
||||
"--pitch", "0",
|
||||
"--yaw", "0",
|
||||
"--frame-id", "map",
|
||||
"--frame-id", "world",
|
||||
"--child-frame-id", "lidar_2_link"]
|
||||
),
|
||||
|
||||
# Node(
|
||||
# package='gazebo_ros',
|
||||
# executable='spawn_entity.py',
|
||||
# arguments=['-entity', robot_name,
|
||||
# '-topic', 'robot_description',
|
||||
# '-x', '-1',
|
||||
# '-y', '-1',
|
||||
# '-z', '1',
|
||||
# '-Y', '0'], # Yaw
|
||||
# output='screen'
|
||||
# ),
|
||||
Node(
|
||||
package='ros_gz_bridge',
|
||||
executable='parameter_bridge',
|
||||
arguments=["/clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock"],
|
||||
),
|
||||
|
||||
# Node(
|
||||
# package="gazebo_ros",
|
||||
# executable="spawn_entity.py",
|
||||
# arguments=[
|
||||
# "-topic", "robot_description",
|
||||
# "-entity", LaunchConfiguration("iisy")
|
||||
# ],
|
||||
# output="screen"
|
||||
# ),
|
||||
Node(
|
||||
package='ros_gz_bridge',
|
||||
executable='parameter_bridge',
|
||||
arguments=["/world/default/model/iisy/joint_state@sensor_msgs/msg/JointState@ignition.msgs.Model"],
|
||||
remappings=[
|
||||
('/world/default/model/iisy/joint_state', '/joint_states'),
|
||||
]
|
||||
),
|
||||
|
||||
# Node(
|
||||
# package='robot_state_publisher',
|
||||
# executable='robot_state_publisher',
|
||||
# name='robot_state_publisher',
|
||||
# output='both',
|
||||
# parameters=[robot_description]
|
||||
# ),
|
||||
#Node(
|
||||
# package="controller_manager",
|
||||
# executable="ros2_control_node",
|
||||
# parameters=[robot_description, robot_controllers],
|
||||
# output="both",
|
||||
#),
|
||||
|
||||
|
||||
|
||||
# ,
|
||||
# Node(
|
||||
# namespace='turtlesim1',
|
||||
# executable='gazebo',
|
||||
# name='gazebo',
|
||||
# arguments=["test.xml"]
|
||||
# )
|
||||
Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
output='both',
|
||||
parameters=[robot_description]
|
||||
),
|
||||
|
||||
# Node(
|
||||
# package='controller_manager',
|
||||
@@ -243,26 +271,28 @@ def generate_launch_description():
|
||||
# ]
|
||||
# ),
|
||||
|
||||
Node(package='moveit_ros_move_group',
|
||||
executable='move_group',
|
||||
Node(
|
||||
package='moveit_ros_move_group',
|
||||
executable='move_group',
|
||||
# prefix='xterm -fs 10 -e gdb --ex run --args',
|
||||
output='screen',
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_planning,
|
||||
ompl_planning_pipeline_config,
|
||||
kinematics_yaml,
|
||||
planning,
|
||||
trajectory_execution,
|
||||
moveit_controllers,
|
||||
planning_scene_monitor_parameters,
|
||||
# octomap_config,
|
||||
# octomap_updater_config
|
||||
]
|
||||
),
|
||||
|
||||
output='screen',
|
||||
parameters=[
|
||||
robot_description,
|
||||
robot_description_semantic,
|
||||
robot_description_planning,
|
||||
ompl_planning_pipeline_config,
|
||||
robot_description_kinematics,
|
||||
planning,
|
||||
trajectory_execution,
|
||||
moveit_controllers,
|
||||
planning_scene_monitor_parameters,
|
||||
# octomap_config,
|
||||
# octomap_updater_config
|
||||
]
|
||||
),
|
||||
#robot_state_publisher,
|
||||
spawn_robot,
|
||||
rviz
|
||||
rviz,
|
||||
|
||||
|
||||
])
|
||||
|
||||
Reference in New Issue
Block a user