hector_imu_attitude_to_tf

妖狐艹你老母 2022-03-16 05:10 170阅读 0赞

Package：hector\_imu\_attitude\_to\_tf

*  **node**:imu\_attitude\_to\_tf\_node  
    将IMU提供的横滚和俯仰角发布给TF
 *  **Subscribed Topic**:imu\_topic(sensor\_msgs/Imu)  
    Imu发布的Topic
 *  **Parameters**  
    1.base\_stabilized\_frame (string, default: “base\_stabilized\_frame”)   
    2.base\_frame (string, default: “base\_frame”)

**hector\_imu\_attitude\_to\_tf包launch文件下的example.launch**

我的修改

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70]  
**源代码：**

#include "ros/ros.h"
    #include "tf/transform_broadcaster.h"
    #include "sensor_msgs/Imu.h"
    
    std::string p_base_stabilized_frame_;//定义base_stabilized_frame(frame_id_)
    std::string p_base_frame_;       //定义base_frame(child_frame_id_)
    tf::TransformBroadcaster* tfB_;  //定义tf::TransformBroadcaster类指针
    tf::StampedTransform transform_; //定义存放变换关系的变量
    tf::Quaternion tmp_;             //定义四元数
    
    #ifndef TF_MATRIX3x3_H
      typedef btScalar tfScalar;
      namespace tf {  typedef btMatrix3x3 Matrix3x3; }
    #endif
    
    void imuMsgCallback(const sensor_msgs::Imu& imu_msg)
    {   //传入的geometry_msgs::Quaternion(imu_msg.orientation) is transformed to a tf::Quaterion(tmp_)
      tf::quaternionMsgToTF(imu_msg.orientation, tmp_);//四元数转欧拉角
     
      tfScalar yaw, pitch, roll;
      tf::Matrix3x3(tmp_).getRPY(roll, pitch, yaw); // the tf::Quaternion has a method to acess roll pitch and yaw
    
      tmp_.setRPY(roll, pitch, 0.0);   //由欧拉角计算四元数，此处消去yaw只用到roll和pitch
    
      transform_.setRotation(tmp_);    //通过四元数得到旋转矩阵
    
      transform_.stamp_ = imu_msg.header.stamp;//获取时间戳
    
      tfB_->sendTransform(transform_); //发布tf变换：sendTransform函数发布transform_变量
    }
    
    int main(int argc, char **argv) { 
      ros::init(argc, argv, ROS_PACKAGE_NAME);
    
      ros::NodeHandle n;
      ros::NodeHandle pn("~"); //pn的命名空间（namespace）是 /ROS_PACKAGE_NAME
      //获取”base_stabilized_frame"、“base_frame"，分别给"p_base_stabilized_frame_"(frame_id_)"p_base_frame_"(child_frame_id_)
      pn.param("base_stabilized_frame", p_base_stabilized_frame_, std::string("base_stabilized"));
      pn.param("base_frame", p_base_frame_, std::string("base_link"));
      
      tfB_ = new tf::TransformBroadcaster();//新建一个tf::TransformBroadcaster类,这个broadcaster就是一个publisher，然后调用sendTransform(),将transform发布到 /tf 的一段transform上
      transform_.getOrigin().setX(0.0);		//初始化
      transform_.getOrigin().setY(0.0);
      transform_.getOrigin().setZ(0.0);
      transform_.frame_id_ = p_base_stabilized_frame_;  //父节点
      transform_.child_frame_id_ = p_base_frame_;	    //子节点
    
      ros::Subscriber imu_subscriber = n.subscribe("imu_topic", 10, imuMsgCallback);//订阅imu话题数据，回调函数将订阅到的四元数转换为欧拉角
    
      ros::spin();
    
      delete tfB_;//释放定义的tf::TransformBroadcaster类
    
      return 0;
    }

param的操作

Get Param的三种方法：
        1.ros::param::get()获取参数"param1"的value，写入到“parameter1”上
    		bool ifget1 = ros::param::get("param1",parameter1)  
        2.ros::NodeHandle::getParam()获取参数,与1作用相同
            bool ifget2 = ros::param::getParam("param2",parameter2)  
        3.ros::NodeHandle nh;
          nh.param()类似于1,2,但是如果get不到指定的param，它可以给param制定一个默认值33333     
            nh.param("param3",parameter3,33333)
        需要判断是否get到param3可以用nh.hasParam("param3")
        Set Param的两种方法：
        1.ros::param::set()设置参数"param4"为parameter4
            ros::param::set("param4",parameter4)
        2.ros::NodeHandle::setParam()
        	nh.setParam("param5",parameter5)
        Check Param的两种方法：
        1.ros::NodeHandle::hasParam()
            bool ifparam5 =nh.hasParam("param5")
        2.ros::param::has()
            bool ifparam6 = ros::param::has("param6")
        Delete Param的两种方法
        1. ros::NodeHandle::deleteParam()
    	    bool ifdeleted5 = nh.deleteParam("param5");
    	2.ros::param::del()
    	    bool ifdeleted6 = ros::param::del("param6");

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMTc2MTE2_size_16_color_FFFFFF_t_70]: /images/20220316/082c8e8dab88400986e772f0b0566153.png

hector_imu_attitude_to_tf

发表评论取消回复

还没有评论，来说两句吧...

相关阅读