nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Set up new virtual surface package

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-03-23 10:28:39 +02:00
parent 53c46c11c0
commit 954020bb36
4 changed files with 351 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

177
src/custom_xarm_gazebo/launch/lite6_virtual_surface.launch.py Normal file
View File

@@ -0,0 +1,177 @@
import os
from ament_index_python import get_package_share_directory
from launch.launch_description_sources import load_python_launch_file_as_module
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.event_handlers import OnProcessExit
from launch.actions import OpaqueFunction
def launch_setup(context, *args, **kwargs):
prefix = LaunchConfiguration('prefix', default='')
hw_ns = LaunchConfiguration('hw_ns', default='xarm')
limited = LaunchConfiguration('limited', default=False)
effort_control = LaunchConfiguration('effort_control', default=False)
velocity_control = LaunchConfiguration('velocity_control', default=False)
add_gripper = LaunchConfiguration('add_gripper', default=False)
add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
dof = LaunchConfiguration('dof', default=7)
robot_type = LaunchConfiguration('robot_type', default='xarm')
ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem')
add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
geometry_type = LaunchConfiguration('geometry_type', default='box')
geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
geometry_height = LaunchConfiguration('geometry_height', default=0.1)
geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
geometry_length = LaunchConfiguration('geometry_length', default=0.1)
geometry_width = LaunchConfiguration('geometry_width', default=0.1)
geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
load_controller = LaunchConfiguration('load_controller', default=False)
ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
# ros2 control params
# xarm_controller/launch/lib/robot_controller_lib.py
mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_controller'), 'launch', 'lib', 'robot_controller_lib.py'))
generate_ros2_control_params_temp_file = getattr(mod, 'generate_ros2_control_params_temp_file')
ros2_control_params = generate_ros2_control_params_temp_file(
os.path.join(get_package_share_directory('xarm_controller'), 'config', '{}{}_controllers.yaml'.format(robot_type.perform(context), dof.perform(context))),
prefix=prefix.perform(context),
add_gripper=add_gripper.perform(context) in ('True', 'true'),
ros_namespace=LaunchConfiguration('ros_namespace', default='').perform(context),
update_rate=1000,
#robot_type=robot_type.perform(context)
)
# robot_description
# xarm_description/launch/lib/robot_description_lib.py
mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('custom_xarm_description'), 'launch', 'lib', 'robot_description_lib.py'))
#mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('draw_svg'), 'launch', 'robots', 'xarm_with_pen.py'))
get_xacro_file_content = getattr(mod, 'get_xacro_file_content')
robot_description = {
'robot_description': get_xacro_file_content(
xacro_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
#xacro_file=PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro']),
arguments={
'prefix': prefix,
'dof': dof,
'robot_type': robot_type,
'add_gripper': add_gripper,
'add_vacuum_gripper': add_vacuum_gripper,
'hw_ns': hw_ns.perform(context).strip('/'),
'limited': limited,
'effort_control': effort_control,
'velocity_control': velocity_control,
'ros2_control_plugin': ros2_control_plugin,
'ros2_control_params': ros2_control_params,
'add_other_geometry': add_other_geometry,
'geometry_type': geometry_type,
'geometry_mass': geometry_mass,
'geometry_height': geometry_height,
'geometry_radius': geometry_radius,
'geometry_length': geometry_length,
'geometry_width': geometry_width,
'geometry_mesh_filename': geometry_mesh_filename,
'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
}
),
}
# robot state publisher node
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[robot_description],
remappings=[
('/tf', 'tf'),
('/tf_static', 'tf_static'),
]
)
# gazebo launch
# gazebo_ros/launch/gazebo.launch.py
#xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'worlds', 'table.world'])
#xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('draw_svg'), 'worlds', 'draw_svg_world.sdf'])
xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('custom_xarm_gazebo'), 'worlds', 'virtual_surface_world.sdf'])
#xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('custom_xarm_gazebo'), 'worlds', 'table.world'])
gazebo_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('gazebo_ros'), 'launch', 'gazebo.launch.py'])),
launch_arguments={
'world': xarm_gazebo_world,
'server_required': 'true',
'gui_required': 'true',
}.items(),
)
# gazebo spawn entity node
gazebo_spawn_entity_node = Node(
package="gazebo_ros",
executable="spawn_entity.py",
output='screen',
arguments=[
'-topic', 'robot_description',
'-entity', '{}{}'.format(robot_type.perform(context), dof.perform(context)),
'-x', '-0.2',
#'-x', '0.0',
'-y', '-0.5',
#'-y', '0.0',
'-z', '1.021',
#'-z', '0.0',
'-Y', '1.571',
#'-Y', '0.0',
],
)
# Load controllers
controllers = [
'joint_state_broadcaster',
'{}{}{}_traj_controller'.format(prefix.perform(context), robot_type.perform(context), dof.perform(context)),
]
if robot_type.perform(context) == 'xarm' and add_gripper.perform(context) in ('True', 'true'):
controllers.append('{}{}_gripper_traj_controller'.format(prefix.perform(context), robot_type.perform(context)))
load_controllers = []
if load_controller.perform(context) in ('True', 'true'):
for controller in controllers:
#print("Attempting to load: ", controller)
#input()
load_controllers.append(Node(
package='controller_manager',
#executable='spawner.py',
executable='spawner',
output='screen',
arguments=[
controller,
'--controller-manager', '{}/controller_manager'.format(ros_namespace)
],
))
return [
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=gazebo_spawn_entity_node,
on_exit=load_controllers,
)
),
gazebo_launch,
robot_state_publisher_node,
gazebo_spawn_entity_node,
]
def generate_launch_description():
return LaunchDescription([
OpaqueFunction(function=launch_setup)
])

