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

Switch to C++

Browse Source
This commit is contained in:
Nicolas Hiillos
2022-11-22 18:11:17 +02:00
parent 1acb8a172b
commit 51ae0b5b9e
4 changed files with 622 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/draw_svg/CMakeLists.txt
View File

@@ -16,7 +16,7 @@ find_package(moveit_ros_planning_interface REQUIRED)
# Install C++
set(SRC_CPP_DIR src/cpp)
# Example 0 - Follow target
set(EXECUTABLE_0 log_position)
set(EXECUTABLE_0 follow)
add_executable(${EXECUTABLE_0} ${SRC_CPP_DIR}/${EXECUTABLE_0}.cpp)
ament_target_dependencies(${EXECUTABLE_0}
rclcpp

119
src/draw_svg/launch/lite6_pen.launch.py
View File

@@ -7,7 +7,7 @@ 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.substitutions import LaunchConfiguration, PathJoinSubstitution, Command, FindExecutable
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.event_handlers import OnProcessExit
@@ -109,14 +109,123 @@ def launch_setup(context, *args, **kwargs):
}.items(),
)
# URDF
_robot_description_xml = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro']
),
#PathJoinSubstitution(
# [
# FindPackageShare('xarm_description'),
# "urdf",
# "lite6",
# #"lite6.urdf.xacro",
# "lite6_robot_macro.xacro",
# ]
#),
" ",
#"name:=", "lite6", " ",
"prefix:=", " ",
"hw_ns:=", hw_ns, " ",
"limited:=", limited, " ",
"effort_control:=", effort_control, " ",
"velocity_control:=", velocity_control, " ",
"add_gripper:=", add_gripper, " ",
"add_vacuum_gripper:=", add_vacuum_gripper, " ",
"dof:=", dof, " ",
"robot_type:=", robot_type, " ",
"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_ip:=", robot_ip, " ",
#"report_type:=", report_type, " ",
#"baud_checkset:=", baud_checkset, " ",
#"default_gripper_baud:=", default_gripper_baud, " ",
]
)
# TODO fix URDF loading
# xacro urdf/xarm_pen.urdf.xacro prefix:= hw_ns:=xarm dof:=6 limited:=false effort_control:=false velocity_control:=false add_gripper:=false add_vacuum_gripper:=false robot_type:=lite ros2_control_plugin:=gazebo_ros2_control/GazeboSystem add_other_geometry:=false geometry_type:=cylinder geometry_mass:=0.05 geometry_height:=0.1 geometry_radius:=0.005 geometry_length:=0.07 geometry_width:=0.1 geometry_mesh_filename:= geometry_mesh_origin_xyz:="0 0 0" geometry_mesh_origin_rpy:="0 0 0" geometry_mesh_tcp_xyz:="0 0 0" geometry_mesh_tcp_rpy:="0 0 0"
_robot_description_xml = Command(['cat ', PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'lite6.tmp.urdf'])])
robot_description = {"robot_description": _robot_description_xml}
# SRDF
_robot_description_semantic_xml = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[
FindPackageShare('xarm_moveit_config'),
"srdf",
#"_lite6_macro.srdf.xacro",
"xarm.srdf.xacro",
]
),
" ",
#"name:=", "lite6", " ",
"prefix:=", " ",
#"hw_ns:=", hw_ns, " ",
#"limited:=", limited, " ",
#"effort_control:=", effort_control, " ",
#"velocity_control:=", velocity_control, " ",
#"add_gripper:=", add_gripper, " ",
#"add_vacuum_gripper:=", add_vacuum_gripper, " ",
"dof:=", dof, " ",
"robot_type:=", robot_type, " ",
#"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_ip:=", robot_ip, " ",
#"report_type:=", report_type, " ",
#"baud_checkset:=", baud_checkset, " ",
#"default_gripper_baud:=", default_gripper_baud, " ",
]
)
robot_description_semantic = {
"robot_description_semantic": _robot_description_semantic_xml
}
nodes = [
Node(
package="draw_svg",
executable="follow.py",
executable="follow",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[{"use_sim_time": use_sim_time}],
parameters=[
robot_description,
robot_description_semantic,
{"use_sim_time": use_sim_time},
],
),
Node(
package="draw_svg",
@@ -134,10 +243,10 @@ def launch_setup(context, *args, **kwargs):
),
]
return nodes + [
return [
robot_moveit_common_launch,
robot_gazebo_launch,
]
] + nodes
def generate_launch_description():

71
src/draw_svg/src/cpp/follow.cpp Normal file
View File

