diff --git a/src/lite6_controller/CMakeLists.txt b/src/lite6_controller/CMakeLists.txt index 05bf62b..f944cbb 100644 --- a/src/lite6_controller/CMakeLists.txt +++ b/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" diff --git a/src/lite6_controller/launch/lite6_real_calibration.launch.py b/src/lite6_controller/launch/lite6_real_calibration.launch.py deleted file mode 100644 index a680e9f..0000000 --- a/src/lite6_controller/launch/lite6_real_calibration.launch.py +++ /dev/null @@ -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) - ]) diff --git a/src/lite6_controller/package.xml b/src/lite6_controller/package.xml index de24f32..69c3acb 100644 --- a/src/lite6_controller/package.xml +++ b/src/lite6_controller/package.xml @@ -16,6 +16,7 @@ moveit moveit_msgs xarm_msgs + xarm_api geometry_msgs moveit_ros_planning_interface tf2_ros diff --git a/src/lite6_controller/src/lite6_calibration.cpp b/src/lite6_controller/src/lite6_calibration.cpp index 40d77c7..3a9add2 100644 --- a/src/lite6_controller/src/lite6_calibration.cpp +++ b/src/lite6_controller/src/lite6_calibration.cpp @@ -1,128 +1,106 @@ -#include -#include -#include +#include #include -#include -#include - -#include +#include +#include #include - -#include -#include -#include - -//#include -//#include -//#include -#include -#include - -#include -#include - -#include -#include -#include -#include +#include -#include -#include -#include -#include -#include -#include - - -#include -#include - -#include - -#include - -#include -#include - -const std::string MOVE_GROUP = "lite6"; - -using namespace std::chrono_literals; - -class Calibration : public rclcpp::Node +void exit_sig_handler(int signum) { - public: - //moveit::planning_interface::MoveGroupInterface move_group; - - rclcpp::TimerBase::SharedPtr status_timer; - - rclcpp::Client::SharedPtr set_joint_client; - rclcpp::Client::SharedPtr get_joint_client; - - Calibration(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : Node("lite6_calibration",options) - //move_group(std::shared_ptr(std::move(this)), MOVE_GROUP) - { - //RCLCPP_INFO(this->get_logger(), "Starting state monitor"); - //move_group.startStateMonitor(30); - //RCLCPP_INFO(this->get_logger(), "Started state monitor"); - //move_group.setPlanningTime(30.0); - - //status_timer = this->create_wall_timer(5s, std::bind(&Calibration::getTree, this)); - - RCLCPP_ERROR(this->get_logger(), "create service client"); - set_joint_client = this->create_client("/ufactory/set_servo_angle"); - get_joint_client = this->create_client("/ufactory/get_servo_angle"); - //xarm_msgs::srv::MoveJoint - while (!set_joint_client->wait_for_service(1s)) { - RCLCPP_ERROR(this->get_logger(), "service not available, waiting again..."); - } - while (!get_joint_client->wait_for_service(1s)) { - RCLCPP_ERROR(this->get_logger(), "service not available, waiting again..."); - } - RCLCPP_ERROR(this->get_logger(), "Client ready"); - - //RCLCPP_ERROR(this->get_logger(), "create request"); - //auto request = std::make_shared(); - //RCLCPP_ERROR(this->get_logger(), "send request"); - //auto result = joint_client->async_send_request(request); - //RCLCPP_ERROR(this->get_logger(), "get request"); - //auto a = result.get()->message; - //RCLCPP_ERROR(this->get_logger(), "A:%s", a.c_str()); - - auto request = std::make_shared(); - RCLCPP_ERROR(this->get_logger(), ""); - auto result = get_joint_client->async_send_request(request); - auto msg = result.get()->message; - RCLCPP_ERROR(this->get_logger(), "A:%s", msg.c_str()); - - } - - void getTree() - { - //rclcpp::sleep_for(20s); - RCLCPP_INFO(this->get_logger(), "Getting robot state"); - //moveit::core::RobotStatePtr move_group_state = move_group.getCurrentState(30); - //RCLCPP_INFO(this->get_logger(), move_group_state->getStateTreeString().c_str()); - //RCLCPP_ERROR(this->get_logger(), "AAAAAAAAAA"); - } - -}; - -/** - * - */ -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - - auto calibration = std::make_shared(); - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(calibration); - executor.spin(); - - rclcpp::shutdown(); - return EXIT_SUCCESS; + fprintf(stderr, "[lite6_calibration] Ctrl-C caught, exit process...\n"); + exit(-1); +} + +void wait() +{ + do + { + std::cout << '\n' << "Press a key to continue..."; + } while (std::cin.get() != '\n'); +} + +void println(std::string s) { + std::cout << s << std::endl; +} + +void print_joints(std::vector jnts) { + for (auto i : jnts) + std::cout << i << std::endl; +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + std::string hw_ns = "ufactory"; + std::shared_ptr node = rclcpp::Node::make_shared("lite6_calibration"); + int ret; + + signal(SIGINT, exit_sig_handler); + + xarm_api::XArmROSClient client; + client.init(node, hw_ns); + client.motion_enable(true); + + println("Setting xArm lite6 to free-drive mode. Attach pen and touch the drawing surface."); + client.set_mode(2); + client.set_state(0); + //rclcpp::sleep_for(std::chrono::seconds(20)); + wait(); + //client.set_mode(0); + //client.set_state(0); + //rclcpp::sleep_for(std::chrono::seconds(2)); + + client.set_mode(0); + client.set_state(0); + rclcpp::sleep_for(std::chrono::seconds(2)); + + std::vector jnt_drawing_pose = {-0.813972, -0.0103697, 0.554617, 3.09979, -0.627334, -0.397301, 0}; + + std::vector jnt_pose = { 0, 0, 0, 0, 0, 0, 0 }; + client.get_servo_angle(jnt_pose); + println("Moving to start drawing pose"); +//XArmROSClient::set_servo_angle(const std::vector& angles, fp32 speed, fp32 acc, fp32 mvtime, bool wait, fp32 timeout, fp32 radius) + 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); + return 0; + + client.set_mode(4); + client.set_state(0); + std::vector jnt_v = { 1, 0, 0, 0, 0, 0, 0 }; + ret = client.vc_set_joint_velocity(jnt_v); + RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_joint: %d", ret); + rclcpp::sleep_for(std::chrono::seconds(2)); + jnt_v[0] = -1; + ret = client.vc_set_joint_velocity(jnt_v); + RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_joint: %d", ret); + rclcpp::sleep_for(std::chrono::seconds(2)); + // 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 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; }