ros机械臂

#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;
}