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

cmake is fixed for control_formance analysis. #53

Closed
wants to merge 2 commits into from
Closed

cmake is fixed for control_formance analysis. #53

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
033f752
cmake is fixed for control_formance analysis.
boyali May 31, 2022
9e83548
Removed various bug.
boyali May 31, 2022
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
64 changes: 32 additions & 32 deletions control/control_performance_analysis/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,47 +9,47 @@ find_package(Boost REQUIRED)
find_package(eigen3_cmake_module REQUIRED)
find_package(Eigen3 REQUIRED)

include_directories(
SYSTEM
${EIGEN3_INCLUDE_DIR}
)
find_package(builtin_interfaces REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)

rosidl_generate_interfaces(control_performance_analysis_msgs
msg/Error.msg
msg/ErrorStamped.msg
DEPENDENCIES
std_msgs
)
rosidl_generate_interfaces(control_performance_analysis
"msg/Error.msg"
"msg/ErrorStamped.msg"
DEPENDENCIES
builtin_interfaces std_msgs
)

ament_auto_add_library(control_performance_analysis_core SHARED
src/control_performance_analysis_utils.cpp
src/control_performance_analysis_core.cpp
)
src/control_performance_analysis_utils.cpp
src/control_performance_analysis_core.cpp
)

ament_auto_add_library(control_performance_analysis_node SHARED
src/control_performance_analysis_node.cpp
)

if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
rosidl_target_interfaces(control_performance_analysis_node
control_performance_analysis_msgs "rosidl_typesupport_cpp")
else()
rosidl_get_typesupport_target(
cpp_typesupport_target control_performance_analysis_msgs "rosidl_typesupport_cpp")
target_link_libraries(control_performance_analysis_node "${cpp_typesupport_target}")
endif()
src/control_performance_analysis_node.cpp
)

if (${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
rosidl_target_interfaces(control_performance_analysis_node
control_performance_analysis "rosidl_typesupport_cpp")
else ()
rosidl_get_typesupport_target(
cpp_typesupport_target control_performance_analysis "rosidl_typesupport_cpp")
target_link_libraries(control_performance_analysis_core "${cpp_typesupport_target}")
target_link_libraries(control_performance_analysis_node "${cpp_typesupport_target}")
endif ()

target_link_libraries(control_performance_analysis_node
control_performance_analysis_core
)
control_performance_analysis_core
)

rclcpp_components_register_node(control_performance_analysis_node
PLUGIN "control_performance_analysis::ControlPerformanceAnalysisNode"
EXECUTABLE control_performance_analysis
)
PLUGIN "control_performance_analysis::ControlPerformanceAnalysisNode"
EXECUTABLE control_performance_analysis_exe
)

ament_auto_package(
INSTALL_TO_SHARE
launch
config
INSTALL_TO_SHARE
launch
config
)
148 changes: 80 additions & 68 deletions ...mance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@

#include <eigen3/Eigen/Core>

#include <autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp>
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
Expand All @@ -31,74 +31,86 @@

