我是靠谱客的博主 重要唇膏，最近开发中收集的这篇文章主要介绍ROS学习记录14【SLAM】仿真学习3——开车，并发布坐标信息零.前言一.用Xbox控制小车运动二.获取坐标系，觉得挺不错的，现在分享给大家，希望可以做个参考。
概述
零.前言
因为是仿真，模型也是怪怪的，所以坐标信息就不用轮子积分的方式了。后面遇到真实的模型过后再具体分析阿克曼运动模型的速度转换。而且真实情况的运动分解更复杂，完全没有理想模型这么理想，所以要么就纯仿真熟悉流程，要么就直接上实物，有针对性的调试。所以，这次坐标信息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 erron");
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;
void LinkStateCallback(const gazebo_msgs::LinkStatesConstPtr& msg)
{
int base_fp_index;
cout << "call back!" << endl;
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();
}
odom_pub.child_frame_id = BASE_FP_STR;
odom_pub.header.frame_id = "odom";
odom_pub.header.stamp = ros::Time::now();
odom_pub.pose.pose = msg->pose[base_fp_index];
odom_pub.twist.twist = msg->twist[base_fp_index];
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "odom_node");
ros::NodeHandle node;
ros::Subscriber sub;
ros::Rate loop_rate(20);
ros::Publisher pub;
tf::TransformBroadcaster tf_pub;
tf::Transform tf_transform;
tf::Quaternion trans_quaternion;
sub = node.subscribe("/gazebo/link_states", 1, &LinkStateCallback);
pub = node.advertise<nav_msgs::Odometry>("odom", 1);
odom_pub.child_frame_id = "N";
while(odom_pub.child_frame_id == "N" && ros::ok())
{
ros::spinOnce(); // wait the first callback while you will pub a NULL tf transform
}
while(ros::ok())
{
pub.publish(odom_pub);
tf_transform.setOrigin(tf::Vector3(odom_pub.pose.pose.position.x, odom_pub.pose.pose.position.y, odom_pub.pose.pose.position.z));
tf_transform.setRotation( tf::Quaternion(odom_pub.pose.pose.orientation.x, odom_pub.pose.pose.orientation.y, odom_pub.pose.pose.orientation.z, odom_pub.pose.pose.orientation.w));
tf_pub.sendTransform(tf::StampedTransform(tf_transform, ros::Time::now(), "odom", BASE_FP_STR));
ros::spinOnce();
loop_rate.sleep();
}
return 0;
};
2.3.1 等待回调
while(odom_pub.child_frame_id == "N" && ros::ok())
{
ros::spinOnce(); // wait the first callback while you will pub a NULL tf transform
}
如果我们还没有接收到第一条来自/gazebo/link_states的消息时，我们发布的内容会是nan，所以我们用死循环，等待第一次消息到来后，再进行后面的逻辑操作
2.3.2 更新odom信息
void LinkStateCallback(const gazebo_msgs::LinkStatesConstPtr& msg)
{
int base_fp_index;
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();
}
odom_pub.child_frame_id = BASE_FP_STR;
odom_pub.header.frame_id = "odom";
odom_pub.header.stamp = ros::Time::now();
odom_pub.pose.pose = msg->pose[base_fp_index];
odom_pub.twist.twist = msg->twist[base_fp_index];
}
我们的link_states是相对ground_plane::link的坐标，也就是相对于gazobo中心的位置关系，我们将这个关系，作为我们odom移动到base_footprint的关系。更新它的信息。
2.3.3 发布消息
while(ros::ok())
{
pub.publish(odom_pub);
tf_transform.setOrigin(tf::Vector3(odom_pub.pose.pose.position.x, odom_pub.pose.pose.position.y, odom_pub.pose.pose.position.z));
tf_transform.setRotation( tf::Quaternion(odom_pub.pose.pose.orientation.x, odom_pub.pose.pose.orientation.y, odom_pub.pose.pose.orientation.z, odom_pub.pose.pose.orientation.w));
tf_pub.sendTransform(tf::StampedTransform(tf_transform, ros::Time::now(), "odom", BASE_FP_STR));
ros::spinOnce();
loop_rate.sleep();
}
我们将odom的信息发布出去后，再将odom的信息添加至我们的tf树里即可，让ros去维护坐标变换的信息。
2.4 修改launch
我们用的是仿真时间，所以我们要在启动gazebo的时候添加一句：
<arg name="use_sim_time" value="true"/>
现在，我们的launch是这样的：
<launch>
<arg name="model_name" value="ackman"/>
<arg name="model_ns" value="ackman"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="use_sim_time" value="true"/>
</include>
<param name="robot_description" textfile="$(find slam_model)/urdf/$(arg model_name).urdf" />
<param name="use_gui" value="true"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
<remap from="/joint_states" to="/$(arg model_ns)/joint_states" />
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find slam_model)/urdf/$(arg model_name).rviz" required="true"></node>
<node name="spawn_model" pkg="gazebo_ros" type="spawn_model" args="-file $(find slam_model)/urdf/$(arg model_name).urdf -urdf -model $(find slam_model)" output="screen"/>
<rosparam file="$(find slam_model)/config/$(arg model_name)_control.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/$(arg model_ns)" args="joint_state_controller right_wheel_velocity_controller left_wheel_velocity_controller right_bridge_position_controller left_bridge_position_controller"/>
</launch>
2.5 运行
- 启动整个系统
roslaunch slam_model ackman_show.launch - 启动
odom发布器rosrun slam_odom odom_pub - 启动遥控器
rosrun slam_keyboard_move xbox_control js1
效果：
话题状态：
最后
以上就是重要唇膏为你收集整理的ROS学习记录14【SLAM】仿真学习3——开车，并发布坐标信息零.前言一.用Xbox控制小车运动二.获取坐标系的全部内容，希望文章能够帮你解决ROS学习记录14【SLAM】仿真学习3——开车，并发布坐标信息零.前言一.用Xbox控制小车运动二.获取坐标系所遇到的程序开发问题。
如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复