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

Compare commits

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

No commits in common. "18031e05925d51653d6b853ca416c45a007b876f" and "67256759ad5101a6a9820f4f066c15bab4e6a8e3" have entirely different histories.
18031e0592 ... 67256759ad

25 changed files with 652 additions and 29486 deletions
Show Stats Expand all files Collapse all files

21
README.md
View File

@ -1,21 +0,0 @@
# 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 Submodule

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

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

@ -3,41 +3,18 @@
<BehaviorTree ID="actorTree">
<Control ID="WeightedRandom" weights="95,5">
<Sequence>
<SubTree ID="WorkOnBench"/>
<SubTree ID="DepositToShelf"/>
<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"/>
</Sequence>
<Sequence>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
<Action ID="ActorMovement" animation_distance="1.5" animation_name="standing_walk" target="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="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,5 +1,6 @@
<?xml version="1.0"?>
<root main_tree_to_execute="actorTree">
<!-- ////////// -->
<BehaviorTree ID="actorTree">
<Fallback>
<ReactiveSequence>
@ -13,15 +14,60 @@
<Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
</Control>
<Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
</Sequence>
</ReactiveSequence>
<Sequence>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="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,26 +1,73 @@
<?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="standing_walk" target="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
</Sequence>
</ReactiveSequence>
<Sequence>
<Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="standing_walk" target="{actorTarget}"/>
<Action ID="ActorMovement" animation_name="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,5 +1,6 @@
<?xml version="1.0"?>
<root main_tree_to_execute="robotTree">
<!-- ////////// -->
<BehaviorTree ID="robotTree">
<ReactiveSequence>
<Inverter>
@ -25,5 +26,48 @@
</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,5 +1,6 @@
<?xml version="1.0"?>
<root main_tree_to_execute="robotTree">
<!-- ////////// -->
<BehaviorTree ID="robotTree">
<ReactiveSequence>
<Inverter>
@ -29,5 +30,48 @@
</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,8 +33,7 @@ target_link_libraries(ign_actor_plugin
install(TARGETS
ign_actor_plugin
DESTINATION share/${PROJECT_NAME}
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY animation DESTINATION share/${PROJECT_NAME})
ament_package()

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

File diff suppressed because one or more lines are too long

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

File diff suppressed because one or more lines are too long

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

File diff suppressed because one or more lines are too long

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

File diff suppressed because one or more lines are too long

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

File diff suppressed because one or more lines are too long

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

File diff suppressed because one or more lines are too long

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

File diff suppressed because one or more lines are too long

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

File diff suppressed because one or more lines are too long

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

File diff suppressed because one or more lines are too long

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

@ -1,30 +1,16 @@
import os
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler
from launch.actions import 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
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(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'],
output='screen'
@ -45,7 +31,7 @@ def generate_launch_description():
],
output='screen'
)
btree = Node(
package='btree',
executable='tree',
@ -53,8 +39,8 @@ def generate_launch_description():
parameters=[
{
"trees": [
[get_package_share_directory('btree'), '/trees/actorTree', scene, '.xml'],
[get_package_share_directory('btree'), '/trees/robotTree', scene, '.xml'],
get_package_share_directory('btree')+'/trees/actorTreeCoex.xml',
get_package_share_directory('btree')+'/trees/robotTreeCoex.xml'
],
"areas": [
"safeArea",
@ -93,24 +79,21 @@ 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 /tmp/gazebo_world.sdf'}.items(),
launch_arguments={'gz_args': '-v 4 -r '+get_package_share_directory('ign_world')+'/world/gaz_new_test.sdf'}.items(),
),
Node(
package='ros_gz_sim',
executable='create',
arguments=[
'-name', 'conveyor',
'-name', 'office',
'-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',
condition=LaunchConfigurationEquals('scene', 'Coop')
output='screen'
),
Node(
@ -128,7 +111,7 @@ def generate_launch_description():
output='both',
emulate_tty=True,
),
btree,
IncludeLaunchDescription(
@ -149,7 +132,7 @@ def generate_launch_description():
on_exit=[load_joint_state_broadcaster],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=load_joint_state_broadcaster,
@ -158,4 +141,4 @@ def generate_launch_description():
),
ignition_spawn_entity,
])
])

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

@ -0,0 +1,311 @@
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 Normal file
View File

@ -0,0 +1,55 @@
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
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="conveyor">
<model name="office">
<static>true</static>
<link name="conveyor">
<link name="office">
<visual name="visual">
<geometry>
<mesh>

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

@ -0,0 +1,75 @@
<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
View File

@ -1,109 +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="$(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>