#include <ros/ros.h>
#include "sensor_msgs/JointState.h"
// #include "std_msgs/Float32.h"
#include "std_msgs/Float64MultiArray.h"
#include "std_msgs/Int32MultiArray.h"
ros::Publisher pub;
float pwm[4];
//回调函数处理jointstate中position数据,且发布pwm消息
void dataCallback(const sensor_msgs::JointState joint){
// myserial::PWM pwm_data;
pwm[0]=joint.position[0]*1500/3.14+1500;
pwm[1]=joint.position[1]*1500/3.14+1500;
pwm[2]=joint.position[2]*1500/3.14+1500;
pwm[3]=joint.position[3]*1500/3.14+1500;
for(int i=0;i < 4;i++){
ROS_INFO("joint%d:data is %f!",(i+1),pwm[i]);
}
for(int i=0;i < 4;i++){
ROS_INFO("joint%d:data is %f!",(i+1),joint.position[i]);
}
// pwm_data.a=pwm[0];
// pwm_data.b=pwm[1];
// pwm_data.c=pwm[2];
// pwm_data.d=pwm[3];
// pwm_data.e=pwm[4];
// pwm_data.f=pwm[5];
// pub.publish(pwm_data);
}
int main(int argc, char** argv){
//初始化节点
ros::init(argc, argv, "get_data");
//创建句柄
ros::NodeHandle nh;
//订阅moveit发布的关节信息,发布处理后的pwm信息
pub = nh.advertise<std_msgs::Int32MultiArray>("/pwm",10);
ros::Subscriber sub = nh.subscribe("/joint_states",10,dataCallback);
ros::Rate loop_rate(1);
while(ros::ok())
{
std_msgs::Int32MultiArray msg;
msg.data.resize(4);
msg.data[0] = pwm[0];
msg.data[1] = pwm[1];
msg.data[2] = pwm[2];
msg.data[3] = pwm[3];
pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}