yenon/ROS_Workspace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Migrate to new ros_control

Browse Source
This commit is contained in:
Bastian Hofmann 2023-02-15 14:11:12 +00:00
parent 17e61a505f
commit 58fd751c64
4 changed files with 134 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

208
src/ign_world/launch/moveit_launch.py
View File

@ -3,7 +3,7 @@ import subprocess
import yaml import yaml
from launch import LaunchDescription 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.actions import IncludeLaunchDescription, SetEnvironmentVariable, RegisterEventHandler, UnsetEnvironmentVariable
from launch_ros.substitutions import FindPackageShare from launch_ros.substitutions import FindPackageShare
from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
@ -51,7 +51,7 @@ def load_yaml(package_name, file_path):
def generate_launch_description(): def generate_launch_description():
robot_name = "iisy" 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") 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") 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') kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yml')
robot_description_kinematics = {'robot_description_kinematics': kinematics_yaml} robot_description_kinematics = {'robot_description_kinematics': kinematics_yaml}
ompl_planning_pipeline_config = {'move_group': { robot_controllers = PathJoinSubstitution(
'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""", FindPackageShare("iisy_config"),
'start_state_max_bounds_error': 0.1}} "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_yaml = load_yaml('iisy_config', 'config/ompl_planning.yml')
ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) 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') 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, moveit_controllers = {
'trajectory_execution.allowed_execution_duration_scaling': 1.2, 'moveit_simple_controller_manager': controllers_yaml,
'trajectory_execution.allowed_goal_duration_margin': 0.5, 'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager'
'trajectory_execution.allowed_start_tolerance': 0.01} }
planning_scene_monitor_parameters = {"publish_planning_scene": True, trajectory_execution = {
"publish_geometry_updates": True, 'moveit_manage_controllers': True,
"publish_state_updates": True, 'trajectory_execution.allowed_execution_duration_scaling': 1.2,
"publish_transforms_updates": True} '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 = { planning = {
"move_group": { "move_group": {
@ -105,13 +123,14 @@ def generate_launch_description():
spawn_robot = Node( spawn_robot = Node(
package='ros_gz_sim', package='ros_gz_sim',
executable='create', executable='create',
arguments=['-world', 'empty', '-string', robot_description_config], arguments=['-world', 'default', '-string', robot_description_config],
output='screen' output='screen'
) )
robot_state_publisher = Node( robot_state_publisher = Node(
package='robot_state_publisher', package='robot_state_publisher',
executable='robot_state_publisher', executable='robot_state_publisher',
output='screen',
parameters=[robot_description] parameters=[robot_description]
) )
@ -124,7 +143,7 @@ def generate_launch_description():
robot_description_semantic, robot_description_semantic,
robot_description_planning, robot_description_planning,
ompl_planning_pipeline_config, ompl_planning_pipeline_config,
kinematics_yaml, robot_description_kinematics,
planning, planning,
trajectory_execution, trajectory_execution,
moveit_controllers, moveit_controllers,
@ -138,27 +157,47 @@ def generate_launch_description():
# octomap_updater_config = load_yaml('moveit_tutorials', 'config/sensors_3d.yaml') # octomap_updater_config = load_yaml('moveit_tutorials', 'config/sensors_3d.yaml')
return LaunchDescription([ pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
# SetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE', value='true'),
# 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')), # PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
# launch_arguments={'world': world_path}.items() # launch_arguments={'world': world_path}.items()
# ), #),
# UnsetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE'), gz_sim,
# Start Gazebo client #Node(package='ros_gz_bridge',
# IncludeLaunchDescription( # 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')) # PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py'))
# ), #),
#RegisterEventHandler(
# event_handler=OnProcessExit(
# target_action=spawn_robot,
# on_exit=gzclient,
# )
#),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=spawn_robot,
on_exit=[robot_state_publisher],
)
),
Node( Node(
@ -173,7 +212,7 @@ def generate_launch_description():
"--roll", "1.5707963267949", "--roll", "1.5707963267949",
"--pitch", "0", "--pitch", "0",
"--yaw", "0", "--yaw", "0",
"--frame-id", "map", "--frame-id", "world",
"--child-frame-id", "lidar_1_link"] "--child-frame-id", "lidar_1_link"]
), ),
@ -189,49 +228,38 @@ def generate_launch_description():
"--roll", "0", "--roll", "0",
"--pitch", "0", "--pitch", "0",
"--yaw", "0", "--yaw", "0",
"--frame-id", "map", "--frame-id", "world",
"--child-frame-id", "lidar_2_link"] "--child-frame-id", "lidar_2_link"]
), ),
# Node( Node(
# package='gazebo_ros', package='ros_gz_bridge',
# executable='spawn_entity.py', executable='parameter_bridge',
# arguments=['-entity', robot_name, arguments=["/clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock"],
# '-topic', 'robot_description', ),
# '-x', '-1',
# '-y', '-1',
# '-z', '1',
# '-Y', '0'], # Yaw
# output='screen'
# ),
# Node( Node(
# package="gazebo_ros", package='ros_gz_bridge',
# executable="spawn_entity.py", executable='parameter_bridge',
# arguments=[ arguments=["/world/default/model/iisy/joint_state@sensor_msgs/msg/JointState@ignition.msgs.Model"],
# "-topic", "robot_description", remappings=[
# "-entity", LaunchConfiguration("iisy") ('/world/default/model/iisy/joint_state', '/joint_states'),
# ], ]
# output="screen" ),
# ),
# Node( #Node(
# package='robot_state_publisher', # package="controller_manager",
# executable='robot_state_publisher', # executable="ros2_control_node",
# name='robot_state_publisher', # parameters=[robot_description, robot_controllers],
# output='both', # output="both",
# parameters=[robot_description] #),
# ),
Node(
package='robot_state_publisher',
# , executable='robot_state_publisher',
# Node( output='both',
# namespace='turtlesim1', parameters=[robot_description]
# executable='gazebo', ),
# name='gazebo',
# arguments=["test.xml"]
# )
# Node( # Node(
# package='controller_manager', # package='controller_manager',
@ -243,26 +271,28 @@ def generate_launch_description():
# ] # ]
# ), # ),
Node(package='moveit_ros_move_group', Node(
executable='move_group', package='moveit_ros_move_group',
executable='move_group',
# prefix='xterm -fs 10 -e gdb --ex run --args', # prefix='xterm -fs 10 -e gdb --ex run --args',
output='screen', output='screen',
parameters=[ parameters=[
robot_description, robot_description,
robot_description_semantic, robot_description_semantic,
robot_description_planning, robot_description_planning,
ompl_planning_pipeline_config, ompl_planning_pipeline_config,
kinematics_yaml, robot_description_kinematics,
planning, planning,
trajectory_execution, trajectory_execution,
moveit_controllers, moveit_controllers,
planning_scene_monitor_parameters, planning_scene_monitor_parameters,
# octomap_config, # octomap_config,
# octomap_updater_config # octomap_updater_config
] ]
), ),
#robot_state_publisher,
spawn_robot, spawn_robot,
rviz rviz,
]) ])

