follower.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <ros/ros.h>
32 #include <nodelet/nodelet.h>
33 #include <geometry_msgs/Twist.h>
34 #include <sensor_msgs/Image.h>
35 #include <visualization_msgs/Marker.h>
36 #include <roch_msgs/SetFollowState.h>
37 
38 #include "dynamic_reconfigure/server.h"
39 #include "roch_follower/FollowerConfig.h"
40 
42 
43 
44 namespace roch_follower
45 {
46 
47 //* The roch follower nodelet.
54 {
55 public:
60  RochFollower() : min_y_(0.1), max_y_(0.5),
61  min_x_(-0.2), max_x_(0.2),
62  max_z_(0.8), goal_z_(0.6),
63  z_scale_(1.0), x_scale_(5.0)
64  {
65 
66  }
67 
69  {
70  delete config_srv_;
71  }
72 
73 private:
74  double min_y_;
75  double max_y_;
76  double min_x_;
77  double max_x_;
78  double max_z_;
79  double goal_z_;
80  double z_scale_;
81  double x_scale_;
82  bool enabled_;
84  // Service for start/stop following
86 
87  // Dynamic reconfigure server
88  dynamic_reconfigure::Server<roch_follower::FollowerConfig>* config_srv_;
89 
95  virtual void onInit()
96  {
98  ros::NodeHandle& private_nh = getPrivateNodeHandle();
99 
100  private_nh.getParam("min_y", min_y_);
101  private_nh.getParam("max_y", max_y_);
102  private_nh.getParam("min_x", min_x_);
103  private_nh.getParam("max_x", max_x_);
104  private_nh.getParam("max_z", max_z_);
105  private_nh.getParam("goal_z", goal_z_);
106  private_nh.getParam("z_scale", z_scale_);
107  private_nh.getParam("x_scale", x_scale_);
108  private_nh.getParam("enabled", enabled_);
109 
110  cmdpub_ = private_nh.advertise<geometry_msgs::Twist> ("cmd_vel", 1);
111  markerpub_ = private_nh.advertise<visualization_msgs::Marker>("marker",1);
112  bboxpub_ = private_nh.advertise<visualization_msgs::Marker>("bbox",1);
113  sub_= nh.subscribe<sensor_msgs::Image>("depth/image_rect", 1, &RochFollower::imagecb, this);
114 
115  switch_srv_ = private_nh.advertiseService("change_state", &RochFollower::changeModeSrvCb, this);
116 
117  config_srv_ = new dynamic_reconfigure::Server<roch_follower::FollowerConfig>(private_nh);
118  dynamic_reconfigure::Server<roch_follower::FollowerConfig>::CallbackType f =
119  boost::bind(&RochFollower::reconfigure, this, _1, _2);
120  config_srv_->setCallback(f);
121  }
122 
123  void reconfigure(roch_follower::FollowerConfig &config, uint32_t level)
124  {
125  min_y_ = config.min_y;
126  max_y_ = config.max_y;
127  min_x_ = config.min_x;
128  max_x_ = config.max_x;
129  max_z_ = config.max_z;
130  goal_z_ = config.goal_z;
131  z_scale_ = config.z_scale;
132  x_scale_ = config.x_scale;
133  }
134 
142  void imagecb(const sensor_msgs::ImageConstPtr& depth_msg)
143  {
144 
145  // Precompute the sin function for each row and column
146  uint32_t image_width = depth_msg->width;
147  float x_radians_per_pixel = 60.0/57.0/image_width;
148  float sin_pixel_x[image_width];
149  for (int x = 0; x < image_width; ++x) {
150  sin_pixel_x[x] = sin((x - image_width/ 2.0) * x_radians_per_pixel);
151  }
152 
153  uint32_t image_height = depth_msg->height;
154  float y_radians_per_pixel = 45.0/57.0/image_width;
155  float sin_pixel_y[image_height];
156  for (int y = 0; y < image_height; ++y) {
157  // Sign opposite x for y up values
158  sin_pixel_y[y] = sin((image_height/ 2.0 - y) * y_radians_per_pixel);
159  }
160 
161  //X,Y,Z of the centroid
162  float x = 0.0;
163  float y = 0.0;
164  float z = 1e6;
165  //Number of points observed
166  unsigned int n = 0;
167 
168  //Iterate through all the points in the region and find the average of the position
169  const float* depth_row = reinterpret_cast<const float*>(&depth_msg->data[0]);
170  int row_step = depth_msg->step / sizeof(float);
171  int counter=0;
172  for (int v = 0; v < (int)depth_msg->height; ++v, depth_row += row_step)
173  {
174  for (int u = 0; u < (int)depth_msg->width; ++u)
175  {
177  if (!depth_image_proc::DepthTraits<float>::valid(depth) || depth > max_z_) continue;
178  float y_val = sin_pixel_y[v] * depth;
179  float x_val = sin_pixel_x[u] * depth;
180  if ( y_val > min_y_ && y_val < max_y_ &&
181  x_val > min_x_ && x_val < max_x_)
182  {
183  x += x_val;
184  y += y_val;
185  z = std::min(z, depth); //approximate depth as forward.
186  n++;
187  }
188  }
189  }
190  //If there are points, find the centroid and calculate the command goal.
191  //If there are no points, simply publish a stop goal.
192  if (n>4000)
193  {
194  x /= n;
195  y /= n;
196  if(z > max_z_){
197  ROS_INFO_THROTTLE(1, "Centroid too far away %f, stopping the robot\n", z);
198  if (enabled_)
199  {
200  cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
201  }
202  return;
203  }
204 
205  ROS_INFO_THROTTLE(1, "Centroid at %f %f %f with %d points", x, y, z, n);
206  publishMarker(x, y, z);
207 
208  if (enabled_)
209  {
210  geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
211  cmd->linear.x = (z - goal_z_) * z_scale_;
212  cmd->angular.z = -x * x_scale_;
213  cmdpub_.publish(cmd);
214  }
215  }
216  else
217  {
218  ROS_INFO_THROTTLE(1, "Not enough points(%d) detected, stopping the robot", n);
219  publishMarker(x, y, z);
220 
221  if (enabled_)
222  {
223  cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
224  }
225  }
226 
227  publishBbox();
228  }
229 
230  bool changeModeSrvCb(roch_msgs::SetFollowState::Request& request,
231  roch_msgs::SetFollowState::Response& response)
232  {
233  if ((enabled_ == true) && (request.state == request.STOPPED))
234  {
235  ROS_INFO("Change mode service request: following stopped");
236  cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
237  enabled_ = false;
238  }
239  else if ((enabled_ == false) && (request.state == request.FOLLOW))
240  {
241  ROS_INFO("Change mode service request: following (re)started");
242  enabled_ = true;
243  }
244 
245  response.result = response.OK;
246  return true;
247  }
248 
249  void publishMarker(double x,double y,double z)
250  {
251  visualization_msgs::Marker marker;
252  marker.header.frame_id = "/camera_rgb_optical_frame";
253  marker.header.stamp = ros::Time();
254  marker.ns = "my_namespace";
255  marker.id = 0;
256  marker.type = visualization_msgs::Marker::SPHERE;
257  marker.action = visualization_msgs::Marker::ADD;
258  marker.pose.position.x = x;
259  marker.pose.position.y = y;
260  marker.pose.position.z = z;
261  marker.pose.orientation.x = 0.0;
262  marker.pose.orientation.y = 0.0;
263  marker.pose.orientation.z = 0.0;
264  marker.pose.orientation.w = 1.0;
265  marker.scale.x = 0.2;
266  marker.scale.y = 0.2;
267  marker.scale.z = 0.2;
268  marker.color.a = 1.0;
269  marker.color.r = 1.0;
270  marker.color.g = 0.0;
271  marker.color.b = 0.0;
272  //only if using a MESH_RESOURCE marker type:
273  markerpub_.publish( marker );
274  }
275 
276  void publishBbox()
277  {
278  double x = (min_x_ + max_x_)/2;
279  double y = (min_y_ + max_y_)/2;
280  double z = (0 + max_z_)/2;
281 
282  double scale_x = (max_x_ - x)*2;
283  double scale_y = (max_y_ - y)*2;
284  double scale_z = (max_z_ - z)*2;
285 
286  visualization_msgs::Marker marker;
287  marker.header.frame_id = "/camera_rgb_optical_frame";
288  marker.header.stamp = ros::Time();
289  marker.ns = "my_namespace";
290  marker.id = 1;
291  marker.type = visualization_msgs::Marker::CUBE;
292  marker.action = visualization_msgs::Marker::ADD;
293  marker.pose.position.x = x;
294  marker.pose.position.y = -y;
295  marker.pose.position.z = z;
296  marker.pose.orientation.x = 0.0;
297  marker.pose.orientation.y = 0.0;
298  marker.pose.orientation.z = 0.0;
299  marker.pose.orientation.w = 1.0;
300  marker.scale.x = scale_x;
301  marker.scale.y = scale_y;
302  marker.scale.z = scale_z;
303  marker.color.a = 0.5;
304  marker.color.r = 0.0;
305  marker.color.g = 1.0;
306  marker.color.b = 0.0;
307  //only if using a MESH_RESOURCE marker type:
308  bboxpub_.publish( marker );
309  }
310 
315 };
316 
318 
319 }
void imagecb(const sensor_msgs::ImageConstPtr &depth_msg)
Callback for point clouds. Callback for depth images. It finds the centroid of the points in a box in...
Definition: follower.cpp:142
dynamic_reconfigure::Server< roch_follower::FollowerConfig > * config_srv_
Definition: follower.cpp:88
void publishMarker(double x, double y, double z)
Definition: follower.cpp:249
void publish(const boost::shared_ptr< M > &message) const
virtual void onInit()
OnInit method from node handle. OnInit method from node handle. Sets up the parameters and topics...
Definition: follower.cpp:95
GLint GLint GLint GLint GLint GLint y
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
string cmd
GLdouble GLdouble z
bool changeModeSrvCb(roch_msgs::SetFollowState::Request &request, roch_msgs::SetFollowState::Response &response)
Definition: follower.cpp:230
GLint GLint GLsizei GLsizei GLsizei depth
PLUGINLIB_DECLARE_CLASS(roch_follower, RochFollower, roch_follower::RochFollower, nodelet::Nodelet)
GLdouble n
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ros::NodeHandle & getPrivateNodeHandle() const
GLint level
GLuint counter
GLfloat f
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
#define ROS_INFO(...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle & getNodeHandle() const
const GLdouble * v
ros::Publisher markerpub_
Definition: follower.cpp:313
ros::ServiceServer switch_srv_
Definition: follower.cpp:85
void reconfigure(roch_follower::FollowerConfig &config, uint32_t level)
Definition: follower.cpp:123
const GLchar * marker
bool getParam(const std::string &key, std::string &s) const
unsigned int uint32_t
RochFollower()
The constructor for the follower. Constructor for the follower.
Definition: follower.cpp:60
#define ROS_INFO_THROTTLE(rate,...)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
GLint GLint GLint GLint GLint x


roch_follower
Author(s): Tony Pratkanis
autogenerated on Mon Jun 10 2019 14:44:24