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

[pull] main from autowarefoundation:main #765

Merged
merged 1 commit into from
Jan 19, 2025
Merged

[pull] main from autowarefoundation:main #765

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
18 changes: 16 additions & 2 deletions ...utoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,13 +18,16 @@
#include <autoware_planning_test_manager/autoware_planning_test_manager.hpp>
#include <autoware_test_utils/autoware_test_utils.hpp>

#include <tier4_planning_msgs/msg/expand_stop_range.hpp>

#include <gtest/gtest.h>

#include <memory>
#include <vector>

using autoware::motion_planning::ObstacleStopPlannerNode;
using autoware::planning_test_manager::PlanningInterfaceTestManager;
using tier4_planning_msgs::msg::ExpandStopRange;

std::shared_ptr<PlanningInterfaceTestManager> generateTestManager()
{
Expand Down Expand Up @@ -66,8 +69,17 @@ void publishMandatoryTopics(
test_manager->publishPointCloud(test_target_node, "obstacle_stop_planner/input/pointcloud");
test_manager->publishAcceleration(test_target_node, "obstacle_stop_planner/input/acceleration");
test_manager->publishPredictedObjects(test_target_node, "obstacle_stop_planner/input/objects");
test_manager->publishExpandStopRange(
test_target_node, "obstacle_stop_planner/input/expand_stop_range");
}

void publishExpandStopRange(
std::shared_ptr<PlanningInterfaceTestManager> test_manager,
std::shared_ptr<ObstacleStopPlannerNode> test_target_node)
{
auto test_node = test_manager->getTestNode();
const auto expand_stop_range = test_manager->getTestNode()->create_publisher<ExpandStopRange>(
"obstacle_stop_planner/input/expand_stop_range", 1);
expand_stop_range->publish(ExpandStopRange{});
autoware::test_utils::spinSomeNodes(test_node, test_target_node, 3);
}

TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
Expand All @@ -78,6 +90,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory)
auto test_target_node = generateNode();

publishMandatoryTopics(test_manager, test_target_node);
publishExpandStopRange(test_manager, test_target_node);

// test for normal trajectory
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node));
Expand All @@ -97,6 +110,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)
auto test_target_node = generateNode();

publishMandatoryTopics(test_manager, test_target_node);
publishExpandStopRange(test_manager, test_target_node);

// test for normal trajectory
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node));
Expand Down
13 changes: 1 addition & 12 deletions ...ng_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,14 +52,10 @@
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <std_msgs/msg/bool.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
#include <tier4_api_msgs/msg/crosswalk_status.hpp>
#include <tier4_api_msgs/msg/intersection_status.hpp>
#include <tier4_planning_msgs/msg/expand_stop_range.hpp>
#include <tier4_planning_msgs/msg/lateral_offset.hpp>
#include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
#include <tier4_planning_msgs/msg/scenario.hpp>
#include <tier4_planning_msgs/msg/velocity_limit.hpp>
#include <tier4_v2x_msgs/msg/virtual_traffic_light_state_array.hpp>

#include <gtest/gtest.h>
#include <tf2_ros/buffer.h>
Expand Down Expand Up @@ -87,14 +83,10 @@ using nav_msgs::msg::OccupancyGrid;
using nav_msgs::msg::Odometry;
using sensor_msgs::msg::PointCloud2;
using tf2_msgs::msg::TFMessage;
using tier4_api_msgs::msg::CrosswalkStatus;
using tier4_api_msgs::msg::IntersectionStatus;
using tier4_planning_msgs::msg::ExpandStopRange;
using tier4_planning_msgs::msg::LateralOffset;
using tier4_planning_msgs::msg::PathWithLaneId;
using tier4_planning_msgs::msg::Scenario;
using tier4_planning_msgs::msg::VelocityLimit;
using tier4_v2x_msgs::msg::VirtualTrafficLightStateArray;

