Skip to content

Commit 614c072

Browse files
jgoppertjgoppert
committed
Wrap rviz node in class to simplify service handling and add motor pwm
param
1 parent 4a54ec7 commit 614c072

2 files changed

Lines changed: 190 additions & 152 deletions

File tree

‎rviz/rplidar.rviz‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,7 @@ Visualization Manager:
7373
Size (Pixels): 5
7474
Size (m): 0.03
7575
Style: Squares
76-
Topic: /scan
76+
Topic: /rplidarNode/scan
7777
Use Fixed Frame: true
7878
Use rainbow: true
7979
Value: true

‎src/node.cpp‎

Lines changed: 189 additions & 151 deletions
Original file line numberDiff line numberDiff line change
@@ -45,14 +45,122 @@
4545

4646
using namespace rp::standalone::rplidar;
4747

48-
RPlidarDriver * drv = NULL;
49-
50-
void publish_scan(ros::Publisher *pub,
51-
rplidar_response_measurement_node_t *nodes,
52-
size_t node_count, ros::Time start,
53-
double scan_time, bool inverted,
54-
float angle_min, float angle_max,
55-
std::string frame_id)
48+
class RPlidarNode {
49+
public:
50+
RPlidarNode();
51+
virtual ~RPlidarNode();
52+
void measure();
53+
private:
54+
// methods
55+
void start();
56+
void stop();
57+
void publish_scan(
58+
rplidar_response_measurement_node_t *nodes,
59+
size_t node_count, ros::Time start,
60+
double scan_time, float angle_min, float angle_max);
61+
bool getRPLIDARDeviceInfo(RPlidarDriver * drv);
62+
bool checkRPLIDARHealth(RPlidarDriver * drv);
63+
bool stop_motor(std_srvs::Empty::Request &req,
64+
std_srvs::Empty::Response &res);
65+
bool start_motor(std_srvs::Empty::Request &req,
66+
std_srvs::Empty::Response &res);
67+
68+
// attributes
69+
RPlidarDriver * drv;
70+
std::string serial_port;
71+
int serial_baudrate ;
72+
std::string frame_id;
73+
bool inverted;
74+
bool angle_compensate;
75+
ros::NodeHandle nh;
76+
// publications
77+
ros::Publisher scan_pub;
78+
// services
79+
ros::ServiceServer stop_motor_service;
80+
ros::ServiceServer start_motor_service;
81+
};
82+
83+
RPlidarNode::RPlidarNode() :
84+
drv(NULL),
85+
serial_port(),
86+
serial_baudrate(),
87+
frame_id(),
88+
inverted(),
89+
angle_compensate(),
90+
nh("~"),
91+
// publications
92+
scan_pub(nh.advertise<sensor_msgs::LaserScan>("scan", 1000)),
93+
// services
94+
stop_motor_service(nh.advertiseService(
95+
"stop_motor", &RPlidarNode::stop_motor, this)),
96+
start_motor_service(nh.advertiseService(
97+
"start_motor", &RPlidarNode::start_motor, this))
98+
{
99+
100+
nh.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0");
101+
nh.param<int>("serial_baudrate", serial_baudrate, 115200);
102+
nh.param<std::string>("frame_id", frame_id, "laser_frame");
103+
nh.param<bool>("inverted", inverted, false);
104+
nh.param<bool>("angle_compensate", angle_compensate, true);
105+
106+
printf("RPLIDAR running on ROS package rplidar_ros\n"
107+
"SDK Version: "RPLIDAR_SDK_VERSION"\n");
108+
109+
u_result op_result;
110+
111+
// create the driver instance
112+
drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
113+
114+
if (!drv) {
115+
throw std::runtime_error("Create Drive fail\n");
116+
}
117+
118+
// make connection...
119+
if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {
120+
char msg[50];
121+
snprintf(msg, 50, "Cannot bind to the specified port %s", serial_port.c_str());
122+
RPlidarDriver::DisposeDriver(drv);
123+
throw std::runtime_error(msg);
124+
}
125+
126+
// get rplidar device info
127+
if (!getRPLIDARDeviceInfo(drv)) {
128+
RPlidarDriver::DisposeDriver(drv);
129+
throw std::runtime_error("Failed to get device info\n");
130+
}
131+
132+
// check health...
133+
if (!checkRPLIDARHealth(drv)) {
134+
RPlidarDriver::DisposeDriver(drv);
135+
throw std::runtime_error("Health check failed\n");
136+
}
137+
138+
// start scanning
139+
start();
140+
}
141+
142+
void RPlidarNode::stop() {
143+
drv->stop();
144+
drv->stopMotor();
145+
}
146+
147+
void RPlidarNode::start() {
148+
int motor_pwm = 0;
149+
nh.param<int>("motor_pwm", motor_pwm, 660);
150+
drv->startMotor();
151+
drv->setMotorPWM(motor_pwm);
152+
drv->startScan();
153+
}
154+
155+
RPlidarNode::~RPlidarNode() {
156+
stop();
157+
RPlidarDriver::DisposeDriver(drv);
158+
}
159+
160+
void RPlidarNode::publish_scan(
161+
rplidar_response_measurement_node_t *nodes,
162+
size_t node_count, ros::Time start,
163+
double scan_time, float angle_min, float angle_max)
56164
{
57165
static int scan_count = 0;
58166
sensor_msgs::LaserScan scan_msg;
@@ -100,10 +208,10 @@ void publish_scan(ros::Publisher *pub,
100208
}
101209
}
102210

