cb_undo_path_backwards2.cpp

Go to the documentation of this file.

#include <move_base_z_client_plugin/client_behaviors/cb_undo_path_backwards2.h>
#include <move_base_z_client_plugin/components/odom_tracker/odom_tracker.h>
#include <move_base_z_client_plugin/components/planner_switcher/planner_switcher.h>
 
namespace cl_move_base_z
{
template <typename T>
int sgn(T val)
{
  return (T(0) < val) - (val < T(0));
}
 
using namespace ::cl_move_base_z::odom_tracker;
 
CbUndoPathBackwards2::CbUndoPathBackwards2() : goalLinePassed_(false)
{
  triggerThreshold_ = 0.01;  // 10 mm
}
 
void CbUndoPathBackwards2::onEntry()
{
  odomTracker_ = moveBaseClient_->getComponent<OdomTracker>();
 
  auto plannerSwitcher = moveBaseClient_->getComponent<PlannerSwitcher>();
  robotPose_ = moveBaseClient_->getComponent<cl_move_base_z::Pose>();
 
  nav_msgs::Path forwardpath = odomTracker_->getPath();
  // ROS_INFO_STREAM("[UndoPathBackward] Current path backwards: " << forwardpath);
 
  odomTracker_->setWorkingMode(WorkingMode::CLEAR_PATH);
 
  // this line is used to flush/reset backward planner in the case it were already there
  // plannerSwitcher->setDefaultPlanners();
  if (forwardpath.poses.size() > 0)
  {
    goal_.target_pose = forwardpath.poses.front();
    initial_plane_side_ = evalPlaneSide(robotPose_->toPoseMsg());
    plannerSwitcher->setUndoPathBackwardsPlannerConfiguration();
    moveBaseClient_->sendGoal(goal_);
  }
}
 
// bool points_on_same_side_of_line(const Vector2d &p1, const Vector2d &p2, const Vector2d &p_line, const Vector2d &normal)
// {
//   return normal.dot(p1 - p_line)*normal.dot(p2 - p_line) > 0.0f;
// }
 
float CbUndoPathBackwards2::evalPlaneSide(const geometry_msgs::Pose& pose)
{
  // check the line was passed
  // y = mx x + y0
  // y = (sin(alpha)/cos(alpha)) (x -x0) + y0
  // cos(alpha)(y - y0) - sin(alpha) (x -x0)= 0 // if greater one side if lower , the other
  auto y = pose.position.y;
  auto x = pose.position.x;
  auto alpha = tf::getYaw(goal_.target_pose.pose.orientation);
  auto y0 = goal_.target_pose.pose.position.y;
  auto x0 = goal_.target_pose.pose.position.x;
 
  auto evalimplicit = cos(alpha) * (y - y0) - sin (alpha) * (x - x0);
  return evalimplicit;
}
 
void CbUndoPathBackwards2::update()
{
  auto pose = robotPose_->toPoseMsg();
 
  float evalimplicit = evalPlaneSide(pose);
  if (sgn(evalimplicit) != sgn(initial_plane_side_))
  {
    ROS_WARN_STREAM("[CbUndoPathBackwards2] goal_ line passed: "
                                      << evalimplicit << "/" << this->initial_plane_side_);
 
    if (fabs(evalimplicit) > triggerThreshold_ /*meters*/)
    {
       ROS_WARN_STREAM("[CbUndoPathBackwards2] virtual goal line passed, stopping behavior and success: "
                                      << evalimplicit << "/" << this->initial_plane_side_);
      moveBaseClient_->cancelGoal();
      // this->postSuccessEvent();
      this->postVirtualLinePassed_();
    }
  }
  else
  {
    ROS_INFO_STREAM_THROTTLE(1.0, "[CbUndoPathBackwards2] goal_ line not passed yet: " << evalimplicit << "/"
                                                                                       << this->initial_plane_side_);
  }
}
 
void CbUndoPathBackwards2::publishMarkers()
{
  double phi = tf::getYaw(goal_.target_pose.pose.orientation);
  visualization_msgs::Marker marker;
  marker.header.frame_id = robotPose_->getReferenceFrame();
 
  marker.header.stamp = ros::Time::now();
  marker.ns = "my_namespace2";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::SPHERE;
  marker.action = visualization_msgs::Marker::ADD;
  marker.scale.x = 3;
  marker.scale.y = 3;
  marker.scale.z = 3;
  marker.color.a = 1.0;
  marker.color.r = 1.0;
  marker.color.g = 0.0;
  marker.color.b = 0.0;
 
  marker.pose = goal_.target_pose.pose;
 
  // geometry_msgs::Point start, end;
  // start.x = pose.position.x;
  // start.y = pose.position.y;
 
  // end.x = pose.position.x + 0.5 * cos(phi);
  // end.y = pose.position.y + 0.5 * sin(phi);
 
  // marker.points.push_back(start);
  // marker.points.push_back(end);
 
  visualization_msgs::MarkerArray ma;
  ma.markers.push_back(marker);
 
  this->visualizationMarkersPub_.publish(ma);
}
 
void CbUndoPathBackwards2::onExit()
{
  auto* odomTracker = moveBaseClient_->getComponent<OdomTracker>();
  odomTracker->popPath();
}
 
}  // namespace cl_move_base_z