lj_laser.cpp
Go to the documentation of this file.
00001 #include <nlj_laser/lj_laser.h>
00002 #include <nlj_laser/hist.h>
00003 
00004 LJLaser::LJLaser(const std::string& name,
00005     const std::string& interface_name,
00006     const std::string& set_service_name) :
00007   lama_jockeys::LearningJockey(name),
00008   interface_name_(interface_name),
00009   set_service_name_(set_service_name)
00010 {
00011 }
00012 
00013 
00014 void LJLaser::onLearn() 
00015 {
00016   ROS_DEBUG(" LJLaser LEARNING");
00017   ros::Time start_time = ros::Time::now();
00018   assigned_ = false;
00019   laser_scan_subscriber_ = nh_.subscribe<sensor_msgs::LaserScan> ("base_scan",50,&LJLaser::handleLaser,this);
00020   // start navigating.
00021   ros::Rate r(10);
00022 /*  ros::spin();
00023   ROS_DEBUG("SPIN ENDS");*/
00024   while (ros::ok())
00025   {
00026     ros::spinOnce();
00027     if (server_.isPreemptRequested() )
00028     {
00029       ROS_INFO_STREAM(jockey_name_ << ": Preempted from LEARNING");
00030       break;
00031     }
00032       if (!ros::ok())
00033     {
00034       ROS_INFO_STREAM(jockey_name_ << ": ros not ok");
00035       break;
00036     }
00037     // update the feedback every 0.5 s.
00038     if (!server_.isActive()) 
00039     {
00040       ROS_DEBUG("Goal succeeded");
00041       break;
00042     }
00043     ros::Time current_time = ros::Time::now();
00044     ros::Duration real_time_elapsed = current_time - start_time;
00045     ros::Duration time_elapsed_for_task;
00046     if (isInterrupted())
00047     {
00048       // time_elapsed_for_task is frozen.
00049       time_elapsed_for_task = getInterruptionTime() - start_time - getInterruptionsDuration();
00050     }
00051     else
00052     {
00053       // time_elapsed_for_task runs.
00054       time_elapsed_for_task = real_time_elapsed - getInterruptionsDuration();
00055     }
00056     
00057     /*feedback_.current_state = lama_jockeys::LearnFeedback::LEARNING;
00058     feedback_.time_elapsed = time_elapsed_for_task;
00059     feedback_.completion = completion();
00060     server_.publishFeedback(feedback_);*/
00061     r.sleep();
00062   }
00063   ROS_DEBUG("LEARNING ENDS");
00064 }
00065 
00066 void LJLaser::onStop()
00067 {
00068   ROS_DEBUG("NJLaser STOP");
00069   laser_scan_subscriber_.shutdown();
00070   
00071   nlj_laser::SetLaserDescriptor ds;
00072   ds.request.descriptor.scans = descriptor_;
00073   ros::service::call(set_service_name_,ds);
00074 
00075   result_.final_state = lama_jockeys::LearnResult::DONE;
00076   result_.completion_time = ros::Duration(0);
00077   server_.setSucceeded(result_);
00078 }
00079 
00080 void LJLaser::handleLaser(sensor_msgs::LaserScan msg) {
00081   if (!assigned_) {
00082     reference_laser_scan_ = msg;
00083     descriptor_.push_back(reference_laser_scan_);
00084     assigned_ = true; 
00085   }
00086   int angle_hist_bin = 100;
00087   float max_distance = 0.3;
00088   float max_error = 0.2; 
00089   ROS_DEBUG("LEARNING laser arrived %lu",msg.ranges.size());
00090   geometry_msgs::Pose2D goal = localize(msg, reference_laser_scan_, angle_hist_bin);
00091   geometry_msgs::Pose2D fix;
00092   fix.x= fix.y=fix.theta = 0;
00093   float err = error (msg,reference_laser_scan_,goal);    
00094   float distance = sqrt (goal.x*goal.x+goal.y*goal.y) ;
00095   if (err > max_error) {
00096     ROS_INFO("error exceeds max limit");
00097     reference_laser_scan_ = previous_laser_scan_;
00098     descriptor_.push_back(reference_laser_scan_);
00099 
00100   } 
00101   if (distance > max_distance) {
00102     ROS_DEBUG("distance is longer than max");
00103     reference_laser_scan_ = msg;
00104     descriptor_.push_back(reference_laser_scan_);
00105   }
00106 
00107   previous_laser_scan_ = msg;
00108 
00109 }
00110 
00111 double LJLaser::completion()
00112 {
00113   return 0;
00114 }
00115 


nlj_laser
Author(s): Gaël Ecorchard , Karel Košnar
autogenerated on Thu Jun 6 2019 17:50:56