autonomous_exploration.cpp
Go to the documentation of this file.
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     //ros::NodeHandle nh_;
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     //ros::ServiceClient tilt_laser_client_;
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     // tilt_laser_client_ = pnh_->serviceClient<pr2_msgs::SetPeriodicCmd>("laser_tilt_controller/set_periodic_cmd");
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     //ros::spin();
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                     //move robot to the received pose
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             //rise the spine up to scan
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                 //setLaserProfile("scan");
00151                 moveRobot(new_pose);
00152                 //setLaserProfile("navigation");
00153 
00154                 received_laser_signal_ = false;
00155                 while(!received_laser_signal_)
00156                   {
00157                     loop.sleep();
00158                   }
00159 
00160                 //TODO: Use laser service to sync
00161                 //Wait 11 seconds before taking a scan
00162                 sleep(wait_for_scan_);
00163                 //get a point cloud
00164                 get_pointcloud_ = true;
00165                 angles += angle_step_;
00166               }//end while                                      
00167         }// end if
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             //sleep(5.0);
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             //lower the spine to navigate some place else
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     //wait for the action server to come up
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         //exit(1);
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     //wait for the action server to come up
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;  //all the way up is 0.3
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         //req.command.period = 20.0;
00292         //req.command.amplitude = 0.8;
00293         //req.command.offset = 0.3;
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         //    ROS_INFO("pointcloud published");
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


autonomous_mapping
Author(s): Gheorghe Rus
autogenerated on Thu May 23 2013 08:58:16