namespace control_performance_analysis
{
using autoware_auto_control_msgs::msg::AckermannLateralCommand;
using autoware_auto_planning_msgs::msg::Trajectory;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::PoseArray;
using geometry_msgs::msg::Twist;
using autoware_auto_control_msgs::msg::AckermannControlCommand;
using autoware_auto_planning_msgs::msg::Trajectory;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::PoseArray;
using geometry_msgs::msg::Twist;

struct TargetPerformanceMsgVars
{
double lateral_error;
double heading_error;
double control_effort_energy;
double error_energy;
double value_approximation;
double curvature_estimate;
double curvature_estimate_pp;
double lateral_error_velocity;
double lateral_error_acceleration;
};

class ControlPerformanceAnalysisCore
{
public:
// See https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ControlPerformanceAnalysisCore();
ControlPerformanceAnalysisCore(double wheelbase, double curvature_interval_length);

// Setters
void setCurrentPose(const Pose & msg);
void setCurrentWaypoints(const Trajectory & trajectory);
void setCurrentVelocities(const Twist & twist_msg);
void setCurrentControlValue(const AckermannLateralCommand & msg);
void setInterpolatedPose(Pose & interpolated_pose);

void findCurveRefIdx();
std::pair<bool, int32_t> findClosestPrevWayPointIdx_path_direction();
double estimateCurvature();
double estimatePurePursuitCurvature();

// Getters
bool isDataReady() const;
std::pair<bool, TargetPerformanceMsgVars> getPerformanceVars();
Pose getPrevWPPose() const;
std::pair<bool, Pose> calculateClosestPose();

private:
double wheelbase_;
double curvature_interval_length_;

// Variables Received Outside
std::shared_ptr<PoseArray> current_waypoints_ptr_;
std::shared_ptr<Pose> current_vec_pose_ptr_;
std::shared_ptr<std::vector<double>> current_velocities_ptr_; // [Vx, Heading rate]
std::shared_ptr<AckermannLateralCommand> current_control_ptr_;

// Variables computed
std::unique_ptr<int32_t> idx_prev_wp_; // the waypoint index, vehicle
std::unique_ptr<int32_t> idx_curve_ref_wp_; // index of waypoint corresponds to front axle center
std::unique_ptr<int32_t> idx_next_wp_; // the next waypoint index, vehicle heading to
std::unique_ptr<TargetPerformanceMsgVars> prev_target_vars_{};
std::shared_ptr<Pose> interpolated_pose_ptr_;
// V = xPx' ; Value function from DARE Lyap matrix P
Eigen::Matrix2d const lyap_P_ = (Eigen::MatrixXd(2, 2) << 2.342, 8.60, 8.60, 64.29).finished();
double const contR{10.0}; // Control weight in LQR

rclcpp::Logger logger_{rclcpp::get_logger("control_performance_analysis")};
rclcpp::Clock clock_{RCL_ROS_TIME};
};
struct TargetPerformanceMsgVars
{
double lateral_error;
double heading_error;
double control_effort_energy;
double error_energy;
double value_approximation;
double curvature_estimate;
double curvature_estimate_pp;
double lateral_error_velocity;
double lateral_error_acceleration;
};

class ControlPerformanceAnalysisCore
{
public:
// See https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ControlPerformanceAnalysisCore();

ControlPerformanceAnalysisCore(double wheelbase, double curvature_interval_length);

// Setters
void setCurrentPose(const Pose& msg);

void setCurrentWaypoints(const Trajectory& trajectory);

void setCurrentVelocities(const Twist& twist_msg);

void setCurrentControlValue(const AckermannControlCommand& msg);

void setInterpolatedPose(Pose& interpolated_pose);

void findCurveRefIdx();

std::pair<bool, int32_t> findClosestPrevWayPointIdx_path_direction();

double estimateCurvature();

double estimatePurePursuitCurvature();

// Getters
bool isDataReady() const;

std::pair<bool, TargetPerformanceMsgVars> getPerformanceVars();

Pose getPrevWPPose() const;

std::pair<bool, Pose> calculateClosestPose();

private:
double wheelbase_;
double curvature_interval_length_;

// Variables Received Outside
std::shared_ptr <PoseArray> current_waypoints_ptr_;
std::shared_ptr <Pose> current_vec_pose_ptr_;
std::shared_ptr <std::vector<double>> current_velocities_ptr_; // [Vx, Heading rate]
std::shared_ptr <AckermannControlCommand> current_control_ptr_;

// Variables computed
std::unique_ptr <int32_t> idx_prev_wp_; // the waypoint index, vehicle
std::unique_ptr <int32_t> idx_curve_ref_wp_; // index of waypoint corresponds to front axle center
std::unique_ptr <int32_t> idx_next_wp_; // the next waypoint index, vehicle heading to
std::unique_ptr <TargetPerformanceMsgVars> prev_target_vars_{};
std::shared_ptr <Pose> interpolated_pose_ptr_;
// V = xPx' ; Value function from DARE Lyap matrix P
Eigen::Matrix2d const lyap_P_ = (Eigen::MatrixXd(2, 2)
<< 2.342, 8.60, 8.60, 64.29).finished();
double const contR{ 10.0 }; // Control weight in LQR

rclcpp::Logger logger_{ rclcpp::get_logger("control_performance_analysis") };
rclcpp::Clock clock_{ RCL_ROS_TIME };
};
} // namespace control_performance_analysis

#endif // CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_
132 changes: 69 additions & 63 deletions ...mance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
#include <rclcpp/rclcpp.hpp>
#include <tier4_autoware_utils/ros/self_pose_listener.hpp>

#include <autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp>
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
Expand All @@ -33,72 +33,78 @@

