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

Compare commits

base: yenon:67256759ad5101a6a9820f4f066c15bab4e6a8e3
Branches Tags
yenon:master
..
compare: yenon:18031e05925d51653d6b853ca416c45a007b876f
Branches Tags
yenon:master

2 Commits
67256759ad ... 18031e0592

Author SHA1 Message Date
Bastian Hofmann
18031e0592 Cleanup and finishing touches 2023-04-30 07:43:08 +00:00
Bastian Hofmann
9d9af7fa6b Removed unused Groot node definitions. 2023-04-21 16:23:00 +00:00
25 changed files with 29486 additions and 652 deletions
Show Stats Expand all files Collapse all files

21
README.md Normal file
View File

@ -0,0 +1,21 @@
# Usage
`ros2 launch ign_world gazebo_controller_launch.py scene:=` and one of the following scene types: `Coex`, `Coop` and `Colab` appended to the beforementioned command.
Depending on your terminal configuration, adding the environment variable `DISPLAY=:0` may be required as a prefix to the command, as not all terminals load the predefined environment.
# Build
Building a package is handled by using the `build.sh` executable.
Many IDE's require a `compile_commands.json` file to offer syntax highlighting, which will be output when using the script. This also includes a symlink, in case your IDE does not search for it in the build directory.
Another addition of the script is the usage of `console_cohesion+` as event handler, which will format output by package, easing debugging in the noisy build output.
# Packages
package name|description
-|-
`btree` | the executable for the behavior trees and the used xml files
`controller_test` | a debugging tool for sending direct commands to the ros_control controller of the `iisy`
`gz_ros2_control` | the git version of that package, as the one in the repos was too outdated
`ign_actor_plugin` | the Gazebo Ignition plugin for controllable Actors
`ign_world` | the world for the simulation
`ros_actor_action_server` | second component of actor plugin, has to be started to enable control functionality
`ros_actor_action_server_msgs` | ros_action messages used by the actor plugin server
`ros_actor_message_queue_msgs` | internal messages of the actor plugin

1
src/Groot

@ -1 +0,0 @@
Subproject commit 05fe640172e3cd447ab5db31f71355789f6a48b3

39
src/btree/trees/actorTreeCoex.xml
View File

@ -3,18 +3,41 @@
<BehaviorTree ID="actorTree">
<Control ID="WeightedRandom" weights="95,5">
<Sequence>
<Control ID="WeightedRandom" weights="95,5">
<Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
</Control>
<Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
<Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
<Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
<SubTree ID="WorkOnBench"/>
<SubTree ID="DepositToShelf"/>
</Sequence>
<Sequence>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
<Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="{actorTarget}"/>
</Sequence>
</Control>
</BehaviorTree>
<BehaviorTree ID="DepositToShelf">
<Sequence>
<WeightedRandom weights="1,1,1">
<Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="5.5,0,0,0.707,0,0,-0.707"/>
<Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="6.5,0,0,0.707,0,0,-0.707"/>
<Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="7.5,0,0,0.707,0,0,-0.707"/>
</WeightedRandom>
<WeightedRandom weights="1,1">
<Sequence>
<Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
<Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
</Sequence>
<Sequence>
<Action ID="ActorAnimation" animation_name="standing_to_low"/>
<Action ID="ActorAnimation" animation_name="low_inspect"/>
<Action ID="ActorAnimation" animation_name="low_put_back"/>
<Action ID="ActorAnimation" animation_name="low_to_standing"/>
</Sequence>
</WeightedRandom>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="WorkOnBench">
<Sequence>
<Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="6.5,6,0,0.707,0,0,0.707"/>
<Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
<Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
</Sequence>
</BehaviorTree>
</root>

50
src/btree/trees/actorTreeColab.xml
View File

