diff --git a/ReadMe.md b/ReadMe.md index a9d0f38a..881469a9 100755 --- a/ReadMe.md +++ b/ReadMe.md @@ -21,7 +21,8 @@ For simplified Chinese version: [简体中文版](./ReadMe_cn.md) - (2022-09-07) Change the parameter type of service (__set_tgpio_modbus_timeout__/__getset_tgpio_modbus_data__), and add parameters to support transparent transmission - (2022-09-07) Change topic name (xarm_states to robot_states) - (2022-09-07) Update submodule xarm-sdk to version 1.11.0 -- (2022-09-09) [Beta]支持Ros Humble版本 +- (2022-09-09) [Beta]Support Humble version +- (2022-10-10) xarm_api adds some services ## 3. Preparation diff --git a/ReadMe_cn.md b/ReadMe_cn.md index 0352f00c..bfaa1009 100755 --- a/ReadMe_cn.md +++ b/ReadMe_cn.md @@ -21,6 +21,7 @@ - (2022-09-07) 变更Topic名字(xarm_states改为robot_states) - (2022-09-07) 更新子模块xarm-sdk到1.11.0版本 - (2022-09-09) [Beta]支持Ros Humble版本 +- (2022-10-10) xarm_api新增一些服务 ## 3. 准备工作 diff --git a/xarm_api/config/xarm_params.yaml b/xarm_api/config/xarm_params.yaml index 20abfef0..6ed1ffdf 100644 --- a/xarm_api/config/xarm_params.yaml +++ b/xarm_api/config/xarm_params.yaml @@ -23,13 +23,31 @@ ufactory_driver: clean_bio_gripper_error: false start_record_trajectory: false stop_record_trajectory: false + ft_sensor_set_zero: false + set_linear_track_stop: false + clean_linear_track_error: false + open_lite6_gripper: false + close_lite6_gripper: false + stop_lite6_gripper: false get_state: false get_cmdnum: false get_vacuum_gripper: false get_gripper_err_code: false get_bio_gripper_status: false get_bio_gripper_error: false + get_reduced_mode: false + get_report_tau_or_i: false + ft_sensor_app_get: false + get_ft_sensor_error: false + get_trajectory_rw_status: false + get_linear_track_pos: false + get_linear_track_status: false + get_linear_track_error: false + get_linear_track_is_enabled: false + get_linear_track_on_zero: false + get_linear_track_sci: false get_err_warn_code: false + get_linear_track_sco: false set_mode: true set_state: true set_collision_sensitivity: false @@ -44,6 +62,16 @@ ufactory_driver: set_self_collision_detection: false set_simulation_robot: false set_baud_checkset_enable: false + set_report_tau_or_i: false + ft_sensor_enable: false + ft_sensor_app_set: false + set_linear_track_enable: false + set_linear_track_speed: false + set_cartesian_velo_continuous: false + set_allow_approx_motion: false + set_only_check_type: false + config_tgpio_reset_when_stop: false + config_cgpio_reset_when_stop: false motion_enable: true set_servo_attach: false set_servo_detach: false @@ -56,6 +84,7 @@ ufactory_driver: get_position: false get_servo_angle: true get_position_aa: false + get_ft_sensor_data: false set_pause_time: false set_tcp_jerk: false set_tcp_maxacc: false @@ -104,4 +133,13 @@ ufactory_driver: getset_tgpio_modbus_data: false save_record_trajectory: false load_trajectory: false - playback_trajectory: false \ No newline at end of file + playback_trajectory: false + iden_tcp_load: false + ft_sensor_iden_load: false + ft_sensor_cali_load: false + config_force_control: false + set_impedance: false + set_impedance_mbk: false + set_impedance_config: false + set_linear_track_back_origin: false + set_linear_track_pos: false \ No newline at end of file diff --git a/xarm_api/include/xarm_api/xarm_driver.h b/xarm_api/include/xarm_api/xarm_driver.h index 07204219..14dd545c 100644 --- a/xarm_api/include/xarm_api/xarm_driver.h +++ b/xarm_api/include/xarm_api/xarm_driver.h @@ -110,6 +110,12 @@ namespace xarm_api rclcpp::Service::SharedPtr service_clean_bio_gripper_error_; rclcpp::Service::SharedPtr service_start_record_trajectory_; rclcpp::Service::SharedPtr service_stop_record_trajectory_; + rclcpp::Service::SharedPtr service_ft_sensor_set_zero_; + rclcpp::Service::SharedPtr service_set_linear_track_stop_; + rclcpp::Service::SharedPtr service_clean_linear_track_error_; + rclcpp::Service::SharedPtr service_open_lite6_gripper_; + rclcpp::Service::SharedPtr service_close_lite6_gripper_; + rclcpp::Service::SharedPtr service_stop_lite6_gripper_; bool _clean_error(const std::shared_ptr req, std::shared_ptr res); bool _clean_warn(const std::shared_ptr req, std::shared_ptr res); bool _clean_conf(const std::shared_ptr req, std::shared_ptr res); @@ -121,6 +127,12 @@ namespace xarm_api bool _clean_bio_gripper_error(const std::shared_ptr req, std::shared_ptr res); bool _start_record_trajectory(const std::shared_ptr req, std::shared_ptr res); bool _stop_record_trajectory(const std::shared_ptr req, std::shared_ptr res); + bool _ft_sensor_set_zero(const std::shared_ptr req, std::shared_ptr res); + bool _set_linear_track_stop(const std::shared_ptr req, std::shared_ptr res); + bool _clean_linear_track_error(const std::shared_ptr req, std::shared_ptr res); + bool _open_lite6_gripper(const std::shared_ptr req, std::shared_ptr res); + bool _close_lite6_gripper(const std::shared_ptr req, std::shared_ptr res); + bool _stop_lite6_gripper(const std::shared_ptr req, std::shared_ptr res); // GetInt16 rclcpp::Service::SharedPtr service_get_state_; @@ -129,16 +141,40 @@ namespace xarm_api rclcpp::Service::SharedPtr service_get_gripper_err_code_; rclcpp::Service::SharedPtr service_get_bio_gripper_status_; rclcpp::Service::SharedPtr service_get_bio_gripper_error_; + rclcpp::Service::SharedPtr service_get_reduced_mode_; + rclcpp::Service::SharedPtr service_get_report_tau_or_i_; + rclcpp::Service::SharedPtr service_ft_sensor_app_get_; + rclcpp::Service::SharedPtr service_get_ft_sensor_error_; + rclcpp::Service::SharedPtr service_get_trajectory_rw_status_; + rclcpp::Service::SharedPtr service_get_linear_track_pos_; + rclcpp::Service::SharedPtr service_get_linear_track_status_; + rclcpp::Service::SharedPtr service_get_linear_track_error_; + rclcpp::Service::SharedPtr service_get_linear_track_is_enabled_; + rclcpp::Service::SharedPtr service_get_linear_track_on_zero_; + rclcpp::Service::SharedPtr service_get_linear_track_sci_; bool _get_state(const std::shared_ptr req, std::shared_ptr res); bool _get_cmdnum(const std::shared_ptr req, std::shared_ptr res); bool _get_vacuum_gripper(const std::shared_ptr req, std::shared_ptr res); bool _get_gripper_err_code(const std::shared_ptr req, std::shared_ptr res); bool _get_bio_gripper_status(const std::shared_ptr req, std::shared_ptr res); bool _get_bio_gripper_error(const std::shared_ptr req, std::shared_ptr res); + bool _get_reduced_mode(const std::shared_ptr req, std::shared_ptr res); + bool _get_report_tau_or_i(const std::shared_ptr req, std::shared_ptr res); + bool _ft_sensor_app_get(const std::shared_ptr req, std::shared_ptr res); + bool _get_ft_sensor_error(const std::shared_ptr req, std::shared_ptr res); + bool _get_trajectory_rw_status(const std::shared_ptr req, std::shared_ptr res); + bool _get_linear_track_pos(const std::shared_ptr req, std::shared_ptr res); + bool _get_linear_track_status(const std::shared_ptr req, std::shared_ptr res); + bool _get_linear_track_error(const std::shared_ptr req, std::shared_ptr res); + bool _get_linear_track_is_enabled(const std::shared_ptr req, std::shared_ptr res); + bool _get_linear_track_on_zero(const std::shared_ptr req, std::shared_ptr res); + bool _get_linear_track_sci(const std::shared_ptr req, std::shared_ptr res); // GetInt16List rclcpp::Service::SharedPtr service_get_err_warn_code_; + rclcpp::Service::SharedPtr service_get_linear_track_sco_; bool _get_err_warn_code(const std::shared_ptr req, std::shared_ptr res); + bool _get_linear_track_sco(const std::shared_ptr req, std::shared_ptr res); // SetInt16 rclcpp::Service::SharedPtr service_set_mode_; @@ -154,6 +190,16 @@ namespace xarm_api rclcpp::Service::SharedPtr service_set_self_collision_detection_; rclcpp::Service::SharedPtr service_set_simulation_robot_; rclcpp::Service::SharedPtr service_set_baud_checkset_enable_; + rclcpp::Service::SharedPtr service_set_report_tau_or_i_; + rclcpp::Service::SharedPtr service_ft_sensor_enable_; + rclcpp::Service::SharedPtr service_ft_sensor_app_set_; + rclcpp::Service::SharedPtr service_set_linear_track_enable_; + rclcpp::Service::SharedPtr service_set_linear_track_speed_; + rclcpp::Service::SharedPtr service_set_cartesian_velo_continuous_; + rclcpp::Service::SharedPtr service_set_allow_approx_motion_; + rclcpp::Service::SharedPtr service_set_only_check_type_; + rclcpp::Service::SharedPtr service_config_tgpio_reset_when_stop_; + rclcpp::Service::SharedPtr service_config_cgpio_reset_when_stop_; bool _set_mode(const std::shared_ptr req, std::shared_ptr res); bool _set_state(const std::shared_ptr req, std::shared_ptr res); bool _set_collision_sensitivity(const std::shared_ptr req, std::shared_ptr res); @@ -167,6 +213,16 @@ namespace xarm_api bool _set_self_collision_detection(const std::shared_ptr req, std::shared_ptr res); bool _set_simulation_robot(const std::shared_ptr req, std::shared_ptr res); bool _set_baud_checkset_enable(const std::shared_ptr req, std::shared_ptr res); + bool _set_report_tau_or_i(const std::shared_ptr req, std::shared_ptr res); + bool _ft_sensor_enable(const std::shared_ptr req, std::shared_ptr res); + bool _ft_sensor_app_set(const std::shared_ptr req, std::shared_ptr res); + bool _set_linear_track_enable(const std::shared_ptr req, std::shared_ptr res); + bool _set_linear_track_speed(const std::shared_ptr req, std::shared_ptr res); + bool _set_cartesian_velo_continuous(const std::shared_ptr req, std::shared_ptr res); + bool _set_allow_approx_motion(const std::shared_ptr req, std::shared_ptr res); + bool _set_only_check_type(const std::shared_ptr req, std::shared_ptr res); + bool _config_tgpio_reset_when_stop(const std::shared_ptr req, std::shared_ptr res); + bool _config_cgpio_reset_when_stop(const std::shared_ptr req, std::shared_ptr res); // SetInt16ById rclcpp::Service::SharedPtr service_motion_enable_; @@ -204,9 +260,11 @@ namespace xarm_api rclcpp::Service::SharedPtr service_get_position_; rclcpp::Service::SharedPtr service_get_servo_angle_; rclcpp::Service::SharedPtr service_get_position_aa_; + rclcpp::Service::SharedPtr service_get_ft_sensor_data_; bool _get_position(const std::shared_ptr req, std::shared_ptr res); bool _get_servo_angle(const std::shared_ptr req, std::shared_ptr res); bool _get_position_aa(const std::shared_ptr req, std::shared_ptr res); + bool _get_ft_sensor_data(const std::shared_ptr req, std::shared_ptr res); // SetFloat32 rclcpp::Service::SharedPtr service_set_pause_time_; @@ -355,6 +413,40 @@ namespace xarm_api // TrajPlay rclcpp::Service::SharedPtr service_playback_trajectory_; bool _playback_trajectory(const std::shared_ptr req, std::shared_ptr res); + + // IdenLoad + rclcpp::Service::SharedPtr service_iden_tcp_load_; + rclcpp::Service::SharedPtr service_ft_sensor_iden_load_; + bool _iden_tcp_load(const std::shared_ptr req, std::shared_ptr res); + bool _ft_sensor_iden_load(const std::shared_ptr req, std::shared_ptr res); + + // FtCaliLoad + rclcpp::Service::SharedPtr service_ft_sensor_cali_load_; + bool _ft_sensor_cali_load(const std::shared_ptr req, std::shared_ptr res); + + // FtForceConfig + rclcpp::Service::SharedPtr service_config_force_control_; + bool _config_force_control(const std::shared_ptr req, std::shared_ptr res); + + // FtForcePid + rclcpp::Service::SharedPtr service_set_force_control_pid_; + bool _set_force_control_pid(const std::shared_ptr req, std::shared_ptr res); + + // FtImpedance + rclcpp::Service::SharedPtr service_set_impedance_; + rclcpp::Service::SharedPtr service_set_impedance_mbk_; + rclcpp::Service::SharedPtr service_set_impedance_config_; + bool _set_impedance(const std::shared_ptr req, std::shared_ptr res); + bool _set_impedance_mbk(const std::shared_ptr req, std::shared_ptr res); + bool _set_impedance_config(const std::shared_ptr req, std::shared_ptr res); + + // LinearTrackBackOrigin + rclcpp::Service::SharedPtr service_set_linear_track_back_origin_; + bool _set_linear_track_back_origin(const std::shared_ptr req, std::shared_ptr res); + + // LinearTrackSetPos + rclcpp::Service::SharedPtr service_set_linear_track_pos_; + bool _set_linear_track_pos(const std::shared_ptr req, std::shared_ptr res); }; } diff --git a/xarm_api/include/xarm_api/xarm_msgs.h b/xarm_api/include/xarm_api/xarm_msgs.h index 5a7034db..b36fe830 100644 --- a/xarm_api/include/xarm_api/xarm_msgs.h +++ b/xarm_api/include/xarm_api/xarm_msgs.h @@ -41,5 +41,12 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include #endif // __XARM_MSGS_H \ No newline at end of file diff --git a/xarm_api/src/xarm_driver_service.cpp b/xarm_api/src/xarm_driver_service.cpp index dc23872d..1c1e5ced 100644 --- a/xarm_api/src/xarm_driver_service.cpp +++ b/xarm_api/src/xarm_driver_service.cpp @@ -40,6 +40,12 @@ namespace xarm_api service_clean_bio_gripper_error_ = _create_service("clean_bio_gripper_error", &XArmDriver::_clean_bio_gripper_error); service_start_record_trajectory_ = _create_service("start_record_trajectory", &XArmDriver::_start_record_trajectory); service_stop_record_trajectory_ = _create_service("stop_record_trajectory", &XArmDriver::_stop_record_trajectory); + service_ft_sensor_set_zero_ = _create_service("ft_sensor_set_zero", &XArmDriver::_ft_sensor_set_zero); + service_set_linear_track_stop_ = _create_service("set_linear_track_stop", &XArmDriver::_set_linear_track_stop); + service_clean_linear_track_error_ = _create_service("clean_linear_track_error", &XArmDriver::_clean_linear_track_error); + service_open_lite6_gripper_ = _create_service("open_lite6_gripper", &XArmDriver::_open_lite6_gripper); + service_close_lite6_gripper_ = _create_service("close_lite6_gripper", &XArmDriver::_close_lite6_gripper); + service_stop_lite6_gripper_ = _create_service("stop_lite6_gripper", &XArmDriver::_stop_lite6_gripper); // GetInt16 service_get_state_ = _create_service("get_state", &XArmDriver::_get_state); @@ -48,9 +54,21 @@ namespace xarm_api service_get_gripper_err_code_ = _create_service("get_gripper_err_code", &XArmDriver::_get_gripper_err_code); service_get_bio_gripper_status_ = _create_service("get_bio_gripper_status", &XArmDriver::_get_bio_gripper_status); service_get_bio_gripper_error_ = _create_service("get_bio_gripper_error", &XArmDriver::_get_bio_gripper_error); - + service_get_reduced_mode_ = _create_service("get_reduced_mode", &XArmDriver::_get_reduced_mode); + service_get_report_tau_or_i_ = _create_service("get_report_tau_or_i", &XArmDriver::_get_report_tau_or_i); + service_ft_sensor_app_get_ = _create_service("ft_sensor_app_get", &XArmDriver::_ft_sensor_app_get); + service_get_ft_sensor_error_ = _create_service("get_ft_sensor_error", &XArmDriver::_get_ft_sensor_error); + service_get_trajectory_rw_status_ = _create_service("get_trajectory_rw_status", &XArmDriver::_get_trajectory_rw_status); + service_get_linear_track_pos_ = _create_service("get_linear_track_pos", &XArmDriver::_get_linear_track_pos); + service_get_linear_track_status_ = _create_service("get_linear_track_status", &XArmDriver::_get_linear_track_status); + service_get_linear_track_error_ = _create_service("get_linear_track_error", &XArmDriver::_get_linear_track_error); + service_get_linear_track_is_enabled_ = _create_service("get_linear_track_is_enabled", &XArmDriver::_get_linear_track_is_enabled); + service_get_linear_track_on_zero_ = _create_service("get_linear_track_on_zero", &XArmDriver::_get_linear_track_on_zero); + service_get_linear_track_sci_ = _create_service("get_linear_track_sci", &XArmDriver::_get_linear_track_sci); + // GetInt16List service_get_err_warn_code_ = _create_service("get_err_warn_code", &XArmDriver::_get_err_warn_code); + service_get_linear_track_sco_ = _create_service("get_linear_track_sco", &XArmDriver::_get_linear_track_sco); // SetInt16 service_set_mode_ = _create_service("set_mode", &XArmDriver::_set_mode); @@ -66,7 +84,17 @@ namespace xarm_api service_set_self_collision_detection_ = _create_service("set_self_collision_detection", &XArmDriver::_set_self_collision_detection); service_set_simulation_robot_ = _create_service("set_simulation_robot", &XArmDriver::_set_simulation_robot); service_set_baud_checkset_enable_ = _create_service("set_baud_checkset_enable", &XArmDriver::_set_baud_checkset_enable); - + service_set_report_tau_or_i_ = _create_service("set_report_tau_or_i", &XArmDriver::_set_report_tau_or_i); + service_ft_sensor_enable_ = _create_service("ft_sensor_enable", &XArmDriver::_ft_sensor_enable); + service_ft_sensor_app_set_ = _create_service("ft_sensor_app_set", &XArmDriver::_ft_sensor_app_set); + service_set_linear_track_enable_ = _create_service("set_linear_track_enable", &XArmDriver::_set_linear_track_enable); + service_set_linear_track_speed_ = _create_service("set_linear_track_speed", &XArmDriver::_set_linear_track_speed); + service_set_cartesian_velo_continuous_ = _create_service("set_cartesian_velo_continuous", &XArmDriver::_set_cartesian_velo_continuous); + service_set_allow_approx_motion_ = _create_service("set_allow_approx_motion", &XArmDriver::_set_allow_approx_motion); + service_set_only_check_type_ = _create_service("set_only_check_type", &XArmDriver::_set_only_check_type); + service_config_tgpio_reset_when_stop_ = _create_service("config_tgpio_reset_when_stop", &XArmDriver::_config_tgpio_reset_when_stop); + service_config_cgpio_reset_when_stop_ = _create_service("config_cgpio_reset_when_stop", &XArmDriver::_config_cgpio_reset_when_stop); + // SetInt16ById service_motion_enable_= _create_service("motion_enable", &XArmDriver::_motion_enable); service_set_servo_attach_ = _create_service("set_servo_attach", &XArmDriver::_set_servo_attach); @@ -94,6 +122,7 @@ namespace xarm_api service_get_position_ = _create_service("get_position", &XArmDriver::_get_position); service_get_servo_angle_ = _create_service("get_servo_angle", &XArmDriver::_get_servo_angle); service_get_position_aa_ = _create_service("get_position_aa", &XArmDriver::_get_position_aa); + service_get_ft_sensor_data_ = _create_service("get_ft_sensor_data", &XArmDriver::_get_ft_sensor_data); // SetFloat32 service_set_pause_time_ = _create_service("set_pause_time", &XArmDriver::_set_pause_time); @@ -192,6 +221,30 @@ namespace xarm_api // TrajPlay service_playback_trajectory_ = _create_service("playback_trajectory", &XArmDriver::_playback_trajectory); + + // IdenLoad + service_iden_tcp_load_ = _create_service("iden_tcp_load", &XArmDriver::_iden_tcp_load); + service_ft_sensor_iden_load_ = _create_service("ft_sensor_iden_load", &XArmDriver::_ft_sensor_iden_load); + + // FtCaliLoad + service_ft_sensor_cali_load_ = _create_service("ft_sensor_cali_load", &XArmDriver::_ft_sensor_cali_load); + + // FtForceConfig + service_config_force_control_ = _create_service("config_force_control", &XArmDriver::_config_force_control); + + // FtForcePid + service_set_force_control_pid_ = _create_service("set_force_control_pid", &XArmDriver::_set_force_control_pid); + + // FtImpedance + service_set_impedance_ = _create_service("set_impedance", &XArmDriver::_set_impedance); + service_set_impedance_mbk_ = _create_service("set_impedance_mbk", &XArmDriver::_set_impedance_mbk); + service_set_impedance_config_ = _create_service("set_impedance_config", &XArmDriver::_set_impedance_config); + + // LinearTrackBackOrigin + service_set_linear_track_back_origin_ = _create_service("set_linear_track_back_origin", &XArmDriver::_set_linear_track_back_origin); + + // LinearTrackSetPos + service_set_linear_track_pos_ = _create_service("set_linear_track_pos", &XArmDriver::_set_linear_track_pos); } bool XArmDriver::_clean_error(const std::shared_ptr req, std::shared_ptr res) @@ -259,6 +312,42 @@ namespace xarm_api return true; } + bool XArmDriver::_ft_sensor_set_zero(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->ft_sensor_set_zero(); + return true; + } + + bool XArmDriver::_set_linear_track_stop(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->set_linear_track_stop(); + return true; + } + + bool XArmDriver::_clean_linear_track_error(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->clean_linear_track_error(); + return true; + } + + bool XArmDriver::_open_lite6_gripper(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->open_lite6_gripper(); + return true; + } + + bool XArmDriver::_close_lite6_gripper(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->close_lite6_gripper(); + return true; + } + + bool XArmDriver::_stop_lite6_gripper(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->stop_lite6_gripper(); + return true; + } + bool XArmDriver::_get_state(const std::shared_ptr req, std::shared_ptr res) { res->ret = arm->get_state((int *)&res->data); @@ -301,6 +390,83 @@ namespace xarm_api return true; } + bool XArmDriver::_get_reduced_mode(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->get_reduced_mode((int *)&res->data); + res->message = "data=" + std::to_string(res->data); + return true; + } + + bool XArmDriver::_get_report_tau_or_i(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->get_report_tau_or_i((int *)&res->data); + res->message = "data=" + std::to_string(res->data); + return true; + } + + bool XArmDriver::_ft_sensor_app_get(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->ft_sensor_app_get((int *)&res->data); + res->message = "data=" + std::to_string(res->data); + return true; + } + + bool XArmDriver::_get_ft_sensor_error(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->get_ft_sensor_error((int *)&res->data); + res->message = "data=" + std::to_string(res->data); + return true; + } + + bool XArmDriver::_get_trajectory_rw_status(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->get_trajectory_rw_status((int *)&res->data); + res->message = "data=" + std::to_string(res->data); + return true; + } + + bool XArmDriver::_get_linear_track_pos(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->get_linear_track_pos((int *)&res->data); + res->message = "data=" + std::to_string(res->data); + return true; + } + + bool XArmDriver::_get_linear_track_status(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->get_linear_track_status((int *)&res->data); + res->message = "data=" + std::to_string(res->data); + return true; + } + + bool XArmDriver::_get_linear_track_error(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->get_linear_track_error((int *)&res->data); + res->message = "data=" + std::to_string(res->data); + return true; + } + + bool XArmDriver::_get_linear_track_is_enabled(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->get_linear_track_is_enabled((int *)&res->data); + res->message = "data=" + std::to_string(res->data); + return true; + } + + bool XArmDriver::_get_linear_track_on_zero(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->get_linear_track_on_zero((int *)&res->data); + res->message = "data=" + std::to_string(res->data); + return true; + } + + bool XArmDriver::_get_linear_track_sci(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->get_linear_track_sci((int *)&res->data); + res->message = "data=" + std::to_string(res->data); + return true; + } + bool XArmDriver::_get_err_warn_code(const std::shared_ptr req, std::shared_ptr res) { int err_warn[2]; @@ -312,6 +478,17 @@ namespace xarm_api return true; } + bool XArmDriver::_get_linear_track_sco(const std::shared_ptr req, std::shared_ptr res) + { + int sco[2]; + res->ret = arm->get_linear_track_sco(sco); + res->datas.resize(2); + res->datas[0] = sco[0]; + res->datas[1]= sco[1]; + res->message = "datas=[ " + std::to_string(res->datas[0]) + ", " + std::to_string(res->datas[1]) + " ]"; + return true; + } + bool XArmDriver::_set_mode(const std::shared_ptr req, std::shared_ptr res) { arm->set_state(XARM_STATE::STOP); @@ -411,6 +588,76 @@ namespace xarm_api return true; } + bool XArmDriver::_set_report_tau_or_i(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->set_report_tau_or_i(req->data); + res->message = "data=" + std::to_string(req->data); + return true; + } + + bool XArmDriver::_ft_sensor_enable(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->ft_sensor_enable(req->data); + res->message = "data=" + std::to_string(req->data); + return true; + } + + bool XArmDriver::_ft_sensor_app_set(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->ft_sensor_app_set(req->data); + res->message = "data=" + std::to_string(req->data); + return true; + } + + bool XArmDriver::_set_linear_track_enable(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->set_linear_track_enable(req->data); + res->message = "data=" + std::to_string(req->data); + return true; + } + + bool XArmDriver::_set_linear_track_speed(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->set_linear_track_speed(req->data); + res->message = "data=" + std::to_string(req->data); + return true; + } + + bool XArmDriver::_set_cartesian_velo_continuous(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->set_cartesian_velo_continuous(req->data); + res->message = "data=" + std::to_string(req->data); + return true; + } + + bool XArmDriver::_set_allow_approx_motion(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->set_allow_approx_motion(req->data); + res->message = "data=" + std::to_string(req->data); + return true; + } + + bool XArmDriver::_set_only_check_type(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->set_only_check_type((unsigned char)req->data); + res->message = "data=" + std::to_string(req->data); + return true; + } + + bool XArmDriver::_config_tgpio_reset_when_stop(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->config_tgpio_reset_when_stop(req->data); + res->message = "data=" + std::to_string(req->data); + return true; + } + + bool XArmDriver::_config_cgpio_reset_when_stop(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->config_cgpio_reset_when_stop(req->data); + res->message = "data=" + std::to_string(req->data); + return true; + } + bool XArmDriver::_motion_enable(const std::shared_ptr req, std::shared_ptr res) { res->ret = arm->motion_enable(req->data, req->id); @@ -522,6 +769,18 @@ namespace xarm_api return true; } + bool XArmDriver::_get_ft_sensor_data(const std::shared_ptr req, std::shared_ptr res) + { + res->datas.resize(6); + res->ret = arm->get_ft_sensor_data(&res->datas[0]); + std::string tmp = ""; + for (int i = 0; i < res->datas.size(); i++) { + tmp += (i == 0 ? "" : ", ") + std::to_string(res->datas[i]); + } + res->message = "datas=[ " + tmp + " ]"; + return true; + } + bool XArmDriver::_set_pause_time(const std::shared_ptr req, std::shared_ptr res) { res->ret = arm->set_pause_time(req->data); @@ -973,4 +1232,113 @@ namespace xarm_api res->ret = arm->playback_trajectory(req->times, filename, req->wait, req->double_speed); return true; } + + bool XArmDriver::_iden_tcp_load(const std::shared_ptr req, std::shared_ptr res) + { + res->datas.resize(4); + res->ret = arm->iden_tcp_load(&res->datas[0], req->estimated_mass); + std::string tmp = ""; + for (int i = 0; i < res->datas.size(); i++) { + tmp += (i == 0 ? "" : ", ") + std::to_string(res->datas[i]); + } + res->message = "datas=[ " + tmp + " ]"; + return true; + } + + bool XArmDriver::_ft_sensor_iden_load(const std::shared_ptr req, std::shared_ptr res) + { + res->datas.resize(10); + res->ret = arm->ft_sensor_iden_load(&res->datas[0]); + std::string tmp = ""; + for (int i = 0; i < res->datas.size(); i++) { + tmp += (i == 0 ? "" : ", ") + std::to_string(res->datas[i]); + } + res->message = "datas=[ " + tmp + " ]"; + return true; + } + + bool XArmDriver::_ft_sensor_cali_load(const std::shared_ptr req, std::shared_ptr res) + { + if (req->datas.size() < 10) { + res->ret = PARAM_ERROR; + return true; + } + + res->ret = arm->ft_sensor_cali_load(&req->datas[0], req->association_setting_tcp_load, req->m, req->x, req->y, req->z); + return true; + } + + bool XArmDriver::_config_force_control(const std::shared_ptr req, std::shared_ptr res) + { + if (req->c_axis.size() < 6 || req->ref.size() < 6 || req->limits.size() < 6) { + res->ret = PARAM_ERROR; + return true; + } + int axis[6] = { 0 }; + for (int i = 0; i < 6; i++) { + axis[i] = req->c_axis[i]; + } + res->ret = arm->config_force_control(req->coord, axis, &req->ref[0], &req->limits[0]); + return true; + } + + bool XArmDriver::_set_force_control_pid(const std::shared_ptr req, std::shared_ptr res) + { + if (req->kp.size() < 6 || req->ki.size() < 6 || req->kd.size() < 6 || req->xe_limit.size() < 6) { + res->ret = PARAM_ERROR; + return true; + } + res->ret = arm->set_force_control_pid(&req->kp[0], &req->ki[0], &req->kd[0], &req->xe_limit[0]); + return true; + } + + bool XArmDriver::_set_impedance(const std::shared_ptr req, std::shared_ptr res) + { + if (req->c_axis.size() < 6 || req->m.size() < 6 || req->k.size() < 6 || req->b.size() < 6) { + res->ret = PARAM_ERROR; + return true; + } + int axis[6] = { 0 }; + for (int i = 0; i < 6; i++) { + axis[i] = req->c_axis[i]; + } + res->ret = arm->set_impedance(req->coord, axis, &req->m[0], &req->k[0], &req->b[0]); + return true; + } + + bool XArmDriver::_set_impedance_mbk(const std::shared_ptr req, std::shared_ptr res) + { + if (req->m.size() < 6 || req->k.size() < 6 || req->b.size() < 6) { + res->ret = PARAM_ERROR; + return true; + } + res->ret = arm->set_impedance_mbk(&req->m[0], &req->k[0], &req->b[0]); + return true; + } + + bool XArmDriver::_set_impedance_config(const std::shared_ptr req, std::shared_ptr res) + { + if (req->c_axis.size() < 6) { + res->ret = PARAM_ERROR; + return true; + } + int axis[6] = { 0 }; + for (int i = 0; i < 6; i++) { + axis[i] = req->c_axis[i]; + } + res->ret = arm->set_impedance_config(req->coord, axis); + return true; + } + + bool XArmDriver::_set_linear_track_back_origin(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->set_linear_track_back_origin(req->wait, req->auto_enable); + return true; + } + + bool XArmDriver::_set_linear_track_pos(const std::shared_ptr req, std::shared_ptr res) + { + res->ret = arm->set_linear_track_pos(req->pos, req->speed, req->wait, req->timeout, req->auto_enable); + return true; + } } \ No newline at end of file diff --git a/xarm_msgs/CMakeLists.txt b/xarm_msgs/CMakeLists.txt index babc70e4..b7b1ab85 100644 --- a/xarm_msgs/CMakeLists.txt +++ b/xarm_msgs/CMakeLists.txt @@ -81,6 +81,14 @@ set(srv_files ${srv_dir}/SetTcpLoad.srv ${srv_dir}/SetModbusTimeout.srv + ${srv_dir}/IdenLoad.srv + ${srv_dir}/FtCaliLoad.srv + ${srv_dir}/FtForceConfig.srv + ${srv_dir}/FtForcePid.srv + ${srv_dir}/FtImpedance.srv + ${srv_dir}/LinearTrackBackOrigin.srv + ${srv_dir}/LinearTrackSetPos.srv + ${srv_dir}/PlanExec.srv ${srv_dir}/PlanJoint.srv ${srv_dir}/PlanPose.srv diff --git a/xarm_msgs/ReadMe.md b/xarm_msgs/ReadMe.md index e62ec496..ad757e0f 100644 --- a/xarm_msgs/ReadMe.md +++ b/xarm_msgs/ReadMe.md @@ -23,6 +23,12 @@ - xarm_api->service: __clean_bio_gripper_error__ - xarm_api->service: __start_record_trajectory__ - xarm_api->service: __stop_record_trajectory__ + - xarm_api->service: __ft_sensor_set_zero__ + - xarm_api->service: __set_linear_track_stop__ + - xarm_api->service: __clean_linear_track_error__ + - xarm_api->service: __open_lite6_gripper__ + - xarm_api->service: __close_lite6_gripper__ + - xarm_api->service: __stop_lite6_gripper__ - [xarm_msgs::srv::GetInt16](./srv/GetInt16.srv) - xarm_api->service: __get_state__ @@ -31,6 +37,21 @@ - xarm_api->service: __get_gripper_err_code__ - xarm_api->service: __get_bio_gripper_status__ - xarm_api->service: __get_bio_gripper_error__ + - xarm_api->service: __get_reduced_mode__ + - xarm_api->service: __get_report_tau_or_i__ + - xarm_api->service: __ft_sensor_app_get__ + - xarm_api->service: __get_ft_sensor_error__ + - xarm_api->service: __get_trajectory_rw_status__ + - xarm_api->service: __get_linear_track_pos__ + - xarm_api->service: __get_linear_track_status__ + - xarm_api->service: __get_linear_track_error__ + - xarm_api->service: __get_linear_track_is_enabled__ + - xarm_api->service: __get_linear_track_on_zero__ + - xarm_api->service: __get_linear_track_sci__ + +- [xarm_msgs::srv::GetInt16List](./srv/GetInt16List.srv) + - xarm_api->service: __get_err_warn_code__ + - xarm_api->service: __get_linear_track_sco__ - [xarm_msgs::srv::SetInt16](./srv/SetInt16.srv) - xarm_api->service: __set_mode__ @@ -44,6 +65,17 @@ - xarm_api->service: __set_reduced_mode__ - xarm_api->service: __set_self_collision_detection__ - xarm_api->service: __set_simulation_robot__ + - xarm_api->service: __set_baud_checkset_enable__ + - xarm_api->service: __set_report_tau_or_i__ + - xarm_api->service: __ft_sensor_enable__ + - xarm_api->service: __ft_sensor_app_set__ + - xarm_api->service: __set_linear_track_enable__ + - xarm_api->service: __set_linear_track_speed__ + - xarm_api->service: __set_cartesian_velo_continuous__ + - xarm_api->service: __set_allow_approx_motion__ + - xarm_api->service: __set_only_check_type__ + - xarm_api->service: __config_tgpio_reset_when_stop__ + - xarm_api->service: __config_cgpio_reset_when_stop__ - [xarm_msgs::srv::SetInt16ById](./srv/SetInt16ById.srv) - xarm_api->service: __motion_enable__ @@ -66,6 +98,7 @@ - xarm_api->service: __get_position__ - xarm_api->service: __get_servo_angle__ - xarm_api->service: __get_position_aa__ + - xarm_api->service: __get_ft_sensor_data__ - [xarm_msgs::srv::SetFloat32](./srv/SetFloat32.srv) - xarm_api->service: __set_pause_time__ @@ -165,6 +198,30 @@ - [xarm_msgs::srv::TrajPlay](./srv/TrajPlay.srv) - xarm_api->service: __playback_trajectory__ +- [xarm_msgs::srv::IdenLoad](./srv/IdenLoad.srv) + - xarm_api->service: __iden_tcp_load__ + - xarm_api->service: __ft_sensor_iden_load__ + +- [xarm_msgs::srv::FtCaliLoad](./srv/FtCaliLoad.srv) + - xarm_api->service: __ft_sensor_cali_load__ + +- [xarm_msgs::srv::FtForceConfig](./srv/FtForceConfig.srv) + - xarm_api->service: __config_force_control__ + +- [xarm_msgs::srv::FtForcePid](./srv/FtForcePid.srv) + - xarm_api->service: __set_force_control_pid__ + +- [xarm_msgs::srv::FtImpedance](./srv/FtImpedance.srv) + - xarm_api->service: __set_impedance__ + - xarm_api->service: __set_impedance_mbk__ + - xarm_api->service: ___set_impedance_config__ + +- [xarm_msgs::srv::LinearTrackBackOrigin](./srv/LinearTrackBackOrigin.srv) + - xarm_api->service: __set_linear_track_back_origin__ + +- [xarm_msgs::srv::LinearTrackSetPos](./srv/LinearTrackSetPos.srv) + - xarm_api->service: __set_linear_track_pos__ + - [xarm_msgs::srv::PlanPose](./srv/PlanPose.srv) - xarm_planner->service: __xarm_pose_plan__ - [xarm_msgs::srv::PlanJoint](./srv/PlanJoint.srv) diff --git a/xarm_msgs/srv/Call.srv b/xarm_msgs/srv/Call.srv index c13ff073..070e323f 100644 --- a/xarm_msgs/srv/Call.srv +++ b/xarm_msgs/srv/Call.srv @@ -10,6 +10,12 @@ # - clean_bio_gripper_error # - start_record_trajectory # - stop_record_trajectory +# - ft_sensor_set_zero +# - set_linear_track_stop +# - clean_linear_track_error +# - open_lite6_gripper +# - close_lite6_gripper +# - stop_lite6_gripper --- diff --git a/xarm_msgs/srv/FtCaliLoad.srv b/xarm_msgs/srv/FtCaliLoad.srv new file mode 100644 index 00000000..354fa32a --- /dev/null +++ b/xarm_msgs/srv/FtCaliLoad.srv @@ -0,0 +1,17 @@ +# This format is suitable for the following services +# - ft_sensor_cali_load + +# iden result([mass(kg)，x_centroid(mm)，y_centroid(mm)，z_centroid(mm)，Fx_offset，Fy_offset，Fz_offset，Tx_offset，Ty_offset，Tz_ffset]) +float32[] datas + +# whether to convert the paramster to the corresponding tcp load and set, default is false +bool association_setting_tcp_load false +float32 m 0.325 +float32 x -17.0 +float32 y 9.0 +float32 z 11.8 + +--- + +int16 ret +string message diff --git a/xarm_msgs/srv/FtForceConfig.srv b/xarm_msgs/srv/FtForceConfig.srv new file mode 100644 index 00000000..fb1058e9 --- /dev/null +++ b/xarm_msgs/srv/FtForceConfig.srv @@ -0,0 +1,12 @@ +# This format is suitable for the following services +# - config_force_control + +int16 coord +int16[] c_axis +float32[] ref +float32[] limits + +--- + +int16 ret +string message \ No newline at end of file diff --git a/xarm_msgs/srv/FtForcePid.srv b/xarm_msgs/srv/FtForcePid.srv new file mode 100644 index 00000000..94643224 --- /dev/null +++ b/xarm_msgs/srv/FtForcePid.srv @@ -0,0 +1,12 @@ +# This format is suitable for the following services +# - set_force_control_pid + +float32[] kp +float32[] ki +float32[] kd +float32[] xe_limit + +--- + +int16 ret +string message \ No newline at end of file diff --git a/xarm_msgs/srv/FtImpedance.srv b/xarm_msgs/srv/FtImpedance.srv new file mode 100644 index 00000000..ee6ca0ce --- /dev/null +++ b/xarm_msgs/srv/FtImpedance.srv @@ -0,0 +1,15 @@ +# This format is suitable for the following services +# - set_impedance +# - set_impedance_mbk +# - set_impedance_config + +int16 coord +int16[] c_axis +float32[] m +float32[] k +float32[] b + +--- + +int16 ret +string message \ No newline at end of file diff --git a/xarm_msgs/srv/GetFloat32List.srv b/xarm_msgs/srv/GetFloat32List.srv index 212d123e..b0019e1c 100644 --- a/xarm_msgs/srv/GetFloat32List.srv +++ b/xarm_msgs/srv/GetFloat32List.srv @@ -2,6 +2,7 @@ # - get_position # - get_servo_angle # - get_position_aa +# - get_ft_sensor_data --- diff --git a/xarm_msgs/srv/GetInt16.srv b/xarm_msgs/srv/GetInt16.srv index 7f51ee38..e4d7aeb6 100644 --- a/xarm_msgs/srv/GetInt16.srv +++ b/xarm_msgs/srv/GetInt16.srv @@ -5,6 +5,17 @@ # - get_gripper_err_code # - get_bio_gripper_status # - get_bio_gripper_error +# - get_reduced_mode +# - get_report_tau_or_i +# - ft_sensor_app_get +# - get_ft_sensor_error +# - get_trajectory_rw_status +# - get_linear_track_pos +# - get_linear_track_status +# - get_linear_track_error +# - get_linear_track_is_enabled +# - get_linear_track_on_zero +# - get_linear_track_sci --- diff --git a/xarm_msgs/srv/GetInt16List.srv b/xarm_msgs/srv/GetInt16List.srv index b0f3cbdd..f8b55915 100644 --- a/xarm_msgs/srv/GetInt16List.srv +++ b/xarm_msgs/srv/GetInt16List.srv @@ -1,5 +1,6 @@ # This format is suitable for the following services # - get_err_warn_code +# - get_linear_track_sco --- diff --git a/xarm_msgs/srv/IdenLoad.srv b/xarm_msgs/srv/IdenLoad.srv new file mode 100644 index 00000000..840a5d6e --- /dev/null +++ b/xarm_msgs/srv/IdenLoad.srv @@ -0,0 +1,16 @@ +# This format is suitable for the following services +# - iden_tcp_load +# - ft_sensor_iden_load + +# estimated mass(kg), only required for Lite6 models via the `iden_tcp_load` service +float32 estimated_mass 0.0 + +--- + +int16 ret +string message + +# the result of identification +# iden_tcp_load: [mass(kg)，x_centroid(mm)，y_centroid(mm)，z_centroid(mm)] +# ft_sensor_iden_load: [mass(kg)，x_centroid(mm)，y_centroid(mm)，z_centroid(mm)，Fx_offset，Fy_offset，Fz_offset，Tx_offset，Ty_offset，Tz_ffset] +float32[] datas \ No newline at end of file diff --git a/xarm_msgs/srv/LinearTrackBackOrigin.srv b/xarm_msgs/srv/LinearTrackBackOrigin.srv new file mode 100644 index 00000000..4b64a234 --- /dev/null +++ b/xarm_msgs/srv/LinearTrackBackOrigin.srv @@ -0,0 +1,10 @@ +# This format is suitable for the following services +# - set_linear_track_back_origin + +bool wait true +bool auto_enable true + +--- + +int16 ret +string message diff --git a/xarm_msgs/srv/LinearTrackSetPos.srv b/xarm_msgs/srv/LinearTrackSetPos.srv new file mode 100644 index 00000000..ab5ae11a --- /dev/null +++ b/xarm_msgs/srv/LinearTrackSetPos.srv @@ -0,0 +1,13 @@ +# This format is suitable for the following services +# - set_linear_track_pos + +int16 pos +int16 speed 0 +bool wait true +float32 timeout 100 +bool auto_enable true + +--- + +int16 ret +string message diff --git a/xarm_msgs/srv/SetInt16.srv b/xarm_msgs/srv/SetInt16.srv index 55d29957..cdaf96ad 100644 --- a/xarm_msgs/srv/SetInt16.srv +++ b/xarm_msgs/srv/SetInt16.srv @@ -11,6 +11,16 @@ # - set_self_collision_detection # - set_simulation_robot # - set_baud_checkset_enable +# - set_report_tau_or_i +# - ft_sensor_enable +# - ft_sensor_app_set +# - set_linear_track_enable +# - set_linear_track_speed +# - set_cartesian_velo_continuous +# - set_allow_approx_motion +# - set_only_check_type +# - config_tgpio_reset_when_stop +# - config_cgpio_reset_when_stop int16 data