Skip to content

Commit

Permalink
Use weak_ptr for frame_tf in rnav API (cc: #66)
Browse files Browse the repository at this point in the history
  • Loading branch information
@jlblancoc
jlblancoc committed Mar 31, 2018
1 parent ef8d116 commit acdcd21
Show file tree
Hide file tree
Showing 3 changed files with 61 additions and 79 deletions.
15 changes: 5 additions & 10 deletions libs/nav/include/mrpt/nav/reactive/CAbstractNavigator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,16 +183,12 @@ class CAbstractNavigator : public mrpt::system::COutputLogger
* targets in a frame ID
* different than the one in which robot odometry is given (both IDs
* default to `"map"`).
* Ownership of the pointee object remains belonging to the user, which is
* responsible of deleting it
* and ensuring its a valid pointer during the lifetime of this navigator
* object.
* \todo [MRPT 2.0: Make this a weak_ptr]
*/
void setFrameTF(mrpt::poses::FrameTransformer<2>* frame_tf);
void setFrameTF(
const std::weak_ptr<mrpt::poses::FrameTransformer<2>>& frame_tf);

/** Get the current frame tf object (defaults to nullptr) \sa setFrameTF */
const mrpt::poses::FrameTransformer<2>* getFrameTF() const
std::weak_ptr<mrpt::poses::FrameTransformer<2>> getFrameTF() const
{
return m_frame_tf;
}
Expand Down Expand Up @@ -313,9 +309,8 @@ class CAbstractNavigator : public mrpt::system::COutputLogger
/** The navigator-robot interface. */
CRobot2NavInterface& m_robot;

/** Optional, user-provided frame transformer.
* Note: We dont have ownership of the pointee object! */
mrpt::poses::FrameTransformer<2>* m_frame_tf;
/** Optional, user-provided frame transformer. */
std::weak_ptr<mrpt::poses::FrameTransformer<2>> m_frame_tf;

/** mutex for all navigation methods */
std::recursive_mutex m_nav_cs;
Expand Down
58 changes: 25 additions & 33 deletions libs/nav/src/reactive/CAbstractNavigator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,6 @@ CAbstractNavigator::CAbstractNavigator(CRobot2NavInterface& react_iterf_impl)
m_counter_check_target_is_blocked(0),
m_navigationState(IDLE),
m_robot(react_iterf_impl),
m_frame_tf(nullptr),
m_curPoseVel(),
m_last_curPoseVelUpdate_robot_time(-1e9),
m_latestPoses(),
Expand Down Expand Up @@ -151,7 +150,8 @@ void CAbstractNavigator::resetNavError()
if (m_navigationState == NAV_ERROR) m_navigationState = IDLE;
}

void CAbstractNavigator::setFrameTF(mrpt::poses::FrameTransformer<2>* frame_tf)
void CAbstractNavigator::setFrameTF(
const std::weak_ptr<mrpt::poses::FrameTransformer<2>>& frame_tf)
{
m_frame_tf = frame_tf;
}
Expand Down Expand Up @@ -215,11 +215,9 @@ void CAbstractNavigator::navigationStep()
if (m_lastNavigationState == NAVIGATING &&
m_navigationState == NAV_ERROR)
{
m_pending_events.push_back(
std::bind(
&CRobot2NavInterface::
sendNavigationEndDueToErrorEvent,
std::ref(m_robot)));
m_pending_events.push_back(std::bind(
&CRobot2NavInterface::sendNavigationEndDueToErrorEvent,
std::ref(m_robot)));
}

// If we just arrived at this state, stop the robot:
Expand Down Expand Up @@ -505,22 +503,20 @@ void CAbstractNavigator::performNavigationStepNavigating(
"[CAbstractNavigator::navigationStep()] Starting Navigation. "
"Watchdog initiated...\n");
if (m_navigationParams)
MRPT_LOG_DEBUG(
mrpt::format(
"[CAbstractNavigator::navigationStep()] Navigation "
"Params:\n%s\n",
m_navigationParams->getAsText().c_str()));
MRPT_LOG_DEBUG(mrpt::format(
"[CAbstractNavigator::navigationStep()] Navigation "
"Params:\n%s\n",
m_navigationParams->getAsText().c_str()));

