Feature/96 go around object

[kentohirogaki]

ブイなどのまわりを旋回する指示をだす機能を追加。時計回りか反時計回りの指定や角度指定も可能。

[wam-v-tan]

Code Review Findings:

1. In CMakeLists.txt, the formatting of the add_executable and add_test sections is inconsistent. It's recommended to align the commands for better readability.
2. In action_node.hpp, the TurningDirection enum class should not have a base type of bool. It should be changed to something like enum class TurningDirection { CLOCKWISE, COUNTER_CLOCKWISE };.
3. In action_node.hpp, the getRelativeAngle method could be improved by using std::hypot function to calculate the distance and angle in a more concise way.
4. In action_node.hpp, consider adding more descriptive comments for the getTurningWaypointPoseOfObject method to explain the purpose of each parameter and the logic behind the calculations.
5. In go_around_object.hpp, the update_position_timer_ member variable is declared but not used. It should be removed if not needed.
6. In go_around_object.hpp, the providedPorts method should be updated to include the new input ports added to the action node.
7. In go_around_object.cpp, the updateWaypointAngleDeg method could be simplified by breaking down the logic into smaller functions for better readability.
8. In test_go_around_object.cpp, the test case could be enhanced by adding more specific assertions to validate the behavior of the GoAroundObject action.
9. In the newly added files like go_around_object.xml and go_around_object.yaml, consider adding more detailed comments to explain the purpose and structure of the files.

Modified Code:

enum class TurningDirection { CLOCKWISE, COUNTER_CLOCKWISE };

double getRelativeAngle(
    const double x1, const double y1, const double x2, const double y2,
    const double minimum_delta = 0.1) const
{
    const double delta_x = x2 - x1;
    const double delta_y = y2 - y1;
    const double relative_angle = std::atan2(delta_y, delta_x);
    return relative_angle;
}

enum class TurningDirection { CLOCKWISE, COUNTER_CLOCKWISE };

// Updated method with more descriptive comments
std::optional<geometry_msgs::msg::Pose> getTurningWaypointPoseOfObject(
    const robotx_behavior_msgs::msg::TaskObject & obj, const double distance = 5.5,
    const TurningDirection turning_direction = TurningDirection::COUNTER_CLOCKWISE,
    const double waypoint_angle_deg = 90.0) const
{
    // Method logic with comments explaining each step
}

static BT::PortsList providedPorts()
{
    return appendPorts(
        ActionROS2Node::providedPorts(),
        {BT::InputPort<std::string>("object_type"), BT::InputPort<std::string>("turning_direction"),
         BT::InputPort<double>("orbit_angle"), BT::InputPort<double>("distance_from_object_to_goal")});
}

void updateWaypointAngleDeg(const BehaviorState BehaviorState)
{
    // Simplified logic with comments
}

TEST(TestSuite, testCase1)
{
    // Test case with specific assertions
}

Note:

• Make sure to update the other parts of the code based on the above suggestions for consistency and clarity.