Go to the documentation of this file.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 <hector_path_follower/hector_path_follower.h>
00032 #include <hector_nav_msgs/GetRobotTrajectory.h>
00033
00034 class SimpleExplorationController
00035 {
00036 public:
00037 SimpleExplorationController()
00038 {
00039 ros::NodeHandle nh;
00040
00041 exploration_plan_service_client_ = nh.serviceClient<hector_nav_msgs::GetRobotTrajectory>("get_exploration_path");
00042
00043 path_follower_.initialize(&tfl_);
00044
00045 exploration_plan_generation_timer_ = nh.createTimer(ros::Duration(15.0), &SimpleExplorationController::timerPlanExploration, this, false );
00046 cmd_vel_generator_timer_ = nh.createTimer(ros::Duration(0.1), &SimpleExplorationController::timerCmdVelGeneration, this, false );
00047
00048 vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 10);
00049
00050 }
00051
00052 void timerPlanExploration(const ros::TimerEvent& e)
00053 {
00054 hector_nav_msgs::GetRobotTrajectory srv_exploration_plan;
00055
00056 if (exploration_plan_service_client_.call(srv_exploration_plan)){
00057 ROS_INFO("Generated exploration path with %u poses", (unsigned int)srv_exploration_plan.response.trajectory.poses.size());
00058 path_follower_.setPlan(srv_exploration_plan.response.trajectory.poses);
00059 }else{
00060 ROS_WARN("Service call for exploration service failed");
00061 }
00062 }
00063
00064 void timerCmdVelGeneration(const ros::TimerEvent& e)
00065 {
00066 geometry_msgs::Twist twist;
00067 path_follower_.computeVelocityCommands(twist);
00068 vel_pub_.publish(twist);
00069 }
00070
00071
00072 protected:
00073 ros::ServiceClient exploration_plan_service_client_;
00074 ros::Publisher vel_pub_;
00075
00076 tf::TransformListener tfl_;
00077
00078 pose_follower::HectorPathFollower path_follower_;
00079
00080 ros::Timer exploration_plan_generation_timer_;
00081 ros::Timer cmd_vel_generator_timer_;
00082 };
00083
00084 int main(int argc, char **argv) {
00085 ros::init(argc, argv, ROS_PACKAGE_NAME);
00086
00087 SimpleExplorationController ec;
00088
00089 ros::spin();
00090
00091 return 0;
00092 }