internal_onStartNewNavigation();
}

// Have we just started the navigation?
if (m_lastNavigationState == IDLE)
{
m_pending_events.push_back(
std::bind(
&CRobot2NavInterface::sendNavigationStartEvent,
std::ref(m_robot)));
m_pending_events.push_back(std::bind(
&CRobot2NavInterface::sendNavigationStartEvent,
std::ref(m_robot)));
}

/* ----------------------------------------------------------------
Expand Down Expand Up @@ -551,10 +547,9 @@ void CAbstractNavigator::performNavigationStepNavigating(
params_abstract_navigator.dist_to_target_for_sending_event)
{
m_navigationEndEventSent = true;
m_pending_events.push_back(
std::bind(
&CRobot2NavInterface::sendNavigationEndEvent,
std::ref(m_robot)));
m_pending_events.push_back(std::bind(
&CRobot2NavInterface::sendNavigationEndEvent,
std::ref(m_robot)));
}

// Have we really reached the target?
Expand All @@ -573,10 +568,9 @@ void CAbstractNavigator::performNavigationStepNavigating(
if (!m_navigationEndEventSent)
{
m_navigationEndEventSent = true;
m_pending_events.push_back(
std::bind(
&CRobot2NavInterface::sendNavigationEndEvent,
std::ref(m_robot)));
m_pending_events.push_back(std::bind(
&CRobot2NavInterface::sendNavigationEndEvent,
std::ref(m_robot)));
}
}
return;
Expand All @@ -603,10 +597,9 @@ void CAbstractNavigator::performNavigationStepNavigating(

m_navigationState = NAV_ERROR;

m_pending_events.push_back(
std::bind(
&CRobot2NavInterface::sendWaySeemsBlockedEvent,
std::ref(m_robot)));
m_pending_events.push_back(std::bind(
&CRobot2NavInterface::sendWaySeemsBlockedEvent,
std::ref(m_robot)));
return;
}
}
Expand All @@ -633,11 +626,10 @@ void CAbstractNavigator::performNavigationStepNavigating(
"Target seems to be blocked by obstacles. Invoking"
" sendCannotGetCloserToBlockedTargetEvent().");

m_pending_events.push_back(
std::bind(
&CRobot2NavInterface::
sendCannotGetCloserToBlockedTargetEvent,
std::ref(m_robot)));
m_pending_events.push_back(std::bind(
&CRobot2NavInterface::
sendCannotGetCloserToBlockedTargetEvent,
std::ref(m_robot)));

m_counter_check_target_is_blocked = 0;
}
Expand Down
67 changes: 31 additions & 36 deletions libs/nav/src/reactive/CAbstractPTGBasedReactive.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,11 +191,10 @@ void CAbstractPTGBasedReactive::enableLogFile(bool enable)
}
}

MRPT_LOG_DEBUG(
mrpt::format(
"[CAbstractPTGBasedReactive::enableLogFile] Logging to "
"file `%s`\n",
aux));
MRPT_LOG_DEBUG(mrpt::format(
"[CAbstractPTGBasedReactive::enableLogFile] Logging to "
"file `%s`\n",
aux));
}
}
catch (std::exception& e)
Expand Down Expand Up @@ -451,16 +450,18 @@ void CAbstractPTGBasedReactive::performNavigationStep()
{
if (t.target_frame_id != m_curPoseVel.pose_frame_id)
{
if (m_frame_tf == nullptr)
auto frame_tf = m_frame_tf.lock();
if (!frame_tf)
{
THROW_EXCEPTION_FMT(
"Different frame_id's but no frame_tf object "
"attached!: target_frame_id=`%s` != pose_frame_id=`%s`",
"attached (or it expired)!: target_frame_id=`%s` != "
"pose_frame_id=`%s`",
t.target_frame_id.c_str(),
m_curPoseVel.pose_frame_id.c_str());
}
mrpt::math::TPose2D robot_frame2map_frame;
m_frame_tf->lookupTransform(
frame_tf->lookupTransform(
t.target_frame_id, m_curPoseVel.pose_frame_id,
robot_frame2map_frame);

Expand Down Expand Up @@ -558,10 +559,9 @@ void CAbstractPTGBasedReactive::performNavigationStep()
}
if (is_all_ptg_collision)
{
m_pending_events.push_back(
std::bind(
&CRobot2NavInterface::sendApparentCollisionEvent,
std::ref(m_robot)));
m_pending_events.push_back(std::bind(
&CRobot2NavInterface::sendApparentCollisionEvent,
std::ref(m_robot)));
}

// Round #2: Evaluate dont sending any new velocity command ("NOP"
Expand Down Expand Up @@ -882,19 +882,17 @@ void CAbstractPTGBasedReactive::performNavigationStep()

if (m_enableConsoleOutput)
{
MRPT_LOG_DEBUG(
mrpt::format(
"CMD: %s "
"speedScale=%.04f "
"T=%.01lfms Exec:%.01lfms|%.01lfms "
"PTG#%i\n",
new_vel_cmd ? new_vel_cmd->asString().c_str() : "NOP",
selectedHolonomicMovement ? selectedHolonomicMovement->speed
: .0,
1000.0 * meanExecutionPeriod.getLastOutput(),
1000.0 * meanExecutionTime.getLastOutput(),
1000.0 * meanTotalExecutionTime.getLastOutput(),
best_ptg_idx));
MRPT_LOG_DEBUG(mrpt::format(
"CMD: %s "
"speedScale=%.04f "
"T=%.01lfms Exec:%.01lfms|%.01lfms "
"PTG#%i\n",
new_vel_cmd ? new_vel_cmd->asString().c_str() : "NOP",
selectedHolonomicMovement ? selectedHolonomicMovement->speed
: .0,
1000.0 * meanExecutionPeriod.getLastOutput(),
1000.0 * meanExecutionTime.getLastOutput(),
1000.0 * meanTotalExecutionTime.getLastOutput(), best_ptg_idx));
}
if (fill_log_record)
{
Expand All @@ -908,10 +906,9 @@ void CAbstractPTGBasedReactive::performNavigationStep()
catch (std::exception& e)
{
doEmergencyStop(
std::string(
"[CAbstractPTGBasedReactive::performNavigationStep] "
"Stopping robot and finishing navigation due to "
"exception:\n") +
std::string("[CAbstractPTGBasedReactive::performNavigationStep] "
"Stopping robot and finishing navigation due to "
"exception:\n") +
std::string(e.what()));
}
catch (...)
Expand Down Expand Up @@ -1021,9 +1018,8 @@ void CAbstractPTGBasedReactive::calc_move_candidate_scores(
double best_trg_angdist = std::numeric_limits<double>::max();
for (size_t i = 0; i < TP_Targets.size(); i++)
{
const double angdist = std::abs(
mrpt::math::angDistance(
TP_Targets[i].target_alpha, cm.direction));
const double angdist = std::abs(mrpt::math::angDistance(
TP_Targets[i].target_alpha, cm.direction));
if (angdist < best_trg_angdist)
{
best_trg_angdist = angdist;
Expand Down Expand Up @@ -1058,7 +1054,7 @@ void CAbstractPTGBasedReactive::calc_move_candidate_scores(
!cm.PTG->supportSpeedAtTarget() // If the PTG is able to handle the
// slow-down on its own, dont change
// speed here
)
)
{
const double TARGET_SLOW_APPROACHING_DISTANCE =
m_holonomicMethod[0]->getTargetApproachSlowDownDistance();
Expand Down Expand Up @@ -1801,9 +1797,8 @@ void CAbstractPTGBasedReactive::TAbstractPTGNavigatorParams::saveToConfigFile(
}
MRPT_SAVE_CONFIG_VAR_COMMENT(
holonomic_method,
string(
"C++ class name of the holonomic navigation method to run in "
"the transformed TP-Space.\n") +
string("C++ class name of the holonomic navigation method to run in "
"the transformed TP-Space.\n") +
lstHoloStr);

// Build list of known decider methods:
Expand Down

0 comments on commit acdcd21

Please sign in to comment.