六维力数据中去除重力、漂移力任务---ros之talker、listener、ur、坑

理论基础

见前面博文
六维力传感器参数辨识方法+matlab代码实现

talker

见前面博文
linux下读取ATI公司力传感器数据&&c++中调用c源文件&&移植到ros+问题解决
最终的talker

/**
 * 该例程将发布chatter话题，消息类型String
 */
 
#include <sstream>
#include "ros/ros.h"
#include "geometry_msgs/WrenchStamped.h"
#include <stdio.h>

#ifdef __cplusplus
extern "C"
{
#endif
#include "netft.h"
#ifdef __cplusplus
}
#endif
int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "talker");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Publisher，发布名为chatter的topic，消息类型为std_msgs::String
    ros::Publisher chatter_pub = n.advertise<geometry_msgs::WrenchStamped>("force0", 1000);

    // 设置循环的频率
    ros::Rate loop_rate(100);

    int nn = 0;

    int count = 0;

    while (ros::ok())
    {
        // 初始化std_msgs::String类型的消息
        geometry_msgs::WrenchStamped msg;
        msg.wrench.force.x = a(argc,argv)[0];
        msg.wrench.force.y = a(argc,argv)[1];
        msg.wrench.force.z = a(argc,argv)[2];
        msg.wrench.torque.x = a(argc,argv)[3];
        msg.wrench.torque.y = a(argc,argv)[4];
        msg.wrench.torque.z = a(argc,argv)[5];
        nn++;
        // 发布消息
ROS_INFO("[%f]", msg.wrench.force.x);
        chatter_pub.publish(msg);

        // 循环等待回调函数
        ros::spinOnce();

        // 按照循环频率延时
        loop_rate.sleep();
        ++count;
    }

    return 0;
}

listener

#include <ros/ros.h>  
#include "std_msgs/String.h"
#include <sstream>
#include "geometry_msgs/WrenchStamped.h"
#include "sensor_msgs/JointState.h"
#include <cmath>
class SubscribeAndPublish  
{  
public:  
  SubscribeAndPublish()  
  {  
    pub_ = n_.advertise<geometry_msgs::WrenchStamped>("/force1", 1000);  
    sub_ = n_.subscribe("force0", 1000, &SubscribeAndPublish::callback1, this);  
    sub2_ = n_.subscribe("joint_states", 1000, &SubscribeAndPublish::callback2, this);  
  }  
//由正向运动学得到变换矩阵的最后一行元素，用于计算实际的接触力
float *qianxiang(const sensor_msgs::JointState::ConstPtr& a)
{
  a1 = a->position[0];
  a2 = a->position[1];
  a3 = a->position[2];
  a4 = a->position[3];
  a5 = a->position[4];
  a6 = a->position[5];
  

  b[0] = -0.5;
  b[1] = 0.707;
  b[2] = -0.5;
  return b;
}
  void callback1(const geometry_msgs::WrenchStamped::ConstPtr& msg1)  
  {  
    force.wrench.force.x = msg1->wrench.force.x/1000000+g*x*qq[0]+g*y*qq[1]-fx;
    force.wrench.force.y = msg1->wrench.force.y/1000000-g*y*qq[0]+g*x*qq[1]-fy;
    force.wrench.force.z = msg1->wrench.force.z/1000000+g*qq[2]-fz;
    force.wrench.torque.x = msg1->wrench.torque.x/1000000+g*y*pz*qq[0]-g*x*pz*qq[1]+g*py*qq[2]-tx;
    force.wrench.torque.y = msg1->wrench.torque.y/1000000+g*x*pz*qq[0]+g*y*pz*qq[1]-g*px*qq[2]-ty;
    force.wrench.torque.z = msg1->wrench.torque.z/1000000-g*x*py*qq[0]-g*y*py*qq[1]-g*y*px*qq[0]+g*x*px*qq[1]-tz;
    pub_.publish(force);
    ros::Rate loop_rate(10);
    loop_rate.sleep();
  }  
  
  void callback2(const sensor_msgs::JointState::ConstPtr& msg2)
  {
  qq[0] = qianxiang(msg2)[0];
  qq[1] = qianxiang(msg2)[1];
  qq[2] = qianxiang(msg2)[2];
for(int i=0;i<3;i++)
      ROS_INFO("[%f]", qq[i]);
  }
private: 
  float a1;
  float a2;
  float a3;
  float a4;
  float a5;
  float a6;
  float g = 2.96;
  float fx = 1.82;
  float fy = -15.41;
  float fz = -14.18;
  float tx = 0.071;
  float ty = 0.349;
  float tz = 0.000139;
  float x = 0.34176;
  float y = -0.24539;
  float px = -0.001740;
  float py = 0.00145;
  float pz = 0.02223;
  float qq[3];
  float b[3];
  ros::NodeHandle n_;   
  ros::Publisher pub_;  
  ros::Subscriber sub_;
  ros::Subscriber sub2_; 
  geometry_msgs::WrenchStamped force;
  sensor_msgs::JointState joint;

};//End of class SubscribeAndPublish  

int main(int argc, char **argv)  
{  
  //Initiate ROS  
  ros::init(argc, argv, "subscribe_and_publish");  

  //Create an object of class SubscribeAndPublish that will take care of everything  
  SubscribeAndPublish test;  
  //ros::spin();
  ros::MultiThreadedSpinner s(2);  //多线程
  ros::spin(s);  

  return 0;  
}  

这里面坑无数：
1、cmath不需要在cmakelists中添加;
2、boost可以不添加;
3、joint_states类型消息必须要先resize大小，才能用！！！，且消息不能用在函数的返回值中;

ur

好久不用ur，也有不少问题，其中经常性会连接不上，sock傻傻啥的。
1、可能本身电脑网段不对;
2、需要添加reserve_port:=30004，端口号可能不对，这个也是一个点
3、可以在common.launch文件中单独param参数，就不用再输入了。

还有个问题，就是需要将ur和传感器网络放在同一网段下，ur机械臂设置ip的方式：
点击文件，然后退出，就可以设置ur ip地址。

坑

现在的一坑是matlab中计算的正向运动学的R3i，即变换矩阵第三行的元素与论文还中的不一样。

May the force be with you!