Skip to content

Commit

Permalink
invert check, add const, add variable names
Browse files Browse the repository at this point in the history
  • Loading branch information
@robomic
robomic committed Oct 1, 2024
1 parent a8bbce3 commit cd28265
Show file tree
Hide file tree
Showing 2 changed files with 70 additions and 61 deletions.
71 changes: 39 additions & 32 deletions simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,12 +51,12 @@ class EntityBase
{
public:
explicit EntityBase(
const std::string & name, const CanonicalizedEntityStatus &,
const std::shared_ptr<hdmap_utils::HdMapUtils> &);
const std::string & name, const CanonicalizedEntityStatus & entity_status,
const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr);

virtual ~EntityBase() = default;

virtual void appendDebugMarker(visualization_msgs::msg::MarkerArray &);
virtual void appendDebugMarker(visualization_msgs::msg::MarkerArray & /*unused*/);

virtual auto asFieldOperatorApplication() const -> concealer::FieldOperatorApplication &;

Expand Down Expand Up @@ -112,7 +112,7 @@ class EntityBase

/* */ auto getCanonicalizedLaneletPose() const -> std::optional<CanonicalizedLaneletPose>;

/* */ auto getCanonicalizedLaneletPose(double matching_distance) const
/* */ auto getCanonicalizedLaneletPose(const double matching_distance) const
-> std::optional<CanonicalizedLaneletPose>;

virtual auto getMaxAcceleration() const -> double = 0;
Expand All @@ -123,7 +123,7 @@ class EntityBase

virtual auto getObstacle() -> std::optional<traffic_simulator_msgs::msg::Obstacle> = 0;

virtual auto getRouteLanelets(double horizon = 100) -> lanelet::Ids = 0;
virtual auto getRouteLanelets(const double horizon = 100.0) -> lanelet::Ids = 0;

virtual auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray = 0;

Expand All @@ -146,67 +146,72 @@ class EntityBase
virtual void requestLaneChange(const traffic_simulator::lane_change::Parameter &){};

/* */ void requestLaneChange(
const lane_change::AbsoluteTarget &, const lane_change::TrajectoryShape,
const lane_change::Constraint &);
const lane_change::AbsoluteTarget & target, const lane_change::TrajectoryShape trajectory_shape,
const lane_change::Constraint & constraint);

/* */ void requestLaneChange(
const lane_change::RelativeTarget &, const lane_change::TrajectoryShape,
const lane_change::Constraint &);
const lane_change::RelativeTarget & target, const lane_change::TrajectoryShape trajectory_shape,
const lane_change::Constraint & constraint);

virtual void requestSpeedChange(
const double, const speed_change::Transition, const speed_change::Constraint, const bool);
const double target_speed, const speed_change::Transition transition,
const speed_change::Constraint constraint, const bool continuous);

virtual void requestSpeedChange(
const speed_change::RelativeTargetSpeed &, const speed_change::Transition,
const speed_change::Constraint, const bool);
const speed_change::RelativeTargetSpeed & target_speed,
const speed_change::Transition transition, const speed_change::Constraint constraint,
const bool continuous);

virtual void requestSpeedChange(double, bool);
virtual void requestSpeedChange(const double target_speed, const bool continuous);

virtual void requestSpeedChange(const speed_change::RelativeTargetSpeed &, bool);
virtual void requestSpeedChange(
const speed_change::RelativeTargetSpeed & target_speed, const bool continuous);

virtual void requestClearRoute() {}

virtual auto isControlledBySimulator() const -> bool;

virtual auto setControlledBySimulator(bool) -> void;
virtual auto setControlledBySimulator(const bool /*unused*/) -> void;

virtual auto requestFollowTrajectory(
const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void;
const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> & /*unused*/) -> void;

virtual void requestWalkStraight();

virtual void setAccelerationLimit(double acceleration) = 0;
virtual void setAccelerationLimit(const double acceleration) = 0;

virtual void setAccelerationRateLimit(double acceleration_rate) = 0;
virtual void setAccelerationRateLimit(const double acceleration_rate) = 0;

virtual void setDecelerationLimit(double deceleration) = 0;
virtual void setDecelerationLimit(const double deceleration) = 0;

