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.

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

Fix/slope inaccuracies #1493

Merged
merged 48 commits into from
Feb 5, 2025
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
48 commits
Select commit Hold shift + click to select a range
8dfa732
fix(traffic_simulator) Fix lanelet slope inaccuracies - follow trajec…
SzymonParapura Dec 17, 2024
0aae035
fix(behavior_tree_plugin) Fix lanelet slope inaccuracies - walk stra…
SzymonParapura Dec 18, 2024
a213185
fix(behavior_tree_plugin) Fix isssue with pedestrian turning
SzymonParapura Dec 18, 2024
1fbb1c1
refactor(behavior_tree_plugin): refactor action_node::calculateUpdate…
SzymonParapura Dec 18, 2024
26f8a65
ref(geometry): add axis header, improve
dmoszynski Dec 19, 2024
6a8f3ce
ref(behavior_tree_plugin, traffic_simulator): separate pose::moveAlon…
dmoszynski Dec 19, 2024
4ee1ebc
ref(traffic_simulator): separete pose::moveToTargetPosition
dmoszynski Dec 19, 2024
f9d067e
ref(traffic_simulator): fix pose::moveToTargetLaneletPose
dmoszynski Dec 19, 2024
72eea6f
fix(traffic_simulator): fix moveToLaneletPose in FollowTrajectoryAction
dmoszynski Dec 19, 2024
bccd3c5
fix(behavior_tree_plugin): use moveToLaneletPose in calculateUpdatedE…
dmoszynski Dec 19, 2024
8b9a76d
Revert "ref(geometry): add axis header, improve"
dmoszynski Dec 19, 2024
ab03b85
ref(geometry): remove rotate
dmoszynski Dec 19, 2024
0977d80
ref(traffic_simulator): cleanup after removal of pose::moveAlongLanelet
dmoszynski Dec 19, 2024
5dfaf19
feat(traffic_simulator): improve moveTowardsLaneletPose to calc Lanel…
dmoszynski Dec 19, 2024
c1df2c5
ref(traffic_simulator): remove irrelevant toMapPosition
dmoszynski Dec 19, 2024
14098e9
fix(traffic_simulator): fix moveTowardsLaneletPose
dmoszynski Dec 19, 2024
311f82a
ref(geometry): remove confusing operator*(quat,vec)
dmoszynski Dec 19, 2024
e3df320
ref(behavior_tree_plugin): refactor ActionNode::calculateUpdatedEntit…
dmoszynski Dec 19, 2024
4efd379
ref(behavior_tree_plugin, traffic_simulator): improve comments
dmoszynski Dec 19, 2024
c59228c
fix(traffis_simulator): use next canonicalized lanelet pose in pose::…
dmoszynski Dec 19, 2024
4b27b38
fix(traffic_simulator) Fix an issue with negative longitudinal displa…
SzymonParapura Dec 20, 2024
32a8838
refactor(traffic_simulator) fix spell check issue
SzymonParapura Dec 20, 2024
8b22d87
Merge branch 'master' into fix/slope_inaccuracies
SzymonParapura Dec 20, 2024
788259c
tmp(behavior_tree_plugin, traffic_simulator): moveTowardsLaneletPose …
dmoszynski Dec 20, 2024
b2074d4
fix(behavior_tree_plugin, traffic_simulator): fix moveTowardsLaneletP…
dmoszynski Dec 20, 2024
ad6c16b
ref(traffic_simulator): tidy up after moveTowardsLaneletPose development
dmoszynski Dec 20, 2024
74bee39
fix(traffic_simulator): revert adjustOrientationAndOzPosition change…
dmoszynski Dec 20, 2024
c3e604f
fix(behavior_tree_plugin) fix sonarquebe issues
SzymonParapura Dec 20, 2024
7ce67d9
Merge branch 'master' into fix/slope_inaccuracies
SzymonParapura Jan 7, 2025
efef508
Merge branch 'master' into fix/slope_inaccuracies
SzymonParapura Jan 18, 2025
e884623
fix(traffic_simulator): Fix lanelet slope inaccuracies
SzymonParapura Jan 23, 2025
72b7c7e
Merge branch 'master' into fix/slope_inaccuracies
SzymonParapura Jan 23, 2025
2ba428f
ref(traffic_simulator) Refactor updateEntityPositionForLaneletTransition
SzymonParapura Jan 23, 2025
c6733e3
fix(traffic_simulator) Improve lanelet transition handling for entity…
SzymonParapura Jan 24, 2025
a41b77c
Merge branch 'master' into fix/slope_inaccuracies
SzymonParapura Jan 24, 2025
404f428
ref(traffic_simulator> Refactor code according to the lastest master …
SzymonParapura Jan 27, 2025
44870e3
reref(behavior_tree_plugin) Update commment
SzymonParapura Jan 27, 2025
d81796b
ref(traffic_simulator): Refactor comments
SzymonParapura Jan 27, 2025
79e7818
ref(traffic_simulator): Refactor comments
SzymonParapura Jan 27, 2025
d38d350
ref(traffic_simulator): Refactor comments
SzymonParapura Jan 27, 2025
5b247b2
Merge branch 'master' into fix/slope_inaccuracies
SzymonParapura Jan 28, 2025
336a7ee
fix(traffic_simulator) Fix an issue with follow trajectory action
SzymonParapura Jan 28, 2025
6d7aa6a
fix(traffic_simulator): Fix an issue with lanelet transition
SzymonParapura Jan 29, 2025
2ea78de
Merge branch 'master' into fix/slope_inaccuracies
HansRobo Jan 30, 2025
c5ed003
Merge branch 'master' into fix/slope_inaccuracies
dmoszynski Jan 31, 2025
63de6f4
Merge branch 'master' into fix/slope_inaccuracies
dmoszynski Jan 31, 2025
403b78b
Merge branch 'master' into fix/slope_inaccuracies
HansRobo Feb 4, 2025
d2d20f6
ref(behavior_tree_plugin): Refactored calculateUpdatedEntityStatusInW…
SzymonParapura Feb 4, 2025
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
Prev Previous commit
Next Next commit
ref(traffic_simulator): Refactor comments
  • Loading branch information
SzymonParapura committed Jan 27, 2025
commit 79e78187e94d521ad61c6cbc97477c48864a9e62
14 changes: 7 additions & 7 deletions simulation/behavior_tree_plugin/src/action_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -254,7 +254,7 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf
const auto quat = math::geometry::getRotation(
canonicalized_entity_status->getMapPose().orientation,
other_entity_status.at(each.first).getMapPose().orientation);
/// @note hard-coded parameter, if the Yaw value of RPY is in ~1.5708 -> 1.5708, entity is a candidate of front entity.
/// @note hard-coded parameter, if the Yaw value of RPY is in [-pi/2, pi/2], entity is a candidate of front entity.
if (
std::fabs(math::geometry::convertQuaternionToEulerAngle(quat).z) <=
boost::math::constants::half_pi<double>()) {
Expand Down Expand Up @@ -480,15 +480,15 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
const geometry_msgs::msg::Twist & desired_twist, const double time_step) {
geometry_msgs::msg::Pose updated_pose;

// Apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * time_step (s)
/// @note Apply yaw change (delta rotation) in radians: yaw_angular_speed (rad/s) * time_step (s)
geometry_msgs::msg::Vector3 delta_rotation;
delta_rotation = desired_twist.angular * time_step;
const auto delta_quaternion = math::geometry::convertEulerAngleToQuaternion(delta_rotation);
updated_pose.orientation = status->getMapPose().orientation * delta_quaternion;

// Apply position change
// @todo first determine global desired_velocity, calculate position change using it
// then pass the same global desired_velocity to updatePositionForLaneletTransition()
/// @note Apply position change
/// @todo first determine global desired_velocity, calculate position change using it
HansRobo marked this conversation as resolved.
Show resolved Hide resolved
/// then pass the same global desired_velocity to updatePositionForLaneletTransition()
const Eigen::Matrix3d rotation_matrix =
math::geometry::getRotationMatrix(updated_pose.orientation);
const auto translation = Eigen::Vector3d(
Expand All @@ -497,7 +497,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
const Eigen::Vector3d delta_position = rotation_matrix * translation;
updated_pose.position = status->getMapPose().position + delta_position;

// If it is the transition between lanelets: overwrite position to improve precision
/// @note If it is the transition between lanelets: overwrite position to improve precision
if (const auto canonicalized_lanelet_pose = status->getCanonicalizedLaneletPose()) {
const auto estimated_next_canonicalized_lanelet_pose =
traffic_simulator::pose::toCanonicalizedLaneletPose(
Expand All @@ -506,7 +506,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame(
const auto next_lanelet_id = static_cast<traffic_simulator::LaneletPose>(
estimated_next_canonicalized_lanelet_pose.value())
.lanelet_id;
if ( // Handle lanelet transition
if ( /// @note Handle lanelet transition
const auto updated_position =
traffic_simulator::pose::updatePositionForLaneletTransition(
canonicalized_lanelet_pose.value(), next_lanelet_id, desired_twist.linear,
Expand Down
Loading
Loading