code update

ik_client.cpp

@@ -42,22 +42,9 @@ 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));
@@ -67,16 +54,32 @@ int main(int argc, char** argv)
     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
+    {
+        // 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_INFO("Robot has reached the target position. Gripper remains open.");
+}
+else
+{
     ROS_ERROR("Failed to call the service ik!");
     return 1;
-    }
-
-    return 0;
+}
+
+
+    return 0;
 }