Skip to content

Commit

Permalink
abstract message-specific functions
Browse files Browse the repository at this point in the history 
Signed-off-by: mitukou1109 <[email protected]>
  • Loading branch information
@mitukou1109
mitukou1109 committed Jan 9, 2025
1 parent 7f4ed3d commit 56da84a
Show file tree
Hide file tree
Showing 2 changed files with 95 additions and 597 deletions.
240 changes: 42 additions & 198 deletions ...ng_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,37 +30,9 @@
<< std::endl; \
}

#include <autoware/component_interface_specs/planning.hpp>
#include <autoware/component_interface_utils/rclcpp.hpp>
#include <autoware_test_utils/autoware_test_utils.hpp>
#include <rclcpp/rclcpp.hpp>

#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
#include <autoware_control_msgs/msg/control.hpp>
#include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
#include <autoware_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_perception_msgs/msg/traffic_light_group_array.hpp>
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
#include <autoware_planning_msgs/msg/path.hpp>
#include <autoware_planning_msgs/msg/trajectory.hpp>
#include <autoware_vehicle_msgs/msg/steering_report.hpp>
#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/msg/odometry.hpp>
#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>
#include <tf2_ros/transform_listener.h>
Expand All @@ -71,197 +43,69 @@

namespace autoware::planning_test_manager
{
using autoware_adapi_v1_msgs::msg::OperationModeState;
using autoware_map_msgs::msg::LaneletMapBin;
using autoware_perception_msgs::msg::PredictedObjects;
using autoware_perception_msgs::msg::TrafficLightGroupArray;
using autoware_planning_msgs::msg::LaneletRoute;
using autoware_planning_msgs::msg::Path;
using autoware_planning_msgs::msg::Trajectory;
using geometry_msgs::msg::AccelWithCovarianceStamped;
using geometry_msgs::msg::Point;
using geometry_msgs::msg::PoseStamped;
using geometry_msgs::msg::Quaternion;
using geometry_msgs::msg::TransformStamped;
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,
START_PLANNER,
};