4
src/ign_world/world/gaz_new_test.sdf
View File

@ -1,5 +1,5 @@
<sdf version="1.6"> <sdf version="1.6">
<world name="empty"> <world name="default">
<physics name="1ms" type="ignored"> <physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size> <max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor> <real_time_factor>1.0</real_time_factor>
@ -24,7 +24,7 @@
</light> </light>
<actor name="actor_walking"> <actor name="actor_walking">
<plugin name="ignition::gazebo::ActorSystem" filename="/home/ros/workspace/install/ign_actor_plugin/lib/ign_actor_plugin/libActorPlugin.so"> <plugin name="ignition::gazebo::ActorSystem" filename="/home/ros/workspace/install/ign_actor_plugin/lib/ign_actor_plugin/libign_actor_plugin.so">
</plugin> </plugin>
<skin> <skin>
<filename>/home/ros/walk.dae</filename> <filename>/home/ros/walk.dae</filename>

7
src/iisy_config/config/iisy_gazebo_controllers.yml Normal file
View File

@ -0,0 +1,7 @@
# see controllers: http://control.ros.org/ros2_controllers/doc/controllers_index.html
controller_manager:
ros__parameters:
update_rate: 500 # Hz
joint_state_controller:
type: joint_state_controller/JointStateController # if left empty, all states are published https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp#L41

10
src/iisy_config/urdf/iisy.urdf
View File

@ -371,12 +371,12 @@
</ros2_control> </ros2_control>
<gazebo> <gazebo>
<plugin name="ign_ros2_control::IgnitionROS2ControlPlugin" filename="$(find ign_ros2_control)/../../lib/libign_ros2_control-system.so"> <plugin filename="ignition-gazebo-joint-state-publisher-system" name="ignition::gazebo::systems::JointStatePublisher"/>
<controller_manager_prefix_node_name>iisy</controller_manager_prefix_node_name> </gazebo>
<gazebo>
<plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
<parameters>$(find iisy_config)/config/iisy_controllers.yml</parameters> <parameters>$(find iisy_config)/config/iisy_controllers.yml</parameters>
<ros>
<namespace>iisy</namespace>
</ros>
</plugin> </plugin>
</gazebo> </gazebo>