@ -1,6 +1,5 @@
<?xml version="1.0"?>
<root main_tree_to_execute="actorTree">
<!-- ////////// -->
<BehaviorTree ID="actorTree">
<Fallback>
<ReactiveSequence>
@ -14,60 +13,15 @@
<Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
</Control>
<Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/>
</Sequence>
</ReactiveSequence>
<Sequence>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/>
<Action ID="SetCalledTo" state="false"/>
</Sequence>
</Fallback>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="ActorAnimation">
<input_port name="animation">Name of animation to play</input_port>
</Action>
<Action ID="ActorMovement">
<input_port name="animation">Name of animation to play</input_port>
<input_port name="target">Pose to move to</input_port>
</Action>
<Condition ID="IsCalled"/>
<Action ID="SetCalledTo">
<input_port name="state">state to set called to</input_port>
</Action>
<Action ID="GenerateXYPose">
<input_port name="area">Area to generate pose in</input_port>
<output_port name="pose">Generated pose in area</output_port>
</Action>
<Condition ID="InAreaTest">
<input_port name="area">Bounds to check in</input_port>
<input_port name="pose">Position of object</input_port>
</Condition>
<Action ID="MoveActorToTarget">
<input_port name="current">Current actor position</input_port>
<input_port name="target">Target to move actor to</input_port>
</Action>
<Action ID="PositionToPose">
<input_port name="offset">offset as a Point</input_port>
<input_port name="orientation">rotation of resulting pose as Quaternion</input_port>
<input_port name="output">generated new pose</input_port>
<input_port name="position">initial position as Position2D</input_port>
</Action>
<Action ID="SetRobotVelocity">
<input_port name="velocity">Target velocity of robot</input_port>
</Action>
<Action ID="RandomFailure">
<input_port name="failureChance">Chance to fail from 0 to 1</input_port>
</Action>
<Action ID="RobotMove">
<input_port name="target">Target pose of robot.</input_port>
</Action>
<Control ID="WeightedRandom">
<input_port name="weights">Weights for the children, separated by semicolon.</input_port>
</Control>
</TreeNodesModel>
<!-- ////////// -->
</root>

51
src/btree/trees/actorTreeCoop.xml
View File

@ -1,73 +1,26 @@
<?xml version="1.0"?>
<root main_tree_to_execute="actorTree">
<!-- ////////// -->
<BehaviorTree ID="actorTree">
<Fallback>
<ReactiveSequence>
<Inverter>
<Condition ID="IsCalled"/>
</Inverter>
<Sequence>
<Control ID="WeightedRandom" weights="100,5,2">
<Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
</Control>
<Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/>
</Sequence>
</ReactiveSequence>
<Sequence>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/>
<Action ID="SetCalledTo" state="false"/>
</Sequence>
</Fallback>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="ActorAnimation">
<input_port name="animation">Name of animation to play</input_port>
</Action>
<Action ID="ActorMovement">
<input_port name="animation">Name of animation to play</input_port>
<input_port name="target">Pose to move to</input_port>
</Action>
<Condition ID="IsCalled"/>
<Action ID="SetCalledTo">
<input_port name="state">state to set called to</input_port>
</Action>
<Action ID="GenerateXYPose">
<input_port name="area">Area to generate pose in</input_port>
<output_port name="pose">Generated pose in area</output_port>
</Action>
<Condition ID="InAreaTest">
<input_port name="area">Bounds to check in</input_port>
<input_port name="pose">Position of object</input_port>
</Condition>
<Action ID="MoveActorToTarget">
<input_port name="current">Current actor position</input_port>
<input_port name="target">Target to move actor to</input_port>
</Action>
<Action ID="PositionToPose">
<input_port name="offset">offset as a Point</input_port>
<input_port name="orientation">rotation of resulting pose as Quaternion</input_port>
<input_port name="output">generated new pose</input_port>
<input_port name="position">initial position as Position2D</input_port>
</Action>
<Action ID="SetRobotVelocity">
<input_port name="velocity">Target velocity of robot</input_port>
</Action>
<Action ID="RandomFailure">
<input_port name="failureChance">Chance to fail from 0 to 1</input_port>
</Action>
<Action ID="RobotMove">
<input_port name="target">Target pose of robot.</input_port>
</Action>
<Control ID="WeightedRandom">
<input_port name="weights">Weights for the children, separated by semicolon.</input_port>
</Control>
</TreeNodesModel>
<!-- ////////// -->
</root>

