00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <ros/ros.h>
00031 #include <pluginlib/class_list_macros.h>
00032 #include <nodelet/nodelet.h>
00033 #include <geometry_msgs/Twist.h>
00034 #include <pcl_ros/point_cloud.h>
00035 #include <pcl/point_types.h>
00036 #include <visualization_msgs/Marker.h>
00037
00038
00039 #include "dynamic_reconfigure/server.h"
00040 #include "oculusprime/FollowerConfig.h"
00041
00042
00043 namespace oculusprime
00044 {
00045 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00046
00047
00053 class OculusprimeFollower : public nodelet::Nodelet
00054 {
00055 public:
00060 OculusprimeFollower() : min_y_(0.1), max_y_(0.5),
00061 min_x_(-0.35), max_x_(0.35),
00062 max_z_(1.3), goal_z_(0.7),
00063 z_scale_(1.0), x_scale_(3.5),
00064 z_tol_(0.1), x_tol_(0.1),
00065 enabled_(true)
00066 {
00067
00068 }
00069
00070 ~OculusprimeFollower()
00071 {
00072 delete config_srv_;
00073 }
00074
00075 private:
00076 double min_y_;
00077 double max_y_;
00078 double min_x_;
00079 double max_x_;
00080 double max_z_;
00081 double goal_z_;
00082 double z_scale_;
00083 double x_scale_;
00084 double z_tol_;
00085 double x_tol_;
00086 bool enabled_;
00088
00089
00090
00091
00092 dynamic_reconfigure::Server<oculusprime::FollowerConfig>* config_srv_;
00093
00099 virtual void onInit()
00100 {
00101 ros::NodeHandle& nh = getNodeHandle();
00102 ros::NodeHandle& private_nh = getPrivateNodeHandle();
00103
00104 private_nh.getParam("min_y", min_y_);
00105 private_nh.getParam("max_y", max_y_);
00106 private_nh.getParam("min_x", min_x_);
00107 private_nh.getParam("max_x", max_x_);
00108 private_nh.getParam("max_z", max_z_);
00109 private_nh.getParam("goal_z", goal_z_);
00110 private_nh.getParam("z_scale", z_scale_);
00111 private_nh.getParam("x_scale", x_scale_);
00112 private_nh.getParam("z_tol", z_tol_);
00113 private_nh.getParam("x_tol", x_tol_);
00114 private_nh.getParam("enabled", enabled_);
00115
00116 cmdpub_ = nh.advertise<geometry_msgs::Twist> ("/cmd_vel", 1);
00117 markerpub_ = nh.advertise<visualization_msgs::Marker>("marker",1);
00118 bboxpub_ = private_nh.advertise<visualization_msgs::Marker>("bbox",1);
00119 sub_= nh.subscribe<PointCloud>("depth/points", 1, &OculusprimeFollower::cloudcb, this);
00120
00121
00122
00123 config_srv_ = new dynamic_reconfigure::Server<oculusprime::FollowerConfig>(private_nh);
00124 dynamic_reconfigure::Server<oculusprime::FollowerConfig>::CallbackType f =
00125 boost::bind(&OculusprimeFollower::reconfigure, this, _1, _2);
00126 config_srv_->setCallback(f);
00127
00128 ROS_INFO("OculusprimeFollower init");
00129 }
00130
00131 void reconfigure(oculusprime::FollowerConfig &config, uint32_t level)
00132 {
00133 min_y_ = config.min_y;
00134 max_y_ = config.max_y;
00135 min_x_ = config.min_x;
00136 max_x_ = config.max_x;
00137 max_z_ = config.max_z;
00138 goal_z_ = config.goal_z;
00139 z_scale_ = config.z_scale;
00140 x_scale_ = config.x_scale;
00141 z_tol_ = config.z_tol;
00142 x_tol_ = config.x_tol;
00143 enabled_ = config.enabled;
00144 }
00145
00153 void cloudcb(const PointCloud::ConstPtr& cloud)
00154 {
00155
00156 float x = 0.0;
00157 float y = 0.0;
00158 float z = 1e6;
00159
00160 unsigned int n = 0;
00161
00162 BOOST_FOREACH (const pcl::PointXYZ& pt, cloud->points)
00163 {
00164
00165
00166 if (!std::isnan(x) && !std::isnan(y) && !std::isnan(z))
00167 {
00168
00169 if (-pt.y > min_y_ && -pt.y < max_y_ && pt.x < max_x_ && pt.x > min_x_ && pt.z < max_z_)
00170 {
00171
00172 x += pt.x;
00173 y += pt.y;
00174 z = std::min(z, pt.z);
00175 n++;
00176 }
00177 }
00178 }
00179
00180
00181
00182 if (n>4000)
00183 {
00184 x /= n;
00185 y /= n;
00186 if(z > max_z_){
00187 ROS_DEBUG("No valid points detected, stopping the robot");
00188 if (enabled_)
00189 {
00190 cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
00191 }
00192 return;
00193 }
00194
00195 ROS_DEBUG("Centroid at %f %f %f with %d points", x, y, z, n);
00196 publishMarker(x, y, z);
00197
00198 if (enabled_)
00199 {
00200 geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
00201
00202 if ( std::abs(z-goal_z_)<z_tol_) { cmd->linear.x = 0; }
00203 else { cmd->linear.x = (z - goal_z_) * z_scale_; }
00204
00205 if (std::abs(x) < x_tol_) { cmd->angular.z = 0; }
00206 else { cmd->angular.z = -x * x_scale_; }
00207
00208 cmdpub_.publish(cmd);
00209 }
00210 }
00211 else
00212 {
00213 ROS_DEBUG("No points detected, stopping the robot, # points= %d", n);
00214 publishMarker(x, y, z);
00215
00216 if (enabled_)
00217 {
00218 cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
00219 }
00220 }
00221
00222 publishBbox();
00223 }
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246 void publishMarker(double x,double y,double z)
00247 {
00248 visualization_msgs::Marker marker;
00249 marker.header.frame_id = "/camera_rgb_optical_frame";
00250 marker.header.stamp = ros::Time();
00251 marker.ns = "my_namespace";
00252 marker.id = 0;
00253 marker.type = visualization_msgs::Marker::SPHERE;
00254 marker.action = visualization_msgs::Marker::ADD;
00255 marker.pose.position.x = x;
00256 marker.pose.position.y = y;
00257 marker.pose.position.z = z;
00258 marker.pose.orientation.x = 0.0;
00259 marker.pose.orientation.y = 0.0;
00260 marker.pose.orientation.z = 0.0;
00261 marker.pose.orientation.w = 1.0;
00262 marker.scale.x = 0.2;
00263 marker.scale.y = 0.2;
00264 marker.scale.z = 0.2;
00265 marker.color.a = 1.0;
00266 marker.color.r = 1.0;
00267 marker.color.g = 0.0;
00268 marker.color.b = 0.0;
00269
00270 markerpub_.publish( marker );
00271 }
00272
00273 void publishBbox()
00274 {
00275 double x = (min_x_ + max_x_)/2;
00276 double y = (min_y_ + max_y_)/2;
00277 double z = (0 + max_z_)/2;
00278
00279 double scale_x = (max_x_ - x)*2;
00280 double scale_y = (max_y_ - y)*2;
00281 double scale_z = (max_z_ - z)*2;
00282
00283 visualization_msgs::Marker marker;
00284 marker.header.frame_id = "/camera_rgb_optical_frame";
00285 marker.header.stamp = ros::Time();
00286 marker.ns = "my_namespace";
00287 marker.id = 1;
00288 marker.type = visualization_msgs::Marker::CUBE;
00289 marker.action = visualization_msgs::Marker::ADD;
00290 marker.pose.position.x = x;
00291 marker.pose.position.y = -y;
00292 marker.pose.position.z = z;
00293 marker.pose.orientation.x = 0.0;
00294 marker.pose.orientation.y = 0.0;
00295 marker.pose.orientation.z = 0.0;
00296 marker.pose.orientation.w = 1.0;
00297 marker.scale.x = scale_x;
00298 marker.scale.y = scale_y;
00299 marker.scale.z = scale_z;
00300 marker.color.a = 0.5;
00301 marker.color.r = 0.0;
00302 marker.color.g = 1.0;
00303 marker.color.b = 0.0;
00304
00305 bboxpub_.publish( marker );
00306 }
00307
00308 ros::Subscriber sub_;
00309 ros::Publisher cmdpub_;
00310 ros::Publisher markerpub_;
00311 ros::Publisher bboxpub_;
00312 };
00313
00314 PLUGINLIB_EXPORT_CLASS(oculusprime::OculusprimeFollower, nodelet::Nodelet)
00315
00316 }