问题描述
在RVIZ中想要去更改机器人模型位置,但不想考虑gazebo的各种因素,只想通过别人给的数据流去实时更改机器人在rviz中的位置。
解决方案
首先,先将原理弄清楚。
在RVIZ中,Fixed Frame是主坐标系,要想展现出机器人随动的状态,需要设置好主坐标系(自己设置),然后再让咱们的机器人tf坐标系跟这个主坐标系有关联,其中非常经典的一个用法是在launch文件中,加入如下代码:
<node name="tf_base" pkg="tf" type="static_transform_publisher" args="0.0 0.0 0.0 0.0 0.0 0.0 map robot_base_link 100" />
但是,这种用法会一直向RVIZ发一个tf广播(是一直),这样的话无论你怎么改,最终它又会重新的让你的机器人tf坐标系回到上面设置的(0,0,0,0,0,0)状态,这可不行。
那么,要让机器人在rviz中运动,实际要做的就是让机器人的tf坐标系运动,要让tf坐标系运动,就要涉及tf坐标系的广播和监听问题,当然,要让tf运动,只需要广播就行。
因为现在tf已经被淘汰了,主要用tf2完成。
示例代码:读取传入的odometry消息,并在rviz中进行实时更改
代码如下:
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
void rviz_Callback(const nav_msgs::Odometry::ConstPtr& odo)
{
static tf2_ros::StaticTransformBroadcaster static_broadcaster;
geometry_msgs::TransformStamped static_transformStamped;
static_transformStamped.header.stamp = ros::Time::now();
static_transformStamped.header.frame_id = "map";
static_transformStamped.child_frame_id = "robot_base_link";
static_transformStamped.transform.translation.x=odo->pose.pose.position.x;
static_transformStamped.transform.translation.y=odo->pose.pose.position.y;
static_transformStamped.transform.translation.z=odo->pose.pose.position.z;
static_transformStamped.transform.rotation.w = odo->pose.pose.orientation.w;
static_transformStamped.transform.rotation.x = odo->pose.pose.orientation.x;
static_transformStamped.transform.rotation.y = odo->pose.pose.orientation.y;
static_transformStamped.transform.rotation.z = odo->pose.pose.orientation.z;
static_broadcaster.sendTransform(static_transformStamped);
}
int main(int argc, char **argv) {
ros::init(argc, argv, "pub_model_state");
ros::NodeHandle nh;
ros::Subscriber uwbSub = nh.subscribe<nav_msgs::Odometry>("topicname", 1000, rviz_Callback);
ROS_INFO("RUNNING");
ros::spin();
return 0;
}
当然,如果你没有map,它也会自动在rviz中创建一个map的坐标系的。
这样理论上就可以实现了。
然后,我说一些自己遇到的坑:
首先,robot_description这个参数是用来加载机器人模型的,要在urdf中显示,就必须要urdf或者xacro文件,sdf只能在gazebo中运行。
只要robot的base_link运动了,一般来说整个机器人都会运动(模型也是会运动的),如果不能就说明,有的东西没有加载。
在launch 文件中添加:
<!--Add a real joint_state publisher from hardware将机器的人joint加进去,这样机器人内部的tf就不用调了-->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<!-- 发布robot数据-->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
然后,调试tf的时候,一定要用 rosrun rqt_tf_tree rqt_tf_tree这个神器,可以看到整个tf结构。而且还能看到什么节点名在对那个tf进行广播。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)