Fatal Frame. preface
Because it is a simulation, the model is also strange, so the coordinate information does not need the way of wheel integration. After meeting the real model, we will analyze the velocity conversion of Ackerman motion model. Moreover, the motion decomposition of the real situation is more complex, which is not as ideal as the ideal model, so either get familiar with the process through pure simulation, or directly go to the real object for targeted debugging. Therefore, this time, the coordinate information odom is obtained from Gazebo.
By the way, the ROS plug-in in VScode solves a visual urdf editing I've always wanted:
I Use Xbox to control trolley movement
I wrote a short piece of code to control the movement of the car through the Xbox handle. Whether genuine or domestic, you can use the handle of Xbox protocol. Another problem is that the mixed programming of C + + and C is really outrageous. FXXK U C++
Create ros_ws/src/slam_keyboard_move/src/xbox_control.cpp this xbox_control node
#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 / * left rocker key*/
#define XBOX_ BUTTON_ RO 0x0a / * right rocker key*/
#define XBOX_BUTTON_ON 0x01
#define XBOX_BUTTON_OFF 0x00
#define XBOX_ AXIS_ LX 0x00 / * left rocker X-axis*/
#define XBOX_ AXIS_ Ly 0x01 / * Y-axis of left rocker*/
#define XBOX_ AXIS_ RX 0x03 / * right rocker X-axis*/
#define XBOX_ AXIS_ Ry 0x04 / * right rocker Y-axis*/
#define XBOX_AXIS_LT 0x02
#define XBOX_AXIS_RT 0x05
#define XBOX_ AXIS_ XX 0x06 / * direction key X-axis*/
#define XBOX_ AXIS_ YY 0x07 / * direction key Y-axis*/
#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 decelerates, rt accelerates, the left rocker controls the left and right directions, and A key emergency stop.
Compile with parameters js? All right. This js? In your / dev/input, you can know the file name of the handle by comparing and analyzing the plug handle.
Then pass
rosrun slam_keyboard_move xbox_control js1 run the handle node.
II Get coordinate system
2.1 message format
The topic of posting location information in Gazebo is / gazebo/link_states
Output look
---
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
---
Then let's look at the types:
rostopic type /gazebo/link_states: gazebo_msgs/LinkStates
Then go to the official website:
# 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
Then: geometry_msgs/Pose.msg
# A representation of pose in free space, composed of position and orientation. Point position Quaternion orientation
Then: geometry_msgs/Twist[] twist
# This expresses velocity in free space broken into its linear and angular parts. Vector3 linear Vector3 angular
2.2 create package
catkin_create_pkg slam_odom roscpp geometry_msgs tf gazebo_msgs
2.3 get gazebo/link_states
We need to know that the index of pose and twist of a link is the same as the index in name, so we have to traverse the name to get the base first_ Subscript of footprint.
I think everyone uses Python, so I'll try to write a version of C. I have an idea. I want to encapsulate ros node again with lua. I don't know if it will be better. It's off the subject.
First, let's take a look at our name:
void LinkStateCallback(const gazebo_msgs::LinkStatesConstPtr& msg)
{
cout << msg->name[1] << endl;
}
/home/kanna/ros_ws/src/slam_model::base_footprint
In order to make the program more portable, we have to deal with it. In this way, I feel it is quite optimized. If you use C + +, you should squeeze the performance to the extreme, otherwise you might as well use 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 complete code
#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 waiting for callback
while(odom_pub.child_frame_id == "N" && ros::ok())
{
ros::spinOnce(); // wait the first callback while you will pub a NULL tf transform
}
If we haven't received the first message from / gazebo / link_ When the States message is sent, the content we publish will be nan, so we use an endless loop to wait for the first message, and then carry out the following logical operations
2.3.2 update odom information
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];
}
Our link_states is the relative ground_ The coordinate of plane:: link, that is, the positional relationship relative to the center of gazobo. We move this relationship as our odom to base_footprint. Update its information.
2.3.3 release message
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();
}
After we publish the information of odom, we can add the information of odom to our tf tree, and let ros maintain the information of coordinate transformation.
2.4 modify launch
We use simulation time, so we need to add a sentence when starting gazebo:
<arg name="use_sim_time" value="true"/>
Now, our launch is as follows:
<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 operation
- Start the entire system roslaunch slam_model ackman_show.launch
- Start the Odom publisher rosrun slam_odom odom_pub
- Start the remote control rosrun slam_keyboard_move xbox_control js1
effect:
Topic status:
