以插件形式获取
可以在机器人的urdf模型中加入插件。
<!-- Fake localization plugin -->
<plugin name="ground_truth_odometry" filename="libgazebo_ros_p3d.so">
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
<bodyName>base_link</bodyName>
<topicName>base_pose_ground_truth</topicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>-->
<!-- initialize odometry for fake localization-->
<xyzOffsets>0 0 0</xyzOffsets>
<rpyOffsets>0 0 0</rpyOffsets>
</plugin>
以订阅者形式获取
订阅话题/gazebo/model_states,找到对应模型的名称,例如机器人名称为turtlebot3。
void _modelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg)
{
int modelCount = msg->name.size();
for(int modelInd = 0; modelInd < modelCount; ++modelInd)
{
if(msg->name[modelInd] == "turtlebot3")
{
_current_pose.pose = msg->pose[modelInd];
_current_velocity.twist = msg->twist[modelInd];
break;
}
}
}