I removed some printing code, but the structure is intact. Since I'm using specific libraries, I doubt you can instantly compile it.
Thanks for helping.
Code:
#include "ros/ros.h"
#include "gazebo_msgs/LinkStates.h"
using namespace std;
double twist_lin_x, twist_lin_y, twist_lin_z,
pos_x, pos_y, pos_z,
twist_ang_x, twist_ang_y, twist_ang_z,
or_x, or_y, or_z, or_w,
effort = 0.0;
int loop = 0;
std_msgs::Float64 msg;
class Pera
{
public:
Pera(ros::NodeHandle* _n):pos_x(0){
//providing callback (a member) and a state (this)
n = *_n;
ROS_INFO("Subscribing to link_states");
ls_sub = n.subscribe("/gazebo/link_states", 1, &Pera::callback, this);
ROS_INFO("Subscribed");
}
void callback(gazebo_msgs::LinkStates msgs_ls){
ROS_INFO("-------------------");
ROS_INFO("Position x hand : %.15lf", msgs_ls.pose[43].position.x);
pos_x = msgs_ls.pose[43].position.x;
}
void run(){
ROS_INFO("Declaring the right hand");
std::string right_hand = (std::string)"amigo::hand_right";
ROS_INFO("Advertising to set_link_state");
ros::Publisher ls_pub = n.advertise<gazebo_msgs::LinkState>("/gazebo/set_link_state", 1);
ROS_INFO("Creating instance");
gazebo_msgs::LinkState linkState;
ROS_INFO("Assigning link");
linkState.link_name = right_hand;
ROS_INFO("Assigning reference");
linkState.reference_frame = right_hand;
while (ros::ok())
{
ROS_INFO("Assigning pos x: %lf", pos_x);
linkState.pose.position.x = pos_x;
}
}
protected:
// state
ros::NodeHandle n;
ros::Subscriber ls_sub;
double pos_x;
};
int main(int argc, char **argv)
{
ROS_INFO("init");
ros::init(argc, argv, "arm_joint_ik_receiver");
ros::NodeHandle n;
ROS_INFO("Calling PERA");
Pera pera(&n);
pera.run();
ROS_INFO("Terminating");
return 0;
}