33 *
44 * @brief Bumper to pointcloud nodelet class implementation.
55 *
6- *
6+ * Copyright 2020
77 *
88 **/
99
1515
1616#include " ca_bumper2pc/ca_bumper2pc.hpp"
1717
18+ #include < string>
19+
1820namespace create_bumper2pc
1921{
2022
@@ -23,13 +25,14 @@ void Bumper2PcNodelet::bumperSensorCB(const ca_msgs::Bumper::ConstPtr& msg_bump)
2325 if (pointcloud_pub_.getNumSubscribers () == 0 )
2426 return ;
2527
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)
2831 return ;
2932
3033 prev_bumper = msg_bump->is_left_pressed || msg_bump->is_right_pressed ;
3134
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
3336 if (msg_bump->is_left_pressed && !(msg_bump->is_right_pressed ))
3437 {
3538 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)
5053 memcpy (&pointcloud_.data [1 * pointcloud_.point_step + pointcloud_.fields [0 ].offset ], &P_INF_X, sizeof (float ));
5154 }
5255
53- if (msg_bump->is_right_pressed && !(msg_bump->is_left_pressed ))
56+ if (msg_bump->is_right_pressed && !(msg_bump->is_left_pressed ))
5457 {
5558 memcpy (&pointcloud_.data [2 * pointcloud_.point_step + pointcloud_.fields [0 ].offset ], &p_side_x_, sizeof (float ));
5659 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)
7174 if (pointcloud_pub_.getNumSubscribers () == 0 )
7275 return ;
7376
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)
7784 return ;
7885
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+
8091
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
8393 if (msg_cliff->is_cliff_left )
8494 {
8595 memcpy (&pointcloud_.data [0 * pointcloud_.point_step + pointcloud_.fields [0 ].offset ], &p_side_x_, sizeof (float ));
@@ -126,15 +136,17 @@ void Bumper2PcNodelet::onInit()
126136 // them will probably fail.
127137 std::string base_link_frame;
128138 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 );
132144 nh.param <std::string>(" base_link_frame" , base_link_frame, " /base_link" );
133145
134146 // 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
138150
139151 // Prepare constant parts of the pointcloud message to be published
140152 pointcloud_.header .frame_id = base_link_frame;
@@ -180,7 +192,7 @@ void Bumper2PcNodelet::onInit()
180192 ROS_INFO (" Bumper/cliff pointcloud configured at distance %f and height %f from base frame" , pc_radius_, pc_height_);
181193}
182194
183- } // namespace create_bumper2pc
195+ } // namespace create_bumper2pc
184196
185197
186198PLUGINLIB_EXPORT_CLASS (create_bumper2pc::Bumper2PcNodelet, nodelet::Nodelet);
0 commit comments