44
src/btree/trees/robotTreeColab.xml
View File

@ -1,6 +1,5 @@
<?xml version="1.0"?>
<root main_tree_to_execute="robotTree">
<!-- ////////// -->
<BehaviorTree ID="robotTree">
<ReactiveSequence>
<Inverter>
@ -26,48 +25,5 @@
</Fallback>
</ReactiveSequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="ActorAnimation">
<input_port name="animation">Name of animation to play</input_port>
</Action>
<Action ID="ActorMovement">
<input_port name="animation">Name of animation to play</input_port>
<input_port name="target">Pose to move to</input_port>
</Action>
<Condition ID="IsCalled"/>
<Action ID="SetCalledTo">
<input_port name="state">state to set called to</input_port>
</Action>
<Action ID="GenerateXYPose">
<input_port name="area">Area to generate pose in</input_port>
<output_port name="pose">Generated pose in area</output_port>
</Action>
<Condition ID="InAreaTest">
<input_port name="area">Bounds to check in</input_port>
<input_port name="pose">Position of object</input_port>
</Condition>
<Action ID="OffsetPose">
<input_port name="input">initial position as Pose</input_port>
<input_port name="offset">offset as a Point</input_port>
<input_port name="orientation">rotation of resulting pose as Quaternion</input_port>
<output_port name="output">generated new pose</output_port>
</Action>
<Action ID="RandomFailure">
<input_port name="failureChance">Chance to fail from 0 to 1</input_port>
</Action>
<Action ID="RobotMove">
<input_port name="target">Target pose of robot.</input_port>
</Action>
<Action ID="SetRobotVelocity">
<input_port name="velocity">Target velocity of robot</input_port>
</Action>
<Control ID="WeightedRandom">
<input_port name="weights">Weights for the children, separated by semicolon.</input_port>
</Control>
<Control ID="InterruptableSequence">
</Control>
</TreeNodesModel>
<!-- ////////// -->
</root>

44
src/btree/trees/robotTreeCoop.xml
View File

@ -1,6 +1,5 @@
<?xml version="1.0"?>
<root main_tree_to_execute="robotTree">
<!-- ////////// -->
<BehaviorTree ID="robotTree">
<ReactiveSequence>
<Inverter>
@ -30,48 +29,5 @@
</Control>
</ReactiveSequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="ActorAnimation">
<input_port name="animation">Name of animation to play</input_port>
</Action>
<Action ID="ActorMovement">
<input_port name="animation">Name of animation to play</input_port>
<input_port name="target">Pose to move to</input_port>
</Action>
<Condition ID="IsCalled"/>
<Action ID="SetCalledTo">
<input_port name="state">state to set called to</input_port>
</Action>
<Action ID="GenerateXYPose">
<input_port name="area">Area to generate pose in</input_port>
<output_port name="pose">Generated pose in area</output_port>
</Action>
<Condition ID="InAreaTest">
<input_port name="area">Bounds to check in</input_port>
<input_port name="pose">Position of object</input_port>
</Condition>
<Action ID="OffsetPose">
<input_port name="input">initial position as Pose</input_port>
<input_port name="offset">offset as a Point</input_port>
<input_port name="orientation">rotation of resulting pose as Quaternion</input_port>
<output_port name="output">generated new pose</output_port>
</Action>
<Action ID="RandomFailure">
<input_port name="failureChance">Chance to fail from 0 to 1</input_port>
</Action>
<Action ID="RobotMove">
<input_port name="target">Target pose of robot.</input_port>
</Action>
<Action ID="SetRobotVelocity">
<input_port name="velocity">Target velocity of robot</input_port>
</Action>
<Control ID="WeightedRandom">
<input_port name="weights">Weights for the children, separated by semicolon.</input_port>
</Control>
<Control ID="InterruptableSequence">
</Control>
</TreeNodesModel>
<!-- ////////// -->
</root>

3
src/ign_actor_plugin/CMakeLists.txt
View File

