hector_imu_attitude_to_tf

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

<launch>
  <node pkg="hector_imu_attitude_to_tf" type="imu_attitude_to_tf_node" name="imu_attitude_to_tf_node" output="screen">
    <!--“hector_imu_attitude_to_tf”包，node名“imu_attitude_to_tf_node”  -->
    <remap from="imu_topic" to="thumper_imu" />
    <!--将“imu_topic”话题映射为“thumper_imu”话题-->
    <param name="base_stabilized_frame" type="string" value="base_stabilized" />
    <!--"base_stabilized"在下面的坐标系上加入了高度，"base_link"则在有高度的基础上加入了俯仰角和横滚角-->
    <param name="base_frame" type="string" value="base_footprint" />
    <!--”base_footprint“没有高度，2D pose of the robot(position and orientation) -->
    <!--”base_footprint“、"base_link"、“base_stabilized”三者的关系参见：http://wiki.ros.org/hector_slam/Tutorials/SettingUpForYourRobot 的附图) -->
  </node>
</launch>

我的修改

<launch>
  <node pkg="hector_imu_attitude_to_tf" type="imu_attitude_to_tf_node" name="imu_attitude_to_tf_node" output="screen">
    <!--“hector_imu_attitude_to_tf”包，node名“imu_attitude_to_tf_node”  -->
    <remap from="imu_topic" to="/imu/data" />
    <!--将“imu_topic”话题映射为“/imu/data”话题-->
    <param name="base_stabilized_frame" type="string" value="base_stabilized" />
    <!--"base_stabilized"在下面的坐标系上加入了高度，"base_link"则在有高度的基础上加入了俯仰角和横滚角-->
    <param name="base_frame" type="string" value="base_footprint" />
    <!--”base_footprint“没有高度，2D pose of the robot(position and orientation) -->
    <!--”base_footprint“、"base_link"、“base_stabilized”三者的关系参见：http://wiki.ros.org/hector_slam/Tutorials/SettingUpForYourRobot 的附图) -->
  </node>
</launch>

源代码：

#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");