Permalink
Browse files

Import feature from mrpt-1.5 branch (Issue#894) (#895)

Implementation of speed_ratio: Desired robot speed at the target, as a ratio of the full robot speed
Fix order of waypoint-reached events
  • Loading branch information...
bear-my-wrath authored and jlblancoc committed Feb 8, 2019
1 parent 3fa290a commit 0750c920280f64c2f6b9df0359c0943610acaaee
@@ -73,6 +73,8 @@ mrpt::nav::CRobot2NavInterface are now invoked *after* `navigationStep()` to
avoid problems if user code invokes the navigator API to change its state.
- Added methods to load/save mrpt::nav::TWaypointSequence to
configuration files.
- Waypoints now have a field `speed_ratio` which is directly
forwarded to the low-level reactive navigator.
- \ref mrpt_comms_grp [NEW IN MRPT 2.0.0]
- This new module has been created to hold all serial devices &
networking classes, with minimal dependencies.
@@ -116,9 +116,6 @@ class CWaypointsNavigator : public mrpt::nav::CAbstractNavigator
/** [rad] Angular error tolerance for waypoints with an assigned heading
* (Default: 5 deg) */
double waypoint_angle_tolerance;
/** [0,1] Relative speed when aiming at a stop-point waypoint
* (Default=0.10) */
double rel_speed_for_stop_waypoints{0.10};
/** >=0 number of waypoints to forward to the underlying navigation
* engine, to ease obstacles avoidance when a waypoint is blocked
* (Default=0 : none). */
@@ -40,6 +40,14 @@ struct TWaypoint
* waypoint for it to be considered reached. */
double allowed_distance;

/** (Default=1.0) Desired robot speed at the target, as a ratio of the full
* robot speed. That is: speed_ratio=1 means that the user wants the robot
* to navigate to the target and smoothly continue to the next one when
* reached. speed_ratio=0 on the other hand means that the robot should
* approach this waypoint slowing down and end up totally stopped.
*/
double speed_ratio;

/** [Default=true] Whether it is allowed to the navigator to proceed to a
* more advanced waypoint
* in the sequence if it determines that it is easier to skip this one
@@ -57,7 +65,8 @@ struct TWaypoint
TWaypoint();
TWaypoint(
double target_x, double target_y, double allowed_distance,
bool allow_skip = true, double target_heading_ = INVALID_NUM);
bool allow_skip = true, double target_heading_ = INVALID_NUM,
double speed_ratio_ = 1.0);
/** get in human-readable format */
std::string getAsText() const;

@@ -156,6 +156,8 @@ void CWaypointsNavigator::waypoints_navigationStep()
}
wps.last_robot_pose = m_curPoseVel.pose; // save for next iters

decltype(m_pending_events) new_events;

if (wps.waypoint_index_current_goal >= 0)
{
auto& wp = wps.waypoints[wps.waypoint_index_current_goal];
@@ -254,7 +256,7 @@ void CWaypointsNavigator::waypoints_navigationStep()
wp.skipped = false;
wp.timestamp_reach = mrpt::system::now();

m_pending_events.emplace_front(std::bind(
new_events.emplace_back(std::bind(
&CRobot2NavInterface::sendWaypointReachedEvent,
std::ref(m_robot), wps.waypoint_index_current_goal,
true /*reason: really reached*/));
@@ -339,13 +341,18 @@ void CWaypointsNavigator::waypoints_navigationStep()
wp.skipped = true;
wp.timestamp_reach = mrpt::system::now();

m_pending_events.emplace_front(std::bind(
new_events.emplace_back(std::bind(
&CRobot2NavInterface::sendWaypointReachedEvent,
std::ref(m_robot), k, false /*reason: skipped*/));
}
}
}

// Insert at the beginning, for these events to be dispatched
// *before* any "end of nav" event:
m_pending_events.insert(
m_pending_events.begin(), new_events.begin(), new_events.end());

// Still not started and no better guess? Start with the first
// waypoint:
if (wps.waypoint_index_current_goal < 0)
@@ -404,12 +411,7 @@ void CWaypointsNavigator::waypoints_navigationStep()
ti.targetAllowedDistance = wp.allowed_distance;
ti.targetIsRelative = false;
ti.targetIsIntermediaryWaypoint = !is_final_wp;
ti.targetDesiredRelSpeed =
(is_final_wp ||
(wp.target_heading != TWaypoint::INVALID_NUM))
? params_waypoints_navigator
.rel_speed_for_stop_waypoints
: 1.;
ti.targetDesiredRelSpeed = wp.speed_ratio;

// For backwards compat. with single-target code, write
// single target info too for the first, next, waypoint:
@@ -498,7 +500,6 @@ void mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::
MRPT_LOAD_CONFIG_VAR(max_distance_to_allow_skip_waypoint, double, c, s);
MRPT_LOAD_CONFIG_VAR(min_timesteps_confirm_skip_waypoints, int, c, s);
MRPT_LOAD_CONFIG_VAR_DEGREES(waypoint_angle_tolerance, c, s);
MRPT_LOAD_CONFIG_VAR(rel_speed_for_stop_waypoints, double, c, s);
MRPT_LOAD_CONFIG_VAR(multitarget_look_ahead, int, c, s);
}

@@ -517,10 +518,6 @@ void mrpt::nav::CWaypointsNavigator::TWaypointsNavigatorParams::
"waypoint_angle_tolerance", waypoint_angle_tolerance,
"Angular error tolerance for waypoints with an assigned heading [deg] "
"(Default: 5 deg)");
MRPT_SAVE_CONFIG_VAR_COMMENT(
rel_speed_for_stop_waypoints,
"[0,1] Relative speed when aiming at a stop-point waypoint "
"(Default=0.10)");
MRPT_SAVE_CONFIG_VAR_COMMENT(
multitarget_look_ahead,
">=0 number of waypoints to forward to the underlying navigation "
@@ -23,18 +23,20 @@ TWaypoint::TWaypoint()
: target(INVALID_NUM, INVALID_NUM),
target_heading(INVALID_NUM),
target_frame_id("map"),
allowed_distance(INVALID_NUM)
allowed_distance(INVALID_NUM),
speed_ratio(1.0)

{
}

TWaypoint::TWaypoint(
double target_x, double target_y, double allowed_distance_,
bool allow_skip_, double target_heading_)
bool allow_skip_, double target_heading_, double speed_ratio_)
: target(target_x, target_y),
target_heading(target_heading_),
target_frame_id("map"),
allowed_distance(allowed_distance_),
speed_ratio(speed_ratio_),
allow_skip(allow_skip_)
{
}
@@ -65,6 +67,7 @@ std::string TWaypoint::getAsText() const

s += (allow_skip ? " allow_skip: YES" : " allow_skip: NO ");

s += mrpt::format(" speed_ratio: %.01f", speed_ratio);
return s;
}

@@ -245,6 +248,7 @@ void TWaypointSequence::save(
NP);
c.write(
s, mrpt::format("wp%03u_target_heading", i), wp.target_heading, NP);
c.write(s, mrpt::format("wp%03u_speed_ratio", i), wp.speed_ratio, NP);
}
}

@@ -274,5 +278,7 @@ void TWaypointSequence::load(
s, mrpt::format("wp%03u_target_heading", i),
mrpt::nav::TWaypoint::INVALID_NUM);
wp.target_heading = (hd > 100) ? mrpt::nav::TWaypoint::INVALID_NUM : hd;
wp.speed_ratio =
c.read_double(s, mrpt::format("wp%03u_speed_ratio", i), 1.0, false);
}
}

0 comments on commit 0750c92

Please sign in to comment.