while (!client->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), "Service not available, waiting again...");
}
auto result = client->async_send_request(request);
// Wait for the result.
if (rclcpp::spin_until_future_complete(node, result) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR_STREAM(rclcpp::get_logger("rclcpp"), "Failed to call service");
return;
}
if (result.get()->ok) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"),"OK");
} else {
RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"),"Not OK");
}
}
int main() {
rclcpp::init(0, nullptr);
auto node = std::make_shared<rclcpp::Node>("demo_send_script");
auto client = node->create_client<tm_msgs::srv::SendScript>("send_script");
// Initial position
double target_position[] = {530, -400, 440, -180, 0, 90};
// Move to initial position
std::string motion_cmd = "PTP(\"CPP\"," + std::to_string(target_position[0]) + "," +
std::to_string(target_position[1]) + "," + std::to_string(target_position[2]) +
",-180,0,90,35,200,0,false)";
send_cmd(motion_cmd, node, client);
std::this_thread::sleep_for(std::chrono::milliseconds(15000)); // Wait to reach target position
// Six specified positions
double positions[][6] = {
{530, -400, 440, -180, 0, 90},
{520, -300, 560, -180, 0, 90},
{480, -60, 650, -180, 0, 90},
{480, 120, 660, -180, 0, 90},
{510, 315, 580, -180, 0, 90},
{600, 370, 370, -180, 0, 90}
};
// Calculate the distance between each point and send the corresponding movement command
for (int i = 0; i < 5; ++i) {
// Calculate the differences in the x, y, and z directions
double dx = positions[i+1][0] - positions[i][0];
double dy = positions[i+1][1] - positions[i][1];
double dz = positions[i+1][2] - positions[i][2];
// Calculate the velocity for each direction
double velocity_x = dx / 5000;
double velocity_y = dy / 5000;
double velocity_z = dz / 5000;
// Set continuous velocity command
std::string velocity_cmd = "SetContinueVLine(" + std::to_string(velocity_x) + ", " + std::to_string(velocity_y) + ", " + std::to_string(velocity_z) + ", 0, 0, 0)";
send_cmd(velocity_cmd, node, client);
// Set the movement duration
std::string distance_cmd = "ContinueVLine(5000, 5000)";
send_cmd(distance_cmd, node, client);
std::this_thread::sleep_for(std::chrono::milliseconds(5000)); // Wait for 5 seconds
}
rclcpp::shutdown();
return 0;
}
But the results I obtained are very different from what I expected.
For example.
the first point should arrive to { 520 , -300 , 560 , -180 , 0 , 90 }
but the result i get from TM-controller { 532 , -377 , 527 , -180 , 0 , 90 }
I have no idea where it went wrong.
What should I do to fix it?
I tried to use Listen node in TMFlow connected to a ubuntu22.04 to use the velocity node. here is my code:
include "rclcpp/rclcpp.hpp"
include "tm_msgs/srv/send_script.hpp"
include
include
include
include
void send_cmd(const std::string& cmd, rclcpp::Node::SharedPtr node, rclcpp::Client::SharedPtr client) {
auto request = std::make_shared();
request->id = "demo";
request->script = cmd;
}
int main() { rclcpp::init(0, nullptr);
}
But the results I obtained are very different from what I expected. For example. the first point should arrive to { 520 , -300 , 560 , -180 , 0 , 90 } but the result i get from TM-controller { 532 , -377 , 527 , -180 , 0 , 90 } I have no idea where it went wrong. What should I do to fix it?