diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/CMakeLists.txt b/planning/scenario_planning/common/motion_velocity_optimizer/CMakeLists.txt index 298c3cacdf8b8..28378138aef57 100644 --- a/planning/scenario_planning/common/motion_velocity_optimizer/CMakeLists.txt +++ b/planning/scenario_planning/common/motion_velocity_optimizer/CMakeLists.txt @@ -5,6 +5,8 @@ project(motion_velocity_optimizer) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -30,11 +32,16 @@ set(MOTION_VELOCITY_OPTIMIZER_HDR include/motion_velocity_optimizer/optimizer/optimizer_base.hpp ) -ament_auto_add_executable(motion_velocity_optimizer - ${MOTION_VELOCITY_OPTIMIZER_SRC} +ament_auto_add_executable(motion_velocity_optimizer + ${MOTION_VELOCITY_OPTIMIZER_SRC} ${MOTION_VELOCITY_OPTIMIZER_HDR} ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/interpolate.hpp b/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/interpolate.hpp index f5d05e49d6401..a48ebbd5ba6dd 100644 --- a/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/interpolate.hpp +++ b/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/interpolate.hpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#pragma once +#ifndef MOTION_VELOCITY_OPTIMIZER__INTERPOLATE_HPP_ +#define MOTION_VELOCITY_OPTIMIZER__INTERPOLATE_HPP_ + #include #include #include @@ -20,8 +22,8 @@ class LinearInterpolate { public: - LinearInterpolate(){}; - ~LinearInterpolate(){}; + LinearInterpolate() {} + ~LinearInterpolate() {} static bool interpolate( const std::vector & base_index, const std::vector & base_value, const std::vector & return_index, std::vector & return_value); @@ -37,7 +39,7 @@ class SplineInterpolate public: SplineInterpolate(); - SplineInterpolate(const std::vector & x); + explicit SplineInterpolate(const std::vector & x); ~SplineInterpolate(); void generateSpline(const std::vector & x); double getValue(const double & s); @@ -46,3 +48,5 @@ class SplineInterpolate const std::vector & return_index, std::vector & return_value); void getValueVector(const std::vector & s_v, std::vector & value_v); }; + +#endif // MOTION_VELOCITY_OPTIMIZER__INTERPOLATE_HPP_ diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/motion_velocity_optimizer.hpp b/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/motion_velocity_optimizer.hpp index 5d42ce85c888d..a2cb25e478f63 100644 --- a/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/motion_velocity_optimizer.hpp +++ b/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/motion_velocity_optimizer.hpp @@ -11,23 +11,30 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_planning_msgs/msg/trajectory.hpp" -#include "autoware_planning_msgs/msg/velocity_limit.hpp" -#include "autoware_debug_msgs/msg/float32_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "rclcpp/rclcpp.hpp" -#include "autoware_debug_msgs/msg/float32_multi_array_stamped.hpp" -#include "tf2/utils.h" -#include "tf2_ros/transform_listener.h" + +#ifndef MOTION_VELOCITY_OPTIMIZER__MOTION_VELOCITY_OPTIMIZER_HPP_ +#define MOTION_VELOCITY_OPTIMIZER__MOTION_VELOCITY_OPTIMIZER_HPP_ + #include +#include #include #include +#include #include "motion_velocity_optimizer/motion_velocity_optimizer_utils.hpp" #include "motion_velocity_optimizer/optimizer/optimizer_base.hpp" +#include "autoware_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_debug_msgs/msg/float32_stamped.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/velocity_limit.hpp" #include "osqp_interface/osqp_interface.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/utils.h" +#include "tf2_ros/transform_listener.h" + class MotionVelocityOptimizer : public rclcpp::Node { public: @@ -35,10 +42,15 @@ class MotionVelocityOptimizer : public rclcpp::Node ~MotionVelocityOptimizer(); private: - rclcpp::Publisher::SharedPtr pub_trajectory_; //!< @brief publisher for output trajectory - rclcpp::Subscription::SharedPtr sub_current_velocity_; //!< @brief subscriber for current velocity - rclcpp::Subscription::SharedPtr sub_current_trajectory_; //!< @brief subscriber for reference trajectory - rclcpp::Subscription::SharedPtr sub_external_velocity_limit_;//!< @brief subscriber for external velocity limit + // publisher for output trajectory + rclcpp::Publisher::SharedPtr pub_trajectory_; + // subscriber for current velocity + rclcpp::Subscription::SharedPtr sub_current_velocity_; + // subscriber for reference trajectory + rclcpp::Subscription::SharedPtr sub_current_trajectory_; + rclcpp::Subscription::SharedPtr + // subscriber for external velocity limit + sub_external_velocity_limit_; tf2::BufferCore tf_buffer_; //!< @brief tf butter tf2_ros::TransformListener tf_listener_; //!< @brief tf listener @@ -48,15 +60,20 @@ class MotionVelocityOptimizer : public rclcpp::Node std::mutex mutex_; - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_ptr_; // current vehicle pose - geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr_; // current vehicle twist - autoware_planning_msgs::msg::Trajectory::ConstSharedPtr base_traj_raw_ptr_; // current base_waypoints - autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr external_velocity_limit_ptr_; // current external_velocity_limit + // current vehicle pose + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_ptr_; + // current vehicle twist + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr_; + // current base_waypoints + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr base_traj_raw_ptr_; + // current external_velocity_limit + autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr external_velocity_limit_ptr_; std::shared_ptr external_velocity_limit_filtered_; autoware_planning_msgs::msg::Trajectory prev_output_; // previously published trajectory - enum class InitializeType { + enum class InitializeType + { INIT = 0, LARGE_DEVIATION_REPLAN = 1, ENGAGING = 2, @@ -96,7 +113,8 @@ class MotionVelocityOptimizer : public rclcpp::Node /* topic callback */ void callbackCurrentVelocity(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); void callbackCurrentTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); - void callbackExternalVelocityLimit(const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + void callbackExternalVelocityLimit( + const autoware_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); void timerCallback(); @@ -113,7 +131,8 @@ class MotionVelocityOptimizer : public rclcpp::Node autoware_planning_msgs::msg::Trajectory optimizeVelocity( const autoware_planning_msgs::msg::Trajectory & input, const int input_closest, - const autoware_planning_msgs::msg::Trajectory & prev_output_traj, const int prev_output_closest); + const autoware_planning_msgs::msg::Trajectory & prev_output_traj, + const int prev_output_closest); void calcInitialMotion( const double & base_speed, const autoware_planning_msgs::msg::Trajectory & base_waypoints, @@ -157,12 +176,19 @@ class MotionVelocityOptimizer : public rclcpp::Node /* debug */ - rclcpp::Publisher::SharedPtr pub_dist_to_stopline_; //!< @brief publisher for stop distance + // publisher for stop distance + rclcpp::Publisher::SharedPtr pub_dist_to_stopline_; rclcpp::Publisher::SharedPtr pub_trajectory_raw_; rclcpp::Publisher::SharedPtr pub_trajectory_vel_lim_; - rclcpp::Publisher::SharedPtr pub_trajectory_latcc_filtered_; + rclcpp::Publisher::SharedPtr + pub_trajectory_latcc_filtered_; rclcpp::Publisher::SharedPtr pub_trajectory_resampled_; rclcpp::Publisher::SharedPtr debug_closest_velocity_; rclcpp::Publisher::SharedPtr debug_closest_acc_; - void publishFloat(const double & data, const rclcpp::Publisher::SharedPtr pub) const; + void publishFloat( + const double & data, + const rclcpp::Publisher::SharedPtr pub) + const; }; + +#endif // MOTION_VELOCITY_OPTIMIZER__MOTION_VELOCITY_OPTIMIZER_HPP_ diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/motion_velocity_optimizer_utils.hpp b/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/motion_velocity_optimizer_utils.hpp index cfd85ea6854b2..2795647ccba70 100644 --- a/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/motion_velocity_optimizer_utils.hpp +++ b/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/motion_velocity_optimizer_utils.hpp @@ -12,7 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifndef MOTION_VELOCITY_OPTIMIZER__MOTION_VELOCITY_OPTIMIZER_UTILS_HPP_ +#define MOTION_VELOCITY_OPTIMIZER__MOTION_VELOCITY_OPTIMIZER_UTILS_HPP_ + #include +#include #include "geometry_msgs/msg/twist_stamped.hpp" #include "rclcpp/rclcpp.hpp" @@ -32,12 +36,15 @@ double calcSquaredDist2d( const autoware_planning_msgs::msg::TrajectoryPoint & b); double calcDist2d(const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b); double calcDist2d(const geometry_msgs::msg::Pose & a, const geometry_msgs::msg::Pose & b); -double calcDist2d(const geometry_msgs::msg::PoseStamped & a, const geometry_msgs::msg::PoseStamped & b); +double calcDist2d( + const geometry_msgs::msg::PoseStamped & a, + const geometry_msgs::msg::PoseStamped & b); double calcDist2d( const autoware_planning_msgs::msg::TrajectoryPoint & a, const autoware_planning_msgs::msg::TrajectoryPoint & b); int calcClosestWaypoint( - const autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Point & point); + const autoware_planning_msgs::msg::Trajectory & trajectory, + const geometry_msgs::msg::Point & point); int calcClosestWaypoint( const autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, const double delta_yaw_threshold); @@ -54,8 +61,12 @@ void calcTrajectoryIntervalDistance( void setZeroVelocity(autoware_planning_msgs::msg::Trajectory & trajectory); double getMaxVelocity(const autoware_planning_msgs::msg::Trajectory & trajectory); double getMaxAbsVelocity(const autoware_planning_msgs::msg::Trajectory & trajectory); -void mininumVelocityFilter(const double & min_vel, autoware_planning_msgs::msg::Trajectory & trajectory); -void maximumVelocityFilter(const double & max_vel, autoware_planning_msgs::msg::Trajectory & trajectory); +void mininumVelocityFilter( + const double & min_vel, + autoware_planning_msgs::msg::Trajectory & trajectory); +void maximumVelocityFilter( + const double & max_vel, + autoware_planning_msgs::msg::Trajectory & trajectory); void multiplyConstantToTrajectoryVelocity( const double & scalar, autoware_planning_msgs::msg::Trajectory & trajectory); void insertZeroVelocityAfterIdx( @@ -70,6 +81,9 @@ void convertEulerAngleToMonotonic(std::vector & a); geometry_msgs::msg::Quaternion getQuaternionFromYaw(double yaw); bool linearInterpTrajectory( const std::vector & base_index, - const autoware_planning_msgs::msg::Trajectory & base_trajectory, const std::vector & out_index, + const autoware_planning_msgs::msg::Trajectory & base_trajectory, + const std::vector & out_index, autoware_planning_msgs::msg::Trajectory & out_trajectory); -} // namespace vpu \ No newline at end of file +} // namespace vpu + +#endif // MOTION_VELOCITY_OPTIMIZER__MOTION_VELOCITY_OPTIMIZER_UTILS_HPP_ diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/optimizer/l2_pseudo_jerk_optimizer.hpp b/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/optimizer/l2_pseudo_jerk_optimizer.hpp index c44589a9b68df..083d6d3121f7a 100644 --- a/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/optimizer/l2_pseudo_jerk_optimizer.hpp +++ b/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/optimizer/l2_pseudo_jerk_optimizer.hpp @@ -12,16 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_OPTIMIZER_L2_PSEUDO_JERK_OPTIMIZER_HPP -#define MOTION_VELOCITY_OPTIMIZER_L2_PSEUDO_JERK_OPTIMIZER_HPP +#ifndef MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__L2_PSEUDO_JERK_OPTIMIZER_HPP_ +#define MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__L2_PSEUDO_JERK_OPTIMIZER_HPP_ -#include "autoware_planning_msgs/msg/trajectory.hpp" -#include "osqp_interface/osqp_interface.hpp" -#include "rclcpp/rclcpp.hpp" #include #include "motion_velocity_optimizer/optimizer/optimizer_base.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "osqp_interface/osqp_interface.hpp" + +#include "rclcpp/rclcpp.hpp" + class L2PseudoJerkOptimizer : public OptimizerBase { public: @@ -34,8 +36,8 @@ class L2PseudoJerkOptimizer : public OptimizerBase double over_a_weight; }; -public: explicit L2PseudoJerkOptimizer(const OptimizerParam & p); + bool solve( const double initial_vel, const double initial_acc, const int closest, const autoware_planning_msgs::msg::Trajectory & input, @@ -50,4 +52,4 @@ class L2PseudoJerkOptimizer : public OptimizerBase osqp::OSQPInterface qp_solver_; }; -#endif // MOTION_VELOCITY_OPTIMIZER_L2_PSEUDO_JERK_OPTIMIZER_HPP +#endif // MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__L2_PSEUDO_JERK_OPTIMIZER_HPP_ diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/optimizer/linf_pseudo_jerk_optimizer.hpp b/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/optimizer/linf_pseudo_jerk_optimizer.hpp index 00d6f39341eb4..3dd73d506cc02 100644 --- a/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/optimizer/linf_pseudo_jerk_optimizer.hpp +++ b/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/optimizer/linf_pseudo_jerk_optimizer.hpp @@ -12,15 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_OPTIMIZER_LINF_PSEUDO_JERK_OPTIMIZER_HPP -#define MOTION_VELOCITY_OPTIMIZER_LINF_PSEUDO_JERK_OPTIMIZER_HPP +#ifndef MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__LINF_PSEUDO_JERK_OPTIMIZER_HPP_ +#define MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__LINF_PSEUDO_JERK_OPTIMIZER_HPP_ + +#include + +#include "motion_velocity_optimizer/optimizer/optimizer_base.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "osqp_interface/osqp_interface.hpp" + #include "rclcpp/rclcpp.hpp" -#include -#include "motion_velocity_optimizer/optimizer/optimizer_base.hpp" class LinfPseudoJerkOptimizer : public OptimizerBase { @@ -34,8 +37,9 @@ class LinfPseudoJerkOptimizer : public OptimizerBase double over_a_weight; }; -public: explicit LinfPseudoJerkOptimizer(const OptimizerParam & p); + virtual ~LinfPseudoJerkOptimizer() = default; + bool solve( const double initial_vel, const double initial_acc, const int closest, const autoware_planning_msgs::msg::Trajectory & input, @@ -50,4 +54,4 @@ class LinfPseudoJerkOptimizer : public OptimizerBase osqp::OSQPInterface qp_solver_; }; -#endif // MOTION_VELOCITY_OPTIMIZER_LINF_PSEUDO_JERK_OPTIMIZER_HPP +#endif // MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__LINF_PSEUDO_JERK_OPTIMIZER_HPP_ diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/optimizer/optimizer_base.hpp b/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/optimizer/optimizer_base.hpp index eca0472e70824..82751d8c384ea 100644 --- a/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/optimizer/optimizer_base.hpp +++ b/planning/scenario_planning/common/motion_velocity_optimizer/include/motion_velocity_optimizer/optimizer/optimizer_base.hpp @@ -12,12 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MOTION_VELOCITY_OPTIMIZER_OPTIMIZER_BASE_HPP -#define MOTION_VELOCITY_OPTIMIZER_OPTIMIZER_BASE_HPP -#include "autoware_planning_msgs/msg/trajectory.hpp" +#ifndef MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__OPTIMIZER_BASE_HPP_ +#define MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__OPTIMIZER_BASE_HPP_ + #include #include +#include "autoware_planning_msgs/msg/trajectory.hpp" + class OptimizerBase { public: @@ -29,6 +31,8 @@ class OptimizerBase virtual void setAccel(const double max_accel) = 0; virtual void setDecel(const double min_decel) = 0; + + virtual ~OptimizerBase() = default; }; -#endif // MOTION_VELOCITY_OPTIMIZER_OPTIMIZER_BASE_HPP +#endif // MOTION_VELOCITY_OPTIMIZER__OPTIMIZER__OPTIMIZER_BASE_HPP_ diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/package.xml b/planning/scenario_planning/common/motion_velocity_optimizer/package.xml index d26319051e5a2..a6d1c5e8de63b 100644 --- a/planning/scenario_planning/common/motion_velocity_optimizer/package.xml +++ b/planning/scenario_planning/common/motion_velocity_optimizer/package.xml @@ -11,6 +11,9 @@ ament_cmake + ament_lint_auto + ament_lint_common + ament_cmake diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/scripts/closest_velocity_checker.py b/planning/scenario_planning/common/motion_velocity_optimizer/scripts/closest_velocity_checker.py index 973d0031945ff..2f3c3807478a8 100755 --- a/planning/scenario_planning/common/motion_velocity_optimizer/scripts/closest_velocity_checker.py +++ b/planning/scenario_planning/common/motion_velocity_optimizer/scripts/closest_velocity_checker.py @@ -1,19 +1,29 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -import math -import sys +# Copyright 2020 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import time -import copy -import numpy as np + import rospy import tf -from autoware_planning_msgs.msg import Path, PathWithLaneId, Trajectory from autoware_control_msgs.msg import ControlCommandStamped +from autoware_planning_msgs.msg import Path, PathWithLaneId, Trajectory from autoware_vehicle_msgs.msg import VehicleCommand -from geometry_msgs.msg import Pose, PoseStamped, PoseWithCovarianceStamped, Quaternion, Twist, TwistStamped -from std_msgs.msg import Bool, Float32, Header, Int32, Float32MultiArray - +from geometry_msgs.msg import Pose, Twist, TwistStamped +from std_msgs.msg import Bool, Float32, Float32MultiArray REF_LINK = "map" SELF_LINK = "base_link" @@ -46,31 +56,40 @@ def __init__(self): self.tfl = tf.TransformListener() # for get self-position # planning path and trajectories + sp = "/planning/scenario_planning" self.sub_behavior_path_w_lid = rospy.Subscriber( - "/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", PathWithLaneId, self.CallBackBehaviorPathWLid, queue_size=1, tcp_nodelay=True + sp + "/lane_driving/behavior_planning/path_with_lane_id", PathWithLaneId, + self.CallBackBehaviorPathWLid, queue_size=1, tcp_nodelay=True ) self.sub_behavior_velocity_path = rospy.Subscriber( - "/planning/scenario_planning/lane_driving/behavior_planning/path", Path, self.CallBackBehaviorPath, queue_size=1, tcp_nodelay=True + sp + "/lane_driving/behavior_planning/path", Path, self.CallBackBehaviorPath, + queue_size=1, tcp_nodelay=True ) self.sub_avoid_trajectory = rospy.Subscriber( - "/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory", Trajectory, self.CallBackAvoidTrajectory, queue_size=1, tcp_nodelay=True + sp + "/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory", Trajectory, + self.CallBackAvoidTrajectory, queue_size=1, tcp_nodelay=True ) self.sub_lane_drive_trajectory = rospy.Subscriber( - "/planning/scenario_planning/lane_driving/trajectory", Trajectory, self.CallBackLaneDriveTrajectory, queue_size=1, tcp_nodelay=True + sp + "/lane_driving/trajectory", Trajectory, self.CallBackLaneDriveTrajectory, + queue_size=1, tcp_nodelay=True ) self.sub_latacc_trajectory = rospy.Subscriber( - "/planning/scenario_planning/motion_velocity_optimizer/debug/trajectory_lateral_acc_filtered", Trajectory, self.CallBackLataccTrajectory, queue_size=1, tcp_nodelay=True + sp + "/motion_velocity_optimizer/debug/trajectory_lateral_acc_filtered", Trajectory, + self.CallBackLataccTrajectory, queue_size=1, tcp_nodelay=True ) self.sub_trajectory = rospy.Subscriber( - "/planning/scenario_planning/trajectory", Trajectory, self.CallBackScenarioTrajectory, queue_size=1, tcp_nodelay=True + sp + "/trajectory", Trajectory, self.CallBackScenarioTrajectory, queue_size=1, + tcp_nodelay=True ) # control commands self.sub_control_cmd = rospy.Subscriber( - "/control/control_cmd", ControlCommandStamped, self.CallBackControlCmd, queue_size=1, tcp_nodelay=True + "/control/control_cmd", ControlCommandStamped, self.CallBackControlCmd, queue_size=1, + tcp_nodelay=True ) self.sub_vehicle_cmd = rospy.Subscriber( - "/control/vehicle_cmd", VehicleCommand, self.CallBackVehicleCmd, queue_size=1, tcp_nodelay=True + "/control/vehicle_cmd", VehicleCommand, self.CallBackVehicleCmd, queue_size=1, + tcp_nodelay=True ) # others related to velocity @@ -78,33 +97,37 @@ def __init__(self): "/autoware/engage", Bool, self.CallBackAwEngage, queue_size=1, tcp_nodelay=True ) self.sub_external_vlim = rospy.Subscriber( - "/planning/scenario_planning/max_velocity", Float32, self.CallBackExternalVelLim, queue_size=1, tcp_nodelay=True + sp + "/max_velocity", Float32, self.CallBackExternalVelLim, + queue_size=1, tcp_nodelay=True ) # self twist self.sub_localization_twist = rospy.Subscriber( - "/localization/twist", TwistStamped, self.CallBackLocalizationTwist, queue_size=1, tcp_nodelay=True + "/localization/twist", TwistStamped, self.CallBackLocalizationTwist, queue_size=1, + tcp_nodelay=True ) self.sub_localization_twist = rospy.Subscriber( - "/vehicle/status/twist", TwistStamped, self.CallBackVehicleTwist, queue_size=1, tcp_nodelay=True + "/vehicle/status/twist", TwistStamped, self.CallBackVehicleTwist, queue_size=1, + tcp_nodelay=True ) # publish data - self.pub_varr = rospy.Publisher("~closest_speeds", Float32MultiArray, queue_size=1) - + self.pub_varr = rospy.Publisher( + "~closest_speeds", Float32MultiArray, queue_size=1) time.sleep(1.0) # wait for ready to publish/subscribe # for publish traffic signal image rospy.Timer(rospy.Duration(1 / 10.0), self.timerCallback) - def printInfo(self): self.count = self.count % 30 if self.count == 0: rospy.loginfo("") rospy.loginfo( - "| Map Limit | Behavior | Obs Avoid | Obs Stop | External Lim | LatAcc Filtered | Optimized | Control VelCmd | Control AccCmd | Vehicle VelCmd | Vehicle AccCmd | Engage | Localization Vel | Vehicle Vel | [km/h]") + "| Map Limit | Behavior | Obs Avoid | Obs Stop | External Lim | LatAcc Filtered " + "| Optimized | Control VelCmd | Control AccCmd | Vehicle VelCmd | Vehicle AccCmd " + "| Engage | Localization Vel | Vehicle Vel | [km/h]") vel_map_lim = self.data_arr[LANE_CHANGE] * 3.6 vel_behavior = self.data_arr[BEHAVIOR_VELOCITY] * 3.6 vel_obs_avoid = self.data_arr[OBSTACLE_AVOID] * 3.6 @@ -118,9 +141,15 @@ def printInfo(self): acc_vehicle_cmd = self.data_arr[VEHICLE_CMD_ACC] vel_localization = self.localization_twist.linear.x * 3.6 vel_vehicle = self.vehicle_twist.linear.x * 3.6 - rospy.loginfo("| {0: 9.2f} | {1: 8.2f} | {2: 9.2f} | {3: 8.2f} | {4: 12.2f} | {5: 15.2f} | {6: 9.2f} | {7: 14.2f} | {8: 14.2f} | {9: 14.2f} | {10: 14.2f} | {11: 6d} | {12: 16.2f} | {13: 11.2f} |".format( - vel_map_lim, vel_behavior, vel_obs_avoid, vel_obs_stop, vel_external_lim, vel_latacc_filtered, vel_optimized, - vel_ctrl_cmd, acc_ctrl_cmd, vel_vehicle_cmd, acc_vehicle_cmd, (bool)(self.autoware_engage), vel_localization, vel_vehicle)) + formatstring = ("| {0: 9.2f} | {1: 8.2f} | {2: 9.2f} | {3: 8.2f} | {4: 12.2f} " + "| {5: 15.2f} | {6: 9.2f} | {7: 14.2f} | {8: 14.2f} | {9: 14.2f} " + "| {10: 14.2f} | {11: 6d} | {12: 16.2f} | {13: 11.2f} |") + rospy.loginfo(formatstring.format(vel_map_lim, vel_behavior, vel_obs_avoid, vel_obs_stop, + vel_external_lim, vel_latacc_filtered, vel_optimized, + vel_ctrl_cmd, acc_ctrl_cmd, vel_vehicle_cmd, + acc_vehicle_cmd, (bool)( + self.autoware_engage), + vel_localization, vel_vehicle)) self.count += 1 def timerCallback(self, t): @@ -189,14 +218,12 @@ def CallBackVehicleCmd(self, msg): self.data_arr[VEHICLE_CMD_ACC] = msg.control.acceleration return - - - def calcClosestPath(self, path): closest = -1 min_dist_squared = 1.0e10 for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) + dist_sq = self.calcSquaredDist2d( + self.self_pose, path.points[i].pose) if dist_sq < min_dist_squared: min_dist_squared = dist_sq closest = i @@ -206,7 +233,8 @@ def calcClosestPathWLid(self, path): closest = -1 min_dist_squared = 1.0e10 for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].point.pose) + dist_sq = self.calcSquaredDist2d( + self.self_pose, path.points[i].point.pose) if dist_sq < min_dist_squared: min_dist_squared = dist_sq closest = i @@ -216,7 +244,8 @@ def calcClosestTrajectory(self, path): closest = -1 min_dist_squared = 1.0e10 for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) + dist_sq = self.calcSquaredDist2d( + self.self_pose, path.points[i].pose) if dist_sq < min_dist_squared: min_dist_squared = dist_sq closest = i @@ -236,8 +265,11 @@ def calcSquaredDist2d(self, p1, p2): def updatePose(self, from_link, to_link): try: - self.tfl.waitForTransform(from_link, to_link, rospy.Time(0), rospy.Duration(0.2)) - (trans, quat) = self.tfl.lookupTransform(from_link, to_link, rospy.Time(0)) # parent, child + self.tfl.waitForTransform( + from_link, to_link, rospy.Time(0), rospy.Duration(0.2)) + # parent, child + (trans, quat) = self.tfl.lookupTransform( + from_link, to_link, rospy.Time(0)) self.self_pose.position.x = trans[0] self.self_pose.position.y = trans[1] self.self_pose.position.z = trans[2] @@ -246,14 +278,13 @@ def updatePose(self, from_link, to_link): self.self_pose.orientation.z = quat[2] self.self_pose.orientation.w = quat[3] return - except: + except Exception: trans = (0.0, 0.0, 0.0) quat = (0.0, 0.0, 0.0, 1.0) rospy.logwarn("cannot get position") return - def main(): rospy.init_node("velocity_checker") VelocityChecker() diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/scripts/trajectory_visualizer.py b/planning/scenario_planning/common/motion_velocity_optimizer/scripts/trajectory_visualizer.py index 98bdccedc8d60..2db99bc435b37 100755 --- a/planning/scenario_planning/common/motion_velocity_optimizer/scripts/trajectory_visualizer.py +++ b/planning/scenario_planning/common/motion_velocity_optimizer/scripts/trajectory_visualizer.py @@ -1,43 +1,48 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -import rospy -import math -import time -from autoware_planning_msgs.msg import Trajectory, PathWithLaneId, Path -from geometry_msgs.msg import PoseStamped, Pose, TwistStamped, Twist +# Copyright 2020 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse + import matplotlib.pyplot as plt import numpy as np +import rospy import tf -import argparse +from autoware_planning_msgs.msg import Path, PathWithLaneId, Trajectory +from geometry_msgs.msg import Pose, Twist, TwistStamped parser = argparse.ArgumentParser() -parser.add_argument('-l', '--length', help='max arclength in plot') -parser.add_argument('-t', '--type', help='Options VEL(default): show velocity only, VEL_ACC_JERK: show vel & acc & jerk') +parser.add_argument('-l', '--length', default=200, type=int, help='max arclength in plot') +parser.add_argument( + '-t', '--type', default="VEL", choices=["VEL", "VEC_ACC_JERK"], + help='Show velocity only (VEL) or vel & acc & jerk (VEL_ACC_JERK)' +) args = parser.parse_args() - -if args.length == None: - PLOT_MAX_ARCLENGTH = 200 -else: - PLOT_MAX_ARCLENGTH = int(args.length) +PLOT_MAX_ARCLENGTH = args.length print('max arclength = ' + str(PLOT_MAX_ARCLENGTH)) -if args.type == None: - PLOT_TYPE = "VEL" -elif args.type == "VEL": - PLOT_TYPE = "VEL" -elif args.type == "VEL_ACC_JERK": - PLOT_TYPE = "VEL_ACC_JERK" -else: - print("invalid type. set default VEL.") - PLOT_TYPE = "VEL" +PLOT_TYPE = args.type print('plot type = ' + PLOT_TYPE) PATH_ORIGIN_FRAME = "map" SELF_POSE_FRAME = "base_link" + class TrajectoryVisualizer(): def __init__(self): @@ -58,19 +63,41 @@ def __init__(self): self.obstacle_stop_traj = Trajectory() self.plotted = [False] * 9 - self.sub_localization_twist = rospy.Subscriber("/localization/twist", TwistStamped, self.CallbackLocalizationTwist, queue_size=1, tcp_nodelay=True) - self.sub_vehicle_twist = rospy.Subscriber("/vehicle/status/twist", TwistStamped, self.CallbackVehicleTwist, queue_size=1, tcp_nodelay=True) - - self.substatus1 = rospy.Subscriber("/planning/scenario_planning/motion_velocity_optimizer/debug/trajectory_external_velocity_limitted", Trajectory, self.CallBackTrajExVelLim, queue_size=1, tcp_nodelay=True) - self.substatus2 = rospy.Subscriber("/planning/scenario_planning/motion_velocity_optimizer/debug/trajectory_lateral_acc_filtered", Trajectory, self.CallBackTrajLatAccFiltered, queue_size=1, tcp_nodelay=True) - self.substatus3 = rospy.Subscriber("/planning/scenario_planning/motion_velocity_optimizer/debug/trajectory_raw", Trajectory, self.CallBackTrajRaw, queue_size=1, tcp_nodelay=True) - self.substatus4 = rospy.Subscriber("/planning/scenario_planning/motion_velocity_optimizer/debug/trajectory_time_resampled", Trajectory, self.CallBackTrajTimeResampled, queue_size=1, tcp_nodelay=True) - self.substatus5 = rospy.Subscriber("/planning/scenario_planning/trajectory", Trajectory, self.CallBackTrajFinal, queue_size=1, tcp_nodelay=True) - - self.substatus6 = rospy.Subscriber("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", PathWithLaneId, self.CallBackLaneChangePath, queue_size=1, tcp_nodelay=True) - self.substatus7 = rospy.Subscriber("/planning/scenario_planning/lane_driving/behavior_planning/path", Path, self.CallBackBehaviorPath, queue_size=1, tcp_nodelay=True) - self.substatus8 = rospy.Subscriber("/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory", Trajectory, self.CallbackObstacleAvoidTraj, queue_size=1, tcp_nodelay=True) - self.substatus8 = rospy.Subscriber("/planning/scenario_planning/lane_driving/trajectory", Trajectory, self.CallbackObstacleStopTraj, queue_size=1, tcp_nodelay=True) + self.sub_localization_twist = rospy.Subscriber( + "/localization/twist", TwistStamped, self.CallbackLocalizationTwist, + queue_size=1, tcp_nodelay=True) + self.sub_vehicle_twist = rospy.Subscriber( + "/vehicle/status/twist", TwistStamped, self.CallbackVehicleTwist, + queue_size=1, tcp_nodelay=True) + + sp = "/planning/scenario_planning" + self.substatus1 = rospy.Subscriber( + sp + "/motion_velocity_optimizer/debug/trajectory_external_velocity_limitted", + Trajectory, self.CallBackTrajExVelLim, queue_size=1, tcp_nodelay=True) + self.substatus2 = rospy.Subscriber( + sp + "/motion_velocity_optimizer/debug/trajectory_lateral_acc_filtered", + Trajectory, self.CallBackTrajLatAccFiltered, queue_size=1, tcp_nodelay=True) + self.substatus3 = rospy.Subscriber( + sp + "/motion_velocity_optimizer/debug/trajectory_raw", Trajectory, + self.CallBackTrajRaw, queue_size=1, tcp_nodelay=True) + self.substatus4 = rospy.Subscriber( + sp + "/motion_velocity_optimizer/debug/trajectory_time_resampled", Trajectory, + self.CallBackTrajTimeResampled, queue_size=1, tcp_nodelay=True) + self.substatus5 = rospy.Subscriber( + sp + "/trajectory", Trajectory, self.CallBackTrajFinal, queue_size=1, tcp_nodelay=True) + + self.substatus6 = rospy.Subscriber( + sp + "/lane_driving/behavior_planning/path_with_lane_id", PathWithLaneId, + self.CallBackLaneChangePath, queue_size=1, tcp_nodelay=True) + self.substatus7 = rospy.Subscriber( + sp + "/lane_driving/behavior_planning/path", Path, self.CallBackBehaviorPath, + queue_size=1, tcp_nodelay=True) + self.substatus8 = rospy.Subscriber( + sp + "/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory", + Trajectory, self.CallbackObstacleAvoidTraj, queue_size=1, tcp_nodelay=True) + self.substatus8 = rospy.Subscriber( + sp + "/lane_driving/trajectory", Trajectory, self.CallbackObstacleStopTraj, + queue_size=1, tcp_nodelay=True) rospy.Timer(rospy.Duration(0.2), self.timerCallback) @@ -116,7 +143,7 @@ def timerCallback(self, event): def plotTrajectoryVelocity(self): plt.clf() - ax1 = plt.subplot(1,1,1) # row, col, index(= 0: x_closest = x[closest] - ax1.plot(x_closest, self.localization_twist.linear.x, label="localization twist vx", color="r", marker="*", ls=":", markersize=10) - ax1.plot(x_closest, self.vehicle_twist.linear.x, label="vehicle twist vx", color="k", marker="+", ls=":", markersize=10) - - + ax1.plot(x_closest, self.localization_twist.linear.x, + label="localization twist vx", color="r", marker="*", ls=":", markersize=10) + ax1.plot(x_closest, self.vehicle_twist.linear.x, + label="vehicle twist vx", color="k", marker="+", ls=":", markersize=10) ax1.set_title("trajectorys velocity") ax1.legend() @@ -183,7 +210,6 @@ def plotTrajectoryVelocity(self): plt.pause(0.01) - def CalcArcLength(self, traj): s_arr = [] ds = 0.0 @@ -315,9 +341,8 @@ def plotTrajectory(self): trajectory_time_resamped = self.trajectory_time_resamped trajectory_final = self.trajectory_final - plt.clf() - ax1 = plt.subplot(3,1,1)#row, col, index( +#include /* * linear interpolation */ @@ -23,11 +25,11 @@ bool LinearInterpolate::interpolate( const std::vector & return_index, std::vector & return_value) { auto isIncrease = [](const std::vector & x) { - for (int i = 0; i < (int)x.size() - 1; ++i) { - if (x[i] > x[i + 1]) return false; - } - return true; - }; + for (int i = 0; i < static_cast(x.size()) - 1; ++i) { + if (x[i] > x[i + 1]) {return false;} + } + return true; + }; if (base_index.size() == 0 || base_value.size() == 0 || return_index.size() == 0) { printf( @@ -41,7 +43,8 @@ bool LinearInterpolate::interpolate( if ( !isIncrease(base_index) || !isIncrease(return_index) || return_index.front() < base_index.front() || base_index.back() < return_index.back() || - base_index.size() != base_value.size()) { + base_index.size() != base_value.size()) + { std::cerr << "[isIncrease] bad index, return false" << std::endl; bool b1 = !isIncrease(base_index); bool b2 = !isIncrease(return_index); @@ -63,8 +66,8 @@ bool LinearInterpolate::interpolate( return_value.push_back(base_value[i]); continue; } - while (base_index[i] < idx) ++i; - if (i <= 0 || (int)base_index.size() - 1 < i) { + while (base_index[i] < idx) {++i;} + if (i <= 0 || static_cast(base_index.size()) - 1 < i) { std::cerr << "? something wrong. skip this idx." << std::endl; continue; } @@ -73,8 +76,8 @@ bool LinearInterpolate::interpolate( const double dist_to_forward = base_index[i] - idx; const double dist_to_backward = idx - base_index[i - 1]; if (dist_to_forward < 0.0 || dist_to_backward < 0.0) { - std::cerr << "?? something wrong. skip this idx. base_index[i - 1] = " << base_index[i - 1] - << ", idx = " << idx << ", base_index[i] = " << base_index[i] << std::endl; + std::cerr << "?? something wrong. skip this idx. base_index[i - 1] = " << base_index[i - 1] << + ", idx = " << idx << ", base_index[i] = " << base_index[i] << std::endl; continue; } @@ -89,9 +92,9 @@ bool LinearInterpolate::interpolate( * spline interpolation */ -SplineInterpolate::SplineInterpolate(){} -SplineInterpolate::SplineInterpolate(const std::vector & x) { generateSpline(x); } -SplineInterpolate::~SplineInterpolate(){} +SplineInterpolate::SplineInterpolate() {} +SplineInterpolate::SplineInterpolate(const std::vector & x) {generateSpline(x);} +SplineInterpolate::~SplineInterpolate() {} void SplineInterpolate::generateSpline(const std::vector & x) { int N = x.size(); @@ -104,7 +107,9 @@ void SplineInterpolate::generateSpline(const std::vector & x) a_ = x; c_.push_back(0.0); - for (int i = 1; i < N - 1; i++) c_.push_back(3.0 * (a_[i - 1] - 2.0 * a_[i] + a_[i + 1])); + for (int i = 1; i < N - 1; i++) { + c_.push_back(3.0 * (a_[i - 1] - 2.0 * a_[i] + a_[i + 1])); + } c_.push_back(0.0); std::vector w_; @@ -117,7 +122,9 @@ void SplineInterpolate::generateSpline(const std::vector & x) w_.push_back(tmp); } - for (int i = N - 2; i > 0; i--) c_[i] = c_[i] - c_[i + 1] * w_[i]; + for (int i = N - 2; i > 0; i--) { + c_[i] = c_[i] - c_[i + 1] * w_[i]; + } for (int i = 0; i < N - 1; i++) { d_.push_back((c_[i + 1] - c_[i]) / 3.0); @@ -131,9 +138,9 @@ void SplineInterpolate::generateSpline(const std::vector & x) double SplineInterpolate::getValue(const double & s) { - if (!initialized_) return 0.0; + if (!initialized_) {return 0.0;} - int j = std::max(std::min(int(std::floor(s)), (int)a_.size() - 1), 0); + int j = std::max(std::min(static_cast(std::floor(s)), static_cast(a_.size()) - 1), 0); const double ds = s - j; return a_[j] + (b_[j] + (c_[j] + d_[j] * ds) * ds) * ds; } @@ -141,9 +148,9 @@ double SplineInterpolate::getValue(const double & s) void SplineInterpolate::getValueVector( const std::vector & s_v, std::vector & value_v) { - if (!initialized_) return; + if (!initialized_) {return;} value_v.clear(); - for (int i = 0; i < (int)s_v.size(); ++i) { + for (int i = 0; i < static_cast(s_v.size()); ++i) { value_v.push_back(getValue(s_v[i])); } } @@ -153,11 +160,11 @@ bool SplineInterpolate::interpolate( const std::vector & return_index, std::vector & return_value) { auto isIncrease = [](const std::vector & x) { - for (int i = 0; i < (int)x.size() - 1; ++i) { - if (x[i] > x[i + 1]) return false; - } - return true; - }; + for (int i = 0; i < static_cast(x.size()) - 1; ++i) { + if (x[i] > x[i + 1]) {return false;} + } + return true; + }; // for (int i = 0; i < base_index.size(); ++i) // { @@ -172,7 +179,8 @@ bool SplineInterpolate::interpolate( if ( !isIncrease(base_index) || !isIncrease(return_index) || return_index.front() < base_index.front() || base_index.back() < return_index.back() || - base_index.size() != base_value.size()) { + base_index.size() != base_value.size()) + { std::cerr << "[isIncrease] bad index, return false" << std::endl; bool b1 = !isIncrease(base_index); bool b2 = !isIncrease(return_index); @@ -196,8 +204,8 @@ bool SplineInterpolate::interpolate( normalized_idx.push_back(i); continue; } - while (base_index[i] < idx) ++i; - if (i <= 0 || (int)base_index.size() - 1 < i) { + while (base_index[i] < idx) {++i;} + if (i <= 0 || static_cast(base_index.size()) - 1 < i) { std::cerr << "? something wrong. skip this idx." << std::endl; continue; } @@ -218,8 +226,8 @@ bool SplineInterpolate::interpolate( generateSpline(base_value); // interpolate by spline with normalized index - for (int i = 0; i < (int)normalized_idx.size(); ++i) { + for (int i = 0; i < static_cast(normalized_idx.size()); ++i) { return_value.push_back(getValue(normalized_idx[i])); } return true; -} \ No newline at end of file +} diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/src/motion_velocity_optimizer.cpp b/planning/scenario_planning/common/motion_velocity_optimizer/src/motion_velocity_optimizer.cpp index 09982593a66b6..f98acb1e72644 100644 --- a/planning/scenario_planning/common/motion_velocity_optimizer/src/motion_velocity_optimizer.cpp +++ b/planning/scenario_planning/common/motion_velocity_optimizer/src/motion_velocity_optimizer.cpp @@ -13,31 +13,39 @@ // limitations under the License. #include "motion_velocity_optimizer/motion_velocity_optimizer.hpp" + +#include +#include +#include +#include +#include +#include + #include "motion_velocity_optimizer/optimizer/l2_pseudo_jerk_optimizer.hpp" #include "motion_velocity_optimizer/optimizer/linf_pseudo_jerk_optimizer.hpp" #include "tf2_ros/create_timer_ros.h" -#include using std::placeholders::_1; #define UPDATE_PARAM(NAME) update_param(parameters, #NAME, planning_param_.NAME); namespace { -template +template void update_param( const std::vector & parameters, const std::string & name, T & value) { auto it = std::find_if( parameters.cbegin(), parameters.cend(), - [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + [&name](const rclcpp::Parameter & parameter) {return parameter.get_name() == name;}); if (it != parameters.cend()) { value = it->template get_value(); } } } // namespace -MotionVelocityOptimizer::MotionVelocityOptimizer() : Node("motion_velocity_optimizer"), tf_listener_(tf_buffer_) +MotionVelocityOptimizer::MotionVelocityOptimizer() +: Node("motion_velocity_optimizer"), tf_listener_(tf_buffer_) { auto & p = planning_param_; p.max_velocity = declare_parameter("max_velocity", 20.0); // 72.0 kmph @@ -134,7 +142,10 @@ MotionVelocityOptimizer::MotionVelocityOptimizer() : Node("motion_velocity_optim /* parameter update */ set_param_res_ = - this->add_on_set_parameters_callback(std::bind(&MotionVelocityOptimizer::paramCallback, this, _1)); + this->add_on_set_parameters_callback( + std::bind( + &MotionVelocityOptimizer::paramCallback, this, + _1)); /* wait to get vehicle position */ @@ -278,7 +289,10 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityOptimizer::calcTrajectoryV vpu::multiplyConstantToTrajectoryVelocity(-1.0, /* out */ traj_resampled); } - /* Calculate the closest point on the previously planned traj (used to get initial planning speed) */ + /* + * Calculate the closest point on the previously planned traj + * (used to get initial planning speed) + */ int prev_output_closest = vpu::calcClosestWaypoint( prev_output_, current_pose_ptr_->pose, planning_param_.delta_yaw_threshold); RCLCPP_DEBUG( @@ -323,7 +337,8 @@ void MotionVelocityOptimizer::insertBehindVelocity( if ( initialize_type_ == InitializeType::INIT || initialize_type_ == InitializeType::LARGE_DEVIATION_REPLAN || - initialize_type_ == InitializeType::ENGAGING) { + initialize_type_ == InitializeType::ENGAGING) + { output.points.at(i).twist.linear.x = output.points.at(output_closest).twist.linear.x; } else { output.points.at(i).twist.linear.x = prev_output.points.at(j).twist.linear.x; @@ -369,9 +384,11 @@ bool MotionVelocityOptimizer::resampleTrajectory( for (int i = 1; i <= N; ++i) { double ds = ds_nominal; if (i > Nt) { - if (dist_i > planning_param_.min_trajectory_length) + if (dist_i > planning_param_.min_trajectory_length) { break; // planning time is enough and planning distance is over min length. - // if the planning time is not enough to see the desired distance, change the interval distance to see far. + } + // if the planning time is not enough to see the desired distance, + // change the interval distance to see far. ds = std::max(1.0, ds_nominal); } dist_i += ds; @@ -435,8 +452,12 @@ void MotionVelocityOptimizer::calcInitialMotion( return; } - /* if current vehicle velocity is low && base_desired speed is high, use engage_velocity for engage vehicle */ - const double engage_vel_thr = planning_param_.engage_velocity * planning_param_.engage_exit_ratio; + /* + * if current vehicle velocity is low && base_desired speed is high, + * use engage_velocity for engage vehicle + */ + const double engage_vel_thr = + planning_param_.engage_velocity * planning_param_.engage_exit_ratio; if (vehicle_speed < engage_vel_thr) { if (target_vel >= planning_param_.engage_velocity) { int idx = 0; @@ -477,7 +498,6 @@ void MotionVelocityOptimizer::calcInitialMotion( get_logger(), "[calcInitialMotion]: normal update. v0 = %f, a0 = %f, vehicle_speed = %f, target_vel = %f", initial_vel, initial_acc, vehicle_speed, target_vel); - return; } autoware_planning_msgs::msg::Trajectory MotionVelocityOptimizer::optimizeVelocity( @@ -504,7 +524,7 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityOptimizer::optimizeVelocit bool stop_point_exists = vpu::searchZeroVelocityIdx(input, stop_idx); RCLCPP_DEBUG( get_logger(), "[replan]: target_vel = %f, stop_idx = %d, closest = %d, stop_point_exists = %d", - target_vel, stop_idx, input_closest, (int)stop_point_exists); + target_vel, stop_idx, input_closest, static_cast(stop_point_exists)); /* for the endpoint of the trajectory */ if (optimized_traj.points.size() > 0) { @@ -547,7 +567,8 @@ bool MotionVelocityOptimizer::lateralAccelerationFilter( output.points.back().twist = input.points.back().twist; // keep the final speed. constexpr double curvature_calc_dist = 3.0; // [m] calc curvature with 3m away points - const unsigned int idx_dist = std::max((int)(curvature_calc_dist / points_interval), 1); + const unsigned int idx_dist = + std::max(static_cast(curvature_calc_dist / points_interval), 1); /* Calculate curvature assuming the trajectory points interval is constant */ std::vector curvature_v; @@ -582,7 +603,7 @@ bool MotionVelocityOptimizer::externalVelocityLimitFilter( autoware_planning_msgs::msg::Trajectory & output) const { output = input; - if (!external_velocity_limit_filtered_) return false; + if (!external_velocity_limit_filtered_) {return false;} vpu::maximumVelocityFilter(*external_velocity_limit_filtered_, output); RCLCPP_DEBUG( @@ -598,8 +619,9 @@ void MotionVelocityOptimizer::preventMoveToCloseStopLine( } int stop_idx = 0; - if (!vpu::searchZeroVelocityIdx(traj, stop_idx)) return; // no obstacle. - + if (!vpu::searchZeroVelocityIdx(traj, stop_idx)) { + return; // no obstacle. + } /* if the desired stop line is ahead of ego-vehicle */ double dist_to_stopline = vpu::calcDist2d(traj.points.at(stop_idx), traj.points.at(closest)); std::string debug_msg; @@ -625,7 +647,7 @@ bool MotionVelocityOptimizer::extractPathAroundIndex( const double ahead_length = planning_param_.extract_ahead_dist; const double behind_length = planning_param_.extract_behind_dist; - if (input.points.size() == 0 || index < 0 || (int)input.points.size() - 1 < index) { + if (input.points.size() == 0 || index < 0 || static_cast(input.points.size()) - 1 < index) { RCLCPP_WARN( get_logger(), "extractPathAroundIndex failed. input.points.size() = %lu, base_index = %d", input.points.size(), index); @@ -636,7 +658,7 @@ bool MotionVelocityOptimizer::extractPathAroundIndex( // calc ahead distance int ahead_index = input.points.size() - 1; - for (int i = index; i < (int)input.points.size() - 1; ++i) { + for (int i = index; i < static_cast(input.points.size()) - 1; ++i) { dist_sum_tmp += vpu::calcDist2d(input.points.at(i), input.points.at(i + 1)); if (dist_sum_tmp > ahead_length) { ahead_index = i + 1; @@ -681,7 +703,7 @@ void MotionVelocityOptimizer::publishFloat( void MotionVelocityOptimizer::updateExternalVelocityLimit(const double dt) { - if (!external_velocity_limit_ptr_) return; + if (!external_velocity_limit_ptr_) {return;} if (external_velocity_limit_ptr_->data < -1.0e-5) { RCLCPP_WARN(get_logger(), "external velocity limit is negative. The command is ignored"); @@ -716,7 +738,8 @@ void MotionVelocityOptimizer::blockUntilVehiclePositionAvailable(const tf2::Dura { static constexpr auto input = "map", output = "base_link"; while (!tf_buffer_.canTransform(input, output, tf2::TimePointZero) && - rclcpp::ok()) { + rclcpp::ok()) + { RCLCPP_INFO( get_logger(), "waiting %d ms for %s->%s transform to become available", std::chrono::duration_cast(duration).count(), input, output); diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/src/motion_velocity_optimizer_utils.cpp b/planning/scenario_planning/common/motion_velocity_optimizer/src/motion_velocity_optimizer_utils.cpp index b208492a10dda..ae693470e0238 100644 --- a/planning/scenario_planning/common/motion_velocity_optimizer/src/motion_velocity_optimizer_utils.cpp +++ b/planning/scenario_planning/common/motion_velocity_optimizer/src/motion_velocity_optimizer_utils.cpp @@ -12,12 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_velocity_optimizer/interpolate.hpp" #include "motion_velocity_optimizer/motion_velocity_optimizer_utils.hpp" +#include +#include +#include + +#include "motion_velocity_optimizer/interpolate.hpp" + namespace vpu { -double square(const double & a) { return a * a; } +double square(const double & a) {return a * a;} double calcSquaredDist2d(const geometry_msgs::msg::Point & a, const geometry_msgs::msg::Point & b) { return square(a.x - b.x) + square(a.y - b.y); @@ -26,7 +31,9 @@ double calcSquaredDist2d(const geometry_msgs::msg::Pose & a, const geometry_msgs { return square(a.position.x - b.position.x) + square(a.position.y - b.position.y); } -double calcSquaredDist2d(const geometry_msgs::msg::PoseStamped & a, const geometry_msgs::msg::PoseStamped & b) +double calcSquaredDist2d( + const geometry_msgs::msg::PoseStamped & a, + const geometry_msgs::msg::PoseStamped & b) { return square(a.pose.position.x - b.pose.position.x) + square(a.pose.position.y - b.pose.position.y); @@ -47,7 +54,9 @@ double calcDist2d(const geometry_msgs::msg::Pose & a, const geometry_msgs::msg:: { return std::sqrt(calcSquaredDist2d(a, b)); } -double calcDist2d(const geometry_msgs::msg::PoseStamped & a, const geometry_msgs::msg::PoseStamped & b) +double calcDist2d( + const geometry_msgs::msg::PoseStamped & a, + const geometry_msgs::msg::PoseStamped & b) { return std::sqrt(calcSquaredDist2d(a, b)); } @@ -64,7 +73,7 @@ int calcClosestWaypoint( double dist_squared_min = std::numeric_limits::max(); int idx_min = -1; - for (int i = 0; i < (int)traj.points.size(); ++i) { + for (int i = 0; i < static_cast(traj.points.size()); ++i) { const double dx = traj.points.at(i).pose.position.x - point.x; const double dy = traj.points.at(i).pose.position.y - point.y; const double dist_squared = dx * dx + dy * dy; @@ -104,7 +113,9 @@ bool extractPathAroundIndex( const double & ahead_length, const double & behind_length, autoware_planning_msgs::msg::Trajectory & extracted_base_trajectory) { - if (trajectory.points.size() == 0 || (int)trajectory.points.size() - 1 < index || index < 0) { + if (trajectory.points.size() == 0 || static_cast(trajectory.points.size()) - 1 < index || + index < 0) + { return false; } @@ -112,7 +123,7 @@ bool extractPathAroundIndex( // calc ahead distance int ahead_index = trajectory.points.size() - 1; - for (int i = index; i < (int)trajectory.points.size() - 1; ++i) { + for (int i = index; i < static_cast(trajectory.points.size()) - 1; ++i) { dist_sum_tmp += vpu::calcDist2d(trajectory.points.at(i), trajectory.points.at(i + 1)); if (dist_sum_tmp > ahead_length) { ahead_index = i + 1; @@ -131,7 +142,7 @@ bool extractPathAroundIndex( } } - // extruct trajectory + // extract trajectory extracted_base_trajectory.points.clear(); for (int i = behind_index; i < ahead_index + 1; ++i) { extracted_base_trajectory.points.push_back(trajectory.points.at(i)); @@ -144,12 +155,14 @@ bool extractPathAroundIndex( double calcLengthOnWaypoints( const autoware_planning_msgs::msg::Trajectory & path, const int idx1, const int idx2) { - if (idx1 == idx2) // zero distance + if (idx1 == idx2) { // zero distance return 0.0; + } if ( - idx1 < 0 || idx2 < 0 || (int)path.points.size() - 1 < idx1 || - (int)path.points.size() - 1 < idx2) { + idx1 < 0 || idx2 < 0 || static_cast(path.points.size()) - 1 < idx1 || + static_cast(path.points.size()) - 1 < idx2) + { std::cerr << "vpu::calcLengthOnWaypoints(): invalid index" << std::endl; return 0.0; } @@ -194,14 +207,13 @@ void setZeroVelocity(autoware_planning_msgs::msg::Trajectory & trajectory) for (auto & tp : trajectory.points) { tp.twist.linear.x = 0.0; } - return; } double getMaxVelocity(const autoware_planning_msgs::msg::Trajectory & trajectory) { double max_vel = 0.0; for (auto & tp : trajectory.points) { - if (tp.twist.linear.x > max_vel) max_vel = tp.twist.linear.x; + if (tp.twist.linear.x > max_vel) {max_vel = tp.twist.linear.x;} } return max_vel; } @@ -211,26 +223,31 @@ double getMaxAbsVelocity(const autoware_planning_msgs::msg::Trajectory & traject double max_vel = 0.0; for (auto & tp : trajectory.points) { double abs_vel = std::fabs(tp.twist.linear.x); - if (abs_vel > max_vel) max_vel = abs_vel; + if (abs_vel > max_vel) {max_vel = abs_vel;} } return max_vel; } -void mininumVelocityFilter(const double & min_vel, autoware_planning_msgs::msg::Trajectory & trajectory) +void mininumVelocityFilter( + const double & min_vel, + autoware_planning_msgs::msg::Trajectory & trajectory) { for (auto & tp : trajectory.points) { - if (tp.twist.linear.x < min_vel) tp.twist.linear.x = min_vel; + if (tp.twist.linear.x < min_vel) {tp.twist.linear.x = min_vel;} } } -void maximumVelocityFilter(const double & max_vel, autoware_planning_msgs::msg::Trajectory & trajectory) +void maximumVelocityFilter( + const double & max_vel, + autoware_planning_msgs::msg::Trajectory & trajectory) { const double abs_max_vel = std::fabs(max_vel); for (auto & tp : trajectory.points) { - if (tp.twist.linear.x > abs_max_vel) + if (tp.twist.linear.x > abs_max_vel) { tp.twist.linear.x = abs_max_vel; - else if (tp.twist.linear.x < -abs_max_vel) + } else if (tp.twist.linear.x < -abs_max_vel) { tp.twist.linear.x = -abs_max_vel; + } } } void multiplyConstantToTrajectoryVelocity( @@ -244,9 +261,9 @@ void multiplyConstantToTrajectoryVelocity( void insertZeroVelocityAfterIdx( const int & stop_idx, autoware_planning_msgs::msg::Trajectory & trajectory) { - if (stop_idx < 0) return; + if (stop_idx < 0) {return;} - for (int i = stop_idx; i < (int)trajectory.points.size(); ++i) { + for (int i = stop_idx; i < static_cast(trajectory.points.size()); ++i) { trajectory.points.at(i).twist.linear.x = 0.0; } } @@ -273,7 +290,8 @@ bool calcTrajectoryCurvatureFrom3Points( { k_arr.clear(); if (trajectory.points.size() < 2 * idx_dist + 1) { - RCLCPP_DEBUG(rclcpp::get_logger("motion_velocity_optimizer_utils"), + RCLCPP_DEBUG( + rclcpp::get_logger("motion_velocity_optimizer_utils"), "[calcTrajectoryCurvatureFrom3Points] cannot calc curvature idx_dist = %d, trajectory.size() " "= %lu", idx_dist, trajectory.points.size()); @@ -340,7 +358,8 @@ geometry_msgs::msg::Quaternion getQuaternionFromYaw(double yaw) bool linearInterpTrajectory( const std::vector & base_index, - const autoware_planning_msgs::msg::Trajectory & base_trajectory, const std::vector & out_index, + const autoware_planning_msgs::msg::Trajectory & base_trajectory, + const std::vector & out_index, autoware_planning_msgs::msg::Trajectory & out_trajectory) { std::vector px, py, pz, pyaw, tlx, taz, alx, aaz; @@ -367,7 +386,8 @@ bool linearInterpTrajectory( !LinearInterpolate::interpolate(base_index, tlx, out_index, tlx_p) || !LinearInterpolate::interpolate(base_index, taz, out_index, taz_p) || !LinearInterpolate::interpolate(base_index, alx, out_index, alx_p) || - !LinearInterpolate::interpolate(base_index, aaz, out_index, aaz_p)) { + !LinearInterpolate::interpolate(base_index, aaz, out_index, aaz_p)) + { RCLCPP_WARN( rclcpp::get_logger("motion_velocity_optimizer_utils"), "[linearInterpTrajectory] interpolation error!!"); diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/src/optimizer/pseudo_jerk_l2_optimizer.cpp b/planning/scenario_planning/common/motion_velocity_optimizer/src/optimizer/pseudo_jerk_l2_optimizer.cpp index 38b5453cee3d0..f5997ed65ae43 100644 --- a/planning/scenario_planning/common/motion_velocity_optimizer/src/optimizer/pseudo_jerk_l2_optimizer.cpp +++ b/planning/scenario_planning/common/motion_velocity_optimizer/src/optimizer/pseudo_jerk_l2_optimizer.cpp @@ -1,32 +1,35 @@ - -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include #include -#include "eigen3/Eigen/Core" + #include "motion_velocity_optimizer/motion_velocity_optimizer_utils.hpp" #include "motion_velocity_optimizer/optimizer/l2_pseudo_jerk_optimizer.hpp" +#include "eigen3/Eigen/Core" + L2PseudoJerkOptimizer::L2PseudoJerkOptimizer(const L2PseudoJerkOptimizer::OptimizerParam & p) { param_ = p; } -void L2PseudoJerkOptimizer::setAccel(const double max_accel) { param_.max_accel = max_accel; } +void L2PseudoJerkOptimizer::setAccel(const double max_accel) {param_.max_accel = max_accel;} -void L2PseudoJerkOptimizer::setDecel(const double min_decel) { param_.min_decel = min_decel; } +void L2PseudoJerkOptimizer::setDecel(const double min_decel) {param_.min_decel = min_decel;} bool L2PseudoJerkOptimizer::solve( const double initial_vel, const double initial_acc, const int closest, @@ -118,11 +121,11 @@ bool L2PseudoJerkOptimizer::solve( /* design constraint matrix */ // 0 < b - delta < vmax^2 - // NOTE: The delta allows b to be negative. This is actully invalid because the definition is b=v^2. - // But mathematically, the strict b>0 constraint may make the problem infeasible, such as the case of - // v=0 & a<0. To avoid the infesibility, we allow b<0. The negative b is dealt as b=0 when it is - // converted to v with sqrt. If the weight of delta^2 is large (the value of delta is very small), - // b is almost 0, and is not a big problem. + // NOTE: The delta allows b to be negative. This is actually invalid because the definition is + // b=v^2. But mathematically, the strict b>0 constraint may make the problem infeasible, such as + // the case of v=0 & a<0. To avoid the infesibility, we allow b<0. The negative b is dealt as b=0 + // when it is converted to v with sqrt. If the weight of delta^2 is large (the value of delta is + // very small), b is almost 0, and is not a big problem. for (unsigned int i = 0; i < N; ++i) { const int j = 2 * N + i; A(i, i) = 1.0; // b_i @@ -176,7 +179,8 @@ bool L2PseudoJerkOptimizer::solve( auto ts2 = std::chrono::system_clock::now(); const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); - // [b0, b1, ..., bN, | a0, a1, ..., aN, | delta0, delta1, ..., deltaN, | sigma0, sigme1, ..., sigmaN] + // [b0, b1, ..., bN, | a0, a1, ..., aN, | delta0, delta1, ..., deltaN, + //| sigma0, sigma1, ..., sigmaN] const std::vector optval = std::get<0>(result); /* get velocity & acceleration */ @@ -195,14 +199,6 @@ bool L2PseudoJerkOptimizer::solve( output->points.at(i).accel.linear.x = 0.0; } - // -- to check the all optimization variables -- - // RCLCPP_DEBUG(rclcpp::get_logger("L2PseudoJerkOptimizer"),"[after optimize Linf] idx, vel, acc, over_vel, over_acc "); - // for (unsigned int i = 0; i < N; ++i) { - // RCLCPP_DEBUG(rclcpp::get_logger("L2PseudoJerkOptimizer"), - // "i = %d, v: %f, vmax: %f a: %f, b: %f, delta: %f, sigma: %f\n", i, std::sqrt(optval.at(i)), - // vmax[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N)); - // } - auto tf2 = std::chrono::system_clock::now(); double dt_ms2 = std::chrono::duration_cast(tf2 - ts2).count() * 1.0e-6; RCLCPP_DEBUG( diff --git a/planning/scenario_planning/common/motion_velocity_optimizer/src/optimizer/pseudo_jerk_linf_optimizer.cpp b/planning/scenario_planning/common/motion_velocity_optimizer/src/optimizer/pseudo_jerk_linf_optimizer.cpp index df1582b280357..6cbc724d3bd4f 100644 --- a/planning/scenario_planning/common/motion_velocity_optimizer/src/optimizer/pseudo_jerk_linf_optimizer.cpp +++ b/planning/scenario_planning/common/motion_velocity_optimizer/src/optimizer/pseudo_jerk_linf_optimizer.cpp @@ -11,19 +11,24 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include "eigen3/Eigen/Core" +#include +#include + #include "motion_velocity_optimizer/motion_velocity_optimizer_utils.hpp" #include "motion_velocity_optimizer/optimizer/linf_pseudo_jerk_optimizer.hpp" +#include "eigen3/Eigen/Core" + LinfPseudoJerkOptimizer::LinfPseudoJerkOptimizer(const LinfPseudoJerkOptimizer::OptimizerParam & p) { param_ = p; } -void LinfPseudoJerkOptimizer::setAccel(const double max_accel) { param_.max_accel = max_accel; } +void LinfPseudoJerkOptimizer::setAccel(const double max_accel) {param_.max_accel = max_accel;} -void LinfPseudoJerkOptimizer::setDecel(const double min_decel) { param_.min_decel = min_decel; } +void LinfPseudoJerkOptimizer::setDecel(const double min_decel) {param_.min_decel = min_decel;} bool LinfPseudoJerkOptimizer::solve( const double initial_vel, const double initial_acc, const int closest, @@ -107,11 +112,11 @@ bool LinfPseudoJerkOptimizer::solve( /* design constraint matrix */ // 0 < b - delta < vmax^2 - // NOTE: The delta allows b to be negative. This is actully invalid because the definition is b=v^2. - // But mathematically, the strict b>0 constraint may make the problem infeasible, such as the case of - // v=0 & a<0. To avoid the infesibility, we allow b<0. The negative b is dealt as b=0 when it is - // converted to v with sqrt. If the weight of delta^2 is large (the value of delta is very small), - // b is almost 0, and is not a big problem. + // NOTE: The delta allows b to be negative. This is actually invalid because the definition is + // b=v^2. But mathematically, the strict b>0 constraint may make the problem infeasible, such as + // the case of v=0 & a<0. To avoid the infesibility, we allow b<0. The negative b is dealt as b=0 + // when it is converted to v with sqrt. If the weight of delta^2 is large (the value of delta is + // very small), b is almost 0, and is not a big problem. for (unsigned int i = 0; i < N; ++i) { const int j = 2 * N + i; A(i, i) = 1.0; // b_i @@ -185,7 +190,8 @@ bool LinfPseudoJerkOptimizer::solve( auto ts2 = std::chrono::system_clock::now(); const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); - // [b0, b1, ..., bN, | a0, a1, ..., aN, | delta0, delta1, ..., deltaN, | sigma0, sigme1, ..., sigmaN] + // [b0, b1, ..., bN, | a0, a1, ..., aN, | delta0, delta1, ..., deltaN, + // | sigma0, sigma1, ..., sigmaN] const std::vector optval = std::get<0>(result); /* get velocity & acceleration */ @@ -204,14 +210,6 @@ bool LinfPseudoJerkOptimizer::solve( output->points.at(i).accel.linear.x = 0.0; } - // -- to check the all optimization variables -- - // RCLCPP_DEBUG(rclcpp::get_logger("LinfPseudoJerkOptimizer"),"[after optimize Linf] idx, vel, acc, over_vel, over_acc "); - // for (unsigned int i = 0; i < N; ++i) { - // RCLCPP_DEBUG(rclcpp::get_logger("LinfPseudoJerkOptimizer"), - // "i = %d, v: %f, vmax: %f a: %f, b: %f, delta: %f, sigma: %f", i, std::sqrt(optval.at(i)), - // vmax[i], optval.at(i + N), optval.at(i), optval.at(i + 2 * N), optval.at(i + 3 * N)); - // } - auto tf2 = std::chrono::system_clock::now(); double dt_ms2 = std::chrono::duration_cast(tf2 - ts2).count() * 1.0e-6; RCLCPP_DEBUG(