Added "support" for controller plugin, currently segfaults.
This commit is contained in:
Bastian Hofmann
28
src/ign_world/CMakeLists.txt
Normal file
28
src/ign_world/CMakeLists.txt
Normal file
@@ -0,0 +1,28 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(ign_world)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
|
||||
install(DIRECTORY world DESTINATION share/${PROJECT_NAME})
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# uncomment the line when a copyright and license is not present in all source files
|
||||
#set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# uncomment the line when this package is not in a git repo
|
||||
#set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
238
src/ign_world/launch/moveit_launch.py
Normal file
238
src/ign_world/launch/moveit_launch.py
Normal 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,
|
||||
],
|
||||
),
|
||||
|
||||
])
|
||||
28
src/ign_world/package.xml
Normal file
28
src/ign_world/package.xml
Normal file
@@ -0,0 +1,28 @@
|
||||
<package format="3">
|
||||
<name>ign_world</name>
|
||||
<version>0.244.1</version>
|
||||
<description>Launch file for simulation of pick and place</description>
|
||||
<license>Apache 2.0</license>
|
||||
<maintainer email="bastian.hofmann@xitaso.com">Bastian Hofmann</maintainer>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>gazebo</exec_depend>
|
||||
|
||||
<exec_depend>image_transport_plugins</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>ros_ign_bridge</exec_depend>
|
||||
<exec_depend>ros_ign_gazebo</exec_depend>
|
||||
<exec_depend>ros_ign_image</exec_depend>
|
||||
<!-- See https://github.com/osrf/ros_ign/issues/40 -->
|
||||
<!--exec_depend>ros_ign_point_cloud</exec_depend-->
|
||||
<exec_depend>rqt_image_view</exec_depend>
|
||||
<exec_depend>rqt_plot</exec_depend>
|
||||
<exec_depend>rqt_topic</exec_depend>
|
||||
<exec_depend>rviz2</exec_depend>
|
||||
<exec_depend>xacro</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
73
src/ign_world/world/gaz_new_test.sdf
Normal file
73
src/ign_world/world/gaz_new_test.sdf
Normal file
@@ -0,0 +1,73 @@
|
||||
<sdf version="1.6">
|
||||
<world name="empty">
|
||||
<physics name="1ms" type="ignored">
|
||||
<max_step_size>0.001</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
</physics>
|
||||
<plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
|
||||
<plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
|
||||
<plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
|
||||
<plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
|
||||
|
||||
<light type="directional" name="sun">
|
||||
<cast_shadows>true</cast_shadows>
|
||||
<pose>0 0 10 0 0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
<attenuation>
|
||||
<range>1000</range>
|
||||
<constant>0.9</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<direction>-0.5 0.1 -0.9</direction>
|
||||
</light>
|
||||
|
||||
<actor name="actor_walking">
|
||||
<plugin name="ignition::gazebo::ActorSystem" filename="/home/ros/workspace/install/ign_actor_plugin/lib/ign_actor_plugin/libActorPlugin.so">
|
||||
</plugin>
|
||||
<skin>
|
||||
<filename>/home/ros/walk.dae</filename>
|
||||
<scale>1.0</scale>
|
||||
</skin>
|
||||
<animation name='walk'>
|
||||
<filename>/home/ros/walk.dae</filename>
|
||||
<interpolate_x>true</interpolate_x>
|
||||
</animation>
|
||||
<animation name='standing_extend_arm'>
|
||||
<filename>/home/ros/standing_extend_arm.dae</filename>
|
||||
</animation>
|
||||
<animation name='standing_retract_arm'>
|
||||
<filename>/home/ros/standing_retract_arm.dae</filename>
|
||||
</animation>
|
||||
|
||||
</actor>
|
||||
|
||||
<model name="ground_plane">
|
||||
<static>true</static>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.8 0.8 0.8 1</ambient>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.8 0.8 0.8 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
</world>
|
||||
</sdf>
|
||||
Reference in New Issue
Block a user