@@ -0,0 +1,71 @@
//#include "follow.h"
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <rclcpp/qos.hpp>
#include <rclcpp/rclcpp.hpp>
const std::string MOVE_GROUP = "lite6";
class MoveItFollowTarget : public rclcpp::Node
{
public:
/// Constructor
MoveItFollowTarget();
/// Move group interface for the robot
moveit::planning_interface::MoveGroupInterface move_group_;
/// Subscriber for target pose
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr target_pose_sub_;
/// Target pose that is used to detect changes
geometry_msgs::msg::Pose previous_target_pose_;
private:
/// Callback that plans and executes trajectory each time the target pose is changed
void target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg);
};
MoveItFollowTarget::MoveItFollowTarget() : Node("follow_target"),
move_group_(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
{
// Use upper joint velocity and acceleration limits
this->move_group_.setMaxAccelerationScalingFactor(1.0);
this->move_group_.setMaxVelocityScalingFactor(1.0);
// Subscribe to target pose
target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
RCLCPP_INFO(this->get_logger(), "Initialization successful.");
}
void MoveItFollowTarget::target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
{
// Return if target pose is unchanged
if (msg->pose == previous_target_pose_)
{
return;
}
RCLCPP_INFO(this->get_logger(), "Target pose has changed. Planning and executing...");
// Plan and execute motion
this->move_group_.setPoseTarget(msg->pose);
this->move_group_.move();
// Update for next callback
previous_target_pose_ = msg->pose;
}
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
auto target_follower = std::make_shared<MoveItFollowTarget>();
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(target_follower);
executor.spin();
rclcpp::shutdown();
return EXIT_SUCCESS;
}

436
src/draw_svg/urdf/lite6.tmp.urdf Normal file
View File

