31 lines
630 B
C++
31 lines
630 B
C++
#include "ros/ros.h"
|
|
#include "poppy_ros/InverseKinematics.h"
|
|
|
|
bool inverseKinematicsService(poppy_ros::InverseKinematics::Request& req, poppy_ros::InverseKinematics::Response& res)
|
|
{
|
|
|
|
res.joint1 = req.x;
|
|
res.joint2 = req.y;
|
|
res.joint3 = req.z;
|
|
res.joint4 = 0.0;
|
|
res.joint5 = 0.0;
|
|
res.joint6 = 0.0;
|
|
|
|
return true;
|
|
}
|
|
|
|
int main(int argc, char** argv)
|
|
{
|
|
ros::init(argc, argv, "ik_server");
|
|
ros::NodeHandle nh;
|
|
|
|
ros::ServiceServer server = nh.advertiseService("ik", inverseKinematicsService);
|
|
|
|
ROS_INFO("ik_server is ready to receive requests.");
|
|
|
|
ros::spin();
|
|
|
|
return 0;
|
|
}
|
|
|