Before I asked Lｚ, how to move the simulation small car? One way is to enter MESSAGE directly in the terminal, and you can control the car through the keyboard.
1. Create a control package
In ROS, there are two working space methods, one is using catkin and the other is ROSBUILD.
It is troublesome to install the ROS program into other systems or architectures with ROSBUILD. This is the main problem existing in Rosbuild, and it is why the Catkin was developed later.
In order to solve the above problem, the key to improving the migrating ability is to provide installation targets for ROS bags and obey the installation layout of FHS. It does not mean that it is impossible to change only if it changes to Rosbuild, but it seems difficult to achieve it.
So, the main design process of Catkin is to write a helping function Catkin designer for CMAKE macro, variables, and Python. Essence The motivation to solve these problems is that the slow construction speed of ROSBUILD is the place where people complain the most.
Reference address:https://blog.csdn.net/chishuideyu/article/details/53581013
lz The working space uses CATKIN. If you use ROSBUILD, you can refer tohttps://www.ncnynl.com/archives/201609/846.html
First of all, we set up a separate package for the keyboard control:
cd ~/catkin_ws/src
catkin_create_pkg smartcar_teleop rospy geometry_msgs std_msgs roscpp
cd ~/catkin_ws
catkin_make2. Write source code
lz is also a novice, so I added a comment and the keyboard control.
#include <termios.h>
#include <signal.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/poll.h>
#include <boost/thread/thread.hpp>
// Ros/Ros.h is a practical header file. It quotes most common head files in ROS systems
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#define KEYCODE_W 0x77
#define KEYCODE_A 0x61
#define KEYCODE_S 0x73
#define KEYCODE_D 0x64
#define KEYCODE_A_CAP 0x41
#define KEYCODE_D_CAP 0x44
#define KEYCODE_S_CAP 0x53
#define KEYCODE_W_CAP 0x57
class SmartCarKeyboardTeleopNode {
private:
double walk_vel_;
double run_vel_;
double yaw_rate_;
double yaw_rate_run_;
geometry_msgs::Twist cmdvel_;
// Create a handle for the nodes of this process. The first Nodehandle created will initialize the node.
// The last destroyed Nodehandle will release all the resources occupied by the node
ros::NodeHandle n_;
// Initialize release node
ros::Publisher pub_;
public:
SmartCarKeyboardTeleopNode() {
// Tell the master we will post Geometry_msgs/Twist message type message on cmd_vel (topic name)
// So the master will tell all nodes subscribing to the cmd_vel topic, and there will be data release.
// The second parameter is the size of the sequence. If the frequency of the news is too high,
// The message in the buffer will start to discard the previously released news when it is greater than 1,000
// nodehandle :: advertise () Return a ROS :: PUBLISHER object, it has two functions:
// 1) It has a publish () member function that allows you to post on top;
// 2) If the message type is not right, it will refuse to release it.
pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
ros::NodeHandle n_private("~");
n_private.param("walk_vel", walk_vel_, 0.5);
n_private.param("run_vel", run_vel_, 1.0);
n_private.param("yaw_rate", yaw_rate_, 1.0);
n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);
}
~SmartCarKeyboardTeleopNode() {}
void keyboardLoop();
void stopRobot() {
cmdvel_.linear.x = 0.0;
cmdvel_.angular.z = 0.0;
pub_.publish(cmdvel_);
}
};
SmartCarKeyboardTeleopNode *tbk;
int kfd = 0;
struct termios cooked, raw;
bool done;
int main(int argc, char **argv) {
// Initialize ROS. It allows ROS to re -mapping the name by command.
// We can specify the name of the node, but the name here must be Basename, which cannot include/other symbols in the name
ros::init(argc, argv, "tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
SmartCarKeyboardTeleopNode tbk;
// Rebate a thread
boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop, &tbk));
// ros :: spin () will not return after calling, that is, your main program will not execute it here,
// and ros :: spinonce () The latter can continue to execute the program after calling.
ros::spin();
// Interrupt thread
t.interrupt();
// Due to thread interruption, return immediately
t.join();
// Speed zero
tbk.stopRobot();
tcsetattr(kfd, TCSANOW, &cooked);
return (0);
}
void SmartCarKeyboardTeleopNode::keyboardLoop() {
char c;
double max_tv = walk_vel_;
double max_rv = yaw_rate_;
bool dirty = false;
int speed = 0;
int turn = 0;
// get the console in raw mode
tcgetattr(kfd, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &= ~(ICANON | ECHO);
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(kfd, TCSANOW, &raw);
puts("Reading from keyboard");
puts("Use WASD keys to control the robot");
puts("Press Shift to move faster");
struct pollfd ufd;
ufd.fd = kfd;
ufd.events = POLLIN;
for (;;) {
boost::this_thread::interruption_point();
// get the next event from the keyboard
int num;
if ((num = poll(&ufd, 1, 250)) < 0) {
perror("poll():");
return;
} else if (num > 0) {
if (read(kfd, &c, 1) < 0) {
perror("read():");
return;
}
} else {
if (dirty == true) {
stopRobot();
dirty = false;
}
continue;
}
switch (c) {
case KEYCODE_W:
max_tv = walk_vel_;
speed = 1;
turn = 0;
dirty = true;
break;
case KEYCODE_S:
max_tv = walk_vel_;
speed = -1;
turn = 0;
dirty = true;
break;
case KEYCODE_A:
max_rv = yaw_rate_;
speed = 0;
turn = 1;
dirty = true;
break;
case KEYCODE_D:
max_rv = yaw_rate_;
speed = 0;
turn = -1;
dirty = true;
break;
case KEYCODE_W_CAP:
max_tv = run_vel_;
speed = 1;
turn = 0;
dirty = true;
break;
case KEYCODE_S_CAP:
max_tv = run_vel_;
speed = -1;
turn = 0;
dirty = true;
break;
case KEYCODE_A_CAP:
max_rv = yaw_rate_run_;
speed = 0;
turn = 1;
dirty = true;
break;
case KEYCODE_D_CAP:
max_rv = yaw_rate_run_;
speed = 0;
turn = -1;
dirty = true;
break;
default:
max_tv = walk_vel_;
max_rv = yaw_rate_;
speed = 0;
turn = 0;
dirty = false;
}
cmdvel_.linear.x = speed * max_tv;
cmdvel_.angular.z = turn * max_rv;
pub_.publish(cmdvel_);
}
}
Add corresponding code to cmakerist.txt:
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
add_executable(smartcar_teleop src/teleop.cpp)
target_link_libraries(smartcar_teleop
${catkin_LIBRARIES}
)Catkin_make, don’t forget
source devel/setup.bashPut the simulation car first according to ROS (6)
． devel/setup.bash
roslaunch smartcar_description smartcar_display.rviz.launch
In the terminal, you can control the simulation car to move according to the control of the keyboard. .. ..
The laboratory just pours the car alone, can’t afford to hurt ＼ (＾ ＾) / ~