Skip to content

Commit

Permalink
ci(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Jun 23, 2022
1 parent 4b6d926 commit 398dc28
Show file tree
Hide file tree
Showing 15 changed files with 260 additions and 355 deletions.
26 changes: 11 additions & 15 deletions planning/recordreplay_planner/design/recordreplay_planner-design.md
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
RecordReplay planner {#recordreplay-planner}
====================
# RecordReplay planner {#recordreplay-planner}

# Purpose / Use cases

Expand All @@ -10,7 +9,6 @@ driving a trajectory, then reset vehicle position, then start replay and see wha
The trajectories can be a loop , which can be determined naively by checking the distance between the first and last
point of the trajectory, but the user has the ultimate control over toggling this behavior.


# Design

Recording will add states at the end of an internal list of states.
Expand All @@ -27,10 +25,10 @@ There is no interpolation between points along the trajectory, and localization
The list of recorded states is simply iterated over.

The stopping concept only works if one can assume that the downstream controller and the vehicle are able
to track the desired velocity going to zero in a single trajectory step.
to track the desired velocity going to zero in a single trajectory step.

Making sure this assumption is satisfied by construction would involve creating a dynamically feasible
velocity profile for stopping - this has not been done yet.
velocity profile for stopping - this has not been done yet.

It is the user's responsibility to check if the trajectory is suitable for looping, and call `set_loop` accordingly
to toggle the looping behavior with regard to the current trajectory that the controller is replaying. `is_loop`,
Expand All @@ -47,30 +45,28 @@ the path file that the user obtained while recording.

Inputs:

* `VehicleKinematicState.msg` is the state that gets recorded
* `BoundingBoxArray.msg` is a list of bounding boxes of obstacles
- `VehicleKinematicState.msg` is the state that gets recorded
- `BoundingBoxArray.msg` is a list of bounding boxes of obstacles

Outputs:

* `Trajectory.msg` is the trajectory that gets published

- `Trajectory.msg` is the trajectory that gets published

## Complexity

Recording is `O(1)` in time and `O(n)` in space, replay is `O(n)` in both time and space, where `n` is the
number of recorded states. Collision checking currently happens on every replay even if obstacles do not
change, and has a complexity that is linear in the number of obstacles but proportional to the product of
change, and has a complexity that is linear in the number of obstacles but proportional to the product of
the number of halfplanes in the ego vehicle and a single obstacle.

# Security considerations
# Security considerations

TBD by a security specialist.

# Future extensions / Unimplemented parts

* Trajectory buffer clearing
* Proper `tf` support
* rosbag2 support for recording and later replaying
- Trajectory buffer clearing
- Proper `tf` support
- rosbag2 support for recording and later replaying

# Related issues

Original file line number Diff line number Diff line change
Expand Up @@ -15,14 +15,15 @@
#ifndef RECORDREPLAY_PLANNER__RECORDREPLAY_PLANNER_HPP_
#define RECORDREPLAY_PLANNER__RECORDREPLAY_PLANNER_HPP_

#include <common/types.hpp>
#include <recordreplay_planner/visibility_control.hpp>
#include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>

#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <common/types.hpp>
#include <autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp>

#include <deque>
#include <string>
#include <map>
#include <string>

using autoware::common::types::bool8_t;
using autoware::common::types::float64_t;
Expand All @@ -36,12 +37,7 @@ namespace recordreplay_planner
using State = autoware_auto_vehicle_msgs::msg::VehicleKinematicState;
using autoware_auto_planning_msgs::msg::Trajectory;

enum class RecordReplayState
{
IDLE,
RECORDING,
REPLAYING
}; // enum class RecordReplayState
enum class RecordReplayState { IDLE, RECORDING, REPLAYING }; // enum class RecordReplayState

/// \brief A class for recording trajectories and replaying them as plans
class RECORDREPLAY_PLANNER_PUBLIC RecordReplayPlanner
Expand Down Expand Up @@ -95,9 +91,10 @@ class RECORDREPLAY_PLANNER_PUBLIC RecordReplayPlanner
/**
* \brief Judges whether current_state has reached the last point in record buffer
* \param current_state current state of the vehicle
* \param distance_thresh threshold of euclidean distance between the current state the and the last point in meters
* \param angle_thresh threshold of difference in the headings of the current_state and the last point in radians
* \return true if both distance and angle conditions are satisfied
* \param distance_thresh threshold of euclidean distance between the current state the and the
* last point in meters \param angle_thresh threshold of difference in the headings of the
* current_state and the last point in radians \return true if both distance and angle conditions
* are satisfied
*/
bool8_t reached_goal(
const State & current_state, const float64_t & distance_thresh,
Expand All @@ -108,14 +105,9 @@ class RECORDREPLAY_PLANNER_PUBLIC RecordReplayPlanner
* \param distance_thresh threshold of euclidean state between the first and last points in meters
* \return true if the distance threshold is met
*/
bool8_t is_loop(
const float64_t & distance_thresh
) const;

auto get_record_buffer()
{
return m_record_buffer;
}
bool8_t is_loop(const float64_t & distance_thresh) const;

auto get_record_buffer() { return m_record_buffer; }

private:
// Obtain a trajectory from the internally-stored recording buffer
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,21 +16,21 @@
#define RECORDREPLAY_PLANNER__VISIBILITY_CONTROL_HPP_

#if defined(__WIN32)
#if defined(RECORDREPLAY_PLANNER_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_EXPORTS)
#define RECORDREPLAY_PLANNER_PUBLIC __declspec(dllexport)
#define RECORDREPLAY_PLANNER_LOCAL
#else // defined(RECORDREPLAY_PLANNER_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_EXPORTS)
#define RECORDREPLAY_PLANNER_PUBLIC __declspec(dllimport)
#define RECORDREPLAY_PLANNER_LOCAL
#endif // defined(RECORDREPLAY_PLANNER_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_EXPORTS)
#if defined(RECORDREPLAY_PLANNER_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_EXPORTS)
#define RECORDREPLAY_PLANNER_PUBLIC __declspec(dllexport)
#define RECORDREPLAY_PLANNER_LOCAL
#else // defined(RECORDREPLAY_PLANNER_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_EXPORTS)
#define RECORDREPLAY_PLANNER_PUBLIC __declspec(dllimport)
#define RECORDREPLAY_PLANNER_LOCAL
#endif // defined(RECORDREPLAY_PLANNER_BUILDING_DLL) || defined(RECORDREPLAY_PLANNER_EXPORTS)
#elif defined(__linux__)
#define RECORDREPLAY_PLANNER_PUBLIC __attribute__((visibility("default")))
#define RECORDREPLAY_PLANNER_LOCAL __attribute__((visibility("hidden")))
#define RECORDREPLAY_PLANNER_PUBLIC __attribute__((visibility("default")))
#define RECORDREPLAY_PLANNER_LOCAL __attribute__((visibility("hidden")))
#elif defined(__APPLE__)
#define RECORDREPLAY_PLANNER_PUBLIC __attribute__((visibility("default")))
#define RECORDREPLAY_PLANNER_LOCAL __attribute__((visibility("hidden")))
#define RECORDREPLAY_PLANNER_PUBLIC __attribute__((visibility("default")))
#define RECORDREPLAY_PLANNER_LOCAL __attribute__((visibility("hidden")))
#else // defined(_LINUX)
#error "Unsupported Build Configuration"
#error "Unsupported Build Configuration"
#endif // defined(_WINDOWS)

#endif // RECORDREPLAY_PLANNER__VISIBILITY_CONTROL_HPP_
8 changes: 4 additions & 4 deletions planning/recordreplay_planner/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,7 @@
<package format="2">
<name>recordreplay_planner</name>
<version>1.0.0</version>
<description>
RecordReplay trajectory planner
</description>
<description>RecordReplay trajectory planner</description>
<maintainer email="opensource@embotech.com">Embotech AG</maintainer>
<license>Apache License 2.0</license>

Expand All @@ -23,5 +21,7 @@
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

<export><build_type>ament_cmake</build_type></export>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
Loading

0 comments on commit 398dc28

Please sign in to comment.