85 lines
2.3 KiB
C++
85 lines
2.3 KiB
C++
#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;
|
|
}
|
|
|
|
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);
|
|
// Add gripper publisher
|
|
|
|
advrobotics_lab3::ikpoppy srv;
|
|
srv.request.x = atof(argv[1]);
|
|
srv.request.y = atof(argv[2]);
|
|
srv.request.z = atof(argv[3]);
|
|
|
|
// Send the target position
|
|
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
|
|
{
|
|
// 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();
|
|
loopRate.sleep();
|
|
}
|
|
|
|
ROS_INFO("Robot has reached the target position. Gripper remains open.");
|
|
}
|
|
else
|
|
{
|
|
ROS_ERROR("Failed to call the service ik!");
|
|
return 1;
|
|
}
|
|
|
|
|
|
return 0;
|
|
} |