Skip to content

Commit

Permalink
feat: change function names
Browse files Browse the repository at this point in the history
Signed-off-by: Berkay Karaman <[email protected]>
  • Loading branch information
brkay54 authored and xmfcx committed Mar 15, 2024
1 parent b955d65 commit 80436e4
Show file tree
Hide file tree
Showing 8 changed files with 184 additions and 212 deletions.
44 changes: 16 additions & 28 deletions tools/reaction_analyzer/include/reaction_analyzer_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -159,45 +159,33 @@ class ReactionAnalyzerNode : public rclcpp::Node
size_t test_iteration_count_{0};

// Functions
void initAnalyzerVariables();

void initPointcloud();

void initPredictedObjects();

void initEgoForTest(
void init_analyzer_variables();
void init_pointcloud();
void init_predicted_objects();
void init_ego(
const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr,
const RouteState::ConstSharedPtr & route_state_ptr,
const OperationModeState::ConstSharedPtr & operation_mode_ptr,
const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr,
const Odometry::ConstSharedPtr & odometry_ptr);

void callOperationModeServiceWithoutResponse();

void spawnObstacle(const geometry_msgs::msg::Point & ego_pose);

void calculateResults(
void call_operation_mode_service_without_response();
void spawn_obstacle(const geometry_msgs::msg::Point & ego_pose);
void calculate_results(
const std::unordered_map<std::string, subscriber::BufferVariant> & message_buffers,
const rclcpp::Time & spawn_cmd_time);

void onTimer();

void dummyPerceptionPublisher();

void on_timer();
void dummy_perception_publisher();
void reset();

void writeResultsToFile();
void write_results();

// Callbacks
void vehiclePoseCallback(Odometry::ConstSharedPtr msg_ptr);

void initializationStateCallback(LocalizationInitializationState::ConstSharedPtr msg_ptr);

void routeStateCallback(RouteState::ConstSharedPtr msg_ptr);

void operationModeCallback(OperationModeState::ConstSharedPtr msg_ptr);
void on_vehicle_pose(Odometry::ConstSharedPtr msg_ptr);
void on_localization_initialization_state(
LocalizationInitializationState::ConstSharedPtr msg_ptr);
void on_route_state(RouteState::ConstSharedPtr msg_ptr);
void on_operation_mode_state(OperationModeState::ConstSharedPtr msg_ptr);
void on_ground_truth_pose(PoseStamped::ConstSharedPtr msg_ptr);

void groundTruthPoseCallback(PoseStamped::ConstSharedPtr msg_ptr);
// Timer
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::TimerBase::SharedPtr dummy_perception_timer_;
Expand Down
58 changes: 20 additions & 38 deletions tools/reaction_analyzer/include/subscriber.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -188,59 +188,41 @@ class SubscriberBase
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

// Functions
void initReactionChainsAndParams();

bool initSubscribers();

bool searchPointcloudNearEntity(const pcl::PointCloud<pcl::PointXYZ> & pcl_pointcloud);

bool searchPredictedObjectsNearEntity(const PredictedObjects & predicted_objects);

bool searchDetectedObjectsNearEntity(const DetectedObjects & detected_objects);

bool searchTrackedObjectsNearEntity(const TrackedObjects & tracked_objects);

void setControlCommandToBuffer(
void init_reaction_chains_and_params();
bool init_subscribers();
bool search_pointcloud_near_entity(const pcl::PointCloud<pcl::PointXYZ> & pcl_pointcloud);
bool search_predicted_objects_near_entity(const PredictedObjects & predicted_objects);
bool search_detected_objects_near_entity(const DetectedObjects & detected_objects);
bool search_tracked_objects_near_entity(const TrackedObjects & tracked_objects);
void set_control_command_to_buffer(
std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd);

std::optional<size_t> findFirstBrakeIdx(const std::vector<AckermannControlCommand> & cmd_array);
std::optional<size_t> find_first_brake_idx(
const std::vector<AckermannControlCommand> & cmd_array);

// Callbacks for modules are subscribed
void controlCommandOutputCallback(
void on_control_command(
const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr);

void trajectoryOutputCallback(
const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr);

void trajectoryOutputCallback(
void on_trajectory(const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr);
void on_trajectory(
const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr,
const PublishedTime::ConstSharedPtr & published_time_ptr);

void pointcloud2OutputCallback(
const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr);

void pointcloud2OutputCallback(
void on_pointcloud(const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr);
void on_pointcloud(
const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr,
const PublishedTime::ConstSharedPtr & published_time_ptr);

void predictedObjectsOutputCallback(
void on_predicted_objects(
const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr);

void predictedObjectsOutputCallback(
void on_predicted_objects(
const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr,
const PublishedTime::ConstSharedPtr & published_time_ptr);

void detectedObjectsOutputCallback(
void on_detected_objects(
const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr);

void detectedObjectsOutputCallback(
void on_detected_objects(
const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr,
const PublishedTime::ConstSharedPtr & published_time_ptr);

void trackedObjectsOutputCallback(
void on_tracked_objects(
const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr);

void trackedObjectsOutputCallback(
void on_tracked_objects(
const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr,
const PublishedTime::ConstSharedPtr & published_time_ptr);
};
Expand Down
26 changes: 13 additions & 13 deletions tools/reaction_analyzer/include/topic_publisher.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ struct PublisherVarAccessor
};

template <typename MessageType>
void publishWithCurrentTime(
void publish_with_current_time(
const PublisherVariables<MessageType> & publisherVar, const rclcpp::Time & current_time,
const bool is_object_spawned) const
{
Expand All @@ -150,25 +150,25 @@ struct PublisherVarAccessor
}

template <typename T>
void setPeriod(T & publisherVar, double newPeriod)
void set_period(T & publisherVar, double newPeriod)
{
publisherVar.period_ns = newPeriod;
}

template <typename T>
double getPeriod(const T & publisherVar) const
double get_period(const T & publisherVar) const
{
return publisherVar.period_ns;
}

template <typename T>
std::shared_ptr<void> getEmptyAreaMessage(const T & publisherVar) const
std::shared_ptr<void> get_empty_area_message(const T & publisherVar) const
{
return std::static_pointer_cast<void>(publisherVar.empty_area_message);
}

template <typename T>
std::shared_ptr<void> getObjectSpawnedMessage(const T & publisherVar) const
std::shared_ptr<void> get_object_spawned_message(const T & publisherVar) const
{
return std::static_pointer_cast<void>(publisherVar.object_spawned_message);
}
Expand Down Expand Up @@ -213,20 +213,20 @@ class TopicPublisher
TopicPublisherParams topic_publisher_params_;

// Functions
void setMessageToVariableMap(
void set_message_to_variable_map(
const PublisherMessageType & message_type, const std::string & topic_name,
rosbag2_storage::SerializedBagMessage & bag_message, const bool is_empty_area_message);
void setPeriodToVariableMap(
void set_period_to_variable_map(
const std::unordered_map<std::string, std::vector<rclcpp::Time>> & time_map);
bool setPublishersAndTimersToVariableMap();
bool checkPublishersInitializedCorrectly();
void initRosbagPublishers();
void pointcloudMessagesSyncPublisher(const PointcloudPublisherType type);
void genericMessagePublisher(const std::string & topic_name);
void pointcloudMessagesAsyncPublisher(
bool set_publishers_and_timers_to_variable_map();
bool check_publishers_initialized_correctly();
void init_rosbag_publishers();
void pointcloud_messages_sync_publisher(const PointcloudPublisherType type);
void pointcloud_messages_async_publisher(
const std::pair<
std::shared_ptr<PublisherVariables<PointCloud2>>,
std::shared_ptr<PublisherVariables<PointCloud2>>> & lidar_output_pair_);
void generic_message_publisher(const std::string & topic_name);

// Variables
PointcloudPublisherType pointcloud_publisher_type_;
Expand Down
7 changes: 4 additions & 3 deletions tools/reaction_analyzer/include/utils.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint;
* @return A vector of tuples. Each tuple contains a string (the test name), a vector of doubles
* (the test results), and a double (the median value).
*/
std::vector<std::tuple<std::string, std::vector<double>, double>> sortResultsByMedian(
std::vector<std::tuple<std::string, std::vector<double>, double>> sort_results_by_median(
const std::unordered_map<std::string, std::vector<double>> test_results);

/**
Expand All @@ -47,7 +47,7 @@ std::vector<std::tuple<std::string, std::vector<double>, double>> sortResultsByM
* @param node A pointer to the node for which the subscription options are being created.
* @return The created SubscriptionOptions.
*/
rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node);
rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node);

/**
* @brief Splits a string by a given delimiter.
Expand All @@ -72,7 +72,8 @@ std::vector<std::string> split(const std::string & str, char delimiter);
* @return The index of the point in the trajectory that is at least the specified distance away
* from the current point.
*/
size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance);
size_t get_index_after_distance(
const Trajectory & traj, const size_t curr_id, const double distance);
} // namespace reaction_analyzer

#endif // UTILS_HPP_
Loading

0 comments on commit 80436e4

Please sign in to comment.