From b997c68b70b4fac7eee4b567d29126a8d8d841ae Mon Sep 17 00:00:00 2001
From: "dorian.veloso" <dorian.veloso@ecam.fr>
Date: Wed, 11 Dec 2024 17:29:50 +0100
Subject: [PATCH] code update

---
 ik_client.cpp | 63 +++++++++++++++++++++++++++------------------------
 1 file changed, 33 insertions(+), 30 deletions(-)

diff --git a/ik_client.cpp b/ik_client.cpp
index c7c9fcf..dc5c26c 100644
--- a/ik_client.cpp
+++ b/ik_client.cpp
@@ -42,41 +42,44 @@ int main(int argc, char** argv)
     srv.request.y = atof(argv[2]);
     srv.request.z = atof(argv[3]);
 
-    // Open the gripper before moving
-    advrobotics_lab3::gripper gripperCmd;
-    gripperCmd.gripper = -40.0; // Assuming -40.0 is the "open" position
-    ROS_INFO("Publishing gripper command: %f", gripperCmd.gripper);
-    ros::Rate rate(10); // Publish at 10 Hz
-    for (int i = 0; i < 10; ++i) // Publish for 1 second
-    {
-    	gripperCmdPublisher.publish(gripperCmd);
-    	ros::spinOnce();
-    	rate.sleep(); // Allow time for the gripper to open
-    }
-    ros::Duration(3.0).sleep(); // Allow time for the gripper to open
-
     // Send the target position
-    if (client.call(srv))
+if (client.call(srv))
+{
+    ROS_INFO("IK: (x, y, z) --> (J1, J2, J3)");
+    ROS_INFO("(%f, %f, %f) --> (%f, %f, %f)", atof(argv[1]), atof(argv[2]), atof(argv[3]),
+             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);
+
+    advrobotics_lab3::gripper gripperCmd;
+    gripperCmd.gripper = -40.0; // Open the gripper
+
+    ros::Rate loopRate(_fps); // Maintain loop rate for movement
+    for (int i = 0; i < 10; ++i) // Assuming movement takes 5 seconds at _fps = 10
     {
-        ROS_INFO("IK: (x, y, z) --> (J1, J2, J3)");
-        ROS_INFO("(%f, %f, %f) --> (%f, %f, %f)", atof(argv[1]), atof(argv[2]), atof(argv[3]),
-                 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);
-
+        // Publish joint commands to move the robot
         jointCmdPublisher.publish(jointCmdMsg);
+
+        // Continuously publish the gripper open command
+        gripperCmdPublisher.publish(gripperCmd);
+
+        ROS_INFO("Moving to target position... Step: %d", i + 1);
+
         ros::spinOnce();
-        ros::Rate loopRate(_fps);
         loopRate.sleep();
     }
-    else
-    {
-        ROS_ERROR("Failed to call the service ik!");
-        return 1;
-    }
 
-    return 0;
+    ROS_INFO("Robot has reached the target position. Gripper remains open.");
+}
+else
+{
+    ROS_ERROR("Failed to call the service ik!");
+    return 1;
+}
+
+
+    return 0;
 }
\ No newline at end of file