RJD-1930 Add see_around for WalkStraightAction

[gmajrobotec]

Description

This PR implements see_around functionality for WalkStraightAction of npc pedestrian.

Abstract

WalkStraightAction::doAction takes into account see_around parameter and if pedestrian_ignore_see_around is also set to aware before changing pedestrian position it checks if there is no other entity in front of it in distance it can stop.

Details

1. Check if any entity is in front of pedestrian.
2. Check if it is in defined minimal distance => minimal distance is calculated based on distance NPC needs to stop.
3. Check if it will collide with pedestrian.

Destructive Changes

--

Known Limitations

--

References

RJD-1930

[dmoszynski]

I'm not sure, but 'see around' might also apply to other entities. Maybe we should move this enum class SeeAroundMode to ActionNode since it's being moved anyway?

[gmajrobotec]

this part is only for pedestrian_ignore_see_around

[dmoszynski]

Suggested change

	bool checkPointIsInfront(
	const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & point)
	{
	const auto self_yaw = math::geometry::convertQuaternionToEulerAngle(pose.orientation).z;
	const auto dx = point.x - pose.position.x;
	const auto dy = point.y - pose.position.y;
	const auto vec_yaw = std::atan2(dy, dx);
	const auto yaw_diff = std::atan2(std::sin(vec_yaw - self_yaw), std::cos(vec_yaw - self_yaw));
	return std::fabs(yaw_diff) <= boost::math::constants::half_pi<double>();
	}
	bool isPointInFront(
	const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point)
	{
	const auto self_yaw = math::geometry::convertQuaternionToEulerAngle(pose.orientation).z;
	const auto angle_to_target_point = std::atan2(target_point.y - pose.position.y, target_point.x - pose.position.x);
	const auto yaw_difference = std::atan2(std::sin(angle_to_target_point - self_yaw), std::cos(angle_to_target_point - self_yaw));
	return std::fabs(yaw_difference) <= boost::math::constants::half_pi<double>();
	}

[gmajrobotec]

done

[dmoszynski]

In general, the names need to be improved to make them more informative - they should refer to what is actually being calculated (what the object contains).

In general, you always need to remember {} for if statements, const for immutable objects, and some relative step-by-step logic.

What is suggested here is only part of the changes; the whole thing needs to be reconsidered.

Suggested change

	bool WalkStraightAction::detectObstacleInFront(const bool see_around) const
	{
	if (should_respect_see_around == SeeAroundMode::blind) {
	return false;
	}
	if (!see_around) {
	return false;
	}
	auto hasObstacleInFrontOfPedestrian = [this](double distance) {
	using math::geometry::operator-;
	using math::geometry::operator*;
	const auto & pedestrian_pose = canonicalized_entity_status_->getMapPose();
	for (const auto & [_, entity_status] : other_entity_status_) {
	const auto & other_position = entity_status.getMapPose().position;
	const auto norm = math::geometry::norm(other_position - pedestrian_pose.position);
	if (norm > distance) continue;
	if (checkPointIsInfront(pedestrian_pose, other_position)) {
	// is in front of pedestrian check if collide
	const auto bounding_box_map_points = math::geometry::transformPoints(
	pedestrian_pose,
	math::geometry::getPointsFromBbox(canonicalized_entity_status_->getBoundingBox()));
	std::vector<geometry_msgs::msg::Point> front_points;
	std::vector<geometry_msgs::msg::Point> check_area_points;
	for (const auto & point : bounding_box_map_points) {
	if (checkPointIsInfront(pedestrian_pose, point)) {
	front_points.push_back(point);
	}
	}
	auto orientation_vector =
	math::geometry::normalize(
	math::geometry::convertQuaternionToEulerAngle(pedestrian_pose.orientation)) *
	distance;
	for (auto & point : front_points) {
	check_area_points.push_back(point);
	point.x += orientation_vector.x;
	point.y += orientation_vector.z;
	point.z += orientation_vector.y;
	check_area_points.push_back(point);
	}
	const auto check_polygon = math::geometry::toBoostPolygon(check_area_points);
	const auto poly =
	math::geometry::toPolygon2D(entity_status.getMapPose(), entity_status.getBoundingBox());
	if (
	boost::geometry::intersects(check_polygon, poly) ||
	boost::geometry::intersects(poly, check_polygon)) {
	return true;
	} else if (boost::geometry::disjoint(check_polygon, poly)) {
	return false;
	}
	return true;
	}
	}
	return false;
	};
	const double min_stop = calculateStopDistance(behavior_parameter_.dynamic_constraints);
	const double stable_distance =
	min_stop + canonicalized_entity_status_->getBoundingBox().dimensions.x;
	return hasObstacleInFrontOfPedestrian(stable_distance);
	}
	bool WalkStraightAction::detectObstacleInFront(const bool see_around) const
	{
	auto isObstacleInFrontOfPedestrian = [this](double detection_horizon) {
	using math::geometry::operator-;
	using math::geometry::operator*;
	const auto & pedestrian_pose = canonicalized_entity_status_->getMapPose();
	for (const auto & [_, entity_status] : other_entity_status_) {
	const auto & other_position = entity_status.getMapPose().position;
	if (const auto distance = math::geometry::norm(other_position - pedestrian_pose.position); distance <= detection_horizon) {
	if (isPointInfront(pedestrian_pose, other_position)) {
	/// @note if in front of pedestrian -> check if collide
	const auto bounding_box_map_points = math::geometry::transformPoints(pedestrian_pose, math::geometry::getPointsFromBbox(canonicalized_entity_status_->getBoundingBox()));
	std::vector<geometry_msgs::msg::Point> bounding_box_front_points;
	for (const auto & point : bounding_box_map_points) {
	if (isPointInfront(pedestrian_pose, point)) {
	bounding_box_front_points.push_back(point);
	}
	}
	/// @attention rename orientation_vector...
	const auto orientation_vector = math::geometry::normalize(math::geometry::convertQuaternionToEulerAngle(pedestrian_pose.orientation)) * detection_horizon;
	/// @attention [reply on github] Why is the point added twice to check_area_points?
	std::vector<geometry_msgs::msg::Point> pedestrian_check_area_points;
	for (auto & point : pedestrian_front_points) {
	pedestrian_check_area_points.push_back(point);
	point.x += orientation_vector.x;
	point.y += orientation_vector.z;
	point.z += orientation_vector.y;
	pedestrian_check_area_points.push_back(point);
	}
	const auto pedestrian_check_area_polygon = math::geometry::toBoostPolygon(pedestrian_check_area_points);
	const auto other_entity_polygon = math::geometry::toPolygon2D(entity_status.getMapPose(), entity_status.getBoundingBox());
	if (boost::geometry::intersects(pedestrian_check_area_polygon, other_entity_polygon) or boost::geometry::intersects(other_entity_polygon, pedestrian_check_area_polygon)) {
	return true;
	} else if (boost::geometry::disjoint(pedestrian_check_area_polygon, other_entity_polygon)) {
	return false;
	} else {
	return true;
	}
	}
	}
	}
	return false;
	};
	if (not see_around or should_respect_see_around == SeeAroundMode::blind) {
	return false;
	} else {
	const double detection_horizon = calculateStopDistance(behavior_parameter_.dynamic_constraints) + canonicalized_entity_status_->getBoundingBox().dimensions.x;
	return isObstacleInFrontOfPedestrian(detection_horizon);
	}
	}

[gmajrobotec]

Changed names and restructured function.

[dmoszynski]

The name “obstacle_detector_result” suggests that it is a complex object, but I don't think it is, is it?

[gmajrobotec]

changed name