Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

RJD-1336/fix_request_speed_change_throws #1404

Open
wants to merge 8 commits into
base: master
Choose a base branch
from
+72 −61
Open

RJD-1336/fix_request_speed_change_throws #1404

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
cd28265
invert check, add const, add variable names
robomic Oct 1, 2024
75797c3
Merge branch 'master' into RJD-1336/fix_request_speed_change
robomic Oct 1, 2024
4466a39
Merge branch 'master' into RJD-1336/fix_request_speed_change
robomic Oct 8, 2024
80da324
add update frame to update other_statuses
robomic Oct 8, 2024
b2bcc57
Merge branch 'master' into RJD-1336/fix_request_speed_change
robomic Oct 10, 2024
fda3031
add constexpr
robomic Oct 15, 2024
69c29a5
Merge branch 'RJD-1336/fix_request_speed_change' of github.com:tier4/…
robomic Oct 15, 2024
d423841
Merge branch 'master' into RJD-1336/fix_request_speed_change
robomic Oct 15, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,7 @@ class RequestSpeedChangeRelativeScenario : public cpp_mock_scenarios::CppScenari
34741, 10.0, 0.0, api_.getHdmapUtils()),
getVehicleParameters());
api_.setLinearVelocity("front", 3);
api_.updateFrame();
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This function is intended to be called on the base type and is not expected to be called within scenarios. Access rights modifications will be implemented in a separate PR, so no changes are needed in this PR.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This line is needed in order to pass this particular test. Without it other_status_ variable is not updated and does not contain other entities. Empty other_status_ results in a throw when calculating the absolute speed from speed_change::RelativeTargetSpeed variable.

api_.requestSpeedChange(
"front",
traffic_simulator::speed_change::RelativeTargetSpeed(
Expand Down
71 changes: 39 additions & 32 deletions simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp
View file
Open in desktop
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 @@ -262,17 +267,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
61 changes: 32 additions & 29 deletions simulation/traffic_simulator/src/entity/entity_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ EntityBase::EntityBase(
[this]() {}, job::Type::STAND_STILL_DURATION, true, job::Event::POST_UPDATE);
}

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

auto EntityBase::asFieldOperatorApplication() const -> concealer::FieldOperatorApplication &
{
Expand All @@ -87,7 +87,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 @@ -106,7 +106,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 @@ -186,10 +186,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 @@ -237,7 +237,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 @@ -292,7 +293,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 @@ -311,9 +312,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 @@ -329,19 +330,21 @@ 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) {
static constexpr double difference_threshold = 0.1;
if (std::abs(diff) <= difference_threshold) {
return true;
}
if (diff > 0) {
if (diff > +difference_threshold) {
setAccelerationLimit(std::abs(acceleration));
return false;
}
if (diff < 0) {
if (diff < -difference_threshold) {
setDecelerationLimit(std::abs(acceleration));
return false;
}
Expand All @@ -365,7 +368,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 @@ -402,7 +405,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 @@ -422,9 +425,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 @@ -463,9 +466,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 @@ -506,14 +509,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 @@ -588,15 +591,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 @@ -683,7 +686,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 @@ -728,7 +731,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 @@ -743,12 +746,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
Loading