ROS学习记录14【SLAM】仿真学习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的posetwistindexname里的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

效果:
在这里插入图片描述
话题状态:
在这里插入图片描述

评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

康娜喵

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值