52 lines
1.5 KiB
C++
52 lines
1.5 KiB
C++
#include <iostream>
|
|
#include "ros/ros.h"
|
|
#include "poppy_ros/InverseKinematics.h"
|
|
#include "geometry_msgs/Twist.h"
|
|
|
|
int main(int argc, char** argv)
|
|
{
|
|
if (argc != 4)
|
|
{
|
|
std::cerr << "Usage: rosrun poppy_ros ik_client <x_target> <y_target> <z_target>" << std::endl;
|
|
return 1;
|
|
}
|
|
|
|
ros::init(argc, argv, "ik_client");
|
|
ros::NodeHandle nh;
|
|
ros::ServiceClient ikClient = nh.serviceClient<poppy_ros::InverseKinematics>("ik");
|
|
|
|
poppy_ros::InverseKinematics srv;
|
|
srv.request.x = std::stof(argv[1]);
|
|
srv.request.y = std::stof(argv[2]);
|
|
srv.request.z = std::stof(argv[3]);
|
|
|
|
if (ikClient.call(srv))
|
|
{
|
|
ROS_INFO("Received joint values from ik_server.");
|
|
|
|
ros::Publisher jointCmdPublisher = nh.advertise<geometry_msgs::Twist>("/joint_cmd", 1);
|
|
geometry_msgs::Twist jointCmdMsg;
|
|
|
|
// Assign the received joint values to the Twist message
|
|
jointCmdMsg.linear.x = srv.response.joint1;
|
|
jointCmdMsg.linear.y = srv.response.joint2;
|
|
jointCmdMsg.linear.z = srv.response.joint3;
|
|
jointCmdMsg.angular.x = srv.response.joint4;
|
|
jointCmdMsg.angular.y = srv.response.joint5;
|
|
jointCmdMsg.angular.z = srv.response.joint6;
|
|
|
|
// Publish joint values to the topic /joint_cmd
|
|
jointCmdPublisher.publish(jointCmdMsg);
|
|
|
|
ROS_INFO("Published joint values to /joint_cmd.");
|
|
}
|
|
else
|
|
{
|
|
ROS_ERROR("Failed to call ik service.");
|
|
return 1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|