hector_imu_attitude_to_tf 妖狐艹你老母 2022-03-16 05:10 179阅读 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** <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> ![在这里插入图片描述][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
还没有评论,来说两句吧...