From 4a967821d8dd0992e65fa5d4738d97bed67a0876 Mon Sep 17 00:00:00 2001 From: Diego Serrapio Date: Wed, 1 Oct 2025 11:29:03 +0200 Subject: [PATCH 01/14] Adds mods required by mc-backend --- tm_driver/CMakeLists.txt | 26 ++++++---- tm_driver/include/tm_driver/tm_command.h | 28 ++++++++++- tm_driver/include/tm_driver/tm_driver.h | 13 ++++- tm_driver/src/tm_command.cpp | 60 ++++++++++++++++++++++++ tm_driver/src/tm_driver.cpp | 38 ++++++++++++++- tm_inspect/COLCON_IGNORE | 0 6 files changed, 151 insertions(+), 14 deletions(-) create mode 100644 tm_inspect/COLCON_IGNORE diff --git a/tm_driver/CMakeLists.txt b/tm_driver/CMakeLists.txt index 4ea38eb..a4c5969 100644 --- a/tm_driver/CMakeLists.txt +++ b/tm_driver/CMakeLists.txt @@ -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..623fffd 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); + + /* 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..45c56ed 100644 --- a/tm_driver/include/tm_driver/tm_driver.h +++ b/tm_driver/include/tm_driver/tm_driver.h @@ -65,7 +65,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 +81,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 +110,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/src/tm_command.cpp b/tm_driver/src/tm_command.cpp index f177676..d7d0299 100644 --- a/tm_driver/src/tm_command.cpp +++ b/tm_driver/src/tm_command.cpp @@ -91,6 +91,23 @@ std::string TmCommand::set_tool_pose_Line(const std::vector &pose, return ss.str(); } +std::string TmCommand::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) +{ + 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); + 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 << fine_goal << ")"; + return ss.str(); +} + std::string TmCommand::set_pvt_point(TmPvtMode mode, double t, const std::vector &pos, const std::vector &vel, int precision) { @@ -181,3 +198,46 @@ std::string TmCommand::set_vel_mode_target(VelMode mode, const std::vector &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..002716a 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); + 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_inspect/COLCON_IGNORE b/tm_inspect/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 From 63c5e26eb61fca4f21bfbf83621409440ace296f Mon Sep 17 00:00:00 2001 From: Diego Serrapio Date: Wed, 8 Oct 2025 11:51:23 +0200 Subject: [PATCH 02/14] Fixes data reporting --- tm_driver/src/tm_robot_state.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/tm_driver/src/tm_robot_state.cpp b/tm_driver/src/tm_robot_state.cpp index b133462..a10d392 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 }; @@ -110,6 +110,7 @@ class TmDataTable 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() @@ -305,7 +306,7 @@ size_t TmRobotState::_deserialize(const char *data, size_t size) } multiThreadCache.set_catch_data(tmRobotStateDataFromEthernet); - + tmRobotStateDataToPublish = *_data_table->get_rsd(); if (boffset > size) { } return boffset; From 100b1f56a6a61b29a9d5caa50a96ab08bb8811e7 Mon Sep 17 00:00:00 2001 From: "Jakub (from KSB robot)" Date: Mon, 13 Oct 2025 12:39:40 +0200 Subject: [PATCH 03/14] Fix to speed rounding --- tm_driver/src/tm_command.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/tm_driver/src/tm_command.cpp b/tm_driver/src/tm_command.cpp index d7d0299..a3eb00e 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 @@ -80,8 +81,8 @@ 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 = int(1000.0 * vel); - int acct_ms = int(1000.0 * acc_time); + int vel_mm = static_cast( std::round(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\","; @@ -202,7 +203,9 @@ std::string TmCommand::set_vel_mode_target(VelMode mode, const std::vector Date: Wed, 15 Oct 2025 14:07:56 +0200 Subject: [PATCH 04/14] Adds reading the model name --- tm_driver/include/tm_driver/tm_robot_state.h | 2 ++ tm_driver/src/tm_robot_state.cpp | 8 ++++++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/tm_driver/include/tm_driver/tm_robot_state.h b/tm_driver/include/tm_driver/tm_robot_state.h index bf47522..f546f29 100755 --- a/tm_driver/include/tm_driver/tm_robot_state.h +++ b/tm_driver/include/tm_driver/tm_robot_state.h @@ -46,6 +46,7 @@ struct TmRobotStateData unsigned char ee_DI[4]; float ee_AO[2]; float ee_AI[2]; + char robot_model[16]; }; class TmDataTable; @@ -97,6 +98,7 @@ 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::string robot_model() { return tmRobotStateDataToPublish.robot_model; } std::string error_content() { return ""; } std::vector flange_pose() { diff --git a/tm_driver/src/tm_robot_state.cpp b/tm_driver/src/tm_robot_state.cpp index b133462..4312566 100755 --- a/tm_driver/src/tm_robot_state.cpp +++ b/tm_driver/src/tm_robot_state.cpp @@ -106,6 +106,7 @@ 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}; } std::map & get() { return _item_map; } std::map::iterator find(const std::string &name) { return _item_map.find(name); } @@ -305,8 +306,11 @@ size_t TmRobotState::_deserialize(const char *data, size_t size) } multiThreadCache.set_catch_data(tmRobotStateDataFromEthernet); - - if (boffset > size) { + static constexpr bool print_model = true; + if (print_model) { + auto msg = std::string("Robot model is: ") + tmRobotStateDataFromEthernet.robot_model; + print_info(msg.c_str()); + print_model = false; } return boffset; } From 03f782e30edc73c01f2238081711f814f78f0080 Mon Sep 17 00:00:00 2001 From: Diego Serrapio Date: Wed, 15 Oct 2025 15:12:37 +0200 Subject: [PATCH 05/14] Fixes build --- tm_driver/src/tm_robot_state.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tm_driver/src/tm_robot_state.cpp b/tm_driver/src/tm_robot_state.cpp index 2c56ca1..04d2cdc 100755 --- a/tm_driver/src/tm_robot_state.cpp +++ b/tm_driver/src/tm_robot_state.cpp @@ -308,7 +308,7 @@ size_t TmRobotState::_deserialize(const char *data, size_t size) multiThreadCache.set_catch_data(tmRobotStateDataFromEthernet); tmRobotStateDataToPublish = *_data_table->get_rsd(); - static constexpr bool print_model = true; + static bool print_model = true; if (print_model) { print_model = false; auto msg = std::string("Robot model is: ") + tmRobotStateDataFromEthernet.robot_model; From cf7c6db37c90f8ef98c8f9833ee59a9dc14a1465 Mon Sep 17 00:00:00 2001 From: Diego Serrapio Date: Wed, 15 Oct 2025 15:28:58 +0200 Subject: [PATCH 06/14] Adds clearing of model buffer on init --- tm_driver/src/tm_robot_state.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tm_driver/src/tm_robot_state.cpp b/tm_driver/src/tm_robot_state.cpp index 04d2cdc..66b7c0f 100755 --- a/tm_driver/src/tm_robot_state.cpp +++ b/tm_driver/src/tm_robot_state.cpp @@ -117,7 +117,7 @@ class TmDataTable 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, From 9fb8ed7b52282f1d1f1921a1b9d766506bbe2dab Mon Sep 17 00:00:00 2001 From: Diego Serrapio Date: Wed, 15 Oct 2025 16:40:11 +0200 Subject: [PATCH 07/14] Makes robot model optional --- tm_driver/include/tm_driver/tm_robot_state.h | 3 ++- tm_driver/src/tm_robot_state.cpp | 19 +++++++++++++++---- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/tm_driver/include/tm_driver/tm_robot_state.h b/tm_driver/include/tm_driver/tm_robot_state.h index f546f29..b553dfb 100755 --- a/tm_driver/include/tm_driver/tm_robot_state.h +++ b/tm_driver/include/tm_driver/tm_robot_state.h @@ -7,6 +7,7 @@ #include #include #include +#include #include "tm_driver_utilities.h" struct TmRobotStateData @@ -98,7 +99,7 @@ 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::string robot_model() { return tmRobotStateDataToPublish.robot_model; } + std::optional robot_model() const; std::string error_content() { return ""; } std::vector flange_pose() { diff --git a/tm_driver/src/tm_robot_state.cpp b/tm_driver/src/tm_robot_state.cpp index 66b7c0f..30f8a0d 100755 --- a/tm_driver/src/tm_robot_state.cpp +++ b/tm_driver/src/tm_robot_state.cpp @@ -106,7 +106,7 @@ 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_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); } @@ -281,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()); } } @@ -309,9 +309,11 @@ 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; - if (print_model) { + const auto model = robot_model(); + + if (print_model && model.has_value()) { print_model = false; - auto msg = std::string("Robot model is: ") + tmRobotStateDataFromEthernet.robot_model; + auto msg = std::string("Robot model is: ") + model.value(); print_info(msg.c_str()); } if (boffset > size) { @@ -326,3 +328,12 @@ 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) { + return std::nullopt; + } + return tmRobotStateDataFromEthernet.robot_model; +} \ No newline at end of file From 15f03a920d46a734ef544d56bbfc5c4fe799d994 Mon Sep 17 00:00:00 2001 From: Diego Serrapio Date: Thu, 16 Oct 2025 09:10:41 +0200 Subject: [PATCH 08/14] Adds function to inform if robot is model s or not --- tm_driver/include/tm_driver/tm_robot_state.h | 1 + tm_driver/src/tm_robot_state.cpp | 9 +++++++++ 2 files changed, 10 insertions(+) diff --git a/tm_driver/include/tm_driver/tm_robot_state.h b/tm_driver/include/tm_driver/tm_robot_state.h index b553dfb..93bb3e5 100755 --- a/tm_driver/include/tm_driver/tm_robot_state.h +++ b/tm_driver/include/tm_driver/tm_robot_state.h @@ -100,6 +100,7 @@ class TmRobotState 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() { diff --git a/tm_driver/src/tm_robot_state.cpp b/tm_driver/src/tm_robot_state.cpp index 30f8a0d..9277e8e 100755 --- a/tm_driver/src/tm_robot_state.cpp +++ b/tm_driver/src/tm_robot_state.cpp @@ -336,4 +336,13 @@ std::optional TmRobotState::robot_model() const 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 From ebf39ada9595234f04164a9565ec0903c7971347 Mon Sep 17 00:00:00 2001 From: Diego Serrapio Date: Thu, 16 Oct 2025 10:02:48 +0200 Subject: [PATCH 09/14] Adjusts tcp speed command based on robot model --- tm_driver/include/tm_driver/tm_command.h | 2 +- tm_driver/include/tm_driver/tm_driver.h | 3 ++- tm_driver/src/tm_command.cpp | 5 +++-- tm_driver/src/tm_driver.cpp | 2 +- tm_driver/src/tm_robot_state.cpp | 2 +- 5 files changed, 8 insertions(+), 6 deletions(-) diff --git a/tm_driver/include/tm_driver/tm_command.h b/tm_driver/include/tm_driver/tm_command.h index 623fffd..7e25e49 100644 --- a/tm_driver/include/tm_driver/tm_command.h +++ b/tm_driver/include/tm_driver/tm_command.h @@ -167,7 +167,7 @@ class TmCommand 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); + 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 */ diff --git a/tm_driver/include/tm_driver/tm_driver.h b/tm_driver/include/tm_driver/tm_driver.h index 45c56ed..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) //////////////////////////////// diff --git a/tm_driver/src/tm_command.cpp b/tm_driver/src/tm_command.cpp index a3eb00e..4399229 100644 --- a/tm_driver/src/tm_command.cpp +++ b/tm_driver/src/tm_command.cpp @@ -200,10 +200,11 @@ std::string TmCommand::set_vel_mode_target(VelMode mode, const std::vector &vel, 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); + 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); diff --git a/tm_driver/src/tm_robot_state.cpp b/tm_driver/src/tm_robot_state.cpp index 9277e8e..0104828 100755 --- a/tm_driver/src/tm_robot_state.cpp +++ b/tm_driver/src/tm_robot_state.cpp @@ -332,7 +332,7 @@ void TmRobotState::set_receive_state(TmCommRC 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) { + 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; From 72b05e194f06430881143cd86b0be6cf6cbfaae6 Mon Sep 17 00:00:00 2001 From: Diego Serrapio Date: Thu, 16 Oct 2025 10:41:08 +0200 Subject: [PATCH 10/14] Fixes to set tcp speed --- tm_driver/src/tm_command.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/tm_driver/src/tm_command.cpp b/tm_driver/src/tm_command.cpp index 4399229..31d2fb3 100644 --- a/tm_driver/src/tm_command.cpp +++ b/tm_driver/src/tm_command.cpp @@ -203,10 +203,8 @@ std::string TmCommand::set_vel_mode_target(VelMode mode, const std::vector Date: Thu, 16 Oct 2025 10:52:52 +0200 Subject: [PATCH 11/14] Fixes set tcp speed command --- tm_driver/src/tm_command.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tm_driver/src/tm_command.cpp b/tm_driver/src/tm_command.cpp index 31d2fb3..43816f0 100644 --- a/tm_driver/src/tm_command.cpp +++ b/tm_driver/src/tm_command.cpp @@ -204,7 +204,7 @@ std::string TmCommand::set_tcp_speed(uint32_t linear_speed, uint32_t rotational_ { std::stringstream ss; const std::string cmd = is_model_s ? "SetTCPSpeedLimit(true," : "SetTCPSpeed("; - ss << linear_speed << "," << rotational_speed << ")"; + ss << cmd << linear_speed << "," << rotational_speed << ")"; return ss.str(); } From f0189077d9502695c4be84741ab117f167ad54ca Mon Sep 17 00:00:00 2001 From: "Jakub (from KSB robot)" Date: Tue, 21 Oct 2025 12:42:26 +0200 Subject: [PATCH 12/14] Fixes for Techman S from KSB tests --- tm_driver/CMakeLists.txt | 2 +- tm_driver/include/tm_driver/tm_robot_state.h | 57 +++++++------------- tm_driver/src/tm_command.cpp | 7 ++- tm_inspect/CMakeLists.txt | 4 +- tm_msgs/CMakeLists.txt | 2 +- 5 files changed, 28 insertions(+), 44 deletions(-) diff --git a/tm_driver/CMakeLists.txt b/tm_driver/CMakeLists.txt index a4c5969..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() diff --git a/tm_driver/include/tm_driver/tm_robot_state.h b/tm_driver/include/tm_driver/tm_robot_state.h index 93bb3e5..fa7b8ec 100755 --- a/tm_driver/include/tm_driver/tm_robot_state.h +++ b/tm_driver/include/tm_driver/tm_robot_state.h @@ -8,6 +8,7 @@ #include #include #include +#include #include "tm_driver_utilities.h" struct TmRobotStateData @@ -39,14 +40,14 @@ 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]; }; @@ -174,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 43816f0..81f072b 100644 --- a/tm_driver/src/tm_command.cpp +++ b/tm_driver/src/tm_command.cpp @@ -81,13 +81,16 @@ 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)); + // 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 << vel_mm << "," << acct_ms << "," << blend_percent << ","; + ss << std::setprecision(precision) << vel_mm << "," << acct_ms << "," << blend_percent << ","; ss << std::boolalpha < Date: Mon, 27 Oct 2025 11:21:56 +0100 Subject: [PATCH 13/14] Fixes multithread concern with robot name --- tm_driver/include/tm_driver/tm_robot_state.h | 1 + tm_driver/src/tm_robot_state.cpp | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/tm_driver/include/tm_driver/tm_robot_state.h b/tm_driver/include/tm_driver/tm_robot_state.h index fa7b8ec..7c60e87 100755 --- a/tm_driver/include/tm_driver/tm_robot_state.h +++ b/tm_driver/include/tm_driver/tm_robot_state.h @@ -275,4 +275,5 @@ class TmRobotState void set_receive_state(TmCommRC state); void update_tm_robot_publish_state(); void print(); + mutable std::mutex _deserialize_mtx; }; diff --git a/tm_driver/src/tm_robot_state.cpp b/tm_driver/src/tm_robot_state.cpp index 0104828..e6c8125 100755 --- a/tm_driver/src/tm_robot_state.cpp +++ b/tm_driver/src/tm_robot_state.cpp @@ -307,7 +307,10 @@ size_t TmRobotState::_deserialize(const char *data, size_t size) } multiThreadCache.set_catch_data(tmRobotStateDataFromEthernet); - tmRobotStateDataToPublish = *_data_table->get_rsd(); + { + std::lock_guard lck(_deserialize_mtx); + tmRobotStateDataToPublish = *_data_table->get_rsd(); + } static bool print_model = true; const auto model = robot_model(); @@ -331,6 +334,7 @@ void TmRobotState::set_receive_state(TmCommRC state){ std::optional TmRobotState::robot_model() const { + std::lock_guard lck(_deserialize_mtx); 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; From 77534d5717570bb6805eb5ca2786bbd1e99336d2 Mon Sep 17 00:00:00 2001 From: Diego Serrapio Date: Mon, 27 Oct 2025 15:07:53 +0100 Subject: [PATCH 14/14] Adds more mutexes for safe multithreading --- tm_driver/include/tm_driver/tm_robot_state.h | 138 ++++++++++++++----- 1 file changed, 100 insertions(+), 38 deletions(-) diff --git a/tm_driver/include/tm_driver/tm_robot_state.h b/tm_driver/include/tm_driver/tm_robot_state.h index 7c60e87..829bd8a 100755 --- a/tm_driver/include/tm_driver/tm_robot_state.h +++ b/tm_driver/include/tm_driver/tm_robot_state.h @@ -91,109 +91,171 @@ class TmRobotState ~TmRobotState(); public: - unsigned char is_linked() { return tmRobotStateDataToPublish.is_linked; } - unsigned char has_error() { return tmRobotStateDataToPublish.has_error; } - bool is_data_table_correct() { return isDataTableCorrect; } - unsigned char is_project_running() { return tmRobotStateDataToPublish.is_proj_running; } - unsigned char is_project_paused() { return tmRobotStateDataToPublish.is_proj_paused; } - unsigned char is_safeguard_A() { return tmRobotStateDataToPublish.is_safeguard_A_triggered; } - 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; } + unsigned char is_linked() const { + std::lock_guard lck(_deserialize_mtx); + return tmRobotStateDataToPublish.is_linked; + } + unsigned char has_error() const { + std::lock_guard lck(_deserialize_mtx); + return tmRobotStateDataToPublish.has_error; + } + bool is_data_table_correct() const { + std::lock_guard lck(_deserialize_mtx); + return isDataTableCorrect; + } + unsigned char is_project_running() const { + std::lock_guard lck(_deserialize_mtx); + return tmRobotStateDataToPublish.is_proj_running; + } + unsigned char is_project_paused() const { + std::lock_guard lck(_deserialize_mtx); + return tmRobotStateDataToPublish.is_proj_paused; + } + unsigned char is_safeguard_A() const { + std::lock_guard lck(_deserialize_mtx); + return tmRobotStateDataToPublish.is_safeguard_A_triggered; + } + unsigned char is_EStop() const { + std::lock_guard lck(_deserialize_mtx); + return tmRobotStateDataToPublish.is_ESTOP_pressed; + } + unsigned char camera_light() const { + std::lock_guard lck(_deserialize_mtx); + return tmRobotStateDataToPublish.camera_light; + } // R/W + int error_code() const { + std::lock_guard lck(_deserialize_mtx); + return tmRobotStateDataToPublish.error_code; + } std::optional robot_model() const; std::optional is_model_s() const; - std::string error_content() { return ""; } + std::string error_content() const { return ""; } - std::vector flange_pose() { + std::vector flange_pose() const { + std::lock_guard lck(_deserialize_mtx); std::vector flangePose; flangePose.assign(6, 0.0); si_pose(flangePose, tmRobotStateDataToPublish.flange_pose, 6); return flangePose; } - std::vector joint_angle(){ + std::vector joint_angle() const { + std::lock_guard lck(_deserialize_mtx); std::vector jointAngle; jointAngle.assign(6, 0.0); jointAngle = rads(tmRobotStateDataToPublish.joint_angle, 6); return jointAngle; } - std::vector tool_pose(){ + std::vector tool_pose() const { + std::lock_guard lck(_deserialize_mtx); std::vector toolPose; toolPose.assign(6, 0.0); si_pose(toolPose, tmRobotStateDataToPublish.tool_pose, 6); return toolPose; } - std::vector tcp_force_vec(){ + std::vector tcp_force_vec() const { + std::lock_guard lck(_deserialize_mtx); std::vector tcpForceVec; tcpForceVec.assign(3, 0.0); for (int i = 0; i < 3; ++i) { tcpForceVec[i] = double(tmRobotStateDataToPublish.tcp_force_vec[i]); } return tcpForceVec; } - double tcp_force() { + double tcp_force() const { + std::lock_guard lck(_deserialize_mtx); return tmRobotStateDataToPublish.tcp_force; } - std::vector tcp_speed_vec(){ + std::vector tcp_speed_vec() const { + std::lock_guard lck(_deserialize_mtx); std::vector tcpSpeedVec; tcpSpeedVec.assign(6, 0.0); si_pose(tcpSpeedVec, tmRobotStateDataToPublish.tcp_speed_vec, 6); return tcpSpeedVec; } - double tcp_speed() { return tmRobotStateDataToPublish.tcp_speed; } - std::vector joint_speed(){ + double tcp_speed() const + { + std::lock_guard lck(_deserialize_mtx); + return tmRobotStateDataToPublish.tcp_speed; + } + std::vector joint_speed() const { + std::lock_guard lck(_deserialize_mtx); std::vector jointSpeed; jointSpeed.assign(6, 0.0); jointSpeed = rads( tmRobotStateDataToPublish.joint_speed, 6); return jointSpeed; - } - std::vector joint_torque(){ + } + + std::vector joint_torque() const { + std::lock_guard lck(_deserialize_mtx); std::vector jointTorque; jointTorque.assign(6, 0.0); jointTorque = meters( tmRobotStateDataToPublish.joint_torque, 6); return jointTorque; } - std::vector joint_torque_average(){ + std::vector joint_torque_average() const { + std::lock_guard lck(_deserialize_mtx); std::vector jointTorqueAverage; jointTorqueAverage.assign(6, 0.0); jointTorqueAverage = meters( tmRobotStateDataToPublish.joint_torque_average, 6); return jointTorqueAverage; } - std::vector joint_torque_min() { + std::vector joint_torque_min() const { + std::lock_guard lck(_deserialize_mtx); std::vector jointTorqueMin; jointTorqueMin.assign(6, 0.0); jointTorqueMin = meters( tmRobotStateDataToPublish.joint_torque_min, 6); return jointTorqueMin; } - std::vector joint_torque_max() { + std::vector joint_torque_max() const { + std::lock_guard lck(_deserialize_mtx); std::vector jointTorqueMax; jointTorqueMax.assign(6, 0.0); jointTorqueMax = meters( tmRobotStateDataToPublish.joint_torque_max, 6); return jointTorqueMax; } - int32_t project_speed() { return tmRobotStateDataToPublish.proj_speed; } - int ma_mode() { return tmRobotStateDataToPublish.ma_mode; } - unsigned char stick_play_pause() { return tmRobotStateDataToPublish.stick_play_pause; } // R/W - int32_t robot_light() { return tmRobotStateDataToPublish.robot_light; } + int32_t project_speed() const { + std::lock_guard lck(_deserialize_mtx); + return tmRobotStateDataToPublish.proj_speed; + } + int ma_mode() const { + std::lock_guard lck(_deserialize_mtx); + return tmRobotStateDataToPublish.ma_mode; + } + unsigned char stick_play_pause() const { + std::lock_guard lck(_deserialize_mtx); + return tmRobotStateDataToPublish.stick_play_pause; + } + int32_t robot_light() const { + std::lock_guard lck(_deserialize_mtx); + return tmRobotStateDataToPublish.robot_light; + } - std::vector ctrller_DO(){ + std::vector ctrller_DO() const { + std::lock_guard lck(_deserialize_mtx); return std::vector (tmRobotStateDataToPublish.ctrller_DO.begin(), tmRobotStateDataToPublish.ctrller_DO.end()); } - std::vector ctrller_DI() { + std::vector ctrller_DI() const { + std::lock_guard lck(_deserialize_mtx); return std::vector (tmRobotStateDataToPublish.ctrller_DI.begin(), tmRobotStateDataToPublish.ctrller_DI.end()); } - std::vector ctrller_AO(){ + std::vector ctrller_AO() const { + std::lock_guard lck(_deserialize_mtx); return std::vector (tmRobotStateDataToPublish.ctrller_AO.begin(), tmRobotStateDataToPublish.ctrller_AO.end()); } - std::vector ctrller_AI(){ - return std::vector (tmRobotStateDataToPublish.ctrller_AI.begin(), tmRobotStateDataToPublish.ctrller_AI.end()); - } - std::vector ee_DO(){ - return std::vector (tmRobotStateDataToPublish.ee_DO.begin(), tmRobotStateDataToPublish.ee_DO.end()); + std::vector ctrller_AI() const { + std::lock_guard lck(_deserialize_mtx); + return std::vector (tmRobotStateDataToPublish.ctrller_AI.begin(), tmRobotStateDataToPublish.ctrller_AI.end()); + } + std::vector ee_DO() const { + std::lock_guard lck(_deserialize_mtx); + return std::vector (tmRobotStateDataToPublish.ee_DO.begin(), tmRobotStateDataToPublish.ee_DO.end()); } - std::vector ee_DI(){ + std::vector ee_DI() const { + std::lock_guard lck(_deserialize_mtx); return std::vector (tmRobotStateDataToPublish.ee_DI.begin(), tmRobotStateDataToPublish.ee_DI.end()); } - std::vector ee_AI(){ - return std::vector (tmRobotStateDataToPublish.ee_AI.begin(), tmRobotStateDataToPublish.ee_AI.end()); + std::vector ee_AI() const { + std::lock_guard lck(_deserialize_mtx); + return std::vector (tmRobotStateDataToPublish.ee_AI.begin(), tmRobotStateDataToPublish.ee_AI.end()); }