ROS学习记录14SLAM仿真学习3——开车，并发布坐标信息
Posted 康娜喵
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了ROS学习记录14SLAM仿真学习3——开车，并发布坐标信息相关的知识，希望对你有一定的参考价值。
零.前言
因为是仿真，模型也是怪怪的，所以坐标信息就不用轮子积分的方式了。后面遇到真实的模型过后再具体分析阿克曼运动模型的速度转换。而且真实情况的运动分解更复杂，完全没有理想模型这么理想，所以要么就纯仿真熟悉流程，要么就直接上实物，有针对性的调试。所以，这次坐标信息odom就从Gazebo里获取。
顺路说一句，VScode里面的ROS插件解决了一个我一直想要的可视化urdf编辑：
一.用Xbox控制小车运动
鄙人写了一小段代码，通过Xbox手柄来控制小车移动。不管是正版还是国产，走Xbox协议的手柄都可以用。还有一个问题就是，C++和C的混合编程真的离谱。FXXK U C++
创建ros_ws/src/slam_keyboard_move/src/xbox_control.cpp这个xbox_control节点
#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include "iostream"
#include "string.h"
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <linux/input.h>
#include <linux/joystick.h>
using namespace std;
#define XBOX_TYPE_BUTTON 0x01
#define XBOX_TYPE_AXIS 0x02
#define XBOX_BUTTON_A 0x00
#define XBOX_BUTTON_B 0x01
#define XBOX_BUTTON_X 0x02
#define XBOX_BUTTON_Y 0x03
#define XBOX_BUTTON_LB 0x04
#define XBOX_BUTTON_RB 0x05
#define XBOX_BUTTON_START 0x06
#define XBOX_BUTTON_BACK 0x07
#define XBOX_BUTTON_HOME 0x08
#define XBOX_BUTTON_LO 0x09 /* 左摇杆按键 */
#define XBOX_BUTTON_RO 0x0a /* 右摇杆按键 */
#define XBOX_BUTTON_ON 0x01
#define XBOX_BUTTON_OFF 0x00
#define XBOX_AXIS_LX 0x00 /* 左摇杆X轴 */
#define XBOX_AXIS_LY 0x01 /* 左摇杆Y轴 */
#define XBOX_AXIS_RX 0x03 /* 右摇杆X轴 */
#define XBOX_AXIS_RY 0x04 /* 右摇杆Y轴 */
#define XBOX_AXIS_LT 0x02
#define XBOX_AXIS_RT 0x05
#define XBOX_AXIS_XX 0x06 /* 方向键X轴 */
#define XBOX_AXIS_YY 0x07 /* 方向键Y轴 */
#define XBOX_AXIS_VAL_UP -32767
#define XBOX_AXIS_VAL_DOWN 32767
#define XBOX_AXIS_VAL_LEFT -32767
#define XBOX_AXIS_VAL_RIGHT 32767
#define XBOX_AXIS_VAL_MIN -32767
#define XBOX_AXIS_VAL_MAX 32767
#define XBOX_AXIS_VAL_MID 0x00
typedef struct xbox_map
int time;
int a;
int b;
int x;
int y;
int lb;
int rb;
int start;
int back;
int home;
int lo;
int ro;
int lx;
int ly;
int rx;
int ry;
int lt;
int rt;
int xx;
int yy;
xbox_map_t;
int xbox_open(char *file_name)
int xbox_fd;
xbox_fd = open(file_name, O_RDONLY);
if (xbox_fd < 0)
perror("open");
return -1;
return xbox_fd;
int xbox_map_read(int xbox_fd, xbox_map_t *map)
int len, type, number, value;
struct js_event js;
len = read(xbox_fd, &js, sizeof(struct js_event));
if (len < 0)
perror("read");
return -1;
type = js.type;
number = js.number;
value = js.value;
map->time = js.time;
if (type == JS_EVENT_BUTTON)
switch (number)
case XBOX_BUTTON_A:
map->a = value;
break;
case XBOX_BUTTON_B:
map->b = value;
break;
case XBOX_BUTTON_X:
map->x = value;
break;
case XBOX_BUTTON_Y:
map->y = value;
break;
case XBOX_BUTTON_LB:
map->lb = value;
break;
case XBOX_BUTTON_RB:
map->rb = value;
break;
case XBOX_BUTTON_START:
map->start = value;
break;
case XBOX_BUTTON_BACK:
map->back = value;
break;
case XBOX_BUTTON_HOME:
map->home = value;
break;
case XBOX_BUTTON_LO:
map->lo = value;
break;
case XBOX_BUTTON_RO:
map->ro = value;
break;
default:
break;
else if (type == JS_EVENT_AXIS)
switch(number)
case XBOX_AXIS_LX:
map->lx = value;
break;
case XBOX_AXIS_LY:
map->ly = value;
break;
case XBOX_AXIS_RX:
map->rx = value;
break;
case XBOX_AXIS_RY:
map->ry = value;
break;
case XBOX_AXIS_LT:
map->lt = value;
break;
case XBOX_AXIS_RT:
map->rt = value;
break;
case XBOX_AXIS_XX:
map->xx = value;
break;
case XBOX_AXIS_YY:
map->yy = value;
break;
default:
break;
else
/* Init do nothing */
return len;
void xbox_close(int xbox_fd)
close(xbox_fd);
return;
int main(int argc, char **argv)
/* xbox variable BEGIN*/
int xbox_fd ;
xbox_map_t map;
int len, type;
int axis_value, button_value;
int number_of_axis, number_of_buttons ;
memset(&map, 0, sizeof(xbox_map_t));
/* xbox variable END*/
/* ros variable BEGIN*/
ros::init(argc, argv, "xbox_control");
ros::NodeHandle h;
std_msgs::Float64 left_pos, right_pos, left_vel, right_vel;
ros::Publisher left_pos_pub = h.advertise<std_msgs::Float64>("/ackman/left_bridge_position_controller/command", 10);
ros::Publisher right_pos_pub = h.advertise<std_msgs::Float64>("/ackman/right_bridge_position_controller/command", 10);
ros::Publisher left_vel_pub = h.advertise<std_msgs::Float64>("/ackman/left_wheel_velocity_controller/command", 10);
ros::Publisher right_vel_pub = h.advertise<std_msgs::Float64>("/ackman/right_wheel_velocity_controller/command", 10);
/* ros variable END*/
xbox_fd = xbox_open((char*)"/dev/input/js1");
if(xbox_fd < 0)
return -1;
perror("open js1 erro\\n");
ROS_INFO("xbox_control shutdown!");
ros::shutdown();
cout << "Open js1 success" << endl;
while(ros::ok())
len = xbox_map_read(xbox_fd, &map);
if (len < 0)
usleep(10*1000);
continue;
// lt subb speed; rt add speed; lt rt from -32767 to 32767;
// key A = 1 was emergent STOP
// lx is is from -32767 to 32767 steer from +0.758 to -0.758
left_pos.data = -map.lx / 32767.0 * 0.758;
right_pos.data = left_pos.data;
if (map.a == 1)
left_vel.data = 0;
else
/*
(-lt + rt) means:
lt max rt min = back up vel max = -32767+(-32767);
lt min rt min = 0 ;
lt min rt max = forward vel max = 32767+32767;
* 10 is the max vel
*/
left_vel.data = (-map.lt + map.rt) / (65534.0) * 10;
if (left_vel.data < 0.1 and left_vel.data > -0.1)
left_vel.data = 0;
right_vel.data = left_vel.data;
left_pos_pub.publish(left_pos);
right_pos_pub.publish(right_pos);
left_vel_pub.publish(left_vel);
right_vel_pub.publish(right_vel);
xbox_close(xbox_fd);
ROS_INFO("xbox_control shutdown!");
ros::shutdown();
return 0;
lt减速，rt加速，左摇杆控制左右方向，A键急停。
编译加上参数即可js?就行。这个js?在你的/dev/input里通过插拔手柄对比分析就可以知道手柄的文件名了。
然后通过
rosrun slam_keyboard_move xbox_control js1运行手柄节点即可。
二.获取坐标系
2.1 消息格式
Gazebo里发布位置信息的话题是/gazebo/link_states
输出看看
---
name:
- ground_plane::link
- /home/kanna/ros_ws/src/slam_model::base_footprint
- /home/kanna/ros_ws/src/slam_model::left_bridge
- /home/kanna/ros_ws/src/slam_model::left_front_wheel
- /home/kanna/ros_ws/src/slam_model::left_rear_wheel
- /home/kanna/ros_ws/src/slam_model::right_bridge
- /home/kanna/ros_ws/src/slam_model::right_front_wheel
- /home/kanna/ros_ws/src/slam_model::right_rear_wheel
pose:
-
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
-
position:
x: 0.0387156240276447
y: -0.0006434379046546359
z: 1.574693242162306e-05
orientation:
x: 3.312143845520465e-07
y: 6.557425849379155e-06
z: -0.00035311039334020347
w: 0.9999999376349684
-
position:
x: 0.33889763674133877
y: 0.2541445693936302
z: 0.10002294814918565
orientation:
x: 3.652178291571738e-07
y: 6.5602419890783786e-06
z: -0.0003531289593156039
w: 0.999999937628382
-
position:
x: 0.3389292272264112
y: 0.299144548143617
z: 0.10001987728948931
orientation:
x: 0.6975386314306447
y: 0.11420190193255057
z: -0.11478579394877221
w: 0.6980128972765485
-
position:
x: -0.2610713069470129
y: 0.29956831544554013
z: 0.10001987961596293
orientation:
x: 0.7068223739872597
y: 0.0020302519763462446
z: -0.0025313756114154285
w: 0.7073836313100841
-
position:
x: 0.33853752655108943
y: -0.2558549877630028
z: 0.10002304528218595
orientation:
x: -1.3274072588362696e-07
y: 6.608511054808344e-06
z: -0.00035331524127595455
w: 0.9999999375623233
-
position:
x: 0.33850552474407763
y: -0.3008550729504472
z: 0.10001990227499637
orientation:
x: 0.6977788040570895
y: 0.11272294589894416
z: -0.11330620712645552
w: 0.6982549545138499
-
position:
x: -0.26149493012562774
y: -0.30043166283581574
z: 0.10001989089495111
orientation:
x: 0.7068217201553735
y: 0.0022166419361558535
z: -0.0027179224643826116
w: 0.7073830329549923
twist:
-
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
-
linear:
x: 5.755967126110963e-05
y: 2.4059535555072117e-05
z: 0.0003404509075145309
angular:
x: 0.0001735916433847387
y: 0.0026237200659893554
z: -2.3125593575503338e-05
-
linear:
x: 0.0005278162426775284
y: -2.0862807273656645e-05
z: 0.005139358908439479
angular:
x: 0.0001979782950360155
y: 0.002605979848441594
z: -2.4899260708480086e-05
-
linear:
x: 0.0004681862086410119
y: -2.054956809143295e-05
z: 0.0035045335930283666
angular:
x: 0.00020525313899467536
y: 0.004689529598689501
z: -2.7280858587093637e-05
-
linear:
x: 0.00028914224941039033
y: 2.5462470341422355e-06
z: 0.0011463643316762643
angular:
x: -3.366716275348254e-05
y: 0.0028853793925617322
z: -4.9346850669416205e-06
-
linear:
x: 0.0005265670826430743
y: 2.7316149254080077e-05
z: 0.005205887601959399
angular:
x: -0.0001850412835686597
y: 0.0026458777083701313
z: 5.220186144899022e-06
-
linear:
x: 0.0004627706375915965
y: 1.73943476321947e-05
z: 0.003549886506426709
angular:
x: -0.00017443288596857062
y: 0.004628201514494329
z: 1.812057183895147e-06
-
linear:
x: 0.00029202005957692324
y: -2.2486217611408557e-06
z: 0.0011770696716055817
angular:
x: 2.75976070767762e-05
y: 0.0029217984738362287
z: -2.2794510301955025e-05
---
然后我们看看类型：
rostopic type /gazebo/link_states 得到：gazebo_msgs/LinkStates
然后去官网看看：
# broadcast all link states in world frame
string[] name # link names
geometry_msgs/Pose[] pose # desired pose in world frame
geometry_msgs/Twist[] twist # desired twist in world frame
然后：geometry_msgs/Pose.msg
# A representation of pose in free space, composed of position and orientation.
Point position
Quaternion orientation
然后：geometry_msgs/Twist[] twist
# This expresses velocity in free space broken into its linear and angular parts.
Vector3 linear
Vector3 angular
2.2 创建包
catkin_create_pkg slam_odom roscpp geometry_msgs tf gazebo_msgs
2.3 获取gazebo/link_states
我们要知道：某个link的pose和twist的index与name里的index相同，所以得先遍历name得到base_footprint的下标。
我看大家都是用的Python，那我就试着写一版C艹的好了，我有个想法，想用lua再封装一次ros node，这样不知道会不会好一些。偏题了。
首先先看看我们的name的样子：
void LinkStateCallback(const gazebo_msgs::LinkStatesConstPtr& msg)
cout << msg->name[1] << endl;
/home/kanna/ros_ws/src/slam_model::base_footprint
为了程序有更好的移植性，我们得处理一下。这样写我感觉还是挺优化的，用C++就要极致得压榨性能，不然不如用python。
void LinkStateCallback(const gazebo_msgs::LinkStatesConstPtr& msg)
int base_fp_index;
string base_fp_str("base_footprint");
for(base_fp_index = 0; base_fp_index < msg->name.size(); ++base_fp_index)
if (msg->name[base_fp_index].find(base_fp_str, msg->name[base_fp_index].length()-15) != string::npos)
break;
if (base_fp_index >= msg->name.size())
ROS_ERROR("Couldn't Find base_footprint");
ros::shutdown();
2.3 完整代码
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <gazebo_msgs/LinkStates.h>
#include <nav_msgs/Odometry.h>
#include "iostream"
#include "string"
using namespace std;
#define BASE_FP_STR "base_footprint"
nav_msgs::Odometry odom_pub以上是关于ROS学习记录14SLAM仿真学习3——开车，并发布坐标信息的主要内容，如果未能解决你的问题，请参考以下文章
ROS学习记录12SLAM仿真学习1——ROS Control
ROS学习记录12SLAM仿真学习1——ROS Control