class PlanningInterfaceTestManager
{
public:
PlanningInterfaceTestManager();

void publishOdometry(
rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift = 0.0);

void publishInitialPose(
rclcpp::Node::SharedPtr target_node, std::string topic_name, const double shift = 0.0,
ModuleName module_name = ModuleName::UNKNOWN);

void publishMaxVelocity(rclcpp::Node::SharedPtr target_node, std::string topic_name);
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);
void publishLaneDrivingScenario(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishParkingScenario(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishParkingState(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishTrajectory(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishRoute(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishTF(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishInitialPoseTF(rclcpp::Node::SharedPtr target_node, std::string topic_name);
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);
void setLaneDrivingTrajectoryInputTopicName(std::string topic_name);
void setRouteInputTopicName(std::string topic_name);
void setPathInputTopicName(std::string topic_name);
void setPathWithLaneIdTopicName(std::string topic_name);
void setPathTopicName(std::string topic_name);
template <typename InputT>
void publishInput(
const rclcpp::Node::SharedPtr target_node, const std::string & topic_name, const InputT & input,
const int repeat_count = 3) const
{
autoware::test_utils::publishToTargetNode(
test_node_, target_node, topic_name, {}, input, repeat_count);
}

void setTrajectorySubscriber(std::string topic_name);
void setScenarioSubscriber(std::string topic_name);
void setPathWithLaneIdSubscriber(std::string topic_name);
void setRouteSubscriber(std::string topic_name);
void setPathSubscriber(std::string topic_name);
template <typename OutputT>
void subscribeOutput(const std::string & topic_name, typename OutputT::ConstSharedPtr buffer = {})
{
test_output_subs_.push_back(test_node_->create_subscription<OutputT>(
topic_name, 1, [&](const typename OutputT::ConstSharedPtr msg) {
if (buffer) {
buffer = msg;
}
received_topic_num_++;
}));
}

void setInitialPoseTopicName(std::string topic_name);
void setOdometryTopicName(std::string topic_name);
void testWithNormalTrajectory(
rclcpp::Node::SharedPtr target_node, const std::string & topic_name);
void testWithAbnormalTrajectory(
rclcpp::Node::SharedPtr target_node, const std::string & topic_name);

void testWithNominalTrajectory(rclcpp::Node::SharedPtr target_node);
void testWithAbnormalTrajectory(rclcpp::Node::SharedPtr target_node);
void testWithNormalRoute(rclcpp::Node::SharedPtr target_node, const std::string & topic_name);
void testWithAbnormalRoute(rclcpp::Node::SharedPtr target_node, const std::string & topic_name);

void testWithNominalRoute(rclcpp::Node::SharedPtr target_node);
void testWithAbnormalRoute(rclcpp::Node::SharedPtr target_node);
void testWithBehaviorNormalRoute(
rclcpp::Node::SharedPtr target_node, const std::string & topic_name);

void testWithBehaviorNominalRoute(
rclcpp::Node::SharedPtr target_node, ModuleName module_name = ModuleName::UNKNOWN);
void testWithNormalPathWithLaneId(
rclcpp::Node::SharedPtr target_node, const std::string & topic_name);
void testWithAbnormalPathWithLaneId(
rclcpp::Node::SharedPtr target_node, const std::string & topic_name);

void testWithNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node);
void testWithAbnormalPathWithLaneId(rclcpp::Node::SharedPtr target_node);
void testWithNormalPath(rclcpp::Node::SharedPtr target_node, const std::string & topic_name);
void testWithAbnormalPath(rclcpp::Node::SharedPtr target_node, const std::string & topic_name);

void testWithNominalPath(rclcpp::Node::SharedPtr target_node);
void testWithAbnormalPath(rclcpp::Node::SharedPtr target_node);
void testWithOffTrackInitialPoses(
rclcpp::Node::SharedPtr target_node, const std::string & topic_name);

// for invalid ego poses, contains some tests inside.
void testRouteWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node);
void testPathWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node);
void testPathWithLaneIdWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node);
void testTrajectoryWithInvalidEgoPose(rclcpp::Node::SharedPtr target_node);
void testWithOffTrackOdometries(
rclcpp::Node::SharedPtr target_node, const std::string & topic_name);

// ego vehicle is located far from trajectory
void testOffTrackFromRoute(rclcpp::Node::SharedPtr target_node);
void testOffTrackFromPath(rclcpp::Node::SharedPtr target_node);
void testOffTrackFromPathWithLaneId(rclcpp::Node::SharedPtr target_node);
void testOffTrackFromTrajectory(rclcpp::Node::SharedPtr target_node);
void resetReceivedTopicNum() { received_topic_num_ = 0; }

int getReceivedTopicNum();
size_t getReceivedTopicNum() const { return received_topic_num_; }

private:
// Publisher (necessary for node running)
rclcpp::Publisher<Odometry>::SharedPtr odom_pub_;
rclcpp::Publisher<Odometry>::SharedPtr initial_pose_pub_;
rclcpp::Publisher<VelocityLimit>::SharedPtr max_velocity_pub_;
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_;
rclcpp::Publisher<Scenario>::SharedPtr scenario_pub_;
rclcpp::Publisher<Scenario>::SharedPtr parking_scenario_pub_;
rclcpp::Publisher<Scenario>::SharedPtr lane_driving_scenario_pub_;
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr parking_state_pub_;
rclcpp::Publisher<Trajectory>::SharedPtr trajectory_pub_;
rclcpp::Publisher<LaneletRoute>::SharedPtr route_pub_;
rclcpp::Publisher<TFMessage>::SharedPtr TF_pub_;
rclcpp::Publisher<TFMessage>::SharedPtr initial_pose_tf_pub_;
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_;
rclcpp::Subscription<LaneletRoute>::SharedPtr route_sub_;
rclcpp::Subscription<Scenario>::SharedPtr scenario_sub_;
rclcpp::Subscription<PathWithLaneId>::SharedPtr path_with_lane_id_sub_;
rclcpp::Subscription<Path>::SharedPtr path_sub_;

// Publisher for testing(trajectory)
rclcpp::Publisher<Trajectory>::SharedPtr normal_trajectory_pub_;
rclcpp::Publisher<Trajectory>::SharedPtr abnormal_trajectory_pub_;

// Publisher for testing(route)
rclcpp::Publisher<LaneletRoute>::SharedPtr normal_route_pub_;
rclcpp::Publisher<LaneletRoute>::SharedPtr abnormal_route_pub_;

// Publisher for testing(route)
rclcpp::Publisher<LaneletRoute>::SharedPtr behavior_normal_route_pub_;

// Publisher for testing(PathWithLaneId)
rclcpp::Publisher<PathWithLaneId>::SharedPtr normal_path_with_lane_id_pub_;
rclcpp::Publisher<PathWithLaneId>::SharedPtr abnormal_path_with_lane_id_pub_;

// Publisher for testing(Path)
rclcpp::Publisher<Path>::SharedPtr normal_path_pub_;
rclcpp::Publisher<Path>::SharedPtr abnormal_path_pub_;

std::string input_trajectory_name_ = "";
std::string input_parking_trajectory_name_ = "";
std::string input_lane_driving_trajectory_name_ = "";
std::string input_route_name_ = "";
std::string input_path_name_ = "";
std::string input_path_with_lane_id_name_ = "";
std::string input_initial_pose_name_ = ""; // for the map based pose
std::string input_odometry_name_ = "";
std::vector<rclcpp::SubscriptionBase::SharedPtr> test_output_subs_;

// Node
rclcpp::Node::SharedPtr test_node_;

std::string map_frame_ = "map";
size_t count_{0};
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
void publishNominalTrajectory(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishAbnormalTrajectory(
rclcpp::Node::SharedPtr target_node, const Trajectory & abnormal_trajectory);

void publishNominalRoute(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishAbnormalRoute(
rclcpp::Node::SharedPtr target_node, const LaneletRoute & abnormal_route);

void publishBehaviorNominalRoute(
rclcpp::Node::SharedPtr target_node, std::string topic_name,
ModuleName module_name = ModuleName::UNKNOWN);
void publishNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishAbNominalPathWithLaneId(rclcpp::Node::SharedPtr target_node, std::string topic_name);

void publishNominalPath(rclcpp::Node::SharedPtr target_node, std::string topic_name);
void publishAbnormalPath(rclcpp::Node::SharedPtr target_node, std::string topic_name);
size_t received_topic_num_ = 0;
}; // class PlanningInterfaceTestManager

} // namespace autoware::planning_test_manager
Expand Down
Loading

0 comments on commit 56da84a

Please sign in to comment.