clovis.arsac
/
Ros2_Lab3_AdvancedRobotics
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Ajouter tower_builder

This commit is contained in:
Clovis ARSAC 2025-12-17 11:42:59 +01:00
parent 3a5974f9f2
commit e99a01b7c1
1 changed files with 167 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

167
tower_builder Normal file
View File

@ -0,0 +1,167 @@
#include "math.h"
#include <chrono>
#include <cstdlib>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "advrobotics_lab3_interfaces/srv/invkin.hpp"
#include "advrobotics_lab3_interfaces/msg/joints.hpp"
#include "advrobotics_lab3_interfaces/msg/gripper.hpp"
#include "Kinematics.h"
using namespace std::chrono_literals;
double deg2rad(double angle)
{
return -angle / 180.0 * M_PI;
}
int main(int argc, char** argv)
{
// inits ROS2
rclcpp::init(argc, argv);
// creates the client node
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("ik_client");
rclcpp::Client<advrobotics_lab3_interfaces::srv::Invkin>::SharedPtr client =
node->create_client<advrobotics_lab3_interfaces::srv::Invkin>("invkin");
// creates a publisher to move the robot joints
auto publisher = node->create_publisher<advrobotics_lab3_interfaces::msg::Joints>("joint_cmd", 10);
auto gripper = node->create_publisher<advrobotics_lab3_interfaces::msg::Gripper>("gripper_cmd", 10);
// opens the gripper
// creates a message to publish
advrobotics_lab3_interfaces::msg::Gripper gripperCmdMsg;
gripperCmdMsg.gripper = -40.0;
// publises the message to move the robot joints
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Publish to gripper_cmd topic...");
gripper->publish(gripperCmdMsg);
rclcpp::Rate rate(1);
rate.sleep();
// creates the request from the input args
auto request = std::make_shared<advrobotics_lab3_interfaces::srv::Invkin::Request>();
request->x = 9.0;
request->y = 0.0;
request->z = 0.0;
// connects to the service
while (!client->wait_for_service(1s))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "[ERROR] Interrupted while waiting for the service. Exiting.");
return 0;
}
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "[WARNING] service not available, waiting again...");
}
// sends the request
auto result = client->async_send_request(request);
// waitw for the result.
if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)
{
// gets the result
auto response = result.get();
// displays i/o values
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "IK: (x,y,z) --> (J1, J2, j3)");
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "(9.0, 0.0, 0.0) --> (%f, %f, %f)", rad2deg(response->q1), rad2deg(response->q2), rad2deg(response->q3));
// creates a message to publish
advrobotics_lab3_interfaces::msg::Joints jointCmdMsg;
jointCmdMsg.q1 = rad2deg(response->q1);
jointCmdMsg.q2 = rad2deg(response->q2);
jointCmdMsg.q3 = rad2deg(response->q3);
// publises the message to move the robot joints
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Publish to joint_cmd topic...");
publisher->publish(jointCmdMsg);
rclcpp::Rate rate(1);
rate.sleep();
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Done!");
}
else
{
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "[ERROR] Failed to call service invkin");
}
// creates a message to publish
gripperCmdMsg.gripper = 10.0;
// publises the message to move the robot joints
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Publish to gripper_cmd topic...");
gripper->publish(gripperCmdMsg);
//rclcpp::Rate rate(1);
rate.sleep();
// creates the request from the input args
//auto request = std::make_shared<advrobotics_lab3_interfaces::srv::Invkin::Request>();
request->x = 9.0;
request->y = 4.0;
request->z = 0.0;
// connects to the service
while (!client->wait_for_service(1s))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "[ERROR] Interrupted while waiting for the service. Exiting.");
return 0;
}
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "[WARNING] service not available, waiting again...");
}
// sends the request
result = client->async_send_request(request);
// waitw for the result.
if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)
{
// gets the result
auto response = result.get();
// displays i/o values
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "IK: (x,y,z) --> (J1, J2, j3)");
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "(9.0,4.0, 0.0) --> (%f, %f, %f)", rad2deg(response->q1), rad2deg(response->q2), rad2deg(response->q3));
// creates a message to publish
advrobotics_lab3_interfaces::msg::Joints jointCmdMsg;
jointCmdMsg.q1 = rad2deg(response->q1);
jointCmdMsg.q2 = rad2deg(response->q2);
jointCmdMsg.q3 = rad2deg(response->q3);
// publises the message to move the robot joints
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Publish to joint_cmd topic...");
publisher->publish(jointCmdMsg);
rclcpp::Rate rate(1);
rate.sleep();
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Done!");
}
else
{
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "[ERROR] Failed to call service invkin");
}
// opens the gripper
// creates a message to publish
gripperCmdMsg.gripper = -40.0;
// publises the message to move the robot joints
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Publish to gripper_cmd topic...");
gripper->publish(gripperCmdMsg);
//rclcpp::Rate rate(1);
rate.sleep();
rclcpp::shutdown();
return 0;
}