103-
pub->publish(scan_msg);
211+
scan_pub.publish(scan_msg);
104212
}
105213

106-
bool getRPLIDARDeviceInfo(RPlidarDriver * drv)
214+
bool RPlidarNode::getRPLIDARDeviceInfo(RPlidarDriver * drv)
107215
{
108216
u_result op_result;
109217
rplidar_response_device_info_t devinfo;
@@ -133,7 +241,7 @@ bool getRPLIDARDeviceInfo(RPlidarDriver * drv)
133241
return true;
134242
}
135243

136-
bool checkRPLIDARHealth(RPlidarDriver * drv)
244+
bool RPlidarNode::checkRPLIDARHealth(RPlidarDriver * drv)
137245
{
138246
u_result op_result;
139247
rplidar_response_device_health_t healthinfo;
@@ -157,162 +265,92 @@ bool checkRPLIDARHealth(RPlidarDriver * drv)
157265
}
158266
}
159267

160-
bool stop_motor(std_srvs::Empty::Request &req,
268+
bool RPlidarNode::stop_motor(std_srvs::Empty::Request &req,
161269
std_srvs::Empty::Response &res)
162270
{
163-
if(!drv)
164-
return false;
165-
166-
ROS_DEBUG("Stop motor");
167-
drv->stop();
168-
drv->stopMotor();
169-
return true;
271+
ROS_DEBUG("Stop motor");
272+
stop();
273+
return true;
170274
}
171275

172-
bool start_motor(std_srvs::Empty::Request &req,
276+
bool RPlidarNode::start_motor(std_srvs::Empty::Request &req,
173277
std_srvs::Empty::Response &res)
174278
{
175-
if(!drv)
176-
return false;
177-
ROS_DEBUG("Start motor");
178-
drv->startMotor();
179-
drv->startScan();;
180-
return true;
279+
ROS_DEBUG("Start motor");
280+
start();
281+
return true;
181282
}
182283

183-
int main(int argc, char * argv[]) {
184-
ros::init(argc, argv, "rplidar_node");
284+
void RPlidarNode::measure() {
285+
rplidar_response_measurement_node_t nodes[360*2];
286+
size_t count = _countof(nodes);
185287

186-
std::string serial_port;
187-
int serial_baudrate = 115200;
188-
std::string frame_id;
189-
bool inverted = false;
190-
bool angle_compensate = true;
191-
192-
ros::NodeHandle nh;
193-
ros::Publisher scan_pub = nh.advertise<sensor_msgs::LaserScan>("scan", 1000);
194-
ros::NodeHandle nh_private("~");
195-
nh_private.param<std::string>("serial_port", serial_port, "/dev/ttyUSB0");
196-
nh_private.param<int>("serial_baudrate", serial_baudrate, 115200);
197-
nh_private.param<std::string>("frame_id", frame_id, "laser_frame");
198-
nh_private.param<bool>("inverted", inverted, false);
199-
nh_private.param<bool>("angle_compensate", angle_compensate, true);
200-
201-
printf("RPLIDAR running on ROS package rplidar_ros\n"
202-
"SDK Version: "RPLIDAR_SDK_VERSION"\n");
203-
204-
u_result op_result;
205-
206-
// create the driver instance
207-
drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT);
208-
209-
if (!drv) {
210-
fprintf(stderr, "Create Driver fail, exit\n");
211-
return -2;
212-
}
213-
214-
// make connection...
215-
if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) {
216-
fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n"
217-
, serial_port.c_str());
218-
RPlidarDriver::DisposeDriver(drv);
219-
return -1;
220-
}
221-
222-
// get rplidar device info
223-
if (!getRPLIDARDeviceInfo(drv)) {
224-
return -1;
225-
}
226-
227-
// check health...
228-
if (!checkRPLIDARHealth(drv)) {
229-
RPlidarDriver::DisposeDriver(drv);
230-
return -1;
231-
}
288+
ros::Time start_scan_time = ros::Time::now();
289+
int op_result = drv->grabScanData(nodes, count);
290+
ros::Time end_scan_time = ros::Time::now();
291+
double scan_duration = (end_scan_time - start_scan_time).toSec() * 1e-3;
232292

233-
ros::ServiceServer stop_motor_service = nh.advertiseService("stop_motor", stop_motor);
234-
ros::ServiceServer start_motor_service = nh.advertiseService("start_motor", start_motor);
235-
236-
drv->startMotor();
237-
drv->startScan();
238-
239-
ros::Time start_scan_time;
240-
ros::Time end_scan_time;
241-
double scan_duration;
242-
while (ros::ok()) {
243-
244-
rplidar_response_measurement_node_t nodes[360*2];
245-
size_t count = _countof(nodes);
246-
247-
start_scan_time = ros::Time::now();
248-
op_result = drv->grabScanData(nodes, count);
249-
end_scan_time = ros::Time::now();
250-
scan_duration = (end_scan_time - start_scan_time).toSec() * 1e-3;
293+
if (op_result == RESULT_OK) {
294+
op_result = drv->ascendScanData(nodes, count);
251295

296+
float angle_min = DEG2RAD(0.0f);
297+
float angle_max = DEG2RAD(359.0f);
252298
if (op_result == RESULT_OK) {
253-
op_result = drv->ascendScanData(nodes, count);
254-
255-
float angle_min = DEG2RAD(0.0f);
256-
float angle_max = DEG2RAD(359.0f);
257-
if (op_result == RESULT_OK) {
258-
if (angle_compensate) {
259-
const int angle_compensate_nodes_count = 360;
260-
const int angle_compensate_multiple = 1;
261-
int angle_compensate_offset = 0;
262-
rplidar_response_measurement_node_t angle_compensate_nodes[angle_compensate_nodes_count];
263-
memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_t));
264-
int i = 0, j = 0;
265-
for( ; i < count; i++ ) {
266-
if (nodes[i].distance_q2 != 0) {
267-
float angle = (float)((nodes[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
268-
int angle_value = (int)(angle * angle_compensate_multiple);
269-
if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value;
270-
for (j = 0; j < angle_compensate_multiple; j++) {
271-
angle_compensate_nodes[angle_value-angle_compensate_offset+j] = nodes[i];
272-
}
299+
if (angle_compensate) {
300+
const int angle_compensate_nodes_count = 360;
301+
const int angle_compensate_multiple = 1;
302+
int angle_compensate_offset = 0;
303+
rplidar_response_measurement_node_t angle_compensate_nodes[angle_compensate_nodes_count];
304+
memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_t));
305+
int i = 0, j = 0;
306+
for( ; i < count; i++ ) {
307+
if (nodes[i].distance_q2 != 0) {
308+
float angle = (float)((nodes[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
309+
int angle_value = (int)(angle * angle_compensate_multiple);
310+
if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value;
311+
for (j = 0; j < angle_compensate_multiple; j++) {
312+
angle_compensate_nodes[angle_value-angle_compensate_offset+j] = nodes[i];
273313
}
274314
}
275-
276-
publish_scan(&scan_pub, angle_compensate_nodes, angle_compensate_nodes_count,
277-
start_scan_time, scan_duration, inverted,
278-
angle_min, angle_max,
279-
frame_id);
280-
} else {
281-
int start_node = 0, end_node = 0;
282-
int i = 0;
283-
// find the first valid node and last valid node
284-
while (nodes[i++].distance_q2 == 0);
285-
start_node = i-1;
286-
i = count -1;
287-
while (nodes[i--].distance_q2 == 0);
288-
end_node = i+1;
289-
290-
angle_min = DEG2RAD((float)(nodes[start_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
291-
angle_max = DEG2RAD((float)(nodes[end_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
292-
293-
publish_scan(&scan_pub, &nodes[start_node], end_node-start_node +1,
294-
start_scan_time, scan_duration, inverted,
295-
angle_min, angle_max,
296-
frame_id);
297-
}
298-
} else if (op_result == RESULT_OPERATION_FAIL) {
299-
// All the data is invalid, just publish them
300-
float angle_min = DEG2RAD(0.0f);
301-
float angle_max = DEG2RAD(359.0f);
302-
303-
publish_scan(&scan_pub, nodes, count,
304-
start_scan_time, scan_duration, inverted,
305-
angle_min, angle_max,
306-
frame_id);
307-
}
308-
}
315+
}
316+
317+
publish_scan(angle_compensate_nodes, angle_compensate_nodes_count,
318+
start_scan_time, scan_duration, angle_min, angle_max);
319+
} else {
320+
int start_node = 0, end_node = 0;
321+
int i = 0;
322+
// find the first valid node and last valid node
323+
while (nodes[i++].distance_q2 == 0);
324+
start_node = i-1;
325+
i = count -1;
326+
while (nodes[i--].distance_q2 == 0);
327+
end_node = i+1;
328+
329+
angle_min = DEG2RAD((float)(nodes[start_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
330+
angle_max = DEG2RAD((float)(nodes[end_node].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f);
331+
332+
publish_scan(&nodes[start_node], end_node-start_node +1,
333+
start_scan_time, scan_duration, angle_min, angle_max);
334+
}
335+
} else if (op_result == RESULT_OPERATION_FAIL) {
336+
// All the data is invalid, just publish them
337+
float angle_min = DEG2RAD(0.0f);
338+
float angle_max = DEG2RAD(359.0f);
309339

310-
ros::spinOnce();
340+
publish_scan(nodes, count,
341+
start_scan_time, scan_duration, angle_min, angle_max);
342+
}
311343
}
344+
ros::spinOnce();
345+
}
312346

313-
// done!
314-
drv->stop();
315-
drv->stopMotor();
316-
RPlidarDriver::DisposeDriver(drv);
347+
int main(int argc, char * argv[]) {
348+
ros::init(argc, argv, "rplidar_node");
349+
RPlidarNode node;
350+
while (ros::ok()) {
351+
node.measure();
352+
}
317353
return 0;
318354
}
355+
356+
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

0 commit comments

Comments
 (0)