robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

update rbs_simulation

Browse source
This commit is contained in:
Ilya Uraev 2023-07-17 16:44:02 +03:00
parent 825dd26bb5
commit da0ef3cb71
2 changed files with 99 additions and 26 deletions
Download patch file Download diff file Expand all files Collapse all files

2
rbs_simulation/CMakeLists.txt
View file

@ -12,7 +12,7 @@ find_package(ament_cmake REQUIRED)
# find_package(<dependency> REQUIRED)
install(
DIRECTORY launch worlds
DIRECTORY launch worlds mujoco_model
DESTINATION share/${PROJECT_NAME}
)

123
rbs_simulation/launch/rbs_simulation.launch.py
View file

@ -1,9 +1,10 @@
import os
from launch import LaunchDescription
from launch import LaunchDescription, LaunchContext
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
ExecuteProcess
ExecuteProcess,
OpaqueFunction
)
from ament_index_python.packages import get_package_share_directory
from launch.conditions import IfCondition, UnlessCondition
@ -12,6 +13,8 @@ from launch.substitutions import Command, FindExecutable, LaunchConfiguration, P
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ur_moveit_config.launch_common import load_yaml
import xacro
def generate_launch_description():
declared_arguments = []
# UR specific arguments
@ -26,7 +29,7 @@ def generate_launch_description():
declared_arguments.append(
DeclareLaunchArgument(
"with_gripper",
default_value="true",
default_value="false",
description="With gripper or not?",
)
)
@ -158,7 +161,7 @@ def generate_launch_description():
# General arguments
runtime_config_package = LaunchConfiguration("runtime_config_package")
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_with_gripper_file")
controllers_file = LaunchConfiguration("controllers_file")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
@ -183,6 +186,10 @@ def generate_launch_description():
world_config_file = PathJoinSubstitution(
[FindPackageShare("rbs_simulation"), "worlds", "mir.sdf"]
)
mujoco_model = PathJoinSubstitution(
[FindPackageShare("rbs_simulation"), "mujoco_model", "current_mj.xml"]
)
robot_description_content = Command(
[
@ -210,16 +217,36 @@ def generate_launch_description():
"prefix:=",
prefix,
" ",
"sim_ignition:=true",
"sim_mujoco:=true",
" ",
"simulation_controllers:=",
initial_joint_controllers_file_path,
" ",
"with_gripper:=",
with_gripper_condition
with_gripper_condition,
" ",
"mujoco_model:=",
mujoco_model,
]
)
robot_description = {"robot_description": robot_description_content}
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, initial_joint_controllers_file_path],
#prefix=['xterm -e gdb -ex run --args'],
output="both",
remappings=[
('motion_control_handle/target_frame', 'target_frame'),
('cartesian_motion_controller/target_frame', 'target_frame'),
# ('cartesian_compliance_controller/target_frame', 'target_frame'),
# ('cartesian_force_controller/target_wrench', 'target_wrench'),
# ('cartesian_compliance_controller/target_wrench', 'target_wrench'),
# ('cartesian_force_controller/ft_sensor_wrench', 'ft_sensor_wrench'),
# ('cartesian_compliance_controller/ft_sensor_wrench', 'ft_sensor_wrench'),
]
)
robot_state_publisher_node = Node(
package="robot_state_publisher",
@ -231,20 +258,20 @@ def generate_launch_description():
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
)
# There may be other controllers of the joints, but this is the initially-started one
initial_joint_controller_spawner_started = Node(
package="controller_manager",
executable="spawner",
arguments=[initial_joint_controller, "--controller-manager", "/controller_manager"],
arguments=[initial_joint_controller, "-c", "/controller_manager"],
condition=IfCondition(start_joint_controller),
)
initial_joint_controller_spawner_stopped = Node(
package="controller_manager",
executable="spawner",
arguments=[initial_joint_controller, "--controller-manager", "/controller_manager", "--stopped"],
arguments=[initial_joint_controller, "-c", "/controller_manager", "--incactive"],
condition=UnlessCondition(start_joint_controller),
)
@ -254,22 +281,32 @@ def generate_launch_description():
output='screen',
condition=IfCondition(with_gripper_condition)
)
cartesian_motion_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["cartesian_motion_controller", "--inactive", "-c", "/controller_manager"],
)
motion_control_handle_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["motion_control_handle", "--inactive", "-c", "/controller_manager"],
)
# Gazebo nodes
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory('ros_ign_gazebo'),
'launch', 'ign_gazebo.launch.py')]),
launch_arguments=[('ign_args', [' -r ',world_config_file, " --physics-engine ignition-physics-dartsim-plugin --render-engine ogre2"])])
# Spawn robot
gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create',
arguments=[
'-name', rbs_robot_type,
'-x', '0.0',
'-z', '0.0',
'-y', '0.0',
'-topic', '/robot_description'],
output='screen')
# gazebo = IncludeLaunchDescription(
# PythonLaunchDescriptionSource(
# [os.path.join(get_package_share_directory('ros_ign_gazebo'),
# 'launch', 'ign_gazebo.launch.py')]),
# launch_arguments=[('ign_args', [' -r ',world_config_file, " --physics-engine ignition-physics-dartsim-plugin --render-engine ogre2"])])
# # Spawn robot
# gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create',
# arguments=[
# '-name', rbs_robot_type,
# '-x', '0.0',
# '-z', '0.0',
# '-y', '0.0',
# '-topic', '/robot_description'],
# output='screen')
# MoveIt Configuration
robot_description_semantic_content = Command(
@ -457,14 +494,46 @@ def generate_launch_description():
# ]
# )
# remappings = [('/camera', '/camera/image'),
# ('/camera_info', '/camera/camera_info')]
# # Bridge
# bridge = Node(
# package='ros_gz_bridge',
# executable='parameter_bridge',
# arguments=[
# '/camera@sensor_msgs/msg/Image@gz.msgs.Image',
# '/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
# '/rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image',
# '/rgbd_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
# '/rgbd_camera/depth_image@sensor_msgs/msg/Image@gz.msgs.Image',
# '/rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
# ],
# output='screen',
# remappings=remappings,
# )
# pc_filter = Node(
# package="rbs_perception",
# executable="pc_filter",
# output="screen",
# #prefix=['xterm -e gdb -ex run --args'],
# )
grasp_marker = Node(
package="rbs_perception",
executable="grasp_marker_publish.py",
name="hyak",
)
nodes_to_start = [
control_node,
robot_state_publisher_node,
joint_state_broadcaster_spawner,
rviz_node,
initial_joint_controller_spawner_stopped,
initial_joint_controller_spawner_started,
gazebo,
gazebo_spawn_robot,
# gazebo,
# gazebo_spawn_robot,
move_group_node,
gripper_controller,
gripper_control_node,
@ -474,6 +543,10 @@ def generate_launch_description():
grasp_pose_loader,
assemble_state,
moveit_planning_scene_init,
#bridge,
cartesian_motion_controller_spawner,
motion_control_handle_spawner,
#pc_filter
#add_planning_scene_object
]