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

Working iisy with ros2_control and actor in same world

Browse Source
This commit is contained in:
Bastian Hofmann
2023-02-22 17:02:58 +00:00
parent d19bb7e053
commit c1f72961f3
38 changed files with 2395 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files

81
src/ign_world/launch/gazebo_controller_launch.py Normal file
View File

@@ -0,0 +1,81 @@
import os
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
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
import xacro
def generate_launch_description():
moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs()
load_joint_state_broadcaster = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'],
output='screen'
)
load_joint_trajectory_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'arm_controller'],
output='screen'
)
ignition_spawn_entity = Node(
package='ros_gz_sim',
executable='create',
arguments=[
'-name', 'iisy',
'-allow_renaming', 'true',
'-string', xacro.process(os.path.join(get_package_share_directory('iisy_config_2'), 'config', 'iisy.urdf.xacro')),
],
output='screen'
)
return LaunchDescription([
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')),
launch_arguments={'gz_args': '-v 4 -r /home/ros/workspace/src/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),
Node(
package='ros_actor_action_server',
executable='ros_actor_action_server'
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
str(moveit_config.package_path / "launch/rsp.launch.py")
),
),
#IncludeLaunchDescription(
# PythonLaunchDescriptionSource(
# str(moveit_config.package_path / "launch/move_group.launch.py")
# ),
#),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=ignition_spawn_entity,
on_exit=[load_joint_state_broadcaster],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=load_joint_state_broadcaster,
on_exit=[load_joint_trajectory_controller],
)
),
ignition_spawn_entity,
])

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

@@ -1,15 +1,17 @@
import os
import subprocess
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node,SetParameter
from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, RegisterEventHandler, UnsetEnvironmentVariable
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 Command, LaunchConfiguration, PathJoinSubstitution
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):
@@ -49,6 +51,19 @@ def load_yaml(package_name, 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')
@@ -60,10 +75,9 @@ def generate_launch_description():
kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yml')
robot_description_kinematics = {'robot_description_kinematics': kinematics_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",
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 = {
@@ -86,7 +100,7 @@ def generate_launch_description():
}
trajectory_execution = {
'moveit_manage_controllers': True,
'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
@@ -126,11 +140,71 @@ def generate_launch_description():
robot_description_kinematics,
planning,
trajectory_execution,
moveit_controllers,
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}
@@ -139,19 +213,55 @@ def generate_launch_description():
pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
return LaunchDescription([
SetParameter(name="use_sim_time",value=True),
#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='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'),
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',
executable='static_transform_publisher',
@@ -182,20 +292,15 @@ def generate_launch_description():
"--child-frame-id", "lidar_2_link"]
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=["/clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock"],
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=["/world/default/model/iisy/joint_state@sensor_msgs/msg/JointState@ignition.msgs.Model"],
remappings=[
('/world/default/model/iisy/joint_state', '/joint_states'),
]
),
#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',
@@ -203,28 +308,4 @@ def generate_launch_description():
output='both',
parameters=[robot_description]
),
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,
moveit_controllers,
planning_scene_monitor_parameters,
# octomap_config,
# octomap_updater_config
]
),
spawn_robot,
rviz,
])

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_2").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 /home/ros/workspace/src/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_2'), 'config', 'iisy_xacro.urdf'))],
# output='screen'
#),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
str(moveit_config.package_path / "launch/demo.launch.py")
),
),
])