TF学习

TF学习

一、tf::TransformListener 类

void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const------Get the transform between two frames by frame ID

二、tf::StampedTransform

TF::stampedtransform是TF的一种特殊情况：它需要frame_id和stamp以及child_frame_id。参见https://www.cnblogs.com/li-yao7758258/p/7672521.html和https://blog.csdn.net/sru_alo/article/details/92804479

三、tf::Transform

包含旋转矩阵和位移向量

四、tf::poseTFToMsg

convert Pose to Pose msg

五、读取任意两个坐标系之间的其次变换关系

这里实现了两个marker的读取及机械臂到达两个marker的位置，并且加入了机械臂末端与marker相对的坐标关系，以下代码都是在这篇博客https://blog.csdn.net/qq_34897331/article/details/100982041和https://blog.csdn.net/qq_23670601/article/details/96704852的基础上修改的

#include "ros/ros.h"
#include <cstdlib> 
#include "std_msgs/Float64.h"
#include <unistd.h>
#include<iostream> //C++标准输入输出库 
#include <vector>
#include "std_msgs/String.h"
#include "geometry_msgs/PoseStamped.h"
#include <kinova_msgs/ArmPoseActionGoal.h> // 注意格式源文件名字为ArmPose.action
#include <ros/ros.h>
#include <kinova_driver/kinova_ros_types.h>

#include <actionlib/client/simple_action_client.h>
#include <kinova_msgs/SetFingersPositionAction.h>
#include <kinova_msgs/ArmPoseAction.h>
#include <kinova_msgs/HomeArm.h>
#include <kinova_msgs/ArmJointAnglesAction.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
using namespace std;
const int FINGER_MAX = 3000;

class tl1{
   
public:
    tl1();
    void registerNodeHandle(ros::NodeHandle& _nh);
    void registerSub();
    void registerSrv();
    void MarkerCallback();
    bool cartesian_pose_client(float x, float y, float z, float qx, float qy, float qz, float qw);
    bool finger_control_client(float finger1, float finger2, float finger3);
    void GotoHome(kinova_msgs::HomeArm& _srv);
    bool getTransform(const std::string& refFrame,
                    const std::string& childFrame)
   {
   
    std::string errMsg;

    if ( !_tfListener.waitForTransform(refFrame,
                                       childFrame,
                                       ros::Time(0),
                                       ros::Duration(0.5),
                                       ros::Duration(0.01),
                                       &errMsg)
         )
    {
   
      ROS_ERROR_STREAM("Unable to get pose from TF: " << errMsg);
      return false;
    }
    else
    {
   
      try
      {
   
        _tfListener.lookupTransform( refFrame, childFrame,
                                     ros::Time(0),                  //get latest available
                                     transform);

       
      }
      catch ( const tf::TransformException& e)
      {
   
        ROS_ERROR_STREAM("Error in lookupTransform of " << childFrame << " in " << refFrame);
        return false;
      }

    }
    return true;
  }
private:
    ros::Subscriber Marke_sub;
    ros::NodeHandle nh;
    ros::ServiceClient start_to_home;
    tf::StampedTransform transform;
    std::string marker_frame;
    std::string camera_frame;
    std::string reference_frame;
    tf::TransformListener _tfListener;
    int tag;
};

int main(int argc, char **argv) {
   


    ros::init(argc, argv, "paragram1");
    ros::NodeHandle node_handle;

    tl1 loandmove;
    kinova_msgs::HomeArm srv;

    loandmove.registerNodeHandle(node_handle);

    loandmove.registerSrv();
    loandmove.GotoHome(srv);
    while ( ros::ok() )
    {
    loandmove.MarkerCallback();
    // ros::Duration( 0.5 ).sleep();
    //sleep(3);
    }
   





    //ros::spin();
    return 0;
}


tl1::tl1(){
   tag=0;};

void tl1::registerNodeHandle(ros::NodeHandle& _nh){
   
    nh = _nh;
};



void tl1::registerSrv(){
   

    start_to_home = nh.serviceClient<kinova_msgs::HomeArm>("j2n6s300_driver/in/home_arm");
    nh.param<std::string>("reference_frame", reference_frame, "j2n6s300_link_base");
    nh.param<std::string>("camera_frame", camera_frame, "j2n6s300_link_base");
    nh.param<std::string>("marker_frame", marker_frame,