Skip to content

Commit

Permalink
Enable cart attachment in slotcar (#120)
Browse files Browse the repository at this point in the history 
* Use JointPositionReset for open loop door control

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* Uncrustify

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* Uncrustify

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* WIP interface for attaching and detaching carts in slotcar

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* Populate data to find closest dispensable object

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* Full prototype of cart deliveries working

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

* Reject attachment if entity is too far away

Signed-off-by: Xi Yu Oh <xiyuoh@intrinsic.ai>

* Use ACTION_COMPLETED mode instead

Signed-off-by: Xi Yu Oh <xiyuoh@intrinsic.ai>

* Skip mode setting if current mode is ACTION_COMPLETED

Signed-off-by: Xi Yu Oh <xiyuoh@intrinsic.ai>

* Use single RobotMode mode for attaching and detaching cart

Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>

* Style

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

* Minor cleanup

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

---------

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>
Signed-off-by: Xi Yu Oh <xiyuoh@intrinsic.ai>
Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
Signed-off-by: Xiyu Oh <xiyu@openrobotics.org>
Co-authored-by: Luca Della Vedova <luca@openrobotics.org>
Co-authored-by: Luca Della Vedova <lucadv@intrinsic.ai>
  • Loading branch information
@xiyuoh @luca-della-vedova
3 people authored Jun 26, 2024
1 parent ee0c022 commit a39e658
Show file tree
Hide file tree
Showing 3 changed files with 164 additions and 31 deletions.
7 changes: 5 additions & 2 deletions rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,10 @@ class SlotcarCommon

void init_ros_node(const rclcpp::Node::SharedPtr node);

using AttachCartCallback = std::function<bool(bool)>;
void set_attach_cart_callback(AttachCartCallback cb)
{ _attach_cart_callback = cb; }

UpdateResult update(const Eigen::Isometry3d& pose,
const std::vector<Eigen::Vector3d>& obstacle_positions,
const double time);
Expand Down Expand Up @@ -231,12 +235,11 @@ class SlotcarCommon
// Straight line distance to charging waypoint within which charging can occur
static constexpr double _charger_dist_thres = 0.3;

bool _docking = false;

Eigen::Vector3d _lookahead_point;
double _lookahead_distance = 8.0;

PathRequestCallback _path_request_callback = nullptr;
AttachCartCallback _attach_cart_callback = nullptr;

std::string get_level_name(const double z);

Expand Down
63 changes: 48 additions & 15 deletions rmf_robot_sim_common/src/slotcar_common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -165,6 +165,7 @@ static Eigen::Vector2d get_closest_point_on_line_segment(
}

using SlotcarCommon = rmf_robot_sim_common::SlotcarCommon;
using RobotMode = rmf_fleet_msgs::msg::RobotMode;

SlotcarCommon::SlotcarCommon()
{
Expand Down Expand Up @@ -195,7 +196,7 @@ std::string SlotcarCommon::model_name() const

void SlotcarCommon::init_ros_node(const rclcpp::Node::SharedPtr node)
{
_current_mode.mode = rmf_fleet_msgs::msg::RobotMode::MODE_MOVING;
_current_mode.mode = RobotMode::MODE_MOVING;
_ros_node = std::move(node);

_tf2_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(_ros_node);
Expand Down Expand Up @@ -490,19 +491,53 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive(
if (stationary && in_charger_vicinity &&
(_enable_instant_charge || _enable_charge))
{
_current_mode.mode = rmf_fleet_msgs::msg::RobotMode::MODE_CHARGING;
_current_mode.mode = RobotMode::MODE_CHARGING;
}
else if (_docking)
else if (_current_mode.mode == RobotMode::MODE_DOCKING)
{
_current_mode.mode = rmf_fleet_msgs::msg::RobotMode::MODE_DOCKING;
// NOOP, keep the current mode
}
else if (_current_mode.mode == RobotMode::MODE_PERFORMING_ACTION)
{
if (_current_mode.performing_action.empty())
{
// Return to IDLE mode, no perform action specified
_current_mode.mode = RobotMode::MODE_IDLE;
}
else
{
if (_current_mode.performing_action == "attach_cart")
{
if (_attach_cart_callback != nullptr && _attach_cart_callback(true))
_current_mode.mode = RobotMode::MODE_ACTION_COMPLETED;
else
_current_mode.mode = RobotMode::MODE_IDLE;
}
else if (_current_mode.performing_action == "detach_cart")
{
if (_attach_cart_callback != nullptr && _attach_cart_callback(false))
_current_mode.mode = RobotMode::MODE_ACTION_COMPLETED;
else
_current_mode.mode = RobotMode::MODE_IDLE;
}
else
{
// Specified performing action not recognized, return to IDLE mode
_current_mode.mode = RobotMode::MODE_IDLE;
}
}
}
else if (_current_mode.mode == RobotMode::MODE_ACTION_COMPLETED)
{
// Do nothing
}
else if (stationary)
{
_current_mode.mode = rmf_fleet_msgs::msg::RobotMode::MODE_IDLE;
_current_mode.mode = RobotMode::MODE_IDLE;
}
else
{
_current_mode.mode = rmf_fleet_msgs::msg::RobotMode::MODE_MOVING;
_current_mode.mode = RobotMode::MODE_MOVING;
}
_old_lin_vel = lin_vel;
_old_ang_vel = ang_vel;
Expand Down Expand Up @@ -565,7 +600,7 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive(
current_heading, goal_heading);
}
result.target_linear_speed_now = 0.0;
_current_mode.mode = rmf_fleet_msgs::msg::RobotMode::MODE_PAUSED;
_current_mode.mode = RobotMode::MODE_PAUSED;
}
else if (close_enough)
{
Expand Down Expand Up @@ -678,9 +713,9 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_ackermann(
bool stationary = lin_vel.norm() < eps && std::abs(ang_vel) < eps;

if (stationary)
_current_mode.mode = rmf_fleet_msgs::msg::RobotMode::MODE_IDLE;
_current_mode.mode = RobotMode::MODE_IDLE;
else
_current_mode.mode = rmf_fleet_msgs::msg::RobotMode::MODE_MOVING;
_current_mode.mode = RobotMode::MODE_MOVING;

_old_lin_vel = lin_vel;
_old_ang_vel = ang_vel;
Expand Down Expand Up @@ -989,12 +1024,14 @@ void SlotcarCommon::publish_state_topic(const rclcpp::Time& t)
robot_state_msg.task_id = _current_task_id;
robot_state_msg.path = _remaining_path;
robot_state_msg.mode = _current_mode;
robot_state_msg.mode.mode_request_id = pause_request.mode_request_id;
// Pick the higher (most recent) one
robot_state_msg.mode.mode_request_id = std::max(pause_request.mode_request_id,
_current_mode.mode_request_id);

if (_adapter_error)
{
robot_state_msg.mode.mode =
rmf_fleet_msgs::msg::RobotMode::MODE_ADAPTER_ERROR;
RobotMode::MODE_ADAPTER_ERROR;
}

robot_state_msg.seq = ++_sequence;
Expand All @@ -1009,10 +1046,6 @@ void SlotcarCommon::mode_request_cb(
return;

_current_mode = msg->mode;
if (msg->mode.mode == msg->mode.MODE_DOCKING)
_docking = true;
else
_docking = false;
}

void SlotcarCommon::map_cb(
Expand Down
125 changes: 111 additions & 14 deletions rmf_robot_sim_gz_plugins/src/slotcar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@
#include <gz/sim/System.hh>
#include <gz/sim/Model.hh>
#include <gz/sim/Util.hh>
#include <gz/sim/components/DetachableJoint.hh>
#include <gz/sim/components/Link.hh>
#include <gz/sim/components/Model.hh>
#include <gz/sim/components/Name.hh>
#include <gz/sim/components/Pose.hh>
Expand Down Expand Up @@ -44,16 +46,22 @@ class GZ_SIM_VISIBLE SlotcarPlugin
void PreUpdate(const UpdateInfo& info, EntityComponentManager& ecm) override;

private:
// Distance to attach cart, if none is found attaching will fail
static constexpr float MIN_ATTACHING_DIST = 1.0;
std::unique_ptr<rmf_robot_sim_common::SlotcarCommon> dataPtr;
gz::transport::Node _gz_node;
rclcpp::Node::SharedPtr _ros_node;

EntityComponentManager* _ecm;

Entity _entity;
Entity _joint_entity = kNullEntity;
Eigen::Isometry3d _pose;
std::unordered_set<Entity> _obstacle_exclusions;
std::unordered_map<Entity, Eigen::Vector3d> _dispensable_positions;
double _height = 0;

bool _read_aabb_dimensions = true;
bool _remove_world_pose_cmd = false;

// Previous velocities, used to do open loop velocity control
double _prev_v_command = 0.0;
Expand All @@ -70,12 +78,18 @@ class GZ_SIM_VISIBLE SlotcarPlugin
void init_obstacle_exclusions(EntityComponentManager& ecm);
bool get_slotcar_height(const gz::msgs::Entity& req,
gz::msgs::Double& rep);
std::vector<Eigen::Vector3d> get_obstacle_positions(
EntityComponentManager& ecm);
std::pair<std::vector<Eigen::Vector3d>, std::unordered_map<Entity,
Eigen::Vector3d>>
get_obstacle_positions(EntityComponentManager& ecm);

void path_request_marker_update(
const rmf_fleet_msgs::msg::PathRequest::SharedPtr);

bool attach_cart(bool attach);

bool attach_entity(const Entity& entity);
bool detach_entity();

void draw_lookahead_marker();

gz::msgs::Marker_V _trajectory_marker_msg;
Expand All @@ -97,15 +111,88 @@ SlotcarPlugin::~SlotcarPlugin()
{
}

bool SlotcarPlugin::attach_cart(bool attach)
{
if (attach)
{
// Find _dispensable_position closest to _pose
if (_dispensable_positions.size() == 0)
return false;
auto min_entity = _dispensable_positions.begin()->first;
auto min_dist = (_pose.translation() -
_dispensable_positions.begin()->second).norm();
for (const auto& [entity, pose] : _dispensable_positions)
{
auto dist = (_pose.translation() - pose).norm();
if (dist < min_dist)
{
min_dist = dist;
min_entity = entity;
}
}
if (min_dist > MIN_ATTACHING_DIST)
return false;

return attach_entity(min_entity);
}
else
{
return detach_entity();
}
return false;
}

bool SlotcarPlugin::detach_entity()
{
_ecm->RequestRemoveEntity(_joint_entity);
_joint_entity = kNullEntity;
return true;
}

bool SlotcarPlugin::attach_entity(const Entity& entity)
{
// A cart was already attached
if (_joint_entity != kNullEntity)
return true;

_joint_entity = _ecm->CreateEntity();

auto robot_link_entities = _ecm->ChildrenByComponents(_entity,
components::Link());
if (robot_link_entities.size() != 1)
{
gzwarn << "Robot should only have one link, using first" << std::endl;
}
auto robot_link_entity = robot_link_entities[0];

auto cart_link_entities = _ecm->ChildrenByComponents(entity,
components::Link());
if (cart_link_entities.size() != 1)
{
gzwarn << "Cart should only have one link, using first" << std::endl;
}
auto cart_link_entity = cart_link_entities[0];

_ecm->CreateComponent(
_joint_entity,
components::DetachableJoint({robot_link_entity,
cart_link_entity, "fixed"}));
return true;
}

void SlotcarPlugin::Configure(const Entity& entity,
const std::shared_ptr<const sdf::Element>& sdf,
EntityComponentManager& ecm, EventManager&)
{
_entity = entity;
_ecm = &ecm;
auto model = Model(entity);
std::string model_name = model.Name(ecm);
dataPtr->set_model_name(model_name);
dataPtr->read_sdf(sdf);
dataPtr->set_attach_cart_callback(std::bind(&SlotcarPlugin::
attach_cart,
this, std::placeholders::_1));

// TODO proper argc argv
char const** argv = NULL;
Expand Down Expand Up @@ -205,32 +292,41 @@ void SlotcarPlugin::init_obstacle_exclusions(EntityComponentManager& ecm)
_obstacle_exclusions.insert(_entity);
}

std::vector<Eigen::Vector3d> SlotcarPlugin::get_obstacle_positions(
EntityComponentManager& ecm)
std::pair<std::vector<Eigen::Vector3d>, std::unordered_map<Entity,
Eigen::Vector3d>>
SlotcarPlugin::get_obstacle_positions(EntityComponentManager& ecm)
{
std::vector<Eigen::Vector3d> obstacle_positions;
std::unordered_map<Entity, Eigen::Vector3d> dispensable_positions;
ecm.Each<components::Model, components::Name, components::Pose,
components::Static>(
[&](const Entity& entity,
const components::Model*,
const components::Name*,
const components::Name* name,
const components::Pose* pose,
const components::Static* is_static
) -> bool
{
const auto& n = name->Data();
// Object should not be static
// It should not be part of obstacle exclusions (doors/lifts/dispensables)
// And it should be closer than the "stop" range (checked by common)
const auto obstacle_position = pose->Data().Pos();
const auto object_position = pose->Data().Pos();
if (is_static->Data() == false &&
_obstacle_exclusions.find(entity) == _obstacle_exclusions.end())
{
obstacle_positions.push_back(rmf_plugins_utils::convert_vec(
obstacle_position));
object_position));
}
// It is a dispensable object
if (n.find("dispensable") != std::string::npos)
{
dispensable_positions.insert({entity, rmf_plugins_utils::convert_vec(
object_position)});
}
return true;
});
return obstacle_positions;
return {obstacle_positions, dispensable_positions};
}

void SlotcarPlugin::charge_state_cb(const gz::msgs::Selection& msg)
Expand Down Expand Up @@ -403,12 +499,13 @@ void SlotcarPlugin::PreUpdate(const UpdateInfo& info,
(std::chrono::duration_cast<std::chrono::nanoseconds>(info.simTime).count())
* 1e-9;

auto pose = ecm.Component<components::Pose>(_entity)->Data();
auto obstacle_positions = get_obstacle_positions(ecm);
_pose = rmf_plugins_utils::convert_pose(
ecm.Component<components::Pose>(_entity)->Data());
auto [obstacle_positions,
dispensable_positions] = get_obstacle_positions(ecm);
_dispensable_positions = std::move(dispensable_positions);

auto update_result =
dataPtr->update(rmf_plugins_utils::convert_pose(pose),
obstacle_positions, time);
auto update_result = dataPtr->update(_pose, obstacle_positions, time);

send_control_signals(ecm, {update_result.v, update_result.w}, dt,
update_result.target_linear_speed_now,
Expand Down

0 comments on commit a39e658

Please sign in to comment.