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
00021 ros::Rate r(10);
00022
00023
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
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
00049 time_elapsed_for_task = getInterruptionTime() - start_time - getInterruptionsDuration();
00050 }
00051 else
00052 {
00053
00054 time_elapsed_for_task = real_time_elapsed - getInterruptionsDuration();
00055 }
00056
00057
00058
00059
00060
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