Permalink
Find file
Fetching contributors…
Cannot retrieve contributors at this time
executable file 317 lines (281 sloc) 10.5 KB
/*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <geometry_msgs/Twist.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <visualization_msgs/Marker.h>
// #include <turtlebot_msgs/SetFollowState.h>
#include "dynamic_reconfigure/server.h"
#include "oculusprime/FollowerConfig.h"
namespace oculusprime
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
//* The oculusprime follower nodelet.
/**
* The oculusprime follower nodelet. Subscribes to point clouds
* from the 3dsensor, processes them, and publishes command vel
* messages.
*/
class OculusprimeFollower : public nodelet::Nodelet
{
public:
/*!
* @brief The constructor for the follower.
* Constructor for the follower.
*/
OculusprimeFollower() : min_y_(0.1), max_y_(0.5),
min_x_(-0.35), max_x_(0.35),
max_z_(1.3), goal_z_(0.7),
z_scale_(1.0), x_scale_(3.5),
z_tol_(0.1), x_tol_(0.1),
enabled_(true)
{
}
~OculusprimeFollower()
{
delete config_srv_;
}
private:
double min_y_; /**< The minimum y position of the points in the box. */
double max_y_; /**< The maximum y position of the points in the box. */
double min_x_; /**< The minimum x position of the points in the box. */
double max_x_; /**< The maximum x position of the points in the box. */
double max_z_; /**< The maximum z position of the points in the box. */
double goal_z_; /**< The distance away from the robot to hold the centroid */
double z_scale_; /**< The scaling factor for translational robot speed */
double x_scale_; /**< The scaling factor for rotational robot speed */
double z_tol_; /**< The target z centroid position allowable tolerance */
double x_tol_; /**< The target x centroid position allowable tolerance */
bool enabled_; /**< Enable/disable following; just prevents motor commands */
// Service for start/stop following
// ros::ServiceServer switch_srv_;
// Dynamic reconfigure server
dynamic_reconfigure::Server<oculusprime::FollowerConfig>* config_srv_;
/*!
* @brief OnInit method from node handle.
* OnInit method from node handle. Sets up the parameters
* and topics.
*/
virtual void onInit()
{
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& private_nh = getPrivateNodeHandle();
private_nh.getParam("min_y", min_y_);
private_nh.getParam("max_y", max_y_);
private_nh.getParam("min_x", min_x_);
private_nh.getParam("max_x", max_x_);
private_nh.getParam("max_z", max_z_);
private_nh.getParam("goal_z", goal_z_);
private_nh.getParam("z_scale", z_scale_);
private_nh.getParam("x_scale", x_scale_);
private_nh.getParam("z_tol", z_tol_);
private_nh.getParam("x_tol", x_tol_);
private_nh.getParam("enabled", enabled_);
cmdpub_ = nh.advertise<geometry_msgs::Twist> ("/cmd_vel", 1);
markerpub_ = nh.advertise<visualization_msgs::Marker>("marker",1);
bboxpub_ = private_nh.advertise<visualization_msgs::Marker>("bbox",1);
sub_= nh.subscribe<PointCloud>("depth/points", 1, &OculusprimeFollower::cloudcb, this);
// switch_srv_ = private_nh.advertiseService("change_state", &OculusprimeFollower::changeModeSrvCb, this);
config_srv_ = new dynamic_reconfigure::Server<oculusprime::FollowerConfig>(private_nh);
dynamic_reconfigure::Server<oculusprime::FollowerConfig>::CallbackType f =
boost::bind(&OculusprimeFollower::reconfigure, this, _1, _2);
config_srv_->setCallback(f);
ROS_INFO("OculusprimeFollower init");
}
void reconfigure(oculusprime::FollowerConfig &config, uint32_t level)
{
min_y_ = config.min_y;
max_y_ = config.max_y;
min_x_ = config.min_x;
max_x_ = config.max_x;
max_z_ = config.max_z;
goal_z_ = config.goal_z;
z_scale_ = config.z_scale;
x_scale_ = config.x_scale;
z_tol_ = config.z_tol;
x_tol_ = config.x_tol;
enabled_ = config.enabled;
}
/*!
* @brief Callback for point clouds.
* Callback for point clouds. Uses PCL to find the centroid
* of the points in a box in the center of the point cloud.
* Publishes cmd_vel messages with the goal from the cloud.
* @param cloud The point cloud message.
*/
void cloudcb(const PointCloud::ConstPtr& cloud)
{
//X,Y,Z of the centroid
float x = 0.0;
float y = 0.0;
float z = 1e6;
//Number of points observed
unsigned int n = 0;
//Iterate through all the points in the region and find the average of the position
BOOST_FOREACH (const pcl::PointXYZ& pt, cloud->points)
{
//First, ensure that the point's position is valid. This must be done in a seperate
//if because we do not want to perform comparison on a nan value.
if (!std::isnan(x) && !std::isnan(y) && !std::isnan(z))
{
//Test to ensure the point is within the aceptable box.
if (-pt.y > min_y_ && -pt.y < max_y_ && pt.x < max_x_ && pt.x > min_x_ && pt.z < max_z_)
{
//Add the point to the totals
x += pt.x;
y += pt.y;
z = std::min(z, pt.z);
n++;
}
}
}
//If there are points, find the centroid and calculate the command goal.
//If there are no points, simply publish a stop goal.
if (n>4000)
{
x /= n;
y /= n;
if(z > max_z_){
ROS_DEBUG("No valid points detected, stopping the robot");
if (enabled_)
{
cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
}
return;
}
ROS_DEBUG("Centroid at %f %f %f with %d points", x, y, z, n);
publishMarker(x, y, z);
if (enabled_)
{
geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
if ( std::abs(z-goal_z_)<z_tol_) { cmd->linear.x = 0; }
else { cmd->linear.x = (z - goal_z_) * z_scale_; }
if (std::abs(x) < x_tol_) { cmd->angular.z = 0; } // avoid backwards arc-turns
else { cmd->angular.z = -x * x_scale_; }
cmdpub_.publish(cmd);
}
}
else
{
ROS_DEBUG("No points detected, stopping the robot, # points= %d", n);
publishMarker(x, y, z);
if (enabled_)
{
cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
}
}
publishBbox();
}
/*
bool changeModeSrvCb(turtlebot_msgs::SetFollowState::Request& request,
turtlebot_msgs::SetFollowState::Response& response)
{
if ((enabled_ == true) && (request.state == request.STOPPED))
{
ROS_INFO("Change mode service request: following stopped");
cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
enabled_ = false;
}
else if ((enabled_ == false) && (request.state == request.FOLLOW))
{
ROS_INFO("Change mode service request: following (re)started");
enabled_ = true;
}
response.result = response.OK;
return true;
}
*/
void publishMarker(double x,double y,double z)
{
visualization_msgs::Marker marker;
marker.header.frame_id = "/camera_rgb_optical_frame";
marker.header.stamp = ros::Time();
marker.ns = "my_namespace";
marker.id = 0;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = x;
marker.pose.position.y = y;
marker.pose.position.z = z;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.x = 0.2;
marker.scale.y = 0.2;
marker.scale.z = 0.2;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
//only if using a MESH_RESOURCE marker type:
markerpub_.publish( marker );
}
void publishBbox()
{
double x = (min_x_ + max_x_)/2;
double y = (min_y_ + max_y_)/2;
double z = (0 + max_z_)/2;
double scale_x = (max_x_ - x)*2;
double scale_y = (max_y_ - y)*2;
double scale_z = (max_z_ - z)*2;
visualization_msgs::Marker marker;
marker.header.frame_id = "/camera_rgb_optical_frame";
marker.header.stamp = ros::Time();
marker.ns = "my_namespace";
marker.id = 1;
marker.type = visualization_msgs::Marker::CUBE;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = x;
marker.pose.position.y = -y;
marker.pose.position.z = z;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
marker.scale.x = scale_x;
marker.scale.y = scale_y;
marker.scale.z = scale_z;
marker.color.a = 0.5;
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
//only if using a MESH_RESOURCE marker type:
bboxpub_.publish( marker );
}
ros::Subscriber sub_;
ros::Publisher cmdpub_;
ros::Publisher markerpub_;
ros::Publisher bboxpub_;
};
PLUGINLIB_DECLARE_CLASS(oculusprime, OculusprimeFollower, oculusprime::OculusprimeFollower, nodelet::Nodelet);
}