virtual void setDecelerationRateLimit(double deceleration_rate) = 0;
virtual void setDecelerationRateLimit(const double deceleration_rate) = 0;

/* */ void setDynamicConstraints(const traffic_simulator_msgs::msg::DynamicConstraints &);
/* */ void setDynamicConstraints(
const traffic_simulator_msgs::msg::DynamicConstraints & constraints);

virtual void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) = 0;

/* */ void setOtherStatus(const std::unordered_map<std::string, CanonicalizedEntityStatus> &);
/* */ void setOtherStatus(
const std::unordered_map<std::string, CanonicalizedEntityStatus> & status);

virtual auto setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void;

virtual auto setStatus(const EntityStatus & status) -> void;

/* */ auto setCanonicalizedStatus(const CanonicalizedEntityStatus &) -> void;
/* */ auto setCanonicalizedStatus(const CanonicalizedEntityStatus & status) -> void;

virtual auto setLinearAcceleration(const double linear_acceleration) -> void;

virtual auto setLinearVelocity(const double linear_velocity) -> void;

virtual void setTrafficLightManager(
const std::shared_ptr<traffic_simulator::TrafficLightManager> &);
const std::shared_ptr<traffic_simulator::TrafficLightManager> & traffic_light_manager);

virtual auto activateOutOfRangeJob(
double min_velocity, double max_velocity, double min_acceleration, double max_acceleration,
double min_jerk, double max_jerk) -> void;
const double min_velocity, const double max_velocity, const double min_acceleration,
const double max_acceleration, const double min_jerk, const double max_jerk) -> void;

virtual auto setVelocityLimit(double) -> void = 0;
virtual auto setVelocityLimit(const double) -> void = 0;

virtual auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void;

Expand Down Expand Up @@ -266,17 +271,19 @@ class EntityBase

private:
virtual auto requestSpeedChangeWithConstantAcceleration(
const double target_speed, const speed_change::Transition, double acceleration,
const double target_speed, const speed_change::Transition, const double acceleration,
const bool continuous) -> void;
virtual auto requestSpeedChangeWithConstantAcceleration(
const speed_change::RelativeTargetSpeed & target_speed,
const speed_change::Transition transition, double acceleration, const bool continuous) -> void;
const speed_change::Transition transition, const double acceleration, const bool continuous)
-> void;
virtual auto requestSpeedChangeWithTimeConstraint(
const double target_speed, const speed_change::Transition, double acceleration_time) -> void;
const double target_speed, const speed_change::Transition, const double acceleration_time)
-> void;
virtual auto requestSpeedChangeWithTimeConstraint(
const speed_change::RelativeTargetSpeed & target_speed,
const speed_change::Transition transition, double time) -> void;
/* */ auto isTargetSpeedReached(double target_speed) const -> bool;
const speed_change::Transition transition, const double time) -> void;
/* */ auto isTargetSpeedReached(const double target_speed) const -> bool;
/* */ auto isTargetSpeedReached(const speed_change::RelativeTargetSpeed & target_speed) const
-> bool;
};
Expand Down
60 changes: 31 additions & 29 deletions simulation/traffic_simulator/src/entity/entity_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ EntityBase::EntityBase(
}
}

void EntityBase::appendDebugMarker(visualization_msgs::msg::MarkerArray &) {}
void EntityBase::appendDebugMarker(visualization_msgs::msg::MarkerArray & /*unused*/) {}

auto EntityBase::asFieldOperatorApplication() const -> concealer::FieldOperatorApplication &
{
Expand All @@ -69,7 +69,7 @@ auto EntityBase::getCanonicalizedLaneletPose() const -> std::optional<Canonicali
return status_->getCanonicalizedLaneletPose();
}