@@ -0,0 +1,436 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from urdf/xarm_pen.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="xarm6">
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
<parameters>/root/ws/install/share/xarm_controller/config/xarm6_controllers.yaml</parameters>
</plugin>
</gazebo>
<link name="world"/>
<joint name="world_joint" type="fixed">
<parent link="world"/>
<child link="link_base"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<ros2_control name="UFRobotSystem" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
<param name="hw_ns">xarm</param>
<param name="velocity_control">False</param>
<param name="prefix">P</param>
<param name="robot_ip">R</param>
<param name="report_type">normal</param>
<param name="dof">6</param>
<param name="baud_checkset">True</param>
<param name="default_gripper_baud">2000000</param>
<param name="robot_type">lite</param>
<param name="add_gripper">False</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min">-6.283185307179586</param>
<param name="max">6.283185307179586</param>
</command_interface>
<command_interface name="velocity">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
<!-- <state_interface name="effort"/> -->
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-2.61799</param>
<param name="max">2.61799</param>
</command_interface>
<command_interface name="velocity">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
<!-- <state_interface name="effort"/> -->
</joint>
<joint name="joint3">
<command_interface name="position">
<param name="min">-0.061087</param>
<param name="max">5.235988</param>
</command_interface>
<command_interface name="velocity">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
<!-- <state_interface name="effort"/> -->
</joint>
<joint name="joint4">
<command_interface name="position">
<param name="min">-6.283185307179586</param>
<param name="max">6.283185307179586</param>
</command_interface>
<command_interface name="velocity">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
<!-- <state_interface name="effort"/> -->
</joint>
<joint name="joint5">
<command_interface name="position">
<param name="min">-2.1642</param>
<param name="max">2.1642</param>
</command_interface>
<command_interface name="velocity">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
<!-- <state_interface name="effort"/> -->
</joint>
<joint name="joint6">
<command_interface name="position">
<param name="min">-6.283185307179586</param>
<param name="max">6.283185307179586</param>
</command_interface>
<command_interface name="velocity">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
<!-- <state_interface name="effort"/> -->
</joint>
</ros2_control>
<material name="White">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<material name="Silver">
<color rgba="0.753 0.753 0.753 1.0"/>
</material>
<link name="link_base">
<inertial>
<origin rpy="0 0 0" xyz="-0.00829544579053192 3.26357432323433E-05 0.0631194584987089"/>
<mass value="1.65393501783165"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/base.stl"/>
</geometry>
<material name="White"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/base.stl"/>
</geometry>
</collision>
</link>
<link name="link1">
<inertial>
<origin rpy="0 0 0" xyz="-0.00036 0.03788 -0.0027"/>
<mass value="1.169"/>
<inertia ixx="1.45164E-03" ixy="1.24E-05" ixz="-6.7E-06" iyy="8.873E-04" iyz="1.255E-04" izz="1.31993E-03"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link1.stl"/>
</geometry>
<material name="White"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link1.stl"/>
</geometry>
</collision>
</link>
<joint name="joint1" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.2435"/>
<parent link="link_base"/>
<child link="link1"/>
<axis xyz="0 0 1"/>
<limit effort="50.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
<link name="link2">
<inertial>
<origin rpy="0 0 0" xyz="0.178 0.0 0.0576"/>
<mass value="1.192"/>
<inertia ixx="1.5854E-03" ixy="-6.766E-06" ixz="-1.15136E-03" iyy="5.6097E-03" iyz="1.14E-06" izz="4.85E-03"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link2.stl"/>
</geometry>
<material name="White"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link2.stl"/>
</geometry>
</collision>
</link>
<joint name="joint2" type="revolute">
<origin rpy="1.5708 -1.5708 3.1416" xyz="0 0 0"/>
<parent link="link1"/>
<child link="link2"/>
<axis xyz="0 0 1"/>
<limit effort="50.0" lower="-2.61799" upper="2.61799" velocity="3.14"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
<link name="link3">
<inertial>
<origin rpy="0 0 0" xyz="0.07285 -0.030 -0.0009"/>
<mass value="0.930"/>
<inertia ixx="8.861E-04" ixy="-3.9287E-04" ixz="7.066E-05" iyy="1.5785E-03" iyz="-2.445E-05" izz="1.84677E-03"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link3.stl"/>
</geometry>
<material name="White"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link3.stl"/>
</geometry>
</collision>
</link>
<joint name="joint3" type="revolute">
<origin rpy="-3.1416 0 1.5708" xyz="0.2002 0 0"/>
<parent link="link2"/>
<child link="link3"/>
<axis xyz="0 0 1"/>
<limit effort="32.0" lower="-0.061087" upper="5.235988" velocity="3.14"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
<link name="link4">
<inertial>
<origin rpy="0 0 0" xyz="-0.0004 -0.0275 -0.0817"/>
<mass value="1.31"/>
<inertia ixx="3.705E-03" ixy="-2.0E-06" ixz="7.17E-06" iyy="3.0455E-03" iyz="-9.3188E-04" izz="1.5413E-03"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link4.stl"/>
</geometry>
<material name="White"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link4.stl"/>
</geometry>
</collision>
</link>
<joint name="joint4" type="revolute">
<origin rpy="1.5708 0 0" xyz="0.087 -0.22761 0"/>
<parent link="link3"/>
<child link="link4"/>
<axis xyz="0 0 1"/>
<limit effort="32.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
<link name="link5">
<inertial>
<origin rpy="0 0 0" xyz="0.0 0.010 0.0019"/>
<mass value="0.784"/>
<inertia ixx="5.668E-04" ixy="6E-07" ixz="-5.3E-06" iyy="5.077E-04" iyz="-4.8E-07" izz="5.3E-04"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link5.stl"/>
</geometry>
<material name="White"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link5.stl"/>
</geometry>
</collision>
</link>
<joint name="joint5" type="revolute">
<origin rpy="1.5708 0 0" xyz="0 0 0"/>
<parent link="link4"/>
<child link="link5"/>
<axis xyz="0 0 1"/>
<limit effort="32.0" lower="-2.1642" upper="2.1642" velocity="3.14"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
<link name="link6">
<inertial>
<origin rpy="0 0 0" xyz="0.0 -0.00194 -0.0102"/>
<mass value="0.180"/>
<inertia ixx="7.726E-05" ixy="1E-06" ixz="4E-07" iyy="8.5665E-05" iyz="-6E-07" izz="1.4814E-04"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link6.stl"/>
</geometry>
<material name="Silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link6.stl"/>
</geometry>
</collision>
</link>
<joint name="joint6" type="revolute">
<origin rpy="-1.5708 0 0" xyz="0 0.0625 0"/>
<parent link="link5"/>
<child link="link6"/>
<axis xyz="0 0 1"/>
<limit effort="20.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
<dynamics damping="1.0" friction="1.0"/>
</joint>
<link name="link_eef"/>
<joint name="joint_eef" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="link6"/>
<child link="link_eef"/>
</joint>
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>100</mechanicalReduction>
</actuator>
</transmission>
<transmission name="tran2">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint2">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor2">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>100</mechanicalReduction>
</actuator>
</transmission>
<transmission name="tran3">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint3">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor3">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>100</mechanicalReduction>
</actuator>
</transmission>
<transmission name="tran4">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint4">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor3">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>100</mechanicalReduction>
</actuator>
</transmission>
<transmission name="tran5">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint5">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor5">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>100</mechanicalReduction>
</actuator>
</transmission>
<transmission name="tran6">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint6">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor6">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>100</mechanicalReduction>
</actuator>
</transmission>
<gazebo reference="link_base">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link1">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link2">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link3">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link4">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link5">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link6">
<selfCollide>true</selfCollide>
</gazebo>
<joint name="eef_joint" type="fixed">
<parent link="link_eef"/>
<child link="pen_link"/>
</joint>
<link name="pen_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.2" radius="0.005"/>
</geometry>
<material name="Cyan">
<color rgba="0.0 1.0 1.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<box size="0 0 0"/>
</geometry>
</collision>
</link>
<!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_p3d.cpp -->
<gazebo>
<plugin filename="libgazebo_ros_p3d.so" name="pen_position">
<!-- <alwaysOn>true</alwaysOn> -->
<ros>
<remapping>odom:=pen_position</remapping>
</ros>
<frame_name>world</frame_name>
<!-- <body_name>pen_link</body_name> -->
<body_name>link6</body_name>
<!-- topic name is always /odom -->
<!-- <topic_name>pen_position</topic_name> -->
<!-- Update rate in Hz -->
<update_rate>10</update_rate>
<!-- <xyzOffsets>0 0 0</xyzOffsets> -->
<!-- <rpyOffsets>0 0 0</rpyOffsets> -->
</plugin>
</gazebo>
</robot>
<!-- <robot name="lite6_robot" xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:xi="http://www.w3.org/2001/XInclude">
<xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.urdf.xacro"/>
<xacro:lite6_urdf prefix=''/> -->
<!--
</robot> -->