@ -33,7 +33,8 @@ target_link_libraries(ign_actor_plugin
install(TARGETS
ign_actor_plugin
DESTINATION lib/${PROJECT_NAME}
DESTINATION share/${PROJECT_NAME}
)
install(DIRECTORY animation DESTINATION share/${PROJECT_NAME})
ament_package()

3538
src/ign_actor_plugin/animation/low_grab.dae Normal file
View File

File diff suppressed because one or more lines are too long

3538
src/ign_actor_plugin/animation/low_inspect.dae Normal file
View File

File diff suppressed because one or more lines are too long

3538
src/ign_actor_plugin/animation/low_put_back.dae Normal file
View File

File diff suppressed because one or more lines are too long

3538
src/ign_actor_plugin/animation/low_to_standing.dae Normal file
View File

File diff suppressed because one or more lines are too long

2899
src/ign_actor_plugin/animation/standing_extend_arm.dae Executable file
View File

File diff suppressed because one or more lines are too long

2899
src/ign_actor_plugin/animation/standing_idle.dae Executable file
View File

File diff suppressed because one or more lines are too long

2899
src/ign_actor_plugin/animation/standing_retract_arm.dae Executable file
View File

File diff suppressed because one or more lines are too long

3538
src/ign_actor_plugin/animation/standing_to_low.dae Normal file
View File

File diff suppressed because one or more lines are too long

2899
src/ign_actor_plugin/animation/standing_walk.dae Executable file
View File

File diff suppressed because one or more lines are too long

31
src/ign_world/launch/gazebo_controller_launch.py
View File

@ -1,14 +1,28 @@
import os
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch_ros.actions import Node, SetParameter
from launch.launch_description_sources import PythonLaunchDescriptionSource
from moveit_configs_utils import MoveItConfigsBuilder
from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch.conditions import LaunchConfigurationEquals
import xacro
def generate_launch_description():
scene_launch_arg = DeclareLaunchArgument(
"scene", default_value=TextSubstitution(text="Coex")
)
worldFile = open('/tmp/gazebo_world.sdf', 'w')
worldFile.write(xacro.process(os.path.join(get_package_share_directory('ign_world'), 'world', 'world.sdf')))
worldFile.flush()
worldFile.close()
scene = LaunchConfiguration('scene')
moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config").to_moveit_configs()
load_joint_state_broadcaster = ExecuteProcess(
@ -39,8 +53,8 @@ def generate_launch_description():
parameters=[
{
"trees": [
get_package_share_directory('btree')+'/trees/actorTreeCoex.xml',
get_package_share_directory('btree')+'/trees/robotTreeCoex.xml'
[get_package_share_directory('btree'), '/trees/actorTree', scene, '.xml'],
[get_package_share_directory('btree'), '/trees/robotTree', scene, '.xml'],
],
"areas": [
"safeArea",
@ -79,21 +93,24 @@ def generate_launch_description():
)
return LaunchDescription([
scene_launch_arg,
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')),
launch_arguments={'gz_args': '-v 4 -r '+get_package_share_directory('ign_world')+'/world/gaz_new_test.sdf'}.items(),
launch_arguments={'gz_args': '-v 4 -r /tmp/gazebo_world.sdf'}.items(),
),
Node(
package='ros_gz_sim',
executable='create',
arguments=[
'-name', 'office',
'-name', 'conveyor',
'-allow_renaming', 'true',
'-string', xacro.process(os.path.join(get_package_share_directory('ign_world'), 'world', 'conveyor.sdf')),
#'-file', 'https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Gazebo',
# '-file', 'https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Gazebo',
],
output='screen'
output='screen',
condition=LaunchConfigurationEquals('scene', 'Coop')
),
Node(

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

@ -1,311 +0,0 @@
import os
import subprocess
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node, SetParameter
from launch.actions import IncludeLaunchDescription, RegisterEventHandler, LogInfo, ExecuteProcess, ExecuteLocal, TimerAction
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch.event_handlers import OnProcessExit
from launch.launch_description_entity import LaunchDescriptionEntity
from pathlib import Path
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 launch_chain(nodes: list[ExecuteLocal]):
generatedNodes: list[LaunchDescriptionEntity] = []
for index in range(1, len(nodes)):
generatedNodes.append(RegisterEventHandler(
OnProcessExit(
target_action=nodes[index-1],
on_exit=nodes[index]
)
))
generatedNodes.append(nodes[0])
return generatedNodes
def generate_launch_description():
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}
controllers = {
"moveit_simple_controller_manager": load_yaml("iisy_config", "config/iisy_controllers.yml"),
"moveit_controller_manager": "moveit_ros_control_interface/Ros2ControlMultiManager",
}
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)
robot_description_planning = {
"robot_description_planning": PathJoinSubstitution(
[
FindPackageShare("iisy_config"),
"config/joint_limits.yml"
]
)
}
trajectory_execution = {
'moveit_manage_controllers': False,
'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
}
}
spawn_robot = Node(
package='ros_gz_sim',
executable='create',
arguments=['-world', 'default', '-string', robot_description_config],
output='screen'
)
rviz = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', os.path.join(get_package_share_directory("ign_world"), 'rviz', 'iisy.rviz')],
parameters=[
robot_description,
robot_description_semantic,
robot_description_planning,
ompl_planning_pipeline_config,
robot_description_kinematics,
planning,
trajectory_execution,
controllers,
planning_scene_monitor_parameters,
],
)
controller_manager = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[
robot_description,
str(Path(get_package_share_directory("iisy_config")+"/config/iisy_controllers.yaml")),
],
)
load_joint_state_broadcaster = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'joint_state_broadcaster'],
output='both'
)
load_joint_trajectory_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
'joint_trajectory_controller'],
output='both'
)
move_group = Node(
package='moveit_ros_move_group',
executable='move_group',
# prefix='xterm -fs 10 -e gdb --ex run --args',
output='both',
parameters=[
robot_description,
robot_description_semantic,
robot_description_planning,
ompl_planning_pipeline_config,
robot_description_kinematics,
planning,
trajectory_execution,
controllers,
planning_scene_monitor_parameters,
# octomap_config,
# octomap_updater_config
]
)
test = Node(
package='moveit_test',
executable='move',
output='both',
parameters=[
robot_description,
robot_description_semantic,
robot_description_planning,
ompl_planning_pipeline_config,
robot_description_kinematics,
planning,
trajectory_execution,
controllers,
planning_scene_monitor_parameters,
# octomap_config,
# octomap_updater_config
],
)
# 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')
pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
return LaunchDescription([
#IncludeLaunchDescription(
# PythonLaunchDescriptionSource(os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
# launch_arguments={'gz_args': '-r '+get_package_share_directory('ign_world')+'/world/gaz_new_test.sdf'}.items(),
#),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=["/clock[rosgraph_msgs/msg/Clock@ignition.msgs.Clock"],
output='both',
),
SetParameter(name="use_sim_time", value=True),
move_group,
#RegisterEventHandler(
# OnProcessExit(
# target_action=spawn_robot,
# on_exit=[load_joint_state_broadcaster]
# )
#),
controller_manager,
RegisterEventHandler(
OnProcessExit(
target_action=load_joint_state_broadcaster,
on_exit=[load_joint_trajectory_controller]
)
),
RegisterEventHandler(
OnProcessExit(
target_action=load_joint_trajectory_controller,
on_exit=TimerAction(period=10.0,actions=[test])
)
),
#spawn_robot,
load_joint_state_broadcaster,
#load_joint_trajectory_controller,
rviz,
Node(
package='ros_actor_action_server',
executable='ros_actor_action_server'),
Node(
package='tf2_ros',
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", "world",
"--child-frame-id", "lidar_1_link"]
),
Node(
package='tf2_ros',
executable='static_transform_publisher',
name='lidar_2_broadcaster',
arguments=[
"--x", "-1.9",
"--y", "1",
"--z", "1",
"--roll", "0",
"--pitch", "0",
"--yaw", "0",
"--frame-id", "world",
"--child-frame-id", "lidar_2_link"]
),
#Node(
# package='ros_gz_bridge',
# executable='parameter_bridge',
# arguments=["/world/default/model/iisy/joint_state[sensor_msgs/msg/JointState@ignition.msgs.Model"],
# output='both',
# remappings=[
# ('/world/default/model/iisy/joint_state', '/joint_states'),
# ]
#),
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='both',
parameters=[robot_description]
),
])

