Better launch process, not as hacky anymore.
This commit is contained in:
Bastian Hofmann
parent
2650b088f6
commit
b02a3a0689
@ -1,10 +1,34 @@
|
||||
import os
|
||||
import yaml
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import ExecuteProcess, IncludeLaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, UnsetEnvironmentVariable
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.substitutions import Command
|
||||
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:
|
||||
return file.read()
|
||||
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
|
||||
print("ERROR: load_file "+package_name+"|"+file_path)
|
||||
return None
|
||||
|
||||
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:
|
||||
return yaml.safe_load(file)
|
||||
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
|
||||
print("ERROR: load_file "+package_name+"|"+file_path)
|
||||
return None
|
||||
|
||||
def generate_launch_description():
|
||||
robot_name = "iisy"
|
||||
@ -13,24 +37,59 @@ def generate_launch_description():
|
||||
path = FindPackageShare(package=package_name).find(package_name)
|
||||
world_path = os.path.join(path, "world/test2.xml")
|
||||
urdf_path = os.path.join(path, "urdf/iisy.urdf")
|
||||
return LaunchDescription([
|
||||
#IncludeLaunchDescription(
|
||||
# PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
|
||||
# launch_arguments={'world': world_path}.items()
|
||||
#),
|
||||
|
||||
ExecuteProcess(
|
||||
cmd=[[
|
||||
'LIBGL_ALWAYS_SOFTWARE=true ros2 launch gazebo_ros gzserver.launch.py "world:='+world_path+'"'
|
||||
]],
|
||||
shell=True
|
||||
robot_description_config = load_file('gaz_simulation', 'urdf/iisy.urdf')
|
||||
robot_description = {'robot_description' : robot_description_config}
|
||||
|
||||
robot_description_semantic_config = load_file('iisy_config', 'config/iisy.srdf')
|
||||
robot_description_semantic = {'robot_description_semantic' : robot_description_semantic_config}
|
||||
|
||||
kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yaml')
|
||||
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.yaml')
|
||||
ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
|
||||
|
||||
controllers_yaml = load_yaml('iisy_config', 'config/gazebo_controllers.yaml')
|
||||
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}
|
||||
|
||||
|
||||
#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='tf2_ros',
|
||||
namespace='gaz_simulation',
|
||||
@ -38,6 +97,7 @@ def generate_launch_description():
|
||||
name='lidar_1_broadcaster',
|
||||
arguments=["1", "-1.9", "1", "1.5707963267949", "0", "0", "map", "lidar_1_link"]
|
||||
),
|
||||
|
||||
Node(
|
||||
package='tf2_ros',
|
||||
namespace='gaz_simulation',
|
||||
@ -45,6 +105,7 @@ def generate_launch_description():
|
||||
name='lidar_2_broadcaster',
|
||||
arguments=["-1.9", "1", "1", "0", "0", "0", "map", "lidar_2_link"]
|
||||
),
|
||||
|
||||
Node(
|
||||
package='gazebo_ros',
|
||||
executable='spawn_entity.py',
|
||||
@ -56,19 +117,39 @@ def generate_launch_description():
|
||||
'-Y', '0'], # Yaw
|
||||
output='screen'
|
||||
),
|
||||
# Subscribe to the joint states of the robot, and publish the 3D pose of each 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='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': Command(['xacro ', urdf_path])}]
|
||||
),
|
||||
|
||||
# Publish the joint states of the robot
|
||||
Node(
|
||||
package='joint_state_publisher',
|
||||
executable='joint_state_publisher',
|
||||
name='joint_state_publisher'
|
||||
)
|
||||
),
|
||||
|
||||
# ,
|
||||
# Node(
|
||||
@ -77,38 +158,22 @@ def generate_launch_description():
|
||||
# name='gazebo',
|
||||
# arguments=["test.xml"]
|
||||
# )
|
||||
|
||||
|
||||
#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
|
||||
# ]
|
||||
#)
|
||||
])
|
||||
|
||||
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="gazebo_gui" default="true"/>
|
||||
<arg name="initial_joint_positions" doc="Initial joint configuration of the robot"
|
||||
default=" -J base_link1_joint 0 -J base_rot 0 -J link1_link2_joint 0 -J link2_link3_joint 0 -J link2_rot 0 -J link3_rot 0"/>
|
||||
|
||||
<!-- startup simulated world -->
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<let name="world_name" default="worlds/empty.world"/>
|
||||
<let name="paused" value="true"/>
|
||||
<let name="gui" value="$(arg gazebo_gui)"/>
|
||||
</include>
|
||||
|
||||
<!-- send robot urdf to param server -->
|
||||
<param name="robot_description" textfile="$(find gaz_simulation)/urdf/iisy.urdf" />
|
||||
|
||||
<!-- unpause only after loading robot model -->
|
||||
<let name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
|
||||
<!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
|
||||
<arg name="world_pose" value="-x 0 -y 0 -z 0" />
|
||||
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg unpause) $(arg world_pose) $(arg initial_joint_positions)"
|
||||
respawn="false" output="screen" />
|
||||
|
||||
<!-- Load joint controller parameters for Gazebo -->
|
||||
<param from="$(find iisy_config)/config/gazebo_controllers.yaml" />
|
||||
<!-- Spawn Gazebo ROS controllers -->
|
||||
<node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />
|
||||
<!-- Load ROS controllers -->
|
||||
<include file="$(dirname)/ros_controllers.launch"/>
|
||||
|
||||
</launch>
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import ExecuteProcess, IncludeLaunchDescription
|
||||
from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable, UnsetEnvironmentVariable
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.substitutions import Command
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
@ -15,18 +15,14 @@ def generate_launch_description():
|
||||
world_path = os.path.join(path, "world/test2.xml")
|
||||
urdf_path = os.path.join(path, "urdf/iisy.urdf")
|
||||
return LaunchDescription([
|
||||
#IncludeLaunchDescription(
|
||||
# PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
|
||||
# launch_arguments={'world': world_path}.items()
|
||||
#),
|
||||
SetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE', value='true'),
|
||||
|
||||
ExecuteProcess(
|
||||
cmd=[[
|
||||
'LIBGL_ALWAYS_SOFTWARE=true ros2 launch gazebo_ros gzserver.launch.py "world:='+world_path+'"'
|
||||
]],
|
||||
shell=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(
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user