ros rviz显示orb |
您所在的位置：网站首页 › ubuntu1404安装ros › ros rviz显示orb |
|
目录
写在前面
代码
使用
参考
完
写在前面
1、本文内容 ros rviz显示orb-slam2保存的轨迹 2、平台 ubuntu1804, ros melodic 3、转载请注明出处： https://blog.csdn.net/qq_41102371/article/details/126838142 代码 mkdir -p trajectory/src cd trajectory/src catkin_create_pkg show_path roscpp rospy tf nav_msgs cd ..把show_path.cpp放进src/show_path/src 在CMakeLists.txt中添加 add_executable(show_path_node src/imu_tf_pose.cpp) target_link_libraries( show_path_node ${catkin_LIBRARIES})show_path.cpp #include #include #include #include #include #include #include "nav_msgs/Path.h" #include ros::Publisher puber; void PublishPath(std::string file_path) { nav_msgs::Path path_msg; geometry_msgs::PoseStamped pose; pose.header.stamp = ros::Time::now(); pose.header.frame_id = "/world"; std::ifstream fin; fin.open(file_path,std::ios::in); if(!fin.is_open()){ std::cout double time,tx,ty,tz,qx,qy,qz,qw; fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw; pose.pose.position.x = tx; pose.pose.position.y = ty; pose.pose.position.z = tz; pose.pose.orientation.x = qx; pose.pose.orientation.y = qy; pose.pose.orientation.z = qz; pose.pose.orientation.w = qw; path_msg.poses.push_back(pose); } } path_msg.header.stamp = ros::Time::now(); path_msg.header.frame_id = "/world"; puber.publish(path_msg); } int main (int argc, char ** argv) { std::cout std::cout PublishPath(file_path); } return 0; } 使用 ./devel/lib/show_path/show_path_node ./orb_slam2.txt /trajectory1打开rviz，add, by topic，选择发布的trajectory1 把fixed frame改成world 其中，/orb_slam2.txt为orb-slam2生成的轨迹文件，如 1662709738.611801 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 1.0000000 1662709738.944941 -0.0026136 0.0170262 -0.0045148 0.0095704 0.0072003 0.0153332 0.9998107 1662709739.544584 0.0069332 0.0336722 0.0215601 0.0175027 -0.0293207 0.0545010 0.9979296 1662709744.308815 0.0545398 0.0866963 0.1380486 -0.0544997 -0.1701589 0.1484294 0.9726481 1662709744.508729 0.0569852 0.0875546 0.1378780 -0.0624662 -0.1643448 0.1828232 0.9672975 1662709744.675315 0.0595775 0.0904402 0.1297402 -0.0711926 -0.1598502 0.2039685 0.9632115 1662709745.308262 0.0525226 0.0983748 0.1075200 -0.0962976 -0.1551851 0.2081083 0.9609034 1662709746.774192 0.0280566 0.0733101 0.1472074 -0.0261320 -0.1733477 0.1026057 0.9791526 ... ... ... 参考https://github.com/gaoxiang12/slambook2/blob/9382d9066029308ff40a539d64880d540135bfd6/ch3/examples/plotTrajectory.cpp https://github.com/robosu12/imu_data_simulation/blob/master/src/vio_data_simulation/src/gener_alldata.cpp 完--------------------------------------------------------------------------------------------202209 |
今日新闻 |
推荐新闻 |
| CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |