SLAM和机器人系统结合可以获取机器人在MAP上的位姿，结合机器人关节数据就可以实现其数字孪生的显示，这样既可以快速完成对机器人关节状态的判断，又可以在导航中更直观的看出机器人的模型是否与障碍物碰撞。其核心是将URDF机器人质心坐标系Link与其在地图上的位姿TF绑定，然后采用joint_state话题发布按URDF Joint定义的角度即可，至于URDF如果构建比较简单的方式是采用SolidWorks导出后再ROS中显示并做微调：
请在感知资料中下载URDF，并将其放置于PC端Launch启动脚本文件夹源码中，然后修改urdf中对应载入STL的包的名字！
新建或者采用我们提供的包，首先包含头文件：
#include "sensor_msgs/JointState.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "tf2_ros/transform_broadcaster.h"新增关节数据结构：
struct joint_st
{
std::vector<std::string> joint_name;
std::vector<float> joint_pos;
}joint;
void state_init(sensor_msgs::JointState &joint_state)
{
joint.joint_name = {
"FR0_J", "FR1_J", "FR2_J",
"HR0_J", "HR1_J", "HR2_J",
"FL0_J", "FL1_J", "FL2_J",
"HL0_J", "HL1_J", "HL2_J",
};
joint.joint_pos = {
0,0,0,
0,0,0,
0,0,0,
0,0,0
};
joint_state.header.stamp = ros::Time::now();
joint_state.name.resize(12);
joint_state.position.resize(12);
for(size_t i=0;i<=11;i++)
{
joint_state.name[i] = joint.joint_name[i];
}
for(int i=0;i<=11;i++)
{
joint_state.position[i]=joint.joint_pos[i];
}
}新增发布话题：
ros::Publisher joint_pub=n.advertise<sensor_msgs::JointState>("joint_states",1);
sensor_msgs::JointState joint_state;
state_init(joint_state);
joint_pub.publish(joint_state);通过UDP采集机器人关节实时数据，并按坐标系定义进行发布：
joint_state.header.stamp = current_time;
joint_state.position[0]=moco.q[0][2]/57.3;
joint_state.position[1]=moco.q[0][0]/57.3;
joint_state.position[2]=(180-moco.q[0][1]-18)/57.3;
joint_state.position[3]=moco.q[1][2]/57.3;
joint_state.position[4]=moco.q[1][0]/57.3;
joint_state.position[5]=(180-moco.q[1][1]-18)/57.3;
joint_state.position[6]=moco.q[2][2]/57.3;
joint_state.position[7]=moco.q[2][0]/57.3;
joint_state.position[8]=(180-moco.q[2][1]-18)/57.3;
joint_state.position[9]=moco.q[3][2]/57.3;
joint_state.position[10]=moco.q[3][0]/57.3;
joint_state.position[11]=(180-moco.q[3][1]-18)/57.3;
joint_pub.publish(joint_state);在本体PC Rviz Launch中增加机器人模型载入、关节话题发布节点，将SLAM感知教程相关代码中URDF进行下载，绑定对应的TF坐标，修正固有偏差：
<launch>
<!-- rviz -->
<node pkg="rviz" type="rviz" name="rviz" required="true"
args="-d $(find robot_navigation)/rviz/multi_navigation.rviz"/>
<node
name="robot_state_publisher"
pkg="robot_state_publisher"
type="robot_state_publisher" />
<arg
name="model" />
<param
name="robot_description"
textfile="$(find robot_navigation)/urdf/tinymal_urdf.urdf" />
<node pkg="tf" type="static_transform_publisher" name="odom_to_robot_urdf" args="0 0 0 0 0 0 /urdf_link /Body_link 100"/>
<node pkg="tf" type="static_transform_publisher" name="odom_to_robot_urdf1" args="0 0 0 0 0 0 /urdf_link /base_link 100"/>
</launch>
