Add freedrive xArm to target height

src/lite6_controller/CMakeLists.txt

@@ -50,6 +50,7 @@ ament_target_dependencies(lite6_controller
 add_executable(lite6_calibration src/lite6_calibration.cpp)
 ament_target_dependencies(lite6_calibration
   "xarm_msgs"
+  "xarm_api"
   "rclcpp"
   "moveit"
   "rclcpp_action"

src/lite6_controller/launch/lite6_real_calibration.launch.py

@@ -1,126 +0,0 @@
-import os
-from launch import LaunchDescription
-from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.substitutions import FindPackageShare
-from launch_ros.actions import Node
-
-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 RegisterEventHandler, EmitEvent
-from launch.event_handlers import OnProcessExit
-from launch.events import Shutdown
-
-
-def launch_setup(context, *args, **kwargs):
-    robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150')
-    report_type = LaunchConfiguration('report_type', default='normal')
-    prefix = LaunchConfiguration('prefix', default='')
-    hw_ns = LaunchConfiguration('hw_ns', default='ufactory')
-    limited = LaunchConfiguration('limited', default=True)
-    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=6)
-    robot_type = LaunchConfiguration('robot_type', default='lite')
-    no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False)
-
-    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"')
-
-    baud_checkset = LaunchConfiguration('baud_checkset', default=True)
-    default_gripper_baud = LaunchConfiguration('default_gripper_baud', default=2000000)
-
-    ros2_control_plugin = 'uf_robot_hardware/UFRobotSystemHardware'
-    controllers_name = LaunchConfiguration('controllers_name', default='controllers')
-    moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_simple_controller_manager')
-    moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_simple_controller_manager/MoveItSimpleControllerManager')
-
-    xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context))
-    ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
-
-    use_sim_time = LaunchConfiguration('use_sim_time', default=False)
-    log_level = LaunchConfiguration("log_level", default='warn')
-
-    # robot_description_parameters
-    # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
-    moveit_config_package_name = 'custom_xarm_moveit_config'
-    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py'))
-    get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters')
-    robot_description_parameters = get_xarm_robot_description_parameters(
-        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
-        urdf_arguments={
-            'prefix': prefix,
-            'hw_ns': hw_ns.perform(context).strip('/'),
-            '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,
-            '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,
-        },
-        srdf_arguments={
-            'prefix': prefix,
-            'dof': dof,
-            'robot_type': robot_type,
-            'add_gripper': add_gripper,
-            'add_vacuum_gripper': add_vacuum_gripper,
-            'add_other_geometry': add_other_geometry,
-        },
-        arguments={
-            'context': context,
-            'xarm_type': xarm_type,
-        }
-    )
-    calibration_node = Node(
-            package="lite6_controller",
-            executable="lite6_calibration",
-            output="screen",
-            arguments=["--ros-args", "--log-level", log_level],
-            parameters=[
-                robot_description_parameters,
-                {"use_sim_time": use_sim_time},
-            ],
-        )
-
-    return [
-        RegisterEventHandler(event_handler=OnProcessExit(
-            target_action=calibration_node,
-            on_exit=[EmitEvent(event=Shutdown())]
-        )),
-        calibration_node
-    ]
-
-
-def generate_launch_description():
-    return LaunchDescription([
-        OpaqueFunction(function=launch_setup)
-    ])

src/lite6_controller/package.xml

@@ -16,6 +16,7 @@
   <depend>moveit</depend>
   <depend>moveit_msgs</depend>
   <depend>xarm_msgs</depend>
+  <depend>xarm_api</depend>
   <depend>geometry_msgs</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>tf2_ros</depend>

src/lite6_controller/src/lite6_calibration.cpp

@@ -1,128 +1,106 @@
-#include <cstdio>
+#include <signal.h>
-#include <rclcpp/rclcpp.hpp>
-#include <robot_controller/robot_controller.hpp>
 #include <chrono>
-#include <cstdlib>
+#include <rclcpp/rclcpp.hpp>
-#include <queue>
+#include <xarm_api/xarm_ros_client.h>
-
-#include <fstream>
 #include <iostream>