enum class ModuleName {
UNKNOWN = 0,
Expand All @@ -117,7 +109,6 @@ class PlanningInterfaceTestManager
void publishPointCloud(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishAcceleration(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishPredictedObjects(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishExpandStopRange(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishOccupancyGrid(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishCostMap(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishMap(rclcpp::Node::SharedPtr target_node, std::string topic_name);
Expand All @@ -131,7 +122,6 @@ class PlanningInterfaceTestManager
void publishLateralOffset(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishOperationModeState(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishTrafficSignals(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishVirtualTrafficLightState(rclcpp::Node::SharedPtr target_node, std::string topic_name);

void setTrajectoryInputTopicName(std::string topic_name);
void setParkingTrajectoryInputTopicName(std::string topic_name);
Expand Down Expand Up @@ -178,6 +168,7 @@ class PlanningInterfaceTestManager
void testOffTrackFromTrajectory(rclcpp::Node::SharedPtr target_node);

int getReceivedTopicNum();
rclcpp::Node::SharedPtr getTestNode() const;

private:
// Publisher (necessary for node running)
Expand All @@ -187,7 +178,6 @@ class PlanningInterfaceTestManager
rclcpp::Publisher<PointCloud2>::SharedPtr point_cloud_pub_;
rclcpp::Publisher<AccelWithCovarianceStamped>::SharedPtr acceleration_pub_;
rclcpp::Publisher<PredictedObjects>::SharedPtr predicted_objects_pub_;
rclcpp::Publisher<ExpandStopRange>::SharedPtr expand_stop_range_pub_;
rclcpp::Publisher<OccupancyGrid>::SharedPtr occupancy_grid_pub_;
rclcpp::Publisher<OccupancyGrid>::SharedPtr cost_map_pub_;
rclcpp::Publisher<LaneletMapBin>::SharedPtr map_pub_;
Expand All @@ -202,7 +192,6 @@ class PlanningInterfaceTestManager
rclcpp::Publisher<LateralOffset>::SharedPtr lateral_offset_pub_;
rclcpp::Publisher<OperationModeState>::SharedPtr operation_mode_state_pub_;
rclcpp::Publisher<TrafficLightGroupArray>::SharedPtr traffic_signals_pub_;
rclcpp::Publisher<VirtualTrafficLightStateArray>::SharedPtr virtual_traffic_light_states_pub_;

// Subscriber
rclcpp::Subscription<Trajectory>::SharedPtr traj_sub_;
Expand Down
1 change: 0 additions & 1 deletion planning/autoware_planning_test_manager/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,6 @@
<depend>rclcpp</depend>
<depend>tf2_msgs</depend>
<depend>tf2_ros</depend>
<depend>tier4_api_msgs</depend>
<depend>tier4_planning_msgs</depend>
<depend>tier4_v2x_msgs</depend>
<depend>unique_identifier_msgs</depend>
Expand Down
20 changes: 5 additions & 15 deletions planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,13 +71,6 @@ void PlanningInterfaceTestManager::publishPredictedObjects(
test_node_, target_node, topic_name, predicted_objects_pub_, PredictedObjects{});
}

void PlanningInterfaceTestManager::publishExpandStopRange(
rclcpp::Node::SharedPtr target_node, std::string topic_name)
{
autoware::test_utils::publishToTargetNode(
test_node_, target_node, topic_name, expand_stop_range_pub_, ExpandStopRange{});
}

void PlanningInterfaceTestManager::publishOccupancyGrid(
rclcpp::Node::SharedPtr target_node, std::string topic_name)
{
Expand Down Expand Up @@ -181,14 +174,6 @@ void PlanningInterfaceTestManager::publishTrafficSignals(
test_node_, target_node, topic_name, traffic_signals_pub_, TrafficLightGroupArray{});
}

void PlanningInterfaceTestManager::publishVirtualTrafficLightState(
rclcpp::Node::SharedPtr target_node, std::string topic_name)
{
autoware::test_utils::publishToTargetNode(
test_node_, target_node, topic_name, virtual_traffic_light_states_pub_,
VirtualTrafficLightStateArray{});
}

void PlanningInterfaceTestManager::publishInitialPoseTF(
rclcpp::Node::SharedPtr target_node, std::string topic_name)
{
Expand Down Expand Up @@ -471,4 +456,9 @@ int PlanningInterfaceTestManager::getReceivedTopicNum()
return count_;
}

rclcpp::Node::SharedPtr PlanningInterfaceTestManager::getTestNode() const
{
return test_node_;
}

} // namespace autoware::planning_test_manager
23 changes: 19 additions & 4 deletions ...nner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,9 @@
// limitations under the License.

#include <autoware/behavior_velocity_planner/test_utils.hpp>
#include <autoware_test_utils/autoware_test_utils.hpp>

#include <tier4_v2x_msgs/msg/virtual_traffic_light_state_array.hpp>

#include <gtest/gtest.h>

Expand All @@ -23,6 +26,19 @@

namespace autoware::behavior_velocity_planner
{
using tier4_v2x_msgs::msg::VirtualTrafficLightStateArray;

void publishVirtualTrafficLightState(
std::shared_ptr<PlanningInterfaceTestManager> test_manager, rclcpp::Node::SharedPtr target_node)
{
auto test_node = test_manager->getTestNode();
const auto pub_virtual_traffic_light =
test_manager->getTestNode()->create_publisher<VirtualTrafficLightStateArray>(
"behavior_velocity_planner_node/input/virtual_traffic_light_states", 1);
pub_virtual_traffic_light->publish(VirtualTrafficLightStateArray{});
autoware::test_utils::spinSomeNodes(test_node, target_node, 3);
}

TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID)
{
rclcpp::init(0, nullptr);
Expand All @@ -35,8 +51,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID)
auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec);

autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node);
test_manager->publishVirtualTrafficLightState(
test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states");
publishVirtualTrafficLightState(test_manager, test_target_node);

// test with nominal path_with_lane_id
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));
Expand All @@ -57,9 +72,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose)

auto test_manager = autoware::behavior_velocity_planner::generateTestManager();
auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec);

autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node);
test_manager->publishVirtualTrafficLightState(
test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states");
publishVirtualTrafficLightState(test_manager, test_target_node);

// test for normal trajectory
ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node));
Expand Down