55
src/ign_world/launch/new_launch.py
View File

@ -1,55 +0,0 @@
import os
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch_ros.actions import Node, SetParameter
from launch.launch_description_sources import PythonLaunchDescriptionSource
from moveit_configs_utils import MoveItConfigsBuilder
from xacro import process as xacro_process
def generate_launch_description():
moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config").to_moveit_configs()
return LaunchDescription([
SetParameter(name="use_sim_time", value=True),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')),
launch_arguments={'gz_args': '-v 4 -r '+get_package_share_directory('ign_world')+'/world/gaz_new_test.sdf'}.items(),
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
output='both',
),
#Node(
# package='ros_gz_bridge',
# executable='parameter_bridge',
# arguments=["/world/default/dynamic_pose/info@tf2_msgs/msg/TFMessage]ignition.msgs.Pose_V"],
# output='both',
# remappings=[
# ('/world/default/dynamic_pose/info', '/tf'),
# ]
#),
Node(
package='ros_actor_action_server',
executable='ros_actor_action_server'
),
#Node(
# package='ros_gz_sim',
# executable='create',
# arguments=['-world', 'default', '-string', xacro_process(os.path.join(get_package_share_directory('iisy_config'), 'config', 'iisy_xacro.urdf'))],
# output='screen'
#),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
str(moveit_config.package_path / "launch/demo.launch.py")
),
),
])