auto EntityBase::getCanonicalizedLaneletPose(double matching_distance) const
auto EntityBase::getCanonicalizedLaneletPose(const double matching_distance) const
-> std::optional<CanonicalizedLaneletPose>
{
const auto include_crosswalk = [](const auto & entity_type) {
Expand All @@ -88,7 +88,7 @@ auto EntityBase::getDefaultMatchingDistanceForLaneletPoseCalculation() const ->
return getBoundingBox().dimensions.y * 0.5 + 1.0;
}

auto EntityBase::isTargetSpeedReached(double target_speed) const -> bool
auto EntityBase::isTargetSpeedReached(const double target_speed) const -> bool
{
return speed_planner_->isTargetSpeedReached(target_speed, getCurrentTwist());
}
Expand Down Expand Up @@ -133,7 +133,7 @@ void EntityBase::requestLaneChange(
const traffic_simulator::lane_change::TrajectoryShape trajectory_shape,
const traffic_simulator::lane_change::Constraint & constraint)
{
lanelet::Id reference_lanelet_id = 0;
lanelet::Id reference_lanelet_id = 0UL;
if (target.entity_name == name) {
if (not laneMatchingSucceed()) {
THROW_SEMANTIC_ERROR(
Expand Down Expand Up @@ -168,10 +168,10 @@ void EntityBase::requestLaneChange(
}

void EntityBase::requestSpeedChangeWithConstantAcceleration(
const double target_speed, const speed_change::Transition transition, double acceleration,
const double target_speed, const speed_change::Transition transition, const double acceleration,
const bool continuous)
{
if (!continuous && isTargetSpeedReached(target_speed)) {
if (isTargetSpeedReached(target_speed) && !continuous) {
return;
}
switch (transition) {
Expand Down Expand Up @@ -219,7 +219,8 @@ void EntityBase::requestSpeedChangeWithConstantAcceleration(
}

void EntityBase::requestSpeedChangeWithTimeConstraint(
const double target_speed, const speed_change::Transition transition, double acceleration_time)
const double target_speed, const speed_change::Transition transition,
const double acceleration_time)
{
if (isTargetSpeedReached(target_speed)) {
return;
Expand Down Expand Up @@ -274,7 +275,7 @@ void EntityBase::requestSpeedChange(
const double target_speed, const speed_change::Transition transition,
const speed_change::Constraint constraint, const bool continuous)
{
if (!continuous && isTargetSpeedReached(target_speed)) {
if (isTargetSpeedReached(target_speed) && !continuous) {
return;
}
switch (constraint.type) {
Expand All @@ -293,9 +294,9 @@ void EntityBase::requestSpeedChange(

void EntityBase::requestSpeedChangeWithConstantAcceleration(
const speed_change::RelativeTargetSpeed & target_speed, const speed_change::Transition transition,
double acceleration, const bool continuous)
const double acceleration, const bool continuous)
{
if (!continuous && isTargetSpeedReached(target_speed)) {
if (isTargetSpeedReached(target_speed) && !continuous) {
return;
}
switch (transition) {
Expand All @@ -311,19 +312,20 @@ void EntityBase::requestSpeedChangeWithConstantAcceleration(
* @brief Checking if the entity reaches target speed.
*/
[this, target_speed, acceleration](double) {
double diff = target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_) -
getCurrentTwist().linear.x;
const double diff =
target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_) -
getCurrentTwist().linear.x;
/**
* @brief Hard coded parameter, threshold for difference
*/
if (std::abs(diff) <= 0.1) {
return true;
}
if (diff > 0) {
if (diff > 0.1) {
setAccelerationLimit(std::abs(acceleration));
return false;
}
if (diff < 0) {
if (diff < 0.1) {
setDecelerationLimit(std::abs(acceleration));
return false;
}
Expand All @@ -347,7 +349,7 @@ void EntityBase::requestSpeedChangeWithConstantAcceleration(

void EntityBase::requestSpeedChangeWithTimeConstraint(
const speed_change::RelativeTargetSpeed & target_speed, const speed_change::Transition transition,
double acceleration_time)
const double acceleration_time)
{
if (isTargetSpeedReached(target_speed)) {
return;
Expand Down Expand Up @@ -384,7 +386,7 @@ void EntityBase::requestSpeedChange(
const speed_change::RelativeTargetSpeed & target_speed, const speed_change::Transition transition,
const speed_change::Constraint constraint, const bool continuous)
{
if (!continuous && isTargetSpeedReached(target_speed)) {
if (isTargetSpeedReached(target_speed) && !continuous) {
return;
}
switch (constraint.type) {
Expand All @@ -404,9 +406,9 @@ void EntityBase::requestSpeedChange(
}
}

void EntityBase::requestSpeedChange(double target_speed, bool continuous)
void EntityBase::requestSpeedChange(const double target_speed, const bool continuous)
{
if (!continuous && isTargetSpeedReached(target_speed)) {
if (isTargetSpeedReached(target_speed) && !continuous) {
return;
}
if (continuous) {
Expand Down Expand Up @@ -445,9 +447,9 @@ void EntityBase::requestSpeedChange(double target_speed, bool continuous)
}

void EntityBase::requestSpeedChange(
const speed_change::RelativeTargetSpeed & target_speed, bool continuous)
const speed_change::RelativeTargetSpeed & target_speed, const bool continuous)
{
if (!continuous && isTargetSpeedReached(target_speed)) {
if (isTargetSpeedReached(target_speed) && !continuous) {
return;
}
if (continuous) {
Expand Down Expand Up @@ -488,14 +490,14 @@ void EntityBase::requestSpeedChange(

auto EntityBase::isControlledBySimulator() const -> bool { return true; }

auto EntityBase::setControlledBySimulator(bool /*unused*/) -> void
auto EntityBase::setControlledBySimulator(const bool /*unused*/) -> void
{
THROW_SEMANTIC_ERROR(
getEntityTypename(), " type entities do not support setControlledBySimulator");
}

auto EntityBase::requestFollowTrajectory(
const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> &) -> void
const std::shared_ptr<traffic_simulator_msgs::msg::PolylineTrajectory> & /*unused*/) -> void
{
THROW_SEMANTIC_ERROR(
getEntityTypename(), " type entities do not support follow trajectory action.");
Expand Down Expand Up @@ -570,15 +572,15 @@ auto EntityBase::setLinearJerk(const double linear_jerk) -> void

auto EntityBase::setAction(const std::string & action) -> void { status_->setAction(action); }

auto EntityBase::setMapPose(const geometry_msgs::msg::Pose &) -> void
auto EntityBase::setMapPose(const geometry_msgs::msg::Pose & /*unused*/) -> void
{
THROW_SEMANTIC_ERROR(
"You cannot set map pose to the vehicle other than ego named ", std::quoted(name), ".");
}

void EntityBase::activateOutOfRangeJob(
double min_velocity, double max_velocity, double min_acceleration, double max_acceleration,
double min_jerk, double max_jerk)
const double min_velocity, const double max_velocity, const double min_acceleration,
const double max_acceleration, const double min_jerk, const double max_jerk)
{
/**
* @brief This value was determined heuristically rather than for
Expand Down Expand Up @@ -679,7 +681,7 @@ auto EntityBase::requestSynchronize(
THROW_SYNTAX_ERROR("Request synchronize is only for non-ego entities.");
}

if (tolerance == 0) {
if (tolerance == 0.0) {
RCLCPP_WARN_ONCE(
rclcpp::get_logger("traffic_simulator"),
"The tolerance is set to 0.0. This may cause the entity to never reach the target lanelet.");
Expand Down Expand Up @@ -724,7 +726,7 @@ auto EntityBase::requestSynchronize(
const auto target_entity_distance = longitudinalDistance(
CanonicalizedLaneletPose(target_entity_lanelet_pose, hdmap_utils_ptr_), target_sync_pose,
true, true, true, hdmap_utils_ptr_);
if (!target_entity_distance.has_value() || target_entity_distance.value() < 0) {
if (!target_entity_distance.has_value() || target_entity_distance.value() < 0.0) {
RCLCPP_WARN_ONCE(
rclcpp::get_logger("traffic_simulator"),
"Failed to get distance between target entity and target lanelet pose. Check if target "
Expand All @@ -739,12 +741,12 @@ auto EntityBase::requestSynchronize(
const auto target_entity_arrival_time =
(std::abs(target_entity_velocity) > std::numeric_limits<double>::epsilon())
? target_entity_distance.value() / target_entity_velocity
: 0;
: 0.0;

auto entity_velocity_to_synchronize = [this, entity_velocity, target_entity_arrival_time,
entity_distance, target_speed]() {
const auto border_distance =
(entity_velocity + target_speed) * target_entity_arrival_time / 2;
(entity_velocity + target_speed) * target_entity_arrival_time / 2.0;
if (border_distance < entity_distance.value()) {
///@brief Making entity speed up.
return entity_velocity + getMaxAcceleration() * step_time_;
Expand Down

0 comments on commit cd28265

Please sign in to comment.