Adds mods required by mc-backend

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()

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<double> &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<double> &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<double> &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 ([email protected]) */
+    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<double> &tcp);
+    static std::string change_tcp(const std::vector<double> &tcp, double weight);
+    static std::string change_tcp(const std::vector<double> &tcp, double weight, const std::vector<double> &inertia);
 };

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<double> &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<double> &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<double> &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<double> &tcp, const std::string &id = "ChangeTCP");
+	bool change_tcp(const std::vector<double> &tcp, double weight, const std::string &id = "ChangeTCP");
+	bool change_tcp(const std::vector<double> &tcp, double weight, const std::vector<double> &inertia, const std::string &id = "ChangeTCP");
 };

tm_driver/include/tm_driver/tm_robot_state.h

@@ -7,6 +7,8 @@
 #include <string>
 #include <mutex>
 #include <functional>
+#include <optional>
+#include <span>
 #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<unsigned char, 16> ctrller_DO{0};
+	std::array<unsigned char, 16> ctrller_DI{0};
+	std::array<float, 2> ctrller_AO{0};
+	std::array<float, 2> ctrller_AI{0};
+	std::array<unsigned char, 4> ee_DO{0};
+	std::array<unsigned char, 4> ee_DI{0};
+    std::array<float, 2> ee_AO{0};
+    std::array<float, 2> 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<std::string> robot_model() const;
+	std::optional<bool> is_model_s() const;
 	std::string error_content() { return ""; }
 
 	std::vector<double> flange_pose() { 
@@ -170,47 +175,27 @@ class TmRobotState
 	int32_t robot_light() { return tmRobotStateDataToPublish.robot_light; }
 
 	std::vector<unsigned char> ctrller_DO(){
-		std::vector<unsigned char>  ctrllerDO;
-		ctrllerDO.assign(8, 0); 
-		for (int i = 0; i < 8; ++i) { ctrllerDO[i] = tmRobotStateDataToPublish.ctrller_DO[i]; }
-		return ctrllerDO;
+		return std::vector<unsigned char>  (tmRobotStateDataToPublish.ctrller_DO.begin(), tmRobotStateDataToPublish.ctrller_DO.end());
 	}
 	std::vector<unsigned char> ctrller_DI() {
-		std::vector<unsigned char>  ctrllerDI;
-		ctrllerDI.assign(8, 0);
-		for (int i = 0; i < 8; ++i) { ctrllerDI[i] = tmRobotStateDataToPublish.ctrller_DI[i]; }
-	    return ctrllerDI; 
+	    return std::vector<unsigned char>  (tmRobotStateDataToPublish.ctrller_DI.begin(), tmRobotStateDataToPublish.ctrller_DI.end());
 	}
 	std::vector<float> ctrller_AO(){
-		std::vector<float>  ctrllerAO;
-		ctrllerAO.assign(1, 0.0);
-		for (int i = 0; i < 1; ++i) { ctrllerAO[i] = tmRobotStateDataToPublish.ctrller_AO[i]; }
-	    return ctrllerAO; 
+		return std::vector<float>  (tmRobotStateDataToPublish.ctrller_AO.begin(), tmRobotStateDataToPublish.ctrller_AO.end());
 	}
 	std::vector<float> ctrller_AI(){
-        std::vector<float>  ctrllerAI;
-		ctrllerAI.assign(2, 0.0);
-		for (int i = 0; i < 2; ++i) { ctrllerAI[i] = tmRobotStateDataToPublish.ctrller_AI[i]; }
-	    return ctrllerAI; 
-	}
+        return std::vector<float>  (tmRobotStateDataToPublish.ctrller_AI.begin(), tmRobotStateDataToPublish.ctrller_AI.end());
+    }
 	std::vector<unsigned char> ee_DO(){
-		std::vector<unsigned char>  eeDO;
-		eeDO.assign(4, 0);
-		for (int i = 0; i < 4; ++i) { eeDO[i] = tmRobotStateDataToPublish.ee_DO[i]; }
-	    return eeDO; 
+        return std::vector<unsigned char>  (tmRobotStateDataToPublish.ee_DO.begin(), tmRobotStateDataToPublish.ee_DO.end());
 	}
 	std::vector<unsigned char> ee_DI(){
-		std::vector<unsigned char>  eeDI;
-		eeDI.assign(4, 0);
-		for (int i = 0; i < 4; ++i) { eeDI[i] = tmRobotStateDataToPublish.ee_DI[i]; }
-	    return eeDI; 
-	}	
+		return std::vector<unsigned char>  (tmRobotStateDataToPublish.ee_DI.begin(), tmRobotStateDataToPublish.ee_DI.end());
+	}
 	std::vector<float> ee_AI(){
-        std::vector<float>  eeAI;
-		eeAI.assign(1, 0.0);
-		for (int i = 0; i < 1; ++i) { eeAI[i] = tmRobotStateDataToPublish.ee_AI[i]; }
-	    return eeAI; 
+        return std::vector<float>  (tmRobotStateDataToPublish.ee_AI.begin(), tmRobotStateDataToPublish.ee_AI.end());
 	}
+
 
     TmCommRC get_receive_state(){return _receive_state;}
 

tm_driver/src/tm_command.cpp

@@ -1,3 +1,4 @@
+#include <ios>
 #ifdef NO_INCLUDE_DIR
 #include "tm_command.h"
 #else
@@ -78,16 +79,36 @@ std::string TmCommand::set_tool_pose_PTP(const std::vector<double> &pose,
 }
 std::string TmCommand::set_tool_pose_Line(const std::vector<double> &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<int>( std::round(1000.0 * vel));
+
+    double vel_mm = 1000.0 * vel;
+	int acct_ms = static_cast<int>( 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 <<fine_goal << ")";
+	return ss.str();
+}
+
+std::string TmCommand::set_tool_pose_Line_rel(const std::vector<double> &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 <<fine_goal << ")";
+	ss << std::boolalpha << fine_goal << ")";
 	return ss.str();
 }
 
@@ -181,3 +202,47 @@ std::string TmCommand::set_vel_mode_target(VelMode mode, const std::vector<doubl
 	}
 	return ss.str();
 }
+
+std::string TmCommand::set_tcp_speed(uint32_t linear_speed, uint32_t rotational_speed, bool is_model_s)
+{
+	std::stringstream ss;
+	const std::string cmd = is_model_s ? "SetTCPSpeedLimit(true," : "SetTCPSpeed(";
+    ss << cmd << linear_speed << "," << rotational_speed << ")";
+	return ss.str();
+}
+
+std::string TmCommand::change_tcp(const std::string &toolname)
+{
+	std::stringstream ss;
+	ss << "ChangeTCP(\"" << toolname << "\")";
+	return ss.str();
+}
+std::string TmCommand::change_tcp(const std::vector<double> &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<double> &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<double> &tcp, double weight, const std::vector<double> &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();
+}