如何获得gazebo仿真中的机器人位姿真值(这里主要讨论轮式移动机器人),这是一个很简单的问题。主要有三种方法。
- 订阅gazebo话题 / 请求gazebo服务
在官网教程Tutorial: ROS Communication中,介绍了gazebo向ros发布的rostopic,例如:
Topics:
/clock : rosgraph_msgs/Clock - Publish simulation time, to be used with /use_sim_time parameter.
~/link_states : gazebo_msgs/LinkStates - Publishes states of all the links in simulation.
~/model_states : gazebo_msgs/ModelStates - Publishes states of all the models in simulation.
其中~/model_states发布的就是此次仿真中的所有模型的实时位姿真值,我们需要从中挑选出所关注的模型的信息。另外,link_states发布的是模型中部件的位姿信息,机械臂仿真中可能会需要此项信息。
同时,gazebo运行后还可以提供若干ros service,例如:
These services allow the user to retrieve state and property information about simulation and objects in simulation:
~/get_model_properties : gazebo_msgs/GetModelProperties- This service returns the properties of a model in simulation.
~/get_model_state : gazebo_msgs/GetModelState - This service returns the states of a model in simulation.
~/get_world_properties : gazebo_msgs/GetWorldProperties - This service returns the properties of the simulation world.
~/get_joint_properties : gazebo_msgs/GetJointProperties - This service returns the properties of a joint in simulation.
~/get_link_properties : gazebo_msgs/GetLinkProperties - This service returns the properties of a link in simulation.
~/get_link_state : gazebo_msgs/GetLinkState - This service returns the states of a link in simulation.
~/get_physics_properties : gazebo_msgs/GetPhysicsProperties - This service returns the properties of the physics engine used in simulation.
其中,get_model_state就是相应的获取机器人位姿真值的rosservice。
我们除了可以在命令行中用rostopic echo和rosservice call来获取返回值,也可以写代码来获取,例如请求一个service的写法:
// 开头别忘了引用头文件:
#include "gazebo_msgs/GetModelState.h"
程序里写:
// ros常规操作:
ros::NodeHandle n2;
ros::ServiceClient client2 = n2.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
gazebo_msgs::GetModelState srv2;
srv2.request.model_name = "robot"; //指定要获取的机器人在gazebo中的名字;
if (client2.call(srv2))
{
// 如果获取服务成功了的话,
// srv2.response就是请求服务后返回的数据,可以随便使用了。
}
else
{
ROS_ERROR("Failed to call service /gazebo/get_model_state");
}
当然,也可以把这些内容放进一个callback函数里,让别的条件来触发获取真值的操作,例如:
//include略
// 注意callback函数的输入得和触发的topic格式对得上:
void aCallback(const geometry_msgs::Twist& msg)
{
// ros常规操作:
ros::NodeHandle n2;
ros::ServiceClient client2 = n2.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
gazebo_msgs::GetModelState srv2;
srv2.request.model_name = "robot"; //指定要获取的机器人在gazebo中的名字;
if (client2.call(srv2))
{
// 如果获取服务成功了的话,
// srv2.response就是请求服务后返回的数据,可以随便使用了。
}
else
{
ROS_ERROR("Failed to call service /gazebo/get_model_state");
}
}
int main(int argc, char** argv){
ros::init(argc, argv, "a_node");
ros::NodeHandle n1;
// 例如让一个速度控制命令rostopic来触发真值获取:
ros::Subscriber sub = n1.subscribe("/cmd_vel",1,&aCallback);
ros::Rate loop_rate1(100);
while (ros::ok())
{
ros::spinOnce();
loop_rate1.sleep();
}
return 0;
}
- 使用tf
这是一种非常规的获取机器人位姿真值的操作。使用的是ros tf里的TransformListener,归根结底是通过获取空间中两个指定坐标系之间的位姿变换(transform)来得到机器人位姿的。非常适合获取机器人身上一个指定部件在某个坐标系下的实时位姿(例如一个机械臂末端执行器,或者IMU),还是非常有实际意义的。
应用的框架如下:
#include <tf/transform_listener.h>
//其他include略
int main(int argc, char** argv){
ros::init(argc, argv, "a_node");
ros::NodeHandle n1;
tf::TransformListener listener; //创建一个listener;
ros::Rate rate(100.0);
while (ros::ok()){
tf::StampedTransform transform; //创建一个transform做储存坐标变换的容器;
try{
//选择监测哪两个坐标系;
listener.lookupTransform("/world", "/aimed_link",
ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(0.5).sleep();
}
//至此transform内就储存了两个指定坐标系间的变换了,可以进行进一步应用:
//position_x = transform.getOrigin().x();
//position_y = transform.getOrigin().y();
//position_z = transform.getOrigin().z();
//orientation_x = transform.getRotation().x();
//orientation_y = transform.getRotation().y();
//orientation_z = transform.getRotation().z();
//orientation_w = transform.getRotation().w();
ros::spinOnce();
rate.sleep();
}
return 0;
}
- 应用插件
最方便的就是直接应用gazebo插件了,插件的概念详见Tutorial: Using Gazebo plugins with ROS
插件libgazebo_ros_p3d.so就具有发布机器人位姿真值的功能。
应用举例:
<!-- 在gazebo仿真模型文件中添加: -->
<gazebo>
<plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
<alwaysOn>true</alwaysOn>
<updateRate>50.0</updateRate>
<bodyName>base_footprint</bodyName>
<topicName>base_pose_ground_truth</topicName>
<gaussianNoise>0.0</gaussianNoise>
<frameName>map</frameName>
<xyzOffsets>0 0 0</xyzOffsets>
<rpyOffsets>0 0 0</rpyOffsets>
</plugin>
</gazebo>
其中bodyName是机器人身上的link,frameName是参考的坐标系,由此我们可以知道,有了这个插件,也可以实现获取机器人身上指定部件位姿真值的目的。