-
+#include <string>
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <geometry_msgs/msg/pose.hpp>
-#include <moveit_msgs/msg/collision_object.hpp>
-
-//#include <moveit/planning_interface/planning_interface.h>
-//#include <moveit/planning_interface/planning_response.h>
-//#include <moveit/kinematic_constraints/kinematic_constraint.h>
-#include <moveit/kinematic_constraints/utils.h>
-#include <moveit/move_group_interface/move_group_interface.h>
-
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#include <tf2_eigen/tf2_eigen.hpp>
-
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/common_planning_interface_objects/common_objects.h>
-#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 
 
-#include <moveit_msgs/msg/constraints.hpp>
+void exit_sig_handler(int signum)
-#include <moveit_msgs/msg/robot_trajectory.hpp>
-#include <moveit_msgs/msg/motion_plan_request.hpp>
-#include <moveit_msgs/msg/motion_sequence_request.hpp>
-#include <moveit_msgs/msg/motion_sequence_item.hpp>
-#include <moveit_msgs/srv/get_motion_sequence.hpp>
-
-
-#include <moveit/moveit_cpp/moveit_cpp.h>
-#include <moveit/moveit_cpp/planning_component.h>
-
-#include <moveit/robot_trajectory/robot_trajectory.h>
-
-#include <pilz_industrial_motion_planner/command_list_manager.h>
-
-#include <xarm_msgs/srv/move_joint.hpp>
-#include <xarm_msgs/srv/get_float32_list.hpp>
-
-const std::string MOVE_GROUP = "lite6";
-
-using namespace std::chrono_literals;
-
-class Calibration : public rclcpp::Node
 {
-  public:
+    fprintf(stderr, "[lite6_calibration] Ctrl-C caught, exit process...\n");
-    //moveit::planning_interface::MoveGroupInterface move_group;
+    exit(-1);
-
+}
-    rclcpp::TimerBase::SharedPtr status_timer;
+
-
+void wait()
-    rclcpp::Client<xarm_msgs::srv::MoveJoint>::SharedPtr set_joint_client;
+{
-    rclcpp::Client<xarm_msgs::srv::GetFloat32List>::SharedPtr get_joint_client;
+ do
-
+ {
-    Calibration(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
+   std::cout << '\n' << "Press a key to continue...";
-      : Node("lite6_calibration",options)
+ } while (std::cin.get() != '\n');
-        //move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
+}
-    {
+
-      //RCLCPP_INFO(this->get_logger(), "Starting state monitor");
+void println(std::string s) {
-      //move_group.startStateMonitor(30);
+  std::cout << s << std::endl;
-      //RCLCPP_INFO(this->get_logger(), "Started state monitor");
+}
-      //move_group.setPlanningTime(30.0);
+
-
+void print_joints(std::vector<float> jnts) {
-      //status_timer = this->create_wall_timer(5s, std::bind(&Calibration::getTree, this));
+    for (auto i : jnts)
-
+      std::cout << i << std::endl;
-      RCLCPP_ERROR(this->get_logger(), "create service client");
+}
-      set_joint_client = this->create_client<xarm_msgs::srv::MoveJoint>("/ufactory/set_servo_angle");
+
-      get_joint_client = this->create_client<xarm_msgs::srv::GetFloat32List>("/ufactory/get_servo_angle");
+int main(int argc, char **argv)
-      //xarm_msgs::srv::MoveJoint
+{
-      while (!set_joint_client->wait_for_service(1s)) {
+    rclcpp::init(argc, argv);
-        RCLCPP_ERROR(this->get_logger(), "service not available, waiting again...");
+    std::string hw_ns = "ufactory";
-      }
+    std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("lite6_calibration");
-      while (!get_joint_client->wait_for_service(1s)) {
+    int ret;
-        RCLCPP_ERROR(this->get_logger(), "service not available, waiting again...");
+
-      }
+    signal(SIGINT, exit_sig_handler);
-      RCLCPP_ERROR(this->get_logger(), "Client ready");
+
-
+    xarm_api::XArmROSClient client;
-      //RCLCPP_ERROR(this->get_logger(), "create request");
+    client.init(node, hw_ns);
-      //auto request = std::make_shared<xarm_msgs::srv::MoveJoint::Request>();
+    client.motion_enable(true);
-      //RCLCPP_ERROR(this->get_logger(), "send request");
+
-      //auto result = joint_client->async_send_request(request);
+    println("Setting xArm lite6 to free-drive mode. Attach pen and touch the drawing surface.");
-      //RCLCPP_ERROR(this->get_logger(), "get request");
+    client.set_mode(2);
-      //auto a = result.get()->message;
+    client.set_state(0);
-      //RCLCPP_ERROR(this->get_logger(), "A:%s", a.c_str());
+    //rclcpp::sleep_for(std::chrono::seconds(20));
-
+    wait();
-      auto request = std::make_shared<xarm_msgs::srv::GetFloat32List::Request>();
+    //client.set_mode(0);
-      RCLCPP_ERROR(this->get_logger(), "");
+    //client.set_state(0);
-      auto result = get_joint_client->async_send_request(request);
+    //rclcpp::sleep_for(std::chrono::seconds(2));
-      auto msg = result.get()->message;
+
-      RCLCPP_ERROR(this->get_logger(), "A:%s", msg.c_str());
+    client.set_mode(0);
-
+    client.set_state(0);
-    }
+    rclcpp::sleep_for(std::chrono::seconds(2));
-
+
-    void getTree()
+    std::vector<float> jnt_drawing_pose = {-0.813972, -0.0103697, 0.554617, 3.09979, -0.627334, -0.397301, 0};
-    {
+
-      //rclcpp::sleep_for(20s);
+    std::vector<float> jnt_pose = { 0, 0, 0, 0, 0, 0, 0 };
-      RCLCPP_INFO(this->get_logger(), "Getting robot state");
+    client.get_servo_angle(jnt_pose);
-      //moveit::core::RobotStatePtr move_group_state  = move_group.getCurrentState(30);
+    println("Moving to start drawing pose");
-      //RCLCPP_INFO(this->get_logger(), move_group_state->getStateTreeString().c_str());
+//XArmROSClient::set_servo_angle(const std::vector<fp32>& angles, fp32 speed, fp32 acc, fp32 mvtime, bool wait, fp32 timeout, fp32 radius)
-      //RCLCPP_ERROR(this->get_logger(), "AAAAAAAAAA");
+    client.set_servo_angle(jnt_drawing_pose, 1, 1, 1, true, 100, -1);
-    }
+    //client.set_servo_angle_j(jnt_drawing_pose, 1, 1, 100);
-
+    //rclcpp::sleep_for(std::chrono::seconds(5));
-};
+
-
+
-/**
+
- *
+    client.set_mode(0);
- */
+    client.set_state(0);
-int main(int argc, char ** argv)
+    return 0;
-{
+
-  rclcpp::init(argc, argv);
+    client.set_mode(4);
-
+    client.set_state(0);
-
+    std::vector<float> jnt_v = { 1, 0, 0, 0, 0, 0, 0 };
-  auto calibration = std::make_shared<Calibration>();
+    ret = client.vc_set_joint_velocity(jnt_v);
-
+    RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_joint: %d", ret);
-  rclcpp::executors::MultiThreadedExecutor executor;
+    rclcpp::sleep_for(std::chrono::seconds(2));
-  executor.add_node(calibration);
+    jnt_v[0] = -1;
-  executor.spin();
+    ret = client.vc_set_joint_velocity(jnt_v);
-
+    RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_joint: %d", ret);
-  rclcpp::shutdown();
+    rclcpp::sleep_for(std::chrono::seconds(2));
-  return EXIT_SUCCESS;
+    // stop
+    jnt_v[0] = 0;
+    ret = client.vc_set_joint_velocity(jnt_v);
+    RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_joint: %d", ret);
+
+    std::vector<float> line_v = { 1, 0, 0, 0, 0, 0};
+    client.set_mode(5);
+	client.set_state(0);
+    ret = client.vc_set_cartesian_velocity(line_v);
+    RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_line: %d", ret);
+    rclcpp::sleep_for(std::chrono::seconds(2));
+    line_v[0] = -1;
+    ret = client.vc_set_cartesian_velocity(line_v);
+    RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_line: %d", ret);
+    rclcpp::sleep_for(std::chrono::seconds(2));
+    // stop
+    line_v[0] = 0;
+    ret = client.vc_set_cartesian_velocity(line_v);
+    RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_line: %d", ret);
+
+    RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "test_xarm_velo_move over");
+    return 0;
 }