BIN
src/ign_world/model/conveyor.stl
View File

Binary file not shown.

BIN
src/ign_world/model/room.stl Normal file
View File

Binary file not shown.

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

@ -1,7 +1,7 @@
<sdf version="1.6">
<model name="office">
<model name="conveyor">
<static>true</static>
<link name="office">
<link name="conveyor">
<visual name="visual">
<geometry>
<mesh>

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

@ -1,75 +0,0 @@
<sdf version="1.6">
<world name="default">
<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/libign_actor_plugin.so">
<x>2</x>
<y>2</y>
<rot>180</rot>
</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>

109
src/ign_world/world/world.sdf Normal file
View File

@ -0,0 +1,109 @@
<sdf version="1.6">
<world name="default">
<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="$(find ign_actor_plugin)/libign_actor_plugin.so">
<x>2</x>
<y>2</y>
<rot>180</rot>
</plugin>
<skin>
<filename>$(find ign_actor_plugin)/animation/walk.dae</filename>
<scale>1.0</scale>
</skin>
<animation name='standing_walk'>
<filename>$(find ign_actor_plugin)/animation/standing_walk.dae</filename>
</animation>
<animation name='standing_extend_arm'>
<filename>$(find ign_actor_plugin)/animation/standing_extend_arm.dae</filename>
</animation>
<animation name='standing_retract_arm'>
<filename>$(find ign_actor_plugin)/animation/standing_retract_arm.dae</filename>
</animation>
<animation name='standing_to_low'>
<filename>$(find ign_actor_plugin)/animation/standing_to_low.dae</filename>
</animation>
<animation name='low_to_standing'>
<filename>$(find ign_actor_plugin)/animation/low_to_standing.dae</filename>
</animation>
<animation name='low_grab'>
<filename>$(find ign_actor_plugin)/animation/low_grab.dae</filename>
</animation>
<animation name='low_inspect'>
<filename>$(find ign_actor_plugin)/animation/low_inspect.dae</filename>
</animation>
<animation name='low_put_back'>
<filename>$(find ign_actor_plugin)/animation/low_put_back.dae</filename>
</animation>
</actor>
<model name="room">
<static>true</static>
<link name="room">
<visual name="visual">
<geometry>
<mesh>
<uri>file://$(find ign_world)/model/room.stl</uri>
</mesh>
</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>
<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>