82 lines
2.3 KiB
C++
82 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]);
|
||
|
||
// 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))
|
||
{
|
||
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);
|
||
|
||
jointCmdPublisher.publish(jointCmdMsg);
|
||
ros::spinOnce();
|
||
ros::Rate loopRate(_fps);
|
||
loopRate.sleep();
|
||
}
|
||
else
|
||
{
|
||
ROS_ERROR("Failed to call the service ik!");
|
||
return 1;
|
||
}
|
||
|
||
return 0;
|
||
} |