Skip to content

Commit

Permalink
Merge branch 'main' into feat/updates-over-ros2
Browse files Browse the repository at this point in the history 
Signed-off-by: Aaron Chong <[email protected]>
  • Loading branch information
@aaronchongth
aaronchongth committed Sep 27, 2024
2 parents 07b9c6a + 76ef117 commit 41d1034
Show file tree
Hide file tree
Showing 5 changed files with 277 additions and 31 deletions.
155 changes: 145 additions & 10 deletions rmf_fleet_adapter/src/rmf_fleet_adapter/agv/EasyFullControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -466,7 +466,7 @@ class EasyFullControl::CommandExecution::Implementation::Data

if (starts.empty())
{
starts = nav_params->compute_plan_starts(graph, map, location, now);
starts = nav_params->compute_plan_starts(*context, map, location, now);
}

if (!starts.empty())
Expand Down Expand Up @@ -860,6 +860,7 @@ class EasyCommandHandle : public RobotCommandHandle,
};

EasyCommandHandle(
std::shared_ptr<Location> reported_location,
std::shared_ptr<NavParams> nav_params,
NavigationRequest handle_nav_request,
StopRequest handle_stop);
Expand Down Expand Up @@ -901,6 +902,8 @@ class EasyCommandHandle : public RobotCommandHandle,
std::weak_ptr<RobotContext> w_context;
std::shared_ptr<NavParams> nav_params;
std::shared_ptr<ProgressTracker> current_progress;
std::shared_ptr<Location> reported_location;
rclcpp::TimerBase::SharedPtr localize_timer;

// Callbacks from user
NavigationRequest handle_nav_request;
Expand All @@ -909,10 +912,12 @@ class EasyCommandHandle : public RobotCommandHandle,

