模板网站禁止右键wordpress描述代码
1.TF之间的管理建立：
<launch>
<!-- 建立base_foot与base(机器人)-gyro(陀螺仪)-laser(雷达)TF之间的关系 --><!--创建静态坐标转换广播器-rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系--><node pkg="tf" type="static_transform_publisher" name="base_to_link" args="0.08 0 0 0 0 0 base_footprint base_link 100" /><node pkg="tf" type="static_transform_publisher" name="base_to_gyro" args="0.08 0 0 0 0 0 base_footprint gyro_link 100" /><node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.14 0 0.11 3.14159 0 0 base_footprint laser 100" /><!-- 创造机器人模型 --><!-- <param name = "robot_description" textfile = "$(find huanyubot_description)/urdf/huanyubot.urdf"/> --><!-- 添加关节状态发布节点 --><!-- <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /> --><!-- 添加机器人状态发布节点 --><!-- <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /> --><!-- 里程计相关设计 --><!--Start the robot's base control code and publish the odometer data--><!-- <node pkg="huanyu_robot_start" type="huanyu_robot_node" name="publish_odom" output="screen"><param name="usart_port" type="string" value="/dev/huanyu_base"/> <param name="baud_data" type="int" value="115200"/><param name="robot_frame_id" type="string" value="base_footprint"/><param name="smoother_cmd_vel" type="string" value="cmd_vel"/><param name="imu_velocity_z" type="bool" value="false"/><param name="filter_Vx_match" type="double" value="1.0"/><param name="filter_Vth_match" type="double" value="1.0"/></node> -->
<!-- 里程计 --><node pkg="huanyu_robot_start" type="huanyu_robot_node" name="publish_odom" output="screen"><!-- <param name="usart_port" type="string" value="/dev/huanyu_base"/> --><!-- <param name="baud_data" type="int" value="115200"/> --><param name="robot_frame_id" type="string" value="base_footprint"/><param name="smoother_cmd_vel" type="string" value="cmd_vel"/><param name="imu_velocity_z" type="bool" value="false"/><param name="filter_Vx_match" type="double" value="1.0"/><param name="filter_Vth_match" type="double" value="1.0"/></node><!-- 雷达1参数以及node启动 --><!-- <node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen"> <param name="serial_port" type="string" value="/dev/huanyu_laser"/><param name="serial_baudrate" type="int" value="115200"/><param name="frame_id" type="string" value="laser"/><param name="inverted" type="bool" value="false"/><param name="angle_compensate" type="bool" value="true"/></node> --><!-- Robot pose ekf --><!-- <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen"><param name="freq" value="30.0"/><param name="sensor_timeout" value="1.0"/><param name="odom_used" value="true"/><param name="imu_used" value="true"/><param name="vo_used" value="false"/><remap from="/imu_data" to="/mobile_base/sensors/imu_data" /></node> --><!-- publish camera image --><!--node pkg="track_detection" type="imageRead_node" name="publish_image" output="screen"/>--> <!-- 雷达2参数以及node启动 --><node pkg="hins_le_driver" type="hins_le_driver" name="hins_le_driver" output="screen" respawn="true"><param name="code_version" type="string" value="2.11.3"/> <!-- 版本信息 请勿修改--><param name="laser_ip" type="string" value="192.168.1.88"/><param name="port" type="int" value="8080"/><param name="topic_name" type="string" value="scan"/><param name="frame_name" type="string" value="laser"/><param name="y_direction" type="bool" value="true"/> <!-- 是否设置雷达的正前方为y轴正方向 false为x轴正方向--><param name="use_udp" type="bool" value="false"/> <!-- 是否使用udp模式 是为true 否则为false--><!--雷达扫描参数--><param name="change_param" type="bool" value="false"/> <!-- 是否改变雷达扫描参数 --><param name="measure_frequency_kHz" type="string" value="200"/> <!-- 激光扫描频率(1s内输出点的数量) --><param name="spin_frequency_Hz" type="string" value="30"/> <!-- 电机转速(完整点云输出频率) --><param name="sampling_size_per_position" type="string" value="2"/> <!-- 单点采样次数(均值滤波窗口大小) --><param name="noise_filter_level" type="string" value="0"/> <!-- 噪声过滤等级(0~3) 值越小越接近原数据，越大点云越平滑 --><!--防拖尾过滤参数--><param name="shadows_filter_level" type="int" value="1"/><param name="shadows_filter_max_angle" value="175"/><param name="shadows_filter_min_angle" value="5"/><param name="shadows_filter_traverse_step" type="int" value="1"/><param name="shadows_filter_window" type="int" value="3"/><!--开始/结束角度 (角度值) 将开始角度～结束角度的值设为无效值(start_angle < end_angle)--> <param name="start_angle" value="1.0"/><param name="end_angle" value="0.0"/><!--避障服务名--><param name="block_enable" type="bool" value="false"/> <!--true为开启避障功能 false为关闭避障功能--><param name="service_name" type="string" value="hins_le_area_data"/></node></launch>内部由TF实现了：
建立base_foot与base(机器人)-gyro(陀螺仪)-laser(雷达)TF之间联系；
base(机器人)内部联系由urdf实现:
odom与basefoot之间海未建立链接，因为gmapping与odom之间建立链接，但是没找道odom与basefoot如何建立连接的。
查阅可知：
odom与basefoot之间属于动态tf转换方式，不在launch里，launch实现静态TF转换，因此再程序内部实现。
map与odom再
void Huanyu_start_object::PublisherOdom()
{geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);std_msgs::Float32 power_msgs;power_msgs.data = this->Power_valtage;power_pub.publish(power_msgs);//first, we'll publish the transform over tf// geometry_msgs::TransformStamped odom_trans;// odom_trans.header.stamp = ros::Time::now();;// odom_trans.header.frame_id = "odom";// odom_trans.child_frame_id = this->robot_frame_id;// odom_trans.transform.translation.x = x;// odom_trans.transform.translation.y = y;// odom_trans.transform.translation.z = 0.0;// odom_trans.transform.rotation = odom_quat;// //send the transform// odom_broadcaster.sendTransform(odom_trans);//next, we'll publish the odometry message over ROSnav_msgs::Odometry odom;odom.header.stamp = ros::Time::now();;odom.header.frame_id = "odom";//set the positionodom.pose.pose.position.x = x;odom.pose.pose.position.y = y;odom.pose.pose.position.z = 0.0;odom.pose.pose.orientation = odom_quat;//set the velocityodom.child_frame_id = this->robot_frame_id;odom.twist.twist.linear.x = this->vx;odom.twist.twist.linear.y = this->vy;odom.twist.twist.angular.z = this->vth; if(this->vx == 0){memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2));memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2));}else{memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance));memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance));} //publish the messageodom_pub.publish(odom);
}里面的：
odom.header.frame_id = "odom";
//set the position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
//set the velocity
odom.child_frame_id = this->robot_frame_id;
//<param name="robot_frame_id" type="string" value="base_footprint"/>已经对应成base_footprint了。
因此实现odom与base_footprint之间的TF转换。
未连接：
map与odom之间连理TF转换是由程序内部实现：如下：
gmapping中map->odom的转换
gmapping 通过“粒子滤波"实现定位。具体通过粒子与已经产生的地图进行scanMatch，矫正里程计误差实现。 在定位的同时，每次经过map_update_interval_时间，进行地图更新updateMap(*scan)。
//gmapping/src/slam_gmapping.cpp
void SlamGMapping::startLiveSlam()
{... // 订阅激光数据scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
}void SlamGMapping::laserCallback(const sensor_msgsList item::LaserScan::ConstPtr& scan)
{// We can't initialize the mapper until we've got the first scanif(!got_first_scan_){if(!initMapper(*scan))return;}GMapping::OrientedPoint odom_pose;if(addScan(*scan, odom_pose))// addscan：对新激光，结合里程计数据进行粒子滤波矫正得出最新位置{ // 以下通过getBestParticle的位置（即，当前激光位置）和里程计最终，算出map-to-odom，发布出去GMapping::OrientedPoint mpose = gsp_->getBestParticleIndex().pose;tf::Transform laser_to_map = tf::Transform(mpose...）;tf::Transform odom_to_laser = tf::Transform(odom_pose...);map_to_odom_ = (odom_to_laser * laser_to_map).inverse();// 当满足 一定时间间隔 更新地图if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_){updateMap(*scan);last_map_update = scan->header.stamp;}}
tf::Transform laser_to_map = tf::Transform(mpose...）;
tf::Transform odom_to_laser = tf::Transform(odom_pose...);
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
进行实现。
TF正确链接：
话题topic链接：
