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 (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<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");
 };