ROS自学实践(5)：GAZEBO建模及添加相机和激光雷达传感器并进行仿真
在搭建完机器人小车的模型之后，需要向其添加传感器，以便提取传感器的数据，进行后续的工作。
一、相机
1.添加camera_gazebo.xacro文件
同添加机器人模型一样，添加一个相机也需要进行定义一个相机的结构，参数，功能的“类”，也就是camera.xacro文件，在总的调用文件“shcRobot2_xacro_camera_gazebo.xacro”中调用即可。具体代码来源于参考文献1
Gazebo/Black
30.0
1.3962634
1280
720
R8G8B8
0.02
300
gaussian
0.0
0.007
true
0.0
/camera
image_raw
camera_info
camera_link
0.07
0.0
0.0
0.0
0.0
0.0
2.调用camera_gazebo.xacro文件生成总模型
在下面的总调用文件“shcRobot2_xacro_camera_gazebo.xacro”中，除了要引用camera.xacro文件，还要记得定义camera和base_link的连接节点的位置和属性。
3.修改相应launch文件，启动仿真
启动键盘控制节点和rqt_image_view节点，可以输出相机的画面
2.激光雷达
1.添加lidar_gazebo.xacro文件
Gazebo/Black
0 0 0 0 0 0
false
5.5
360
1
-3
3
0.10
6.0
0.01
gaussian
0.0
0.01
/scan
laser_link
2.调用lidar_gazebo.xacro文件生成总模型shcRobot2_xacro_lida_gazebo.xacro
大家注意一下，上面的文件中，使用的是shcRobot2_base_lida_gazebo.xacro作为小车的基础模型，和之前的shcRobot2_base_gazebo.xacro不太一样，其实就是添加了支撑雷达的杆，为了方便起见，还是把代码粘在下面
Gazebo/Black
transmission_interface/SimpleTransmission
hardware_interface/VelocityJointInterface
hardware_interface/VelocityJointInterface
1
Gazebo/Black
transmission_interface/SimpleTransmission
hardware_interface/VelocityJointInterface
hardware_interface/VelocityJointInterface
1
Gazebo/White
Gazebo/Gray
Gazebo/Blue
Debug
true
/
1
true
true
100.0
true
left_rear_drive_joint
right_rear_drive_joint
0.8
${2*wheel_radius}
1
80
3
cmd_vel
odom
odom
base_link
Debug
true
/
1
true
true
100.0
true
left_front_steer_joint
right_front_steer_joint
0.8
${2*wheel_radius}
1
80
3
cmd_vel
odom
odom
base_link
3.修改相应launch文件，启动仿真
启动键盘控制节点和rviz节点，可以输出激光雷达的点云图像
附键盘控制节点的cpp文件[2]
#include
#include
#include
#include
#include
#include
#include
#include
#include
#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_;
ros::NodeHandle n_;
ros::Publisher pub_;
public:
SmartCarKeyboardTeleopNode()
{
pub_ = n_.advertise("cmd_vel", 2);
ros::NodeHandle n_private("~");
n_private.param("walk_vel", walk_vel_, 2.5);
n_private.param("run_vel", run_vel_, 2.0);
n_private.param("yaw_rate", yaw_rate_, 3.0);
n_private.param("yaw_rate_run", yaw_rate_run_, 3.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)
{
ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
SmartCarKeyboardTeleopNode tbk;
boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop, &tbk));
ros::spin();
t.interrupt();
t.join();
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_);
}
}
参考文献：
1.深蓝学院-ROS理论与实践
2.https://www.guyuehome.com/253