diff --git a/tm_driver/CMakeLists.txt b/tm_driver/CMakeLists.txt index 4ea38eb..77ebe16 100644 --- a/tm_driver/CMakeLists.txt +++ b/tm_driver/CMakeLists.txt @@ -8,7 +8,7 @@ endif() # Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() @@ -71,7 +71,7 @@ include_directories(include if (moveit2_lib_auto_judge) if (moveit_ros_planning_interface_FOUND) - add_executable(tm_driver + add_library(tm_driver #src/tm_ros2_node.cpp src/tm_ros2_composition_moveit.cpp src/tm_ros2_sct.cpp @@ -100,7 +100,7 @@ if (moveit2_lib_auto_judge) rclcpp_action ) else() - add_executable(tm_driver + add_library(tm_driver src/tm_ros2_composition.cpp src/tm_ros2_sct.cpp src/tm_ros2_svr.cpp @@ -125,7 +125,7 @@ if (moveit2_lib_auto_judge) ) endif (moveit_ros_planning_interface_FOUND) else() - add_executable(tm_driver + add_library(tm_driver src/tm_ros2_composition.cpp src/tm_ros2_sct.cpp src/tm_ros2_svr.cpp @@ -149,15 +149,21 @@ else() tf2_geometry_msgs ) endif() - -install(TARGETS - tm_driver - DESTINATION lib/${PROJECT_NAME} + +ament_export_targets(export_tm_driver HAS_LIBRARY_TARGET) + +install(DIRECTORY include/ + DESTINATION include ) -install( - DIRECTORY launch - DESTINATION share/${PROJECT_NAME} + +install(TARGETS + tm_driver + EXPORT export_tm_driver + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) ament_package() diff --git a/tm_driver/include/tm_driver/tm_command.h b/tm_driver/include/tm_driver/tm_command.h index 391a4a1..7e25e49 100644 --- a/tm_driver/include/tm_driver/tm_command.h +++ b/tm_driver/include/tm_driver/tm_command.h @@ -73,7 +73,15 @@ class TmCommand /* Leaving external control mode. More Detail please refer to the TM_Robot_Expression.pdf Chapter 8.2 */ - static std::string script_exit() { return "ScriptExit()"; } + static std::string script_exit(int priority = -1) + { + std::string prio_str = ""; + if (priority >= 0) + { + prio_str = std::to_string(priority); + } + return "ScriptExit(" + prio_str + ")"; + } // More details please refer to the TM_Robot_Expression.pdf Chapter 9.1 static std::string set_tag(int tag, int wait = 0); @@ -116,6 +124,9 @@ class TmCommand static std::string set_tool_pose_Line(const std::vector &pose, double vel, double acc_time, int blend_percent, bool fine_goal, int precision = 5); + static std::string set_tool_pose_Line_rel(const std::vector &pose, + bool tool_frame, double vel, double acc_time, int blend_percent, bool fine_goal, int precision = 5); + /* PVT start. More details please refer to the TM_Robot_Expression.pdf Chapter 9.16 */ static std::string set_pvt_enter(int mode) { return "PVTEnter(" + std::to_string(mode) + ")"; } @@ -149,4 +160,19 @@ class TmCommand static std::string set_vel_mode_start(VelMode mode, double timeout_zero_vel, double timeout_stop); static std::string set_vel_mode_stop() { return "StopContinueVmode()"; } static std::string set_vel_mode_target(VelMode mode, const std::vector &vel, int precision = 5); + + /* + Sets the maximum speed of the robot during motion commands. + linear_speed : not implemented yet. The number must be >0 + rotational_speed : in degrees per second + + More details please refer to Jakub Sikorski (jakub@ramlab.com) */ + static std::string set_tcp_speed(uint32_t linear_speed, uint32_t rotational_speed, bool is_model_s); + + /* Changing the TCP of the robot + More details please refer to the TM_Robot_Expression.pdf Chapter 8.14 */ + static std::string change_tcp(const std::string &toolname); + static std::string change_tcp(const std::vector &tcp); + static std::string change_tcp(const std::vector &tcp, double weight); + static std::string change_tcp(const std::vector &tcp, double weight, const std::vector &inertia); }; \ No newline at end of file diff --git a/tm_driver/include/tm_driver/tm_driver.h b/tm_driver/include/tm_driver/tm_driver.h index 7533d64..6db5d2c 100644 --- a/tm_driver/include/tm_driver/tm_driver.h +++ b/tm_driver/include/tm_driver/tm_driver.h @@ -29,6 +29,7 @@ class TmDriver double _max_payload = 4.0; bool isOnListenNode = false; bool connect_recovery_is_halt = false; //false: do the recovery; true: stop the recovery + bool _is_model_s = false; public: explicit TmDriver(const std::string &ip); @@ -55,7 +56,7 @@ class TmDriver void set_this_max_velocity(double max_vel) { _max_velocity = max_vel; } void set_this_max_tcp_speed(double max_spd) { _max_tcp_speed = max_spd; } void set_this_max_payload(double payload) { _max_payload = payload; } - + void set_is_model_s(bool is_model_s) { _is_model_s = is_model_s; } //////////////////////////////// // SVR Robot Function (write_XXX) //////////////////////////////// @@ -65,7 +66,7 @@ class TmDriver // SCT Robot Function (set_XXX) //////////////////////////////// bool is_on_listen_node(); - bool script_exit(const std::string &id = "Exit"); + bool script_exit(int priority = -1, const std::string &id = "Exit"); bool set_tag(int tag, int wait = 0, const std::string &id = "Tag"); bool set_wait_tag(int tag, int timeout_ms = 0, const std::string &id = "WaitTag"); bool set_stop(int level=-1, const std::string &id = "Stop"); @@ -81,6 +82,9 @@ class TmDriver double vel, double acc_time, int blend_percent, bool fine_goal = false, const std::string &id = "PTPT"); bool set_tool_pose_Line(const std::vector &pose, double vel, double acc_time, int blend_percent, bool fine_goal = false, const std::string &id = "Line"); + bool set_tool_pose_Line_rel(const std::vector &pose, + bool tool_frame, + double vel, double acc_time, int blend_percent, bool fine_goal = false, const std::string &id = "LineRel"); // set_tool_pose_PLINE // @@ -107,4 +111,12 @@ class TmDriver bool set_vel_mode_start(VelMode mode, double timeout_zero_vel, double timeout_stop, const std::string &id = "VModeStart"); bool set_vel_mode_stop(const std::string &id = "VModeStop"); bool set_vel_mode_target(VelMode mode, const std::vector &vel, const std::string &id = "VModeTrgt"); + + + bool set_tcp_speed(uint32_t linear_speed, uint32_t rotational_speed, const std::string &id = "SetTCPSpeed"); + + bool change_tcp(const std::string &toolname, const std::string &id = "ChangeTCP"); + bool change_tcp(const std::vector &tcp, const std::string &id = "ChangeTCP"); + bool change_tcp(const std::vector &tcp, double weight, const std::string &id = "ChangeTCP"); + bool change_tcp(const std::vector &tcp, double weight, const std::vector &inertia, const std::string &id = "ChangeTCP"); }; diff --git a/tm_driver/include/tm_driver/tm_robot_state.h b/tm_driver/include/tm_driver/tm_robot_state.h index bf47522..fa7b8ec 100755 --- a/tm_driver/include/tm_driver/tm_robot_state.h +++ b/tm_driver/include/tm_driver/tm_robot_state.h @@ -7,6 +7,8 @@ #include #include #include +#include +#include #include "tm_driver_utilities.h" struct TmRobotStateData @@ -38,14 +40,15 @@ struct TmRobotStateData int ma_mode = 0; char stick_play_pause; int32_t robot_light = 0; - unsigned char ctrller_DO[16]; - unsigned char ctrller_DI[16]; - float ctrller_AO[2]; - float ctrller_AI[2]; - unsigned char ee_DO[4]; - unsigned char ee_DI[4]; - float ee_AO[2]; - float ee_AI[2]; + std::array ctrller_DO{0}; + std::array ctrller_DI{0}; + std::array ctrller_AO{0}; + std::array ctrller_AI{0}; + std::array ee_DO{0}; + std::array ee_DI{0}; + std::array ee_AO{0}; + std::array ee_AI{0}; + char robot_model[16]; }; class TmDataTable; @@ -97,6 +100,8 @@ class TmRobotState unsigned char is_EStop() { return tmRobotStateDataToPublish.is_ESTOP_pressed; } unsigned char camera_light() { return tmRobotStateDataToPublish.camera_light; } // R/W int error_code() { return tmRobotStateDataToPublish.error_code; } + std::optional robot_model() const; + std::optional is_model_s() const; std::string error_content() { return ""; } std::vector flange_pose() { @@ -170,47 +175,27 @@ class TmRobotState int32_t robot_light() { return tmRobotStateDataToPublish.robot_light; } std::vector ctrller_DO(){ - std::vector ctrllerDO; - ctrllerDO.assign(8, 0); - for (int i = 0; i < 8; ++i) { ctrllerDO[i] = tmRobotStateDataToPublish.ctrller_DO[i]; } - return ctrllerDO; + return std::vector (tmRobotStateDataToPublish.ctrller_DO.begin(), tmRobotStateDataToPublish.ctrller_DO.end()); } std::vector ctrller_DI() { - std::vector ctrllerDI; - ctrllerDI.assign(8, 0); - for (int i = 0; i < 8; ++i) { ctrllerDI[i] = tmRobotStateDataToPublish.ctrller_DI[i]; } - return ctrllerDI; + return std::vector (tmRobotStateDataToPublish.ctrller_DI.begin(), tmRobotStateDataToPublish.ctrller_DI.end()); } std::vector ctrller_AO(){ - std::vector ctrllerAO; - ctrllerAO.assign(1, 0.0); - for (int i = 0; i < 1; ++i) { ctrllerAO[i] = tmRobotStateDataToPublish.ctrller_AO[i]; } - return ctrllerAO; + return std::vector (tmRobotStateDataToPublish.ctrller_AO.begin(), tmRobotStateDataToPublish.ctrller_AO.end()); } std::vector ctrller_AI(){ - std::vector ctrllerAI; - ctrllerAI.assign(2, 0.0); - for (int i = 0; i < 2; ++i) { ctrllerAI[i] = tmRobotStateDataToPublish.ctrller_AI[i]; } - return ctrllerAI; - } + return std::vector (tmRobotStateDataToPublish.ctrller_AI.begin(), tmRobotStateDataToPublish.ctrller_AI.end()); + } std::vector ee_DO(){ - std::vector eeDO; - eeDO.assign(4, 0); - for (int i = 0; i < 4; ++i) { eeDO[i] = tmRobotStateDataToPublish.ee_DO[i]; } - return eeDO; + return std::vector (tmRobotStateDataToPublish.ee_DO.begin(), tmRobotStateDataToPublish.ee_DO.end()); } std::vector ee_DI(){ - std::vector eeDI; - eeDI.assign(4, 0); - for (int i = 0; i < 4; ++i) { eeDI[i] = tmRobotStateDataToPublish.ee_DI[i]; } - return eeDI; - } + return std::vector (tmRobotStateDataToPublish.ee_DI.begin(), tmRobotStateDataToPublish.ee_DI.end()); + } std::vector ee_AI(){ - std::vector eeAI; - eeAI.assign(1, 0.0); - for (int i = 0; i < 1; ++i) { eeAI[i] = tmRobotStateDataToPublish.ee_AI[i]; } - return eeAI; + return std::vector (tmRobotStateDataToPublish.ee_AI.begin(), tmRobotStateDataToPublish.ee_AI.end()); } + TmCommRC get_receive_state(){return _receive_state;} diff --git a/tm_driver/src/tm_command.cpp b/tm_driver/src/tm_command.cpp index f177676..81f072b 100644 --- a/tm_driver/src/tm_command.cpp +++ b/tm_driver/src/tm_command.cpp @@ -1,3 +1,4 @@ +#include #ifdef NO_INCLUDE_DIR #include "tm_command.h" #else @@ -78,16 +79,36 @@ std::string TmCommand::set_tool_pose_PTP(const std::vector &pose, } std::string TmCommand::set_tool_pose_Line(const std::vector &pose, double vel, double acc_time, int blend_percent, bool fine_goal, int precision) +{ + auto pose_mmdeg = mmdeg_pose(pose); + // int vel_mm = static_cast( std::round(1000.0 * vel)); + + double vel_mm = 1000.0 * vel; + int acct_ms = static_cast( std::round(1000.0 * acc_time)); + + std::stringstream ss; + ss << std::fixed << std::setprecision(precision); + ss << "Line(\"CAP\","; + for (auto &value : pose_mmdeg) { ss << value << ","; } + ss << std::setprecision(precision) << vel_mm << "," << acct_ms << "," << blend_percent << ","; + ss << std::boolalpha < &pose, + bool tool_frame, + double vel, double acc_time, int blend_percent, bool fine_goal, int precision) { auto pose_mmdeg = mmdeg_pose(pose); int vel_mm = int(1000.0 * vel); int acct_ms = int(1000.0 * acc_time); std::stringstream ss; ss << std::fixed << std::setprecision(precision); - ss << "Line(\"CAP\","; + std::string frame = tool_frame ? "T" : "C"; + ss << "Move_Line(\"" << frame << "AP\","; for (auto &value : pose_mmdeg) { ss << value << ","; } ss << vel_mm << "," << acct_ms << "," << blend_percent << ","; - ss << std::boolalpha < &tcp) +{ + std::stringstream ss; + ss << "ChangeTCP("; + ss << tcp[0] << "," << tcp[1] << "," << tcp[2] << "," << deg(tcp[3]) << "," << deg(tcp[4]) << "," << deg(tcp[5]); + ss << ")"; + return ss.str(); +} +std::string TmCommand::change_tcp(const std::vector &tcp, double weight) +{ + std::stringstream ss; + ss << "ChangeTCP("; + ss << tcp[0] << "," << tcp[1] << "," << tcp[2] << "," << deg(tcp[3]) << "," << deg(tcp[4]) << "," << deg(tcp[5]); + ss << "," << weight; + ss << ")"; + return ss.str(); +} +std::string TmCommand::change_tcp(const std::vector &tcp, double weight, const std::vector &inertia) +{ + std::stringstream ss; + ss << "ChangeTCP("; + ss << tcp[0] << "," << tcp[1] << "," << tcp[2] << "," << deg(tcp[3]) << "," << deg(tcp[4]) << "," << deg(tcp[5]); + ss << "," << weight; + ss << "," << inertia[0] << "," << inertia[1] << "," << inertia[2]; + ss << "," << inertia[3] << "," << inertia[4] << "," << inertia[5]; + ss << "," << deg(inertia[6]) << "," << deg(inertia[7]) << "," << deg(inertia[8]); + ss << ")"; + return ss.str(); +} diff --git a/tm_driver/src/tm_driver.cpp b/tm_driver/src/tm_driver.cpp index d994a19..fd31156 100644 --- a/tm_driver/src/tm_driver.cpp +++ b/tm_driver/src/tm_driver.cpp @@ -90,9 +90,9 @@ void TmDriver::back_to_listen_node(){ isOnListenNode = true; } -bool TmDriver::script_exit(const std::string &id) +bool TmDriver::script_exit(int priority, const std::string &id) { - return (sct.send_script_str(id, TmCommand::script_exit()) == RC_OK); + return (sct.send_script_str(id, TmCommand::script_exit(priority)) == RC_OK); } bool TmDriver::set_tag(int tag, int wait, const std::string &id) @@ -152,6 +152,15 @@ bool TmDriver::set_tool_pose_Line(const std::vector &pose, ) == RC_OK); } +bool TmDriver::set_tool_pose_Line_rel(const std::vector &pose, + bool tool_frame, + double vel, double acc_time, int blend_percent, bool fine_goal, const std::string &id) +{ + return (sct.send_script_str( + id, TmCommand::set_tool_pose_Line_rel(pose, tool_frame, vel, acc_time, blend_percent, fine_goal) + ) == RC_OK); +} + bool TmDriver::set_pvt_enter(TmPvtMode mode, const std::string &id) { return (sct.send_script_str(id, TmCommand::set_pvt_enter(int(mode))) == RC_OK); @@ -324,3 +333,28 @@ bool TmDriver::set_vel_mode_target(VelMode mode, const std::vector &vel, { return (sct.send_script_str_silent(id, TmCommand::set_vel_mode_target(mode, vel)) == RC_OK); } + +bool TmDriver::set_tcp_speed(uint32_t linear_speed, uint32_t rotational_speed, const std::string &id) +{ + std::string script = TmCommand::set_tcp_speed(linear_speed, rotational_speed, _is_model_s); + print_info("TM_DRV: send set tcp speed.:\n"); + printf("%s\n", script.c_str()); + return (sct.send_script_str(id, script) == RC_OK); +} + +bool TmDriver::change_tcp(const std::string &toolname, const std::string &id) +{ + return (sct.send_script_str(id, TmCommand::change_tcp(toolname)) == RC_OK); +} +bool TmDriver::change_tcp(const std::vector &tcp, const std::string &id) +{ + return (sct.send_script_str(id, TmCommand::change_tcp(tcp)) == RC_OK); +} +bool TmDriver::change_tcp(const std::vector &tcp, double weight, const std::string &id) +{ + return (sct.send_script_str(id, TmCommand::change_tcp(tcp, weight)) == RC_OK); +} +bool TmDriver::change_tcp(const std::vector &tcp, double weight, const std::vector &inertia, const std::string &id) +{ + return (sct.send_script_str(id, TmCommand::change_tcp(tcp, weight, inertia)) == RC_OK); +} diff --git a/tm_driver/src/tm_robot_state.cpp b/tm_driver/src/tm_robot_state.cpp index b133462..0104828 100755 --- a/tm_driver/src/tm_robot_state.cpp +++ b/tm_driver/src/tm_robot_state.cpp @@ -27,12 +27,12 @@ class TmDataTable }; private: std::map _item_map; - + const TmRobotStateData *_rsd; public: TmDataTable(TmRobotState *rs) { print_debug("Create DataTable"); - + _rsd = &rs->tmRobotStateDataFromEthernet; _item_map.clear(); //_item_map[""] = { Item:, &rs- }; _item_map["Robot_Link" ] = { &rs->tmRobotStateDataFromEthernet.is_linked }; @@ -106,16 +106,18 @@ class TmDataTable _item_map["End_AO1" ] = { &rs->tmRobotStateDataFromEthernet.ee_AO[1],Item::NOT_REQUIRE }; _item_map["End_AI0" ] = { &rs->tmRobotStateDataFromEthernet.ee_AI[0] }; _item_map["End_AI1" ] = { &rs->tmRobotStateDataFromEthernet.ee_AI[1],Item::NOT_REQUIRE }; + _item_map["Robot_Model" ] = { &rs->tmRobotStateDataFromEthernet.robot_model,Item::NOT_REQUIRE}; } std::map & get() { return _item_map; } std::map::iterator find(const std::string &name) { return _item_map.find(name); } std::map::iterator end() { return _item_map.end(); } + const TmRobotStateData* get_rsd() { return _rsd; } }; TmRobotState::TmRobotState() { print_debug("TmRobotState::TmRobotState"); - + std::memset(&tmRobotStateDataFromEthernet.robot_model[0], 0, sizeof(tmRobotStateDataFromEthernet.robot_model)); _data_table = new TmDataTable(this); _f_deserialize_item[0] = std::bind(&TmRobotState::_deserialize_skip, @@ -279,7 +281,7 @@ size_t TmRobotState::_deserialize_first_time(const char *data, size_t size) for (auto iter : _data_table->get()) { if (iter.second.required && !iter.second.checked) { isDataTableCorrect = false; - std::string msg = "Required item" + iter.first + " is NOT checked"; + std::string msg = "Required item " + iter.first + " is NOT checked"; print_error(msg.c_str()); } } @@ -305,7 +307,15 @@ size_t TmRobotState::_deserialize(const char *data, size_t size) } multiThreadCache.set_catch_data(tmRobotStateDataFromEthernet); - + tmRobotStateDataToPublish = *_data_table->get_rsd(); + static bool print_model = true; + const auto model = robot_model(); + + if (print_model && model.has_value()) { + print_model = false; + auto msg = std::string("Robot model is: ") + model.value(); + print_info(msg.c_str()); + } if (boffset > size) { } return boffset; @@ -318,3 +328,21 @@ void TmRobotState::update_tm_robot_publish_state(){ void TmRobotState::set_receive_state(TmCommRC state){ _receive_state = state; } + +std::optional TmRobotState::robot_model() const +{ + auto robot_name_item_it = _data_table->find("Robot_Model"); + if (robot_name_item_it == _data_table->end() || !robot_name_item_it->second.checked || tmRobotStateDataFromEthernet.robot_model[0] == '\0') { + return std::nullopt; + } + return tmRobotStateDataFromEthernet.robot_model; +} + +std::optional TmRobotState::is_model_s() const +{ + auto model = robot_model(); + if (!model.has_value()) { + return std::nullopt; + } + return model.value().find("S-") != std::string::npos; +} \ No newline at end of file diff --git a/tm_inspect/CMakeLists.txt b/tm_inspect/CMakeLists.txt index ef0ad3b..b6d2921 100755 --- a/tm_inspect/CMakeLists.txt +++ b/tm_inspect/CMakeLists.txt @@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() diff --git a/tm_inspect/COLCON_IGNORE b/tm_inspect/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/tm_msgs/CMakeLists.txt b/tm_msgs/CMakeLists.txt index 5ce8374..0dd2f62 100755 --- a/tm_msgs/CMakeLists.txt +++ b/tm_msgs/CMakeLists.txt @@ -8,7 +8,7 @@ endif() # Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) endif()