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 <ios>
 #ifdef NO_INCLUDE_DIR
 #include "tm_command.h"
 #else
@@ -80,8 +81,8 @@ 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 = int(1000.0 * vel);
-	int acct_ms = int(1000.0 * acc_time);
+	int vel_mm = static_cast<int>( std::round(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\",";
@@ -202,7 +203,9 @@ std::string TmCommand::set_vel_mode_target(VelMode mode, const std::vector<doubl
 std::string TmCommand::set_tcp_speed(uint32_t linear_speed, uint32_t rotational_speed)
 {
 	std::stringstream ss;
-	ss << "SetTCPSpeed(" << linear_speed << "," << rotational_speed << ")";
+	ss << "SetTCPSpeedLimit(";
+    ss << std::boolalpha << true;
+    ss << "," << linear_speed << "," << rotational_speed << ")";
 	return ss.str();
 }
 
diff --git a/tm_driver/src/tm_robot_state.cpp b/tm_driver/src/tm_robot_state.cpp
index 4312566..2c56ca1 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<std::string, Item> _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 };
@@ -111,6 +111,7 @@ class TmDataTable
 	std::map<std::string, Item>  & get() { return _item_map; }
 	std::map<std::string, Item>::iterator find(const std::string &name) { return _item_map.find(name); }
 	std::map<std::string, Item>::iterator end() { return _item_map.end(); }
+	const TmRobotStateData* get_rsd() { return _rsd; }
 };
 
 TmRobotState::TmRobotState()
@@ -306,11 +307,14 @@ 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;
 	if (print_model) {
+		print_model = false;
 		auto msg = std::string("Robot model is: ") + tmRobotStateDataFromEthernet.robot_model;
 		print_info(msg.c_str());
-		print_model = false;
+	}
+	if (boffset > size) {
 	}
 	return boffset;
 }