walk_analyzer.cpp
Go to the documentation of this file.
00001 #include <vigir_walk_monitor/walk_analyzer.h>
00002 
00003 namespace vigir_footstep_planning
00004 {
00005 WalkAnalyzer::WalkAnalyzer(ros::NodeHandle& nh)
00006 {
00007   nh.getParam("logging_dir", logging_dir);
00008   //data_header = "index step_x step_y step_z droll dpitch dyaw distance_2D distance_3D duration swing_height";
00009   data_header = "index | step_x step_y step_yaw | dx dy dyaw | swing_distance duration";
00010   reset();
00011 }
00012 
00013 WalkAnalyzer::~WalkAnalyzer()
00014 {
00015 }
00016 
00017 void WalkAnalyzer::reset()
00018 {
00019   current_goal_id = actionlib_msgs::GoalID();
00020   current_step_plan = msgs::StepPlan();
00021   step_feedback_map.clear();
00022 }
00023 
00024 void WalkAnalyzer::executeStepPlanGoalCallback(const msgs::ExecuteStepPlanActionGoal& execute_goal)
00025 {
00026   // check if new step plan is executed
00027   if (execute_goal.goal_id.id != current_goal_id.id || execute_goal.goal_id.stamp != current_goal_id.stamp)
00028   {
00029     if (!step_feedback_map.empty())
00030       writeResult(logging_dir, data_header);
00031     reset();
00032   }
00033   else
00034     return;
00035 
00036   if (!execute_goal.goal.step_plan.steps.empty())
00037   {
00038     current_goal_id = execute_goal.goal_id;
00039     current_step_plan = execute_goal.goal.step_plan;
00040 
00041     const msgs::Step& initial = current_step_plan.steps.front();
00042 
00043     msgs::Step start_left, start_right;
00044     start_left.foot = current_step_plan.start.left;
00045     start_right.foot = current_step_plan.start.right;
00046 
00047     // adding initial states
00048     step_feedback_map[initial.step_index-1] = initial.foot.foot_index == msgs::Foot::LEFT ? start_right : start_left;
00049     step_feedback_map[initial.step_index] = initial.foot.foot_index == msgs::Foot::LEFT ? start_left : start_right;
00050 
00051     ROS_INFO("[WalkAnalyzer] Got new step plan with %lu steps.", current_step_plan.steps.size());
00052   }
00053 }
00054 
00055 void WalkAnalyzer::executeStepPlanResultCallback(const msgs::ExecuteStepPlanActionResult& execute_result)
00056 {
00057   if (!step_feedback_map.empty())
00058     writeResult(logging_dir, data_header);
00059   reset();
00060 }
00061 
00062 void WalkAnalyzer::stepFeedbackCallback(const msgs::StepPlanFeedback& feedback)
00063 {
00064   if (current_step_plan.steps.empty() || feedback.steps.empty())
00065     return;
00066 
00067   for (std::vector<msgs::Step>::const_iterator itr = feedback.steps.begin(); itr != feedback.steps.end(); itr++)
00068   {
00069     if (itr->step_index <= current_step_plan.steps.back().step_index)
00070       step_feedback_map[itr->step_index] = *itr;
00071   }
00072 }
00073 
00074 bool WalkAnalyzer::analyze(const msgs::Step& previous, const msgs::Step& current, const msgs::Step& next, std::string& result) const
00075 {
00076   std::ostringstream ss;
00077   ss << std::fixed << std::setprecision(6);
00078   std::ostringstream temp;
00079   temp << std::fixed << std::setprecision(6);
00080 
00081   // get travel distance
00082   geometry_msgs::Pose dswing;
00083   getDeltaStep(previous.foot, next.foot, dswing);
00084 
00085   tf::Quaternion swing_q;
00086   tf::quaternionMsgToTF(dswing.orientation, swing_q);
00087 
00088   double swing_r, swing_p, swing_y;
00089   tf::Matrix3x3(swing_q).getRPY(swing_r, swing_p, swing_y);
00090 
00091   // reconstruct delta step
00092   geometry_msgs::Pose dstep;
00093   getDeltaStep(current.foot, next.foot, dstep);
00094 
00095   tf::Quaternion step_q;
00096   tf::quaternionMsgToTF(dstep.orientation, step_q);
00097 
00098   double step_r, step_p, step_y;
00099   tf::Matrix3x3(step_q).getRPY(step_r, step_p, step_y);
00100 
00101   // generate message
00102   ss << next.step_index << " | "
00103      << dstep.position.x << " " << dstep.position.y << " "
00104      /*<< r << " " << p << " "*/ << step_y << " | "
00105      << dswing.position.x << " " << dswing.position.y << " " //<< dstep.position.z << " "
00106      /*<< r << " " << p << " "*/ << swing_y << " | "
00107      << sqrt(dswing.position.x*dswing.position.x + dswing.position.y*dswing.position.y) << " "
00108      //<< sqrt(dstep.position.x*dstep.position.x + dstep.position.y*dstep.position.y + dstep.position.z*dstep.position.z) << " "
00109      << next.step_duration;// << " " <<  next.swing_height;
00110 
00111   if (!temp.str().empty())
00112     temp << ", ";
00113 
00114   temp << "{" << sqrt(dswing.position.x*dswing.position.x + dswing.position.y*dswing.position.y) << ", " << next.step_duration << "}";
00115 
00116   //ss << "\n" << temp.str();
00117 
00118   result = ss.str();
00119 
00120   return true;
00121 }
00122 
00123 bool WalkAnalyzer::analyzeData(const std::map<int, msgs::Step>& data, std::string& result) const
00124 {
00125   result.clear();
00126 
00127   if (data.empty())
00128     return false;
00129 
00130   if (data.size() < 3)
00131     return true;
00132 
00133   std::map<int, msgs::Step>::const_iterator itr = data.begin();
00134   msgs::Step previous = itr->second; itr++;
00135   msgs::Step current = itr->second; itr++;
00136 
00137   for (; itr != data.end(); itr++)
00138   {
00139     const msgs::Step& next = itr->second;
00140 
00141     if (current.step_index+1 != next.step_index)
00142     {
00143       ROS_ERROR("[WalkAnalyzer] analyzeData: Inconsistent data record! Stopping analysis.");
00144       return false;
00145     }
00146 
00147     std::string temp;
00148     if (!analyze(previous, current, next, temp))
00149       return false;
00150 
00151     if (!result.empty())
00152       result += "\n";
00153     result += temp;
00154 
00155     previous = current;
00156     current = next;
00157   }
00158 
00159   return true;
00160 }
00161 
00162 void WalkAnalyzer::writeResult(const std::string& logging_dir, const std::string& data_header)
00163 {
00164   std::string result;
00165   if (!analyzeData(step_feedback_map, result))
00166   {
00167     ROS_ERROR("[WalkAnalyzer] Error while analyzing data!");
00168     return;
00169   }
00170   if (result.empty())
00171     return;
00172 
00173   std::string date_time = currentDateTime();
00174 
00175   // write to seperate file
00176   std::string filename = logging_dir + "/" + date_time + ".txt";
00177   std::ofstream file(filename);
00178 
00179   if (file.is_open())
00180   {
00181     file << data_header + "\n";
00182     file << result;
00183     file.close();
00184     ROS_INFO("[WalkAnalyzer] Written step plan result in '%s'", filename.c_str());
00185   }
00186   else
00187     ROS_ERROR("[WalkAnalyzer] Can't open file '%s'", filename.c_str());
00188 
00189   // append to total file
00190   filename = logging_dir + "/overall.txt";
00191   file.open(filename, std::ios_base::app);
00192 
00193   if (file.is_open())
00194   {
00195     file << result << "\n";
00196     file.close();
00197     ROS_INFO("[WalkAnalyzer] Written step plan result in '%s'", filename.c_str());
00198   }
00199   else
00200     ROS_ERROR("[WalkAnalyzer] Can't open file '%s'", filename.c_str());
00201 }
00202 
00203 std::string WalkAnalyzer::currentDateTime() const
00204 {
00205   time_t     now = time(0);
00206   struct tm  tstruct;
00207   char       buf[80];
00208   tstruct = *localtime(&now);
00209   strftime(buf, sizeof(buf), "%Y-%m-%d.%X", &tstruct);
00210 
00211   return buf;
00212 }
00213 }


vigir_walk_monitor
Author(s): Alexander Stumpf
autogenerated on Thu Jun 6 2019 18:38:20