00001 #include "ros/ros.h"
00002
00003 #include "sensor_msgs/PointCloud.h"
00004 #include "geometry_msgs/Pose.h"
00005 #include "move_base_msgs/MoveBaseAction.h"
00006 #include "actionlib/client/simple_action_client.h"
00007 #include "pcl/common/angles.h"
00008 #include "LinearMath/btQuaternion.h"
00009 #include <pr2_controllers_msgs/SingleJointPositionAction.h>
00010 #include <pr2_msgs/SetPeriodicCmd.h>
00011 #include <pr2_msgs/PeriodicCmd.h>
00012 #include <pr2_msgs/LaserTrajCmd.h>
00013 #include <pr2_msgs/SetLaserTrajCmd.h>
00014 #include "pr2_msgs/LaserScannerSignal.h"
00015 #include <pcl_ros/publisher.h>
00016 #include <pcl_ros/pcl_nodelet.h>
00017 #include <pluginlib/class_list_macros.h>
00018 #include "pcl/io/pcd_io.h"
00019 #include "pcl/filters/voxel_grid.h"
00020 #include <geometry_msgs/PoseArray.h>
00021 #include <visualization_msgs/Marker.h>
00022
00023 namespace autonomous_exploration
00024 {
00025
00026 class AutonomousExploration:public pcl_ros::PCLNodelet
00027 {
00028 public:
00029 AutonomousExploration();
00030 ~AutonomousExploration();
00031 void autonomousExplorationCallBack(const geometry_msgs::PoseArray& pose_msg);
00032 void pointcloudCallBack(const sensor_msgs::PointCloud2& pointcloud_msg);
00033 void laserScannerSignalCallBack(const pr2_msgs::LaserScannerSignal laser_scanner_signal_msg);
00034 void moveRobot(geometry_msgs::Pose goal_pose);
00035 void moveTorso(double position, double velocity, std::string direction);
00036 void setLaserProfile(std::string mode);
00037 void onInit();
00038 void spin();
00039 protected:
00040 using pcl_ros::PCLNodelet::pnh_;
00041 private:
00042
00043 ros::Subscriber pose_subscriber_;
00044 ros::Subscriber pointcloud_subscriber_;
00045 ros::Subscriber laser_signal_subscriber_;
00046 ros::Publisher pointcloud_publisher_;
00047 ros::Publisher tilt_laser_publisher_;
00048 ros::Publisher tilt_laser_traj_cmd_publisher_;
00049 ros::Publisher goal_pub_;
00050 ros::Publisher incremental_pointcloud_pub_;
00051 std::string subscribe_pose_topic_;
00052 boost::thread spin_thread_;
00053 geometry_msgs::PoseArray pose_msg_;
00054 visualization_msgs::Marker marker_;
00055 bool get_pointcloud_, received_pose_, received_laser_signal_,move_robot_, publish_cloud_incrementally_;
00056 double angle_step_, wait_for_scan_, downsample_leaf_size_, rotate_amount_;
00057 int scans_count_;
00058
00059 pcl::PointCloud<pcl::PointXYZ> cloud_merged_;
00060 };
00061
00062 AutonomousExploration::AutonomousExploration()
00063 {
00064
00065 }
00066 AutonomousExploration::~AutonomousExploration()
00067 {
00068 ROS_INFO("Shutting down autonomous_exploration node.");
00069 }
00070
00071 void AutonomousExploration::onInit()
00072 {
00073 pcl_ros::PCLNodelet::onInit ();
00074 move_robot_=false;
00075 scans_count_=0;
00076 pnh_->param("subscribe_pose_topic", subscribe_pose_topic_, std::string("/robot_pose"));
00077 pnh_->param("publish_cloud_incrementally", publish_cloud_incrementally_, false);
00078 pnh_->param("angle_step", angle_step_, 30.0);
00079 pnh_->param("wait_for_scan", wait_for_scan_, 11.0);
00080 pnh_->param("downsample_leaf_size", downsample_leaf_size_, 0.02);
00081 pnh_->param("rotate_amount_", rotate_amount_, 360.0);
00082 ROS_INFO("autonomous_exploration node is up and running.");
00083 get_pointcloud_ = false;
00084 received_pose_ = false;
00085 received_laser_signal_ = false;
00086 pose_subscriber_ = pnh_->subscribe(subscribe_pose_topic_, 100, &AutonomousExploration::autonomousExplorationCallBack, this);
00087 pointcloud_publisher_ = pnh_->advertise<sensor_msgs::PointCloud2>("pointcloud", 100);
00088 incremental_pointcloud_pub_ = pnh_->advertise<sensor_msgs::PointCloud2>("incremental_pointcloud",100);
00089 goal_pub_=pnh_->advertise<visualization_msgs::Marker>("goal_pose",100,true);
00090 tilt_laser_publisher_ = pnh_->advertise<pr2_msgs::PeriodicCmd>("/laser_tilt_controller/set_periodic_cmd", 1);
00091 pointcloud_subscriber_ = pnh_->subscribe("/points2_out", 100, &AutonomousExploration::pointcloudCallBack, this);
00092 laser_signal_subscriber_ = pnh_->subscribe(
00093 "/laser_tilt_controller/laser_scanner_signal", 1, &AutonomousExploration::laserScannerSignalCallBack, this);
00094
00095 tilt_laser_traj_cmd_publisher_ = pnh_->advertise<pr2_msgs::LaserTrajCmd>("/laser_tilt_controller/set_traj_cmd", 1);
00096 spin_thread_ = boost::thread (boost::bind (&AutonomousExploration::spin, this));
00097
00098 moveTorso(0.01, 1.0, "down");
00099 setLaserProfile("navigation");
00100 }
00101 void AutonomousExploration::spin()
00102 {
00103 ros::Rate loop(1);
00104 while(pnh_->ok())
00105 {
00106 loop.sleep();
00107 if(received_pose_ )
00108 {
00109 move_robot_=false;
00110 geometry_msgs::Pose new_pose;
00111 for (unsigned int i=0;i<pose_msg_.poses.size();i++)
00112 {
00113 if (!move_robot_)
00114 {
00115
00116 ROS_INFO("Moving the robot");
00117 moveRobot(pose_msg_.poses[i]);
00118 ROS_INFO("End of Moving the robot");
00119 new_pose = pose_msg_.poses[i];
00120
00121 }
00122 else break;
00123 }
00124 if (!move_robot_)
00125 {
00126 ROS_WARN("No good pose to navigate to. Exiting.");
00127 exit(1);
00128 }
00129 scans_count_++;
00130
00131
00132 setLaserProfile("scan");
00133 moveTorso(0.3, 1.0, "up");
00134 if (scans_count_==1)
00135 {
00136 double angles =0.0 ;
00137 btQuaternion initial_rotation( new_pose.orientation.x,new_pose.orientation.y,new_pose.orientation.z,new_pose.orientation.w);
00138 btScalar angle = - initial_rotation.getAngle();
00139 std::cerr<<"initial angle: "<<angle<<std::endl;
00140 while(angles <= rotate_amount_)
00141 {
00142 std::cerr<<"angle before: "<<angle<<std::endl;
00143 btQuaternion q (angle, 0, 0);
00144 angle= angle + pcl::deg2rad(angle_step_);
00145
00146 new_pose.orientation.x = q.x();
00147 new_pose.orientation.y = q.y();
00148 new_pose.orientation.z = q.z();
00149 new_pose.orientation.w = q.w();
00150
00151 moveRobot(new_pose);
00152
00153
00154 received_laser_signal_ = false;
00155 while(!received_laser_signal_)
00156 {
00157 loop.sleep();
00158 }
00159
00160
00161
00162 sleep(wait_for_scan_);
00163
00164 get_pointcloud_ = true;
00165 angles += angle_step_;
00166 }
00167 }
00168 else
00169 {
00170 btQuaternion initial_rotation( new_pose.orientation.x,new_pose.orientation.y,new_pose.orientation.z,new_pose.orientation.w);
00171 btScalar angle = - initial_rotation.getAngle();
00172 btQuaternion q (angle, 0, 0);
00173 new_pose.orientation.x = q.x();
00174 new_pose.orientation.y = q.y();
00175 new_pose.orientation.z = q.z();
00176 new_pose.orientation.w = q.w();
00177 get_pointcloud_ = true;
00178 moveRobot(new_pose);
00179
00180 }
00181
00182
00183 while (!get_pointcloud_)
00184 {
00185 loop.sleep();
00186 }
00187 ROS_INFO("Published aggregated cloud with %ld points", cloud_merged_.points.size());
00188 cloud_merged_.header.stamp = ros::Time::now();
00189 pointcloud_publisher_.publish(cloud_merged_);
00190
00191 received_pose_ = false;
00192
00193 moveTorso(0.01, 1.0, "down");
00194 setLaserProfile("navigation");
00195
00196 }
00197 }
00198 }
00199 void AutonomousExploration::moveRobot(geometry_msgs::Pose goal_pose)
00200 {
00201 actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base", true);
00202
00203
00204 while(!ac.waitForServer(ros::Duration(5.0)))
00205 {
00206 ROS_INFO("Waiting for the move_base action server to come up");
00207 }
00208
00209 move_base_msgs::MoveBaseGoal goal;
00210 goal.target_pose.header.frame_id = "/map";
00211 goal.target_pose.header.stamp = ros::Time::now();
00212 goal.target_pose.pose = goal_pose;
00213 marker_.header.frame_id="/map";
00214 marker_.header.stamp=ros::Time::now();
00215 marker_.type=visualization_msgs::Marker::ARROW;
00216 marker_.action=visualization_msgs::Marker::ADD;
00217 marker_.ns="autonomosu_exploration";
00218 marker_.pose=goal_pose;
00219 marker_.id= 0;
00220 marker_.scale.x= 1.0;
00221 marker_.scale.y= 4.0;
00222 marker_.scale.z= 1.0;
00223 marker_.color.r= 1.0f;
00224 marker_.color.g= 1.0f;
00225 marker_.color.b= 0.0f;
00226 marker_.color.a = 1.0f;
00227 marker_.lifetime= ros::Duration::Duration();
00228 goal_pub_.publish(marker_);
00229 ROS_INFO("Sending goal");
00230 ac.sendGoal(goal);
00231 ac.waitForResult();
00232
00233 ROS_INFO("Pose Position x: %f, y:%f, z:%f Orientation x:%f, y:%f, z:%f, w:%f", goal.target_pose.pose.position.x,
00234 goal.target_pose.pose.position.y, goal.target_pose.pose.position.z, goal.target_pose.pose.orientation.x,
00235 goal.target_pose.pose.orientation.y, goal.target_pose.pose.orientation.z, goal.target_pose.pose.orientation.w);
00236
00237 if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00238 {
00239 ROS_INFO("Robot has moved to the goal pose");
00240 move_robot_=true;
00241 }
00242 else
00243 {
00244 ROS_ERROR("Error in moving robot to goal pose.");
00245
00246 move_robot_=false;
00247 }
00248 }
00249
00250 void AutonomousExploration::moveTorso(double position, double velocity, std::string direction)
00251 {
00252 actionlib::SimpleActionClient<pr2_controllers_msgs::SingleJointPositionAction> tc ("torso_controller/position_joint_action", true);
00253
00254
00255 while(!tc.waitForServer(ros::Duration(5.0)))
00256 {
00257 ROS_INFO("Waiting for the torso action server to come up");
00258 }
00259 pr2_controllers_msgs::SingleJointPositionGoal goal;
00260 goal.position = position;
00261 goal.min_duration = ros::Duration(2.0);
00262 goal.max_velocity = velocity;
00263
00264 ROS_INFO("Sending '%s' goal", direction.c_str());
00265 tc.sendGoal(goal);
00266 tc.waitForResult();
00267
00268 if(tc.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00269 {
00270 ROS_INFO("Torso moved %s.", direction.c_str());
00271 }
00272 else
00273 {
00274 ROS_ERROR("Error in moving torso %s.", direction.c_str());
00275 }
00276 }
00277
00278 void AutonomousExploration::setLaserProfile(std::string mode)
00279 {
00280 if(mode == "scan")
00281 {
00282 pr2_msgs::PeriodicCmd periodic_cmd_msg;
00283
00284 periodic_cmd_msg.header.frame_id = "/map";
00285 periodic_cmd_msg.header.stamp = ros::Time::now();
00286
00287 periodic_cmd_msg.profile = "linear";
00288 periodic_cmd_msg.offset = 0.3;
00289 periodic_cmd_msg.period = 20.0;
00290 periodic_cmd_msg.amplitude = 0.8;
00291
00292
00293
00294 tilt_laser_publisher_.publish(periodic_cmd_msg);
00295 ROS_INFO("Tilt laser successfully published. Mode: %s", mode.c_str());
00296 }
00297 else if(mode == "navigation")
00298 {
00299 pr2_msgs::LaserTrajCmd laser_traj_cmd_msg;
00300
00301 laser_traj_cmd_msg.header.frame_id = "/map";
00302 laser_traj_cmd_msg.header.stamp = ros::Time::now();
00303
00304 laser_traj_cmd_msg.profile = "blended_linear";
00305 laser_traj_cmd_msg.position.resize(3);
00306 ROS_INFO("Resized");
00307 laser_traj_cmd_msg.position[0] = 1.05;
00308 laser_traj_cmd_msg.position[1] = -0.7;
00309 laser_traj_cmd_msg.position[2] = 1.05;
00310
00311 ros::Duration dur;
00312 laser_traj_cmd_msg.time_from_start.resize(3);
00313 ROS_INFO("Resized time_from_start");
00314 laser_traj_cmd_msg.time_from_start[0] = dur.fromSec(0.0);
00315 laser_traj_cmd_msg.time_from_start[1] = dur.fromSec(1.8);
00316 laser_traj_cmd_msg.time_from_start[2] = dur.fromSec(2.3125);
00317
00318 laser_traj_cmd_msg.max_velocity = 10;
00319 laser_traj_cmd_msg.max_acceleration = 30;
00320 tilt_laser_traj_cmd_publisher_.publish(laser_traj_cmd_msg);
00321 ROS_INFO("Tilt Laser Traj Command successfully called. Mode: %s\n", mode.c_str());
00322
00323 }
00324 }
00325 void AutonomousExploration::laserScannerSignalCallBack(const pr2_msgs::LaserScannerSignal laser_scanner_signal_msg)
00326 {
00327 if(!received_laser_signal_)
00328 {
00329 received_laser_signal_ = true;
00330 }
00331 }
00332 void AutonomousExploration::pointcloudCallBack(const sensor_msgs::PointCloud2& pointcloud_msg)
00333 {
00334 if(get_pointcloud_)
00335 {
00336 ROS_INFO("pointcloud received");
00337
00338 if (publish_cloud_incrementally_)
00339 {
00340 incremental_pointcloud_pub_.publish(pointcloud_msg);
00341 }
00342 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp (new pcl::PointCloud<pcl::PointXYZ> ());
00343 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp_filtered (new pcl::PointCloud<pcl::PointXYZ> ());
00344 pcl::fromROSMsg(pointcloud_msg, *cloud_tmp);
00345 pcl::VoxelGrid<pcl::PointXYZ> sor;
00346 sor.setInputCloud (cloud_tmp);
00347 sor.setLeafSize (downsample_leaf_size_, downsample_leaf_size_, downsample_leaf_size_);
00348 sor.filter (*cloud_tmp_filtered);
00349
00350
00351 if (cloud_merged_.points.size() == 0)
00352 cloud_merged_ = *cloud_tmp_filtered;
00353 else
00354 cloud_merged_ += *cloud_tmp_filtered;
00355
00356 get_pointcloud_ = false;
00357 }
00358 }
00359 void AutonomousExploration::autonomousExplorationCallBack(const geometry_msgs::PoseArray& pose_msg)
00360 {
00361 pose_msg_=pose_msg;
00362 received_pose_ = true;
00363 ROS_INFO("Received a Pose");
00364
00365 }
00366
00367 }
00368 PLUGINLIB_DECLARE_CLASS(autonomous_mapping,AutonomousExploration,autonomous_exploration::AutonomousExploration,nodelet::Nodelet);
00369