From 51ae0b5b9e3308f8403ceccb7f4ae08c1bef58ec Mon Sep 17 00:00:00 2001 From: Nicolas Hiillos Date: Tue, 22 Nov 2022 18:11:17 +0200 Subject: [PATCH] Switch to C++ --- src/draw_svg/CMakeLists.txt | 2 +- src/draw_svg/launch/lite6_pen.launch.py | 119 ++++++- src/draw_svg/src/cpp/follow.cpp | 71 ++++ src/draw_svg/urdf/lite6.tmp.urdf | 436 ++++++++++++++++++++++++ 4 files changed, 622 insertions(+), 6 deletions(-) create mode 100644 src/draw_svg/src/cpp/follow.cpp create mode 100644 src/draw_svg/urdf/lite6.tmp.urdf diff --git a/src/draw_svg/CMakeLists.txt b/src/draw_svg/CMakeLists.txt index 5a96960..2081abb 100644 --- a/src/draw_svg/CMakeLists.txt +++ b/src/draw_svg/CMakeLists.txt @@ -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 diff --git a/src/draw_svg/launch/lite6_pen.launch.py b/src/draw_svg/launch/lite6_pen.launch.py index 00b68e7..5b583d1 100644 --- a/src/draw_svg/launch/lite6_pen.launch.py +++ b/src/draw_svg/launch/lite6_pen.launch.py @@ -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(): diff --git a/src/draw_svg/src/cpp/follow.cpp b/src/draw_svg/src/cpp/follow.cpp new file mode 100644 index 0000000..ec626e3 --- /dev/null +++ b/src/draw_svg/src/cpp/follow.cpp @@ -0,0 +1,71 @@ +//#include "follow.h" + +#include +#include +#include +#include + +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::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(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("/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(); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(target_follower); + executor.spin(); + + rclcpp::shutdown(); + return EXIT_SUCCESS; +} diff --git a/src/draw_svg/urdf/lite6.tmp.urdf b/src/draw_svg/urdf/lite6.tmp.urdf new file mode 100644 index 0000000..5035766 --- /dev/null +++ b/src/draw_svg/urdf/lite6.tmp.urdf @@ -0,0 +1,436 @@ + + + + + + + + + gazebo_ros2_control/GazeboSystem + /root/ws/install/share/xarm_controller/config/xarm6_controllers.yaml + + + + + + + + + + + gazebo_ros2_control/GazeboSystem + xarm + False + P + R + normal + 6 + True + 2000000 + lite + False + + + + -6.283185307179586 + 6.283185307179586 + + + -3.14 + 3.14 + + + + + + + + -2.61799 + 2.61799 + + + -3.14 + 3.14 + + + + + + + + -0.061087 + 5.235988 + + + -3.14 + 3.14 + + + + + + + + -6.283185307179586 + 6.283185307179586 + + + -3.14 + 3.14 + + + + + + + + -2.1642 + 2.1642 + + + -3.14 + 3.14 + + + + + + + + -6.283185307179586 + 6.283185307179586 + + + -3.14 + 3.14 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 100 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 100 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 100 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 100 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 100 + + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + hardware_interface/PositionJointInterface + 100 + + + + true + + + true + + + true + + + true + + + true + + + true + + + true + + + + + + + + + + + + + + + + + + + + + + + + + + + odom:=pen_position + + world + + link6 + + + + 10 + + + + + + +