//==============================================================================
EasyCommandHandle::EasyCommandHandle(
std::shared_ptr<Location> reported_location_,
std::shared_ptr<NavParams> nav_params_,
NavigationRequest handle_nav_request_,
StopRequest handle_stop_)
: nav_params(std::move(nav_params_)),
reported_location(std::move(reported_location_)),
handle_nav_request(std::move(handle_nav_request_)),
handle_stop(std::move(handle_stop_))
{
Expand Down Expand Up @@ -1303,6 +1308,117 @@ void EasyCommandHandle::follow_new_path(
this->current_progress = ProgressTracker::make(
queue,
path_finished_callback_);

if (initial_map != reported_location->map)
{
// The robot has not localized to the current map yet. This should only
// happen when a replan happens while the robot is inside of a lift.
RCLCPP_WARN(
context->node()->get_logger(),
"Relocalizing robot [%s] from map [%s] to [%s] because of a mismatch "
"while starting to follow a new path",
context->requester_id().c_str(),
initial_map.c_str(),
reported_location->map.c_str());

auto localize = EasyFullControl::Destination::Implementation::make(
initial_map,
reported_location->position,
std::nullopt,
"",
std::nullopt,
nullptr);

// Create a shared reference to the current progress. We will swap this
// out for a nullptr after the first time we trigger the current_progress
// to make sure it's impossible for the user to do anything that would
// double-trigger it.
auto progress_ref =
std::make_shared<std::shared_ptr<ProgressTracker>>(current_progress);

auto begin_following_path =
[w = weak_from_this(), progress_ref]()
{
if (!progress_ref)
{
throw std::runtime_error(
"[rmf_fleet_adapter::agv::EasyFullControl::follow_new_path] "
"progress_ref was not initialized. This should never happen. "
"please report this bug to the RMF maintainers.");
}

if (!*progress_ref)
{
// This was already triggered
return;
}

const auto self = w.lock();
if (!self)
{
// The adapter is deconstructing
return;
}

if (self->current_progress != *progress_ref)
{
// The robot is following a new path
return;
}

self->localize_timer = nullptr;
self->current_progress->next();
*progress_ref = nullptr;
};

auto localize_cmd = EasyFullControl::CommandExecution::Implementation
::make_hold(
context,
waypoints.front().time(),
context->itinerary().current_plan_id(),
[worker = context->worker(), begin_following_path]()
{
worker.schedule([begin_following_path](const auto&)
{
begin_following_path();
});
});

if (context->localize(std::move(localize), std::move(localize_cmd)))
{
localize_timer = context->node()->try_create_wall_timer(
std::chrono::seconds(300),
[begin_following_path, w = weak_from_this()]()
{
const auto self = w.lock();
if (!self)
return;

const auto context = self->w_context.lock();
if (!context)
return;

RCLCPP_ERROR(
context->node()->get_logger(),
"Waiting for robot [%s] to localize timed out. Please ensure that "
"your localization function triggers execution.finished() when the "
"robot's localization process is finished.",
context->requester_id().c_str());

begin_following_path();
});

// Return here to avoid triggering current_progress->next() at the end of
// this function so that it can be triggered when the downstream
// integrator triggers the finished callback.
return;
}

// If context->localize(...) returned false then that means the downstream
// integrator isn't handling localization commands and therefore will never
// trigger the finish callback, so we should proceed to call
// current_progress->next() below.
}
this->current_progress->next();
}

Expand Down Expand Up @@ -1510,11 +1626,15 @@ class EasyFullControl::EasyRobotUpdateHandle::Implementation

struct Updater
{
std::shared_ptr<Location> reported_location;
std::shared_ptr<RobotUpdateHandle> handle;
std::shared_ptr<NavParams> nav_params;

Updater(std::shared_ptr<NavParams> params_)
: handle(nullptr),
Updater(
std::shared_ptr<Location> reported_location_,
std::shared_ptr<NavParams> params_)
: reported_location(std::move(reported_location_)),
handle(nullptr),
nav_params(std::move(params_))
{
// Do nothing
Expand Down Expand Up @@ -1550,22 +1670,24 @@ class EasyFullControl::EasyRobotUpdateHandle::Implementation
}

Implementation(
std::shared_ptr<Location> reported_location_,
std::shared_ptr<NavParams> params_,
rxcpp::schedulers::worker worker_)
: updater(std::make_shared<Updater>(params_)),
: updater(std::make_shared<Updater>(std::move(reported_location_), params_)),
worker(worker_)
{
// Do nothing
}

static std::shared_ptr<EasyRobotUpdateHandle> make(
std::shared_ptr<Location> reported_location_,
std::shared_ptr<NavParams> params_,
rxcpp::schedulers::worker worker_)
{
auto handle = std::shared_ptr<EasyRobotUpdateHandle>(
new EasyRobotUpdateHandle);
handle->_pimpl = rmf_utils::make_unique_impl<Implementation>(
std::move(params_), std::move(worker_));
std::move(reported_location_), std::move(params_), std::move(worker_));
return handle;
}
};
Expand Down Expand Up @@ -1596,6 +1718,12 @@ void EasyFullControl::EasyRobotUpdateHandle::update(
const auto position = updater->to_rmf_coordinates(
state.map(), state.position(), *context);

*updater->reported_location = Location {
context->now(),
state.map(),
position
};

if (current_activity)
{
const auto update_fn =
Expand Down Expand Up @@ -2895,11 +3023,19 @@ auto EasyFullControl::add_robot(
{
const auto node = _pimpl->node();

rmf_traffic::Time now = std::chrono::steady_clock::time_point(
std::chrono::nanoseconds(node->now().nanoseconds()));

auto reported_location = std::make_shared<Location>(Location {
now,
initial_state.map(),
initial_state.position()
});
auto worker =
FleetUpdateHandle::Implementation::get(*_pimpl->fleet_handle).worker;
auto robot_nav_params = std::make_shared<NavParams>(_pimpl->nav_params);
auto easy_updater = EasyRobotUpdateHandle::Implementation::make(
robot_nav_params, worker);
reported_location, robot_nav_params, worker);

LocalizationRequest localization = nullptr;
if (callbacks.localize())
Expand Down Expand Up @@ -2981,9 +3117,6 @@ auto EasyFullControl::add_robot(
charger_index = charger_wp->index();
}

rmf_traffic::Time now = std::chrono::steady_clock::time_point(
std::chrono::nanoseconds(node->now().nanoseconds()));

const auto position_opt = robot_nav_params->to_rmf_coordinates(
initial_state.map(), initial_state.position());
if (!position_opt.has_value())
Expand Down Expand Up @@ -3017,7 +3150,7 @@ auto EasyFullControl::add_robot(

robot_nav_params->find_stacked_vertices(graph);
const Eigen::Vector3d position = *position_opt;
auto starts = robot_nav_params->compute_plan_starts(
auto starts = robot_nav_params->unfiltered_compute_plan_starts(
graph, initial_state.map(), position, now);

if (starts.empty())
Expand Down Expand Up @@ -3057,6 +3190,7 @@ auto EasyFullControl::add_robot(
}

const auto cmd_handle = std::make_shared<EasyCommandHandle>(
reported_location,
robot_nav_params,
std::move(handle_nav_request),
std::move(handle_stop));
Expand Down Expand Up @@ -3106,6 +3240,7 @@ auto EasyFullControl::add_robot(
](const auto&)
{
cmd_handle->w_context = context;
context->_set_reported_location(cmd_handle->reported_location);
context->set_nav_params(nav_params);
EasyRobotUpdateHandle::Implementation::get(*easy_updater)
.updater->handle = handle;
Expand Down
Loading

0 comments on commit 41d1034

Please sign in to comment.