dorian.veloso
/
LAB3_SRONG_PHERWANI_VELOSO
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

code update

This commit is contained in:
Dorian VELOSO 2024-12-11 17:29:50 +01:00
parent 41d7664cfd
commit b997c68b70
1 changed files with 33 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
ik_client.cpp
View File

@ -42,41 +42,44 @@ int main(int argc, char** argv)
srv.request.y = atof(argv[2]); srv.request.y = atof(argv[2]);
srv.request.z = atof(argv[3]); 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 // 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)"); // Publish joint commands to move the robot
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);
jointCmdPublisher.publish(jointCmdMsg); 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::spinOnce();
ros::Rate loopRate(_fps);
loopRate.sleep(); 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;
} }