namespace control_performance_analysis
{
using autoware_auto_control_msgs::msg::AckermannLateralCommand;
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_vehicle_msgs::msg::SteeringReport;
using control_performance_analysis::msg::ErrorStamped;
using geometry_msgs::msg::PoseStamped;
using nav_msgs::msg::Odometry;
using autoware_auto_control_msgs::msg::AckermannControlCommand;
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_vehicle_msgs::msg::SteeringReport;
using control_performance_analysis::msg::ErrorStamped;
using geometry_msgs::msg::PoseStamped;
using nav_msgs::msg::Odometry;

// Parameters Struct
struct Param
{
// Global parameters
double wheel_base;
double curvature_interval_length;
struct Param
{
// Global parameters
double wheel_base;
double curvature_interval_length;

// Control Method Parameters
double control_period;
};
// Control Method Parameters
double control_period;
};

class ControlPerformanceAnalysisNode : public rclcpp::Node
{
public:
explicit ControlPerformanceAnalysisNode(const rclcpp::NodeOptions & node_options);

private:
// Subscribers and Local Variable Assignment
rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_; // subscribe to trajectory
rclcpp::Subscription<AckermannLateralCommand>::SharedPtr
sub_control_steering_; // subscribe to steering control value
rclcpp::Subscription<Odometry>::SharedPtr sub_velocity_; // subscribe to velocity
rclcpp::Subscription<SteeringReport>::SharedPtr sub_vehicle_steering_;

// Self Pose listener.
tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; // subscribe to pose listener.

// Publishers
rclcpp::Publisher<ErrorStamped>::SharedPtr pub_error_msg_; // publish error message

// Node Methods
bool isDataReady() const; // check if data arrive
static bool isValidTrajectory(const Trajectory & traj);
boost::optional<TargetPerformanceMsgVars> computeTargetPerformanceMsgVars() const;

// Callback Methods
void onTrajectory(const Trajectory::ConstSharedPtr msg);
void publishErrorMsg(const TargetPerformanceMsgVars & control_performance_vars);
void onControlRaw(const AckermannLateralCommand::ConstSharedPtr control_msg);
void onVecSteeringMeasured(const SteeringReport::ConstSharedPtr meas_steer_msg);
void onVelocity(const Odometry::ConstSharedPtr msg);

// Timer - To Publish In Control Period
rclcpp::TimerBase::SharedPtr timer_publish_;
void onTimer();

// Parameters
Param param_{}; // wheelbase, control period and feedback coefficients.

// Subscriber Parameters
Trajectory::ConstSharedPtr current_trajectory_ptr_; // ConstPtr to local traj.
AckermannLateralCommand::ConstSharedPtr current_control_msg_ptr_;
SteeringReport::ConstSharedPtr current_vec_steering_msg_ptr_;
Odometry::ConstSharedPtr current_odom_ptr_;
PoseStamped::ConstSharedPtr current_pose_; // pose of the vehicle, x, y, heading

// Algorithm
std::unique_ptr<ControlPerformanceAnalysisCore> control_performance_core_ptr_;
};
class ControlPerformanceAnalysisNode : public rclcpp::Node
{
public:
explicit ControlPerformanceAnalysisNode(const rclcpp::NodeOptions& node_options);

private:
// Subscribers and Local Variable Assignment
rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_; // subscribe to trajectory
rclcpp::Subscription<AckermannControlCommand>::SharedPtr
sub_control_steering_; // subscribe to steering control value
rclcpp::Subscription<Odometry>::SharedPtr sub_velocity_; // subscribe to velocity
rclcpp::Subscription<SteeringReport>::SharedPtr sub_vehicle_steering_;

// Self Pose listener.
tier4_autoware_utils::SelfPoseListener self_pose_listener_{ this }; // subscribe to pose listener.

// Publishers
rclcpp::Publisher<ErrorStamped>::SharedPtr pub_error_msg_; // publish error message

// Node Methods
bool isDataReady() const; // check if data arrive
static bool isValidTrajectory(const Trajectory& traj);

boost::optional <TargetPerformanceMsgVars> computeTargetPerformanceMsgVars() const;

// Callback Methods
void onTrajectory(const Trajectory::ConstSharedPtr msg);

void publishErrorMsg(const TargetPerformanceMsgVars& control_performance_vars);

void onControlRaw(const AckermannControlCommand::ConstSharedPtr control_msg);

void onVecSteeringMeasured(const SteeringReport::ConstSharedPtr meas_steer_msg);

void onVelocity(const Odometry::ConstSharedPtr msg);

// Timer - To Publish In Control Period
rclcpp::TimerBase::SharedPtr timer_publish_;

void onTimer();

// Parameters
Param param_{}; // wheelbase, control period and feedback coefficients.

// Subscriber Parameters
Trajectory::ConstSharedPtr current_trajectory_ptr_; // ConstPtr to local traj.
AckermannControlCommand::ConstSharedPtr current_control_msg_ptr_;
SteeringReport::ConstSharedPtr current_vec_steering_msg_ptr_;
Odometry::ConstSharedPtr current_odom_ptr_;
PoseStamped::ConstSharedPtr current_pose_; // pose of the vehicle, x, y, heading

// Algorithm
std::unique_ptr <ControlPerformanceAnalysisCore> control_performance_core_ptr_;
};
} // namespace control_performance_analysis

#endif // CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_
Loading