Skip to content

Commit

Permalink
Address feedback
Browse files Browse the repository at this point in the history 
Signed-off-by: Luca Della Vedova <[email protected]>
  • Loading branch information
@luca-della-vedova
luca-della-vedova committed Sep 8, 2021
1 parent 2fcbd61 commit de0f865
Show file tree
Hide file tree
Showing 2 changed files with 20 additions and 34 deletions.
16 changes: 8 additions & 8 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 @@ -112,13 +112,13 @@ inline Eigen::Isometry3d convert_pose(const IgnPoseT& _pose)
return tf;
}

typedef struct TrajectoryPoint
struct TrajectoryPoint
{
Eigen::Vector3d pos;
Eigen::Quaterniond quat;
TrajectoryPoint(const Eigen::Vector3d& _pos, const Eigen::Quaterniond& _quat)
: pos(_pos), quat(_quat) {}
} TrajectoryPoint;
};

// steering type constants
enum class SteeringType
Expand All @@ -130,12 +130,12 @@ enum class SteeringType
class SlotcarCommon
{
public:
typedef struct UpdateResult
struct UpdateResult
{
double v = 0.0; // Target displacement in X (forward)
double w = 0.0; // Target displacement in yaw
double speed = 0.0; // Target speed
} UpdateResult;
};

SlotcarCommon();

Expand Down Expand Up @@ -310,10 +310,10 @@ class SlotcarCommon

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

void diff_drive_path_request_cb(
void handle_diff_drive_path_request(
const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg);

void ackermann_path_request_cb(
void handle_ackermann_path_request(
const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg);

void pause_request_cb(const rmf_fleet_msgs::msg::PauseRequest::SharedPtr msg);
Expand All @@ -322,11 +322,11 @@ class SlotcarCommon

void map_cb(const rmf_building_map_msgs::msg::BuildingMap::SharedPtr msg);

UpdateResult update_diff_drive(const Eigen::Isometry3d& pose,
UpdateResult update_diff_drive(
const std::vector<Eigen::Vector3d>& obstacle_positions,
const double time);

UpdateResult update_ackermann(const Eigen::Isometry3d& pose,
UpdateResult update_ackermann(
const std::vector<Eigen::Vector3d>& obstacle_positions,
const double time);

Expand Down
38 changes: 12 additions & 26 deletions rmf_robot_sim_common/src/slotcar_common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,18 +186,17 @@ void SlotcarCommon::path_request_cb(
switch (this->_steering_type)
{
case SteeringType::DIFF_DRIVE:
diff_drive_path_request_cb(msg);
handle_diff_drive_path_request(msg);
break;
case SteeringType::ACKERMANN:
ackermann_path_request_cb(msg);
handle_ackermann_path_request(msg);
break;
default:
break;
// NOOP
}
}

void SlotcarCommon::diff_drive_path_request_cb(
void SlotcarCommon::handle_diff_drive_path_request(
const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg)
{
const auto old_path = _remaining_path;
Expand Down Expand Up @@ -259,7 +258,7 @@ void SlotcarCommon::diff_drive_path_request_cb(
}
}

void SlotcarCommon::ackermann_path_request_cb(
void SlotcarCommon::handle_ackermann_path_request(
const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg)
{
// yaw is ignored
Expand Down Expand Up @@ -458,19 +457,17 @@ SlotcarCommon::UpdateResult SlotcarCommon::update(const Eigen::Isometry3d& pose,
switch (this->_steering_type)
{
case SteeringType::DIFF_DRIVE:
return update_diff_drive(pose, obstacle_positions, time);
return update_diff_drive(obstacle_positions, time);
case SteeringType::ACKERMANN:
// TODO(anyone) use obstacle_positions for emergency stop for ackermann
return update_ackermann(pose, obstacle_positions, time);
return update_ackermann(obstacle_positions, time);
default:
return UpdateResult();
// NOOP
}
}

// First value of par is x_target, second is yaw_target
SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive(
const Eigen::Isometry3d& pose,
const std::vector<Eigen::Vector3d>& obstacle_positions,
const double time)
{
Expand Down Expand Up @@ -648,13 +645,6 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive(
current_heading,
goal_heading);

// Put in a deadzone if yaw is small enough. This essentially locks the
// tires. COMMENTED OUT as it breaks rotations for some reason...
// if(std::abs(result.w) < std::max(0.1*M_PI/180.00, goal_yaw_tolerance))
// {
// result.w = 0.0;
// }

result.v = 0.0;
}

Expand Down Expand Up @@ -688,9 +678,8 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_diff_drive(
}

SlotcarCommon::UpdateResult SlotcarCommon::update_ackermann(
const Eigen::Isometry3d& pose,
const std::vector<Eigen::Vector3d>& /*obstacle_positions*/,
const double time)
const double /*time*/)
{

UpdateResult result;
Expand All @@ -706,7 +695,7 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_ackermann(
if (traj.turning == false)
{
Eigen::Vector3d to_waypoint = Eigen::Vector3d(traj.x1.x(), traj.x1.y(), 0) -
pose.translation();
_pose.translation();
to_waypoint(2) = 0.0;

const Eigen::Vector3d dpos = to_waypoint;
Expand All @@ -716,7 +705,7 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_ackermann(
result.v = dpos_mag >= wp_range ? dpos_mag : 0.0;

// figure out where we are relative to the goal point
Eigen::Vector2d position(pose.translation().x(), pose.translation().y());
Eigen::Vector2d position(_pose.translation().x(), _pose.translation().y());
Eigen::Vector2d dest_pt = traj.x1;
Eigen::Vector2d forward = traj.v1;
Eigen::Vector2d dest_pt_to_current_position = position - dest_pt;
Expand All @@ -725,7 +714,7 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_ackermann(
// we behind the goal point, turn to suit our needs
if (dotp_location < 0.0)
{
Eigen::Vector2d heading = pose.linear().block<2, 1>(0, 0);
Eigen::Vector2d heading = _pose.linear().block<2, 1>(0, 0);
heading = heading.normalized();
Eigen::Vector2d dpos_norm(dpos.x(), dpos.y());
dpos_norm = dpos_norm.normalized();
Expand All @@ -737,19 +726,16 @@ SlotcarCommon::UpdateResult SlotcarCommon::update_ackermann(
else
result.w = 0.0;

// printf("_ackermann_traj_idx %d dpos_mag: %g displacements %g, %g\n",
// _ackermann_traj_idx, dpos_mag, result.v, result.w);

close_enough = (dpos_mag < wp_range) || dotp_location >= 0.0;
if (_ackermann_traj_idx != (ackermann_trajectory.size() - 1))
result.speed = _nominal_drive_speed;
}
else
{
Eigen::Vector2d position(pose.translation().x(), pose.translation().y());
Eigen::Vector2d position(_pose.translation().x(), _pose.translation().y());
result.speed = _nominal_drive_speed;

Eigen::Vector2d heading = pose.linear().block<2, 1>(0, 0);
Eigen::Vector2d heading = _pose.linear().block<2, 1>(0, 0);
heading = heading.normalized();
Eigen::Vector2d target_heading = traj.v1;

Expand Down

0 comments on commit de0f865

Please sign in to comment.