From 08abb0d06f07bcf401cf6ca2b688fc0bdcd1d5a3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E2=80=9CSRONG?= <“ougy.srong@ecam.fr”>
Date: Wed, 11 Dec 2024 17:15:32 +0100
Subject: [PATCH] code

---
 ik_client.cpp | 111 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 111 insertions(+)
 create mode 100644 ik_client.cpp

diff --git a/ik_client.cpp b/ik_client.cpp
new file mode 100644
index 0000000..37fcc95
--- /dev/null
+++ b/ik_client.cpp
@@ -0,0 +1,111 @@
+#include "math.h"
+
+#include "ros/ros.h"
+#include "advrobotics_lab3/ikpoppy.h"
+#include "advrobotics_lab3/joints.h"
+#include "advrobotics_lab3/gripper.h" // Include the gripper message header
+
+#include "Kinematics.h"
+
+float _fps = 10.0;
+
+double deg2rad(double angle)
+{
+    return -angle / 180.0 * M_PI;
+}
+
+double rad2deg(double rad)
+{
+    return rad * 180.0 / M_PI;
+}
+
+// Function to calculate the Euclidean distance
+double calculateDistance(double x1, double y1, double z1, double x2, double y2, double z2)
+{
+    return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2) + pow(z2 - z1, 2));
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "ik_client");
+
+    if (argc != 4)
+    {
+        ROS_INFO("Usage: rosrun advrobotics_lab3 ik_client x y z");
+        ROS_INFO("Example: rosrun advrobotics_lab3 ik_client 6 9 0");
+        return 1;
+    }
+
+    ros::NodeHandle nh;
+    ros::ServiceClient client = nh.serviceClient<advrobotics_lab3::ikpoppy>("ik");
+
+    ros::Publisher jointCmdPublisher = nh.advertise<advrobotics_lab3::joints>("joint_cmd", 1);
+    ros::Publisher gripperCmdPublisher = nh.advertise<advrobotics_lab3::gripper>("gripper_cmd", 1);
+
+    // Parse the target position
+    double targetX = atof(argv[1]);
+    double targetY = atof(argv[2]);
+    double targetZ = atof(argv[3]);
+
+    advrobotics_lab3::ikpoppy srv;
+    srv.request.x = targetX;
+    srv.request.y = targetY;
+    srv.request.z = targetZ;
+
+    // Call the IK service to get the target joint positions
+    if (!client.call(srv))
+    {
+        ROS_ERROR("Failed to call the service ik!");
+        return 1;
+    }
+
+    ROS_INFO("IK: (x, y, z) --> (J1, J2, J3)");
+    ROS_INFO("(%f, %f, %f) --> (%f, %f, %f)", targetX, targetY, targetZ,
+             rad2deg(srv.response.q1), rad2deg(srv.response.q2), rad2deg(srv.response.q3));
+
+    advrobotics_lab3::joints jointCmdMsg;
+    jointCmdMsg.q1 = rad2deg(srv.response.q1);
+    jointCmdMsg.q2 = rad2deg(srv.response.q2);
+    jointCmdMsg.q3 = rad2deg(srv.response.q3);
+
+    // Gripper command
+    advrobotics_lab3::gripper gripperCmd;
+
+    // Initialize robot's current position
+    double currentX = 0.0, currentY = 0.0, currentZ = 0.0; // Assuming starting at (0, 0, 0)
+
+    ros::Rate loopRate(10); // Loop at 10 Hz
+    double distanceThreshold = 1.0; // Threshold for "near target" in units
+
+    while (ros::ok())
+    {
+        // Calculate the distance to the target
+        double distanceToTarget = calculateDistance(currentX, currentY, currentZ, targetX, targetY, targetZ);
+
+        // Publish joint commands
+        jointCmdPublisher.publish(jointCmdMsg);
+
+        // If the robot is near the target, open the gripper
+        if (distanceToTarget <= distanceThreshold)
+        {
+            gripperCmd.gripper = -40.0; // Open position
+            gripperCmdPublisher.publish(gripperCmd);
+            ROS_INFO("Gripper opened as robot reached near the target position.");
+            break;
+        }
+
+        // Simulate robot progress (for demonstration purposes, adjust this for real-world feedback)
+        currentX += (targetX - currentX) * 0.1; // Simulated step toward target
+        currentY += (targetY - currentY) * 0.1;
+        currentZ += (targetZ - currentZ) * 0.1;
+
+        ROS_INFO("Current Position: (%f, %f, %f), Distance to Target: %f", currentX, currentY, currentZ, distanceToTarget);
+
+        ros::spinOnce();
+        loopRate.sleep();
+    }
+
+    ROS_INFO("Robot has reached the target position.");
+    return 0;
+}
+