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

Added "support" for controller plugin, currently segfaults.

Browse Source
This commit is contained in:
Bastian Hofmann
2023-01-24 15:19:00 +00:00
parent 3199b400d1
commit e0e44b1ffa
13 changed files with 409 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files

238
src/ign_world/launch/moveit_launch.py Normal file
View File

@@ -0,0 +1,238 @@
import os
import subprocess
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, UnsetEnvironmentVariable
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import Command, LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
def load_file(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, 'r') as file:
print("Opened file")
return file.read()
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
print("ERROR: load_file " + package_name + " | " + file_path + " -> " + absolute_file_path)
return None
def load_xacro(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
process = subprocess.run(["xacro", absolute_file_path], capture_output=True)
if process.returncode != 0:
print(process.stderr)
return process.stdout.decode("UTF-8")
def load_yaml(package_name, file_path):
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
try:
with open(absolute_file_path, 'r') as file:
print("Opened file")
return yaml.safe_load(file)
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
print("ERROR: load_yaml " + package_name + " | " + file_path + " -> " + absolute_file_path)
return None
def generate_launch_description():
robot_name = "iisy"
# 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")
robot_description_config = load_xacro('iisy_config', 'urdf/iisy.urdf')
robot_description = {'robot_description': robot_description_config}
robot_description_semantic_config = load_file('iisy_config', 'srdf/iisy.srdf')
robot_description_semantic = {'robot_description_semantic': robot_description_semantic_config}
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}}
ompl_planning_yaml = load_yaml('iisy_config', 'config/ompl_planning.yml')
ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
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}
planning_scene_monitor_parameters = {"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True}
planning = {
"move_group": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters": "default_planner_request_adapters/AddTimeParameterization 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
}
}
# octomap_config = {'octomap_frame': 'camera_rgb_optical_frame',
# 'octomap_resolution': 0.05,
# 'max_range': 5.0}
# octomap_updater_config = load_yaml('moveit_tutorials', 'config/sensors_3d.yaml')
return LaunchDescription([
# SetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE', value='true'),
# IncludeLaunchDescription(
# PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
# launch_arguments={'world': world_path}.items()
# ),
# UnsetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE'),
# Start Gazebo client
# IncludeLaunchDescription(
# PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py'))
# ),
Node(
package='ros_gz_sim',
executable='create',
arguments=['-world', 'empty', '-string', robot_description_config],
output='screen'
),
Node(
package='tf2_ros',
namespace='gaz_simulation',
executable='static_transform_publisher',
name='lidar_1_broadcaster',
arguments=[
"--x", "1",
"--y", "-1.9",
"--z", "1",
"--roll", "1.5707963267949",
"--pitch", "0",
"--yaw", "0",
"--frame-id", "map",
"--child-frame-id", "lidar_1_link"]
),
Node(
package='tf2_ros',
namespace='gaz_simulation',
executable='static_transform_publisher',
name='lidar_2_broadcaster',
arguments=[
"--x", "-1.9",
"--y", "1",
"--z", "1",
"--roll", "0",
"--pitch", "0",
"--yaw", "0",
"--frame-id", "map",
"--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="gazebo_ros",
# executable="spawn_entity.py",
# arguments=[
# "-topic", "robot_description",
# "-entity", LaunchConfiguration("iisy")
# ],
# output="screen"
# ),
# Node(
# package='robot_state_publisher',
# executable='robot_state_publisher',
# name='robot_state_publisher',
# output='both',
# parameters=[robot_description]
# ),
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[robot_description]
),
# ,
# Node(
# namespace='turtlesim1',
# executable='gazebo',
# name='gazebo',
# arguments=["test.xml"]
# )
# Node(
# package='controller_manager',
# name='gazebo_controller_spawner',
# executable='spawner',
# output='screen',
# parameters=[
# controllers_yaml,
# ]
# ),
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,
kinematics_yaml,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
# octomap_config,
# octomap_updater_config
]
),
Node(
package='rviz2',
executable='rviz2',
# arguments=['-d', os.path.join(pkg_ros_gz_sim_demos, 'rviz', 'gpu_lidar_bridge.rviz')],
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
planning,
],
),
])