170
src/custom_xarm_gazebo/worlds/virtual_surface_world.sdf Normal file
View File

@@ -0,0 +1,170 @@
<?xml version="1.0"?>
<!-- <sdf version="1.9"> -->
<sdf version="1.7">
<world name="virtual_surface_world">
<!-- Physics -->
<plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
<engine>
<filename>ignition-physics-dartsim-plugin</filename>
</engine>
<dart>
<collision_detector>bullet</collision_detector>
</dart>
</plugin>
<physics name="physics_config" type="dart">
<max_step_size>0.005</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<!-- Scene -->
<plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>
<scene>
<ambient>0.8 0.8 0.8</ambient>
<grid>false</grid>
</scene>
<!-- User Commands (transform control) -->
<plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
</plugin>
<!-- -->
<!-- Illumination -->
<!-- -->
<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.3 0.3 -0.9</direction>
</light>
<!-- -->
<!-- Models -->
<!-- -->
<!-- Ground -->
<model name="ground_plane">
<pose>0 0 0 0 0 0</pose>
<static>true</static>
<link name="ground_plane_link">
<collision name="ground_plane_collision">
<geometry>
<plane>
<normal>0 0 1</normal>
</plane>
</geometry>
</collision>
<visual name="ground_plane_visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>4 4</size>
</plane>
</geometry>
<material>
<diffuse>0.2 0.2 0.2 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
</link>
</model>
<model name="drawing_surface">
<pose>-0.1485 -0.3 0.5 0 0 0</pose>
<static>true</static>
<link name="drawing_surface_link">
<collision name="drawing_surface_collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>0.297 0.21</size>
</plane>
</geometry>
</collision>
<visual name="drawing_surface_visual">
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<plane>
<normal>0 0 1</normal>
<size>0.297 0.21</size>
</plane>
</geometry>
<!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_video.cpp -->
<!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/worlds/gazebo_ros_video_demo.world -->
<!-- TODO: note docs outdated: https://classic.gazebosim.org/tutorials?tut=ros_gzplugins -->
<plugin name="drawing_surface" filename="libgazebo_ros_video.so">
<!-- <ros>
<remapping>~/image_raw:=/camera1/image_raw</remapping>
</ros> -->
<!-- 300dpi A4 paper is 2480x3508 px -->
<height>2480</height>
<width>3508</width>
</plugin>
</visual>
<!-- <inertial>
<origin xyz="0 0 1" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0"/>
</inertial> -->
</link>
</model>
<!-- <include>
<uri>model://ground_plane</uri>
<pose>0.0 -0.84 0 0 0 0</pose>
</include> -->
<!-- <include>
<uri>model://sun</uri>
</include> -->
<!-- <include>
<uri>model://table</uri>
<name>table</name>
<pose>0.0 -0.84 0 0 0 0</pose>
</include>
-->
<!-- Static target -->
<!-- <model name="target">
<static>true</static>
<pose>0.5 -0.25 0.5 3.1415927 0 0</pose>
<link name="target_link">
<visual name="target_visual">
<cast_shadows>false</cast_shadows>
<geometry>
<box>
<size>0.04 0.04 0.04</size>
</box>
</geometry>
<material>
<diffuse>0.1 0.1 0.1 1</diffuse>
<specular>0.4 0.4 0.4 1</specular>
</material>
</visual>
</link>
-->
<!-- Pose publisher plugin, used to determine the target pose to reach -->
<!-- <plugin filename="ignition-gazebo-pose-publisher-system" name="ignition::gazebo::systems::PosePublisher">
<publish_nested_model_pose>true</publish_nested_model_pose>
<publish_link_pose>false</publish_link_pose>
<publish_collision_pose>false</publish_collision_pose>
<publish_visual_pose>false</publish_visual_pose>
</plugin>
</model> -->
<!-- <plugin filename="ignition-gazebo-pose-publisher-system" name="ignition::gazebo::systems::PosePublisher">
<publish_link_pose>true</publish_link_pose>
<publish_collision_pose>false</publish_collision_pose>
<publish_visual_pose>false</publish_visual_pose>
<publish_nested_model_pose>false</publish_nested_model_pose>
</plugin> -->
</world>
</sdf>