3
3
*
4
4
* @brief Bumper to pointcloud nodelet class implementation.
5
5
*
6
- *
6
+ * Copyright 2020
7
7
*
8
8
**/
9
9
15
15
16
16
#include " ca_bumper2pc/ca_bumper2pc.hpp"
17
17
18
+ #include < string>
19
+
18
20
namespace create_bumper2pc
19
21
{
20
22
@@ -23,13 +25,14 @@ void Bumper2PcNodelet::bumperSensorCB(const ca_msgs::Bumper::ConstPtr& msg_bump)
23
25
if (pointcloud_pub_.getNumSubscribers () == 0 )
24
26
return ;
25
27
26
- // We publish just one "no events" pc (with all three points far away) and stop spamming when bumper/cliff conditions disappear
27
- if (! (msg_bump->is_left_pressed || msg_bump->is_right_pressed ) && ! prev_bumper)
28
+ // We publish just one "no events" pc (with all three points far away)
29
+ // and stop spamming when bumper/cliff conditions disappear
30
+ if (!(msg_bump->is_left_pressed || msg_bump->is_right_pressed ) && !prev_bumper)
28
31
return ;
29
32
30
33
prev_bumper = msg_bump->is_left_pressed || msg_bump->is_right_pressed ;
31
34
32
- // For any of {left/center/right} with no bumper/cliff event, we publish a faraway point that won't get used
35
+ // For any of {left/center/right} with no bumper/cliff event, we publish a faraway point that won't get used
33
36
if (msg_bump->is_left_pressed && !(msg_bump->is_right_pressed ))
34
37
{
35
38
memcpy (&pointcloud_.data [0 * pointcloud_.point_step + pointcloud_.fields [0 ].offset ], &p_side_x_, sizeof (float ));
@@ -50,7 +53,7 @@ void Bumper2PcNodelet::bumperSensorCB(const ca_msgs::Bumper::ConstPtr& msg_bump)
50
53
memcpy (&pointcloud_.data [1 * pointcloud_.point_step + pointcloud_.fields [0 ].offset ], &P_INF_X, sizeof (float ));
51
54
}
52
55
53
- if (msg_bump->is_right_pressed && !(msg_bump->is_left_pressed ))
56
+ if (msg_bump->is_right_pressed && !(msg_bump->is_left_pressed ))
54
57
{
55
58
memcpy (&pointcloud_.data [2 * pointcloud_.point_step + pointcloud_.fields [0 ].offset ], &p_side_x_, sizeof (float ));
56
59
memcpy (&pointcloud_.data [2 * pointcloud_.point_step + pointcloud_.fields [1 ].offset ], &n_side_y_, sizeof (float ));
@@ -71,15 +74,22 @@ void Bumper2PcNodelet::cliffSensorCB(const ca_msgs::Cliff::ConstPtr& msg_cliff)
71
74
if (pointcloud_pub_.getNumSubscribers () == 0 )
72
75
return ;
73
76
74
- // We publish just one "no events" pc (with all three points far away) and stop spamming when bumper/cliff conditions disappear
75
- if ( !(msg_cliff->is_cliff_left || msg_cliff->is_cliff_front_left || msg_cliff->is_cliff_front_right || msg_cliff->is_cliff_right )
76
- && !prev_cliff)
77
+ // We publish just one "no events" pc (with all three points far away)
78
+ // and stop spamming when bumper/cliff conditions disappear
79
+ if (!(msg_cliff->is_cliff_left ||
80
+ msg_cliff->is_cliff_front_left ||
81
+ msg_cliff->is_cliff_front_right ||
82
+ msg_cliff->is_cliff_right )
83
+ && !prev_cliff)
77
84
return ;
78
85
79
- prev_cliff = msg_cliff->is_cliff_left || msg_cliff->is_cliff_front_left || msg_cliff->is_cliff_front_right || msg_cliff->is_cliff_right ;
86
+ prev_cliff = msg_cliff->is_cliff_left ||
87
+ msg_cliff->is_cliff_front_left ||
88
+ msg_cliff->is_cliff_front_right ||
89
+ msg_cliff->is_cliff_right ;
90
+
80
91
81
-
82
- // For any of {left/center/right} with no bumper/cliff event, we publish a faraway point that won't get used
92
+ // For any of {left/center/right} with no bumper/cliff event, we publish a faraway point that won't get used
83
93
if (msg_cliff->is_cliff_left )
84
94
{
85
95
memcpy (&pointcloud_.data [0 * pointcloud_.point_step + pointcloud_.fields [0 ].offset ], &p_side_x_, sizeof (float ));
@@ -126,15 +136,17 @@ void Bumper2PcNodelet::onInit()
126
136
// them will probably fail.
127
137
std::string base_link_frame;
128
138
double r, h, angle;
129
- nh.param (" pointcloud_radius" , r, 0.25 ); pc_radius_ = r;
130
- nh.param (" pointcloud_height" , h, 0.04 ); pc_height_ = h;
131
- nh.param (" side_point_angle" , angle, 0.34906585 );
139
+ nh.param (" pointcloud_radius" , r, 0.25 );
140
+ pc_radius_ = r;
141
+ nh.param (" pointcloud_height" , h, 0.04 );
142
+ pc_height_ = h;
143
+ nh.param (" side_point_angle" , angle, 0.34906585 );
132
144
nh.param <std::string>(" base_link_frame" , base_link_frame, " /base_link" );
133
145
134
146
// Lateral points x/y coordinates; we need to store float values to memcopy later
135
- p_side_x_ = + pc_radius_* sin (angle); // angle degrees from vertical
136
- p_side_y_ = + pc_radius_* cos (angle); // angle degrees from vertical
137
- n_side_y_ = - pc_radius_* cos (angle); // angle degrees from vertical
147
+ p_side_x_ = + pc_radius_ * sin (angle); // angle degrees from vertical
148
+ p_side_y_ = + pc_radius_ * cos (angle); // angle degrees from vertical
149
+ n_side_y_ = - pc_radius_ * cos (angle); // angle degrees from vertical
138
150
139
151
// Prepare constant parts of the pointcloud message to be published
140
152
pointcloud_.header .frame_id = base_link_frame;
@@ -180,7 +192,7 @@ void Bumper2PcNodelet::onInit()
180
192
ROS_INFO (" Bumper/cliff pointcloud configured at distance %f and height %f from base frame" , pc_radius_, pc_height_);
181
193
}
182
194
183
- } // namespace create_bumper2pc
195
+ } // namespace create_bumper2pc
184
196
185
197
186
198
PLUGINLIB_EXPORT_CLASS (create_bumper2pc::Bumper2PcNodelet, nodelet::Nodelet);
0 commit comments