Skip to content

Commit

Permalink
Update target lanelet poses and velocities
Browse files Browse the repository at this point in the history
  • Loading branch information
@curious-jp
curious-jp committed Feb 20, 2024
1 parent 0c5fb8b commit f18fc68
Show file tree
Hide file tree
Showing 3 changed files with 86 additions and 55 deletions.
43 changes: 27 additions & 16 deletions mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,45 +43,56 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode
private:
bool requested = false;
const traffic_simulator::CanonicalizedLaneletPose ego_target =
getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34510, 0, 0, 0, 0, 0));
getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34585, 0, 0, 0, 0, 0));
const traffic_simulator::CanonicalizedLaneletPose npc_target =
getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34495, 15, 0, 0, 0, 0));
getSampleLaneletPose(traffic_simulator::helper::constructLaneletPose(34570, 0, 0, 0, 0, 0));

void onUpdate() override
{
// SUCCESS
// check if npc is in the target lanelet when ego is in the target lanelet and npc is stopped
if (
api_.requestSynchronize("npc", ego_target, npc_target, 0.2) &&
// api_.getIfArrivedToTargetLaneletPose("ego", ego_target, 0.2) &&
// api_.getCurrentTwist("npc").linear.x == 0.0) {
true) {
api_.requestSynchronize("npc", ego_target, npc_target, 0.2);
// SUCCESS
// check if npc is in the target lanelet when ego is in the target lanelet and npc is stopped
if (
// api_.getIfArrivedToTargetLaneletPose("ego", ego_target, 0.2) &&
// api_.getIfArrivedToTargetLaneletPose("npc", npc_target, 0.2) &&
// api_.getCurrentTwist("npc").linear.x <= 0.1
false
)
{
stop(cpp_mock_scenarios::Result::SUCCESS);
}

// FAILURES
if (api_.getCurrentTime() >= 20.0) {
if (api_.getCurrentTime() >= 40.0) {
stop(cpp_mock_scenarios::Result::FAILURE);
}
if (api_.checkCollision("ego", "npc")) {
stop(cpp_mock_scenarios::Result::FAILURE);
}

}
void onInitialize() override
{
api_.spawn(
"ego",
api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34513, 15, 0, 0, 0, 0)),
api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34976, 10, 0, 0, 0, 0)),
getVehicleParameters());
api_.setLinearVelocity("ego", 5);
// api_.requestSpeedChange("ego", 5, true);
// api_.requestLaneChange("ego", 34513);
api_.setLinearVelocity("ego", 10);
api_.requestSpeedChange("ego", 10, true);

std::vector<geometry_msgs::msg::Pose> goal_poses;
goal_poses.emplace_back(api_.toMapPose(
api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34579, 10, 0, 0, 0, 0))));
// goal_poses.emplace_back(api_.toMapPose(
// api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34579, 10, 0, 0, 0, 0))));
api_.requestAssignRoute("ego", goal_poses);


api_.spawn(
"npc",
api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34564, 5, 0, 0, 0, 0)),
api_.canonicalize(traffic_simulator::helper::constructLaneletPose(34576, 5, 0, 0, 0, 0)),
getVehicleParameters());
api_.setLinearVelocity("npc", 5);
api_.setLinearVelocity("npc", 0);
}

auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose)
Expand Down
3 changes: 2 additions & 1 deletion simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -229,7 +229,8 @@ class EntityBase

/* */ auto updateTraveledDistance(const double step_time) -> double;

/* */ auto getDistanceToTargetLaneletPose(const CanonicalizedLaneletPose & target_lanelet_pose)
/* */ auto getDistanceToTargetLaneletPose(
const CanonicalizedLaneletPose & target_lanelet_pose, const double matching_distance)
-> std::optional<double>;

/* */ auto getIfArrivedToTargetLaneletPose(
Expand Down
95 changes: 57 additions & 38 deletions simulation/traffic_simulator/src/entity/entity_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -819,77 +819,91 @@ auto EntityBase::updateTraveledDistance(const double step_time) -> double
}

