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

OMPL planning working on occasion, race during startup suspected

Browse Source
This commit is contained in:
Bastian Hofmann 2023-02-16 10:17:54 +00:00
parent 722a67cb9d
commit d19bb7e053
2 changed files with 14 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -50,10 +50,6 @@ def load_yaml(package_name, file_path):
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}
@ -64,13 +60,11 @@ def generate_launch_description():
kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yml')
robot_description_kinematics = {'robot_description_kinematics': kinematics_yaml}
robot_controllers = PathJoinSubstitution(
[
FindPackageShare("iisy_config"),
"config",
"iisy_controllers.yaml",
]
)
moveit_controllers_yaml = load_yaml("iisy_config", "config/iisy_moveit_controllers.yaml")
moveit_controllers = {
"moveit_simple_controller_manager": moveit_controllers_yaml,
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
ompl_planning_pipeline_config = {
'move_group': {
@ -91,13 +85,6 @@ 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,
@ -127,13 +114,6 @@ def generate_launch_description():
output='screen'
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[robot_description]
)
rviz = Node(
package='rviz2',
executable='rviz2',
@ -159,50 +139,21 @@ def generate_launch_description():
pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
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'),
Node(
package='ros_actor_action_server',
executable='ros_actor_action_server'),
#IncludeLaunchDescription(
# PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
# launch_arguments={'world': world_path}.items()
#),
gz_sim,
#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=gzclient,
# )
#),
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(),
),
Node(
package='tf2_ros',
namespace='gaz_simulation',
executable='static_transform_publisher',
name='lidar_1_broadcaster',
arguments=[
@ -218,7 +169,6 @@ def generate_launch_description():
Node(
package='tf2_ros',
namespace='gaz_simulation',
executable='static_transform_publisher',
name='lidar_2_broadcaster',
arguments=[
@ -247,13 +197,6 @@ def generate_launch_description():
]
),
#Node(
# package="controller_manager",
# executable="ros2_control_node",
# parameters=[robot_description, robot_controllers],
# output="both",
#),
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
@ -261,21 +204,11 @@ def generate_launch_description():
parameters=[robot_description]
),
# 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',
output='both',
parameters=[
robot_description,
robot_description_semantic,
@ -290,7 +223,6 @@ def generate_launch_description():
# octomap_updater_config
]
),
#robot_state_publisher,
spawn_robot,
rviz,

2
src/iisy_config/config/iisy_gazebo_controllers.yml
View File

@ -4,4 +4,4 @@ controller_manager:
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
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