auto EntityBase::getDistanceToTargetLaneletPose(
const CanonicalizedLaneletPose & target_lanelet_pose) -> std::optional<double>
const CanonicalizedLaneletPose & target_lanelet_pose, const double matching_distance)
-> std::optional<double>
{
constexpr double matching_distance = 100.0; // may be better to use a parameter
const auto entity_lanelet_pose_ = getLaneletPose(matching_distance);
const double matching_distance_ = matching_distance;
const auto entity_lanelet_pose_ = getLaneletPose(matching_distance_);
const auto target_lanelet_pose_ = target_lanelet_pose;

// check if the entity is on the lanelet
if (entity_lanelet_pose_) {
const auto entity_distance_to_target = hdmap_utils_ptr_->getLongitudinalDistance(
static_cast<LaneletPose>(entity_lanelet_pose_.value()),
static_cast<LaneletPose>(target_lanelet_pose_));
if (!entity_lanelet_pose_) {
THROW_SEMANTIC_ERROR(
"Failed to get entity's lanelet pose. Please check entity lanelet pose exists.");
}

const auto entity_distance_to_target = hdmap_utils_ptr_->getLongitudinalDistance(
static_cast<LaneletPose>(entity_lanelet_pose_.value()),
static_cast<LaneletPose>(target_lanelet_pose_));

return entity_distance_to_target;
if (!entity_distance_to_target.has_value()) {
/**
* This is not an error, means that the entity is already over the target lanelet.
*/
return std::nullopt;
}

// may be give error here too?
return std::nullopt;
return entity_distance_to_target;
}

auto EntityBase::getIfArrivedToTargetLaneletPose(
const CanonicalizedLaneletPose & target_lanelet_pose, const double threshold) -> bool
{
if (!getDistanceToTargetLaneletPose(target_lanelet_pose).has_value()) {
// error here
}
const auto entity_lanelet_pose_ = getLaneletPose();
const auto target_lanelet_pose_ = target_lanelet_pose;
/**
* WARNING*****WIP*********
* mesuring distance is set to 100 manually
*/
const auto entity_distance_to_target = getDistanceToTargetLaneletPose(target_lanelet_pose, 100);

const auto entity_distance_to_target = hdmap_utils_ptr_->getLongitudinalDistance(
static_cast<LaneletPose>(entity_lanelet_pose_.value()),
static_cast<LaneletPose>(target_lanelet_pose_));
if (!entity_distance_to_target.has_value()) {
/**
* @brief meaning that the entity is already over the target lanelet.
*
*/
return true;
}

if (entity_distance_to_target <= threshold) {
return true;
} else {
return false;
}
return false;
}

/** WARNING***************
* This should not be called from ego entity
*/
auto EntityBase::requestSynchronize(
const CanonicalizedLaneletPose & ego_target, const CanonicalizedLaneletPose & entity_target,
const double threshold) -> bool
{
if (name == "ego") {
/**
* *********************CHECK*********************
* is this correct to throw syntax error?
*/
THROW_SYNTAX_ERROR("The entity that is requested to synchronize is the ego. Please check.");
}
if (getIfArrivedToTargetLaneletPose(entity_target, threshold)) {
return true;
}

job_list_.append(
[this, ego_target, entity_target](double) -> bool {
// WARNING****************
// not checking job_duration_ at all
const auto entity_distance = getDistanceToTargetLaneletPose(entity_target);
[this, ego_target, entity_target](double) {
/**
* @brief This is draft function for synchronization.
* It is not a good way to synchronize the entity with the ego.
* WARNING****************
* not checking job_duration_ at all
*/
const auto entity_distance = getDistanceToTargetLaneletPose(entity_target, 100);
const auto ego_distance = this->hdmap_utils_ptr_->getLongitudinalDistance(
static_cast<LaneletPose>(other_status_.find("ego")->second.getLaneletPose()),
static_cast<LaneletPose>(ego_target));

if (!entity_distance.has_value() ){
if (!entity_distance.has_value()) {
THROW_SEMANTIC_ERROR(
"Failed to get entity's distance to target lanelet pose. Please check entity distance exists.");
"Entity is already over the target lanelet.");
}
if (!ego_distance.has_value() ){
THROW_SEMANTIC_ERROR(
"Failed to get ego's distance to target lanelet pose. Please check ego distance exists.");
}

const auto ego_velocity = other_status_.find("ego")->second.getTwist().linear.x;
// be better to use acceleration,jerk to estimate the arrival time

RCLCPP_ERROR(rclcpp::get_logger("simulation"), "#####################");
// estimate ego's arrival time to the target point
// can estimate it more precisely by using accel,jerk
const auto ego_arrival_time = (ego_velocity != 0) ? ego_distance.value() / ego_velocity : 0;
Expand All @@ -898,13 +912,18 @@ auto EntityBase::requestSynchronize(
// really just a simple example, kind of like a joke
const auto entity_velocity_to_synchronize = entity_distance.value() / ego_arrival_time;

this->requestSpeedChange(entity_velocity_to_synchronize, true);
RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "speed change");
// using this->requestSpeedChange here does not work in some kind of reason.
// it seems that after this function is called by some reason, func_on_cleanup will be deleted and become nullptr
// this->requestSpeedChange(entity_velocity_to_synchronize, true);
target_speed_ = entity_velocity_to_synchronize;
return true;
},
// after this im not sure it is correct, just an draft
[this]() {}, job::Type::LINEAR_ACCELERATION, true, job::Event::POST_UPDATE);
[this]() { RCLCPP_ERROR(rclcpp::get_logger("traffic_simulator"), "sync job done"); },
job::Type::LINEAR_VELOCITY, false, job::Event::POST_UPDATE);
return false;
}

} // namespace entity
} // namespace traffic_simulator
} // namespace traffic_simulator

0 comments on commit f18fc68

Please sign in to comment.