├── include
    └── speed_planner
    │   ├── utils.h
    │   ├── vehicle_info.h
    │   ├── convex_speed_optimizer.h
    │   ├── obstacle.h
    │   ├── collision_checker.h
    │   ├── trajectory.h
    │   └── speed_planner_node.h
├── README.md
├── src
    ├── utils.cpp
    ├── obstacle.cpp
    ├── collision_checker.cpp
    ├── convex_speed_optimizer.cpp
    └── speed_planner_node.cpp
├── CMakeLists.txt
├── launch
    └── speed_planner.launch
└── package.xml


/include/speed_planner/utils.h:
--------------------------------------------------------------------------------
 1 | #ifndef UTILS_H
 2 | #define UTILS_H 
 3 | #include <vector>
 4 | #include <memory>
 5 | #include <cmath>
 6 | #include <limits>
 7 | 
 8 | int getNearestId(const double current_x,
 9 |                  const double current_y,
10 |                  const std::vector<double>& trajectory_x,
11 |                  const std::vector<double>& trajectory_y,
12 |                  const int ini_id = 0);
13 | 
14 | #endif


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # autoware_speed_planner
 2 | 
 3 | ## Overview
 4 | 
 5 | ### Description
 6 | This repository is to create speed planner node for Autoware
 7 | 
 8 | ### Requirement
 9 | 1. ROS Melodic
10 | 
11 | 2. Gurobi 8.1.1
12 | 
13 | ### Usage
14 | 1. `git clone https://github.com/purewater0901/autoware_speed_planner`
15 | 
16 | 2. `colcon build --packages-select speed_planner`
17 | 
18 | 3. `roslaunch speed_planner speed_planner.launch`
19 | 
20 | ### Reference
21 | https://www.researchgate.net/publication/326238347_Toward_a_More_Complete_Flexible_and_Safer_Speed_Planning_for_Autonomous_Driving_via_Convex_Optimization
22 | 


--------------------------------------------------------------------------------
/src/utils.cpp:
--------------------------------------------------------------------------------
 1 | #include "speed_planner/utils.h"
 2 | 
 3 | int getNearestId(const double current_x,
 4 |                  const double current_y,
 5 |                  const std::vector<double>& trajectory_x,
 6 |                  const std::vector<double>& trajectory_y,
 7 |                  const int ini_id)
 8 | {
 9 |     int min_id = -1;
10 |     double min_distance = std::numeric_limits<double>::max();
11 |     if(ini_id>trajectory_x.size())
12 |         return trajectory_x.size()-1;
13 | 
14 |     for(size_t i=ini_id; i<trajectory_x.size(); ++i)
15 |     {
16 |         double tmp_dis = std::sqrt(std::pow(current_x-trajectory_x[i], 2)+std::pow(current_y-trajectory_y[i], 2));
17 |         if(tmp_dis<min_distance)
18 |         {
19 |             min_id = i;
20 |             min_distance = tmp_dis;
21 |         }
22 |     }
23 | 
24 |     if(min_id==-1)
25 |         min_id = 0;
26 | 
27 |     return min_id;
28 | }


--------------------------------------------------------------------------------
/include/speed_planner/vehicle_info.h:
--------------------------------------------------------------------------------
 1 | #ifndef SPEED_PLANNER_VEHICLE_INFO_H
 2 | #define SPEED_PLANNER_VEHICLE_INFO_H
 3 | 
 4 | #include <iostream>
 5 | #include <vector>
 6 | #include <cmath>
 7 | 
 8 | class VehicleInfo
 9 | {
10 | public:
11 |     VehicleInfo(const double length, const double width, const double wheelBase, const double safety_distance)
12 |     : wheel_base_(wheelBase), safety_distance_(safety_distance)
13 |     {
14 |         if(length>width)
15 |         {
16 |             length_ = length;
17 |             width_ = width;
18 |         }
19 |         else
20 |         {
21 |             length_ = width;
22 |             width_ = length;
23 |         }
24 | 
25 |         circumcircle_radius_ = std::sqrt(std::pow(length_/2.0, 2) + std::pow(width_/2.0, 2));    // circumcircle radius
26 |         middlecircle_radius_ = std::sqrt((width_/4)*(width_/4)+(length_/4)*(length_/4));
27 |         footprintcircle_radius_ = std::sqrt(std::pow(width_/4,2)+std::pow(length_/8,2));
28 | 
29 |         middlecircle_deviation_ = length_/8;
30 |         footprintcircle_deviation_ = std::sqrt(std::pow(width_/4,2)+std::pow(3*length_/8, 2));
31 | 
32 |         footprint_deviation_yaw_ = std::atan2(3*length_/8, width_/4);
33 |     }
34 |     ~VehicleInfo() = default;
35 | 
36 |     double length_;
37 |     double width_;
38 |     double wheel_base_;
39 |     double safety_distance_;
40 |     double circumcircle_radius_;
41 |     double middlecircle_radius_;
42 |     double footprintcircle_radius_;
43 |     double middlecircle_deviation_;
44 |     double footprintcircle_deviation_;
45 |     double footprint_deviation_yaw_;
46 | };
47 | 
48 | #endif
49 | 


--------------------------------------------------------------------------------
/include/speed_planner/convex_speed_optimizer.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <iostream>
 4 | #include <vector>
 5 | #include <array>
 6 | #include <cassert>
 7 | #include "speed_planner/trajectory.h"
 8 | #include "speed_planner/collision_checker.h"
 9 | #include "gurobi_c++.h"
10 | 
11 | class ConvexSpeedOptimizer
12 | {
13 |     public:
14 |         ConvexSpeedOptimizer(const double previewDistance, 
15 |                         const double ds, 
16 |                         const double mass,
17 |                         const double mu,
18 |                         std::array<double, 5>& weight);
19 | 
20 |         bool calcOptimizedSpeed(const Trajectory& trajectory,
21 |                                 std::vector<double>& result_speed, 
22 |                                 std::vector<double>& result_acceleration, 
23 |                                 const std::vector<double>& Vr,
24 |                                 const std::vector<double>& Vd,
25 |                                 const std::vector<double>& Arlon,
26 |                                 const std::vector<double>& Arlat,
27 |                                 const std::vector<double>& Aclon,
28 |                                 const std::vector<double>& Aclat,
29 |                                 const double a0,
30 |                                 const bool is_collide,
31 |                                 const std::unique_ptr<CollisionInfo>& collisioin_info,
32 |                                 const double safeTime);
33 | 
34 |         double epsilon_;
35 |         double gravity_;
36 |         double mass_;
37 |         double mu_;
38 | 
39 |         double previewDistance_;
40 |         double ds_;
41 |         std::array<double, 5> weight_;
42 | };


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(speed_planner)
 3 | 
 4 | add_compile_options(-std=c++11)
 5 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -m64 -std=c++11 -g -D_GLIBCXX_USE_CXX11_ABI=0")
 6 | set(GUROBI_INCLUDE_DIRECTORY "/opt/gurobi811/linux64/include")
 7 | set(GUROBI_LIB_DIRECTORY "/opt/gurobi811/linux64/lib")
 8 | 
 9 | find_package(catkin REQUIRED COMPONENTS
10 |   autoware_msgs
11 |   roscpp
12 |   std_msgs
13 |   tf2
14 |   tf2_ros
15 |   vector_map
16 | )
17 | 
18 | find_package(Eigen3 REQUIRED)
19 | 
20 | catkin_package(
21 |   CATKIN_DEPENDS
22 |   roscpp
23 |   std_msgs
24 |   tf2_ros
25 |   #tf2
26 |   autoware_msgs
27 |   vector_map
28 | )
29 | 
30 | include_directories(
31 |   include
32 |   ${catkin_INCLUDE_DIRS}
33 |   ${EIGEN3_INCLUDE_DIR}
34 |   ${GUROBI_INCLUDE_DIRECTORY}
35 | )
36 | 
37 | #link_directories(${GUROBI_LIB_DIRECTORY})
38 | 
39 | add_executable(speed_planner src/speed_planner_node.cpp src/convex_speed_optimizer.cpp src/collision_checker.cpp src/utils.cpp src/obstacle.cpp)
40 | #target_link_libraries (speed_planner gurobi_c++ gurobi81 ${catkin_LIBRARIES} /opt/gurobi811/linux64/lib/libgurobi_g++5.2.a)
41 | target_link_libraries (speed_planner ${catkin_LIBRARIES} /opt/gurobi811/linux64/lib/libgurobi_g++5.2.a /opt/gurobi811/linux64/lib/libgurobi81.so)
42 | add_dependencies(speed_planner ${catkin_EXPORTED_TARGETS})
43 | 
44 | install(TARGETS
45 |         speed_planner
46 |         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
47 |         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
48 |         RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
49 |         )
50 | 
51 | install(DIRECTORY launch/
52 |         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
53 |         PATTERN ".svn" EXCLUDE)
54 | 
55 | 


--------------------------------------------------------------------------------
/include/speed_planner/obstacle.h:
--------------------------------------------------------------------------------
 1 | #ifndef SPEED_PLANNER_OBJECTS_H
 2 | #define SPEED_PLANNER_OBJECTS_H
 3 | 
 4 | #include <vector>
 5 | #include <cmath>
 6 | 
 7 | class Obstacle
 8 | {
 9 | public:
10 |     enum TYPE
11 |     {
12 |         STATIC  = 0,
13 |         DYNAMIC = 1,
14 |     };
15 | 
16 |     Obstacle(const double x, 
17 |              const double y, 
18 |              const double angle, 
19 |              const double radius);
20 | 
21 |     virtual std::vector<std::pair<double, std::pair<double, double>>> getPosition() = 0;
22 | 
23 |     TYPE getType() { return type_; }
24 |     double getRadius() { return radius_; }
25 | 
26 | 
27 |     double x_;
28 |     double y_;
29 |     double angle_;
30 |     double radius_;
31 |     TYPE type_;
32 | };
33 | 
34 | class StaticObstacle : public Obstacle
35 | {
36 | public:
37 |     StaticObstacle(const double x,
38 |                    const double y,
39 |                    const double angle,
40 |                    const double radius);
41 | 
42 |     std::vector<std::pair<double, std::pair<double, double>>> getPosition();
43 | };
44 | 
45 | class DynamicObstacle : public Obstacle
46 | {
47 | public:
48 |     DynamicObstacle(const double x,
49 |                     const double y,
50 |                     const double angle,
51 |                     const double radius,
52 |                     const double translational_velocity,
53 |                     const double predicted_time_horizon,
54 |                     const double dt);
55 | 
56 |     void setFutureTrajectory();
57 |     std::vector<std::pair<double, std::pair<double, double>>> getPosition();
58 | 
59 | private:
60 |     double predicted_time_horizon_;
61 |     double dt_;
62 |     double translationa_velocity_;
63 |     std::vector<std::pair<double, std::pair<double, double>>> predicted_trajectory_;
64 | };
65 | 
66 | #endif
67 | 


--------------------------------------------------------------------------------
/include/speed_planner/collision_checker.h:
--------------------------------------------------------------------------------
 1 | #ifndef SPEED_PLANNER_COLLISION_CHECKER_H
 2 | #define SPEED_PLANNER_COLLISION_CHECKER_H
 3 | 
 4 | #include <cmath>
 5 | #include <vector>
 6 | #include <memory>
 7 | #include "speed_planner/obstacle.h"
 8 | #include "speed_planner/trajectory.h"
 9 | #include "speed_planner/vehicle_info.h"
10 | 
11 | class CollisionInfo
12 | {
13 | public:
14 |     CollisionInfo(const Obstacle::TYPE type, int collision_position_id, double collision_time)
15 |                  : type_(type), collision_position_id_(collision_position_id), collision_time_(collision_time), traversal_time_(10.0)
16 |                  {}
17 | 
18 |     int getId() { return collision_position_id_; }
19 |     double getCollisionTime() { return collision_time_; }
20 |     double getTraversalTime() { return traversal_time_; }
21 |     Obstacle::TYPE getType() { return type_; }
22 | 
23 |     Obstacle::TYPE type_;
24 |     int collision_position_id_;
25 |     double collision_time_;
26 |     double traversal_time_;
27 | };
28 | 
29 | class CollisionChecker
30 | {
31 | public:
32 |     CollisionChecker() = default;
33 |     
34 |     bool check(const Trajectory& trajectory, 
35 |                const std::vector<std::shared_ptr<Obstacle>>& obstacles,
36 |                const std::unique_ptr<VehicleInfo>& ego_vehicle,
37 |                std::unique_ptr<CollisionInfo>& result);
38 | 
39 | private:
40 |     bool static_obstacle_check(const Trajectory& trajectory,
41 |                                const std::shared_ptr<Obstacle>& obstacle,
42 |                                const std::unique_ptr<VehicleInfo>& ego_vehicle,
43 |                                std::unique_ptr<CollisionInfo>& result);
44 | 
45 |     bool dynamic_obstacle_check(const Trajectory& trajectory,
46 |                                const std::shared_ptr<Obstacle>& obstacle,
47 |                                const std::unique_ptr<VehicleInfo>& ego_vehicle,
48 |                                std::unique_ptr<CollisionInfo>& result);
49 | 
50 |     int collision_check(const Trajectory& trajectory,
51 |                         const std::unique_ptr<VehicleInfo>& ego_vehicle,
52 |                         const double obstacle_x,
53 |                         const double obstacle_y,
54 |                         const double obstacle_radius);
55 | };
56 | 
57 | #endif


--------------------------------------------------------------------------------
/launch/speed_planner.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     <arg name="mass" default="1500.0" />
 3 |     <arg name="mu" default="0.8" />
 4 |     <arg name="ds" default="0.1" />
 5 |     <arg name="preview_distance" default="30" />
 6 |     <arg name="callback_dt" default="0.2" />
 7 |     <arg name="time_weight" default="0.0" />
 8 |     <arg name="smooth_weight" default="15.0" />
 9 |     <arg name="velocity_weight" default="15.0" />
10 |     <arg name="longitudinal_slack_weight" default="1.0" />
11 |     <arg name="lateral_slack_weight" default="10.0" />
12 |     <arg name="curvature_weight" default="10" />
13 |     <arg name="decay_factor" default="0.8" />
14 |     <arg name="lateral_g" default="0.4" />
15 |     <arg name="skip_size" default="10" />
16 |     <arg name="smooth_size" default="50" />
17 |     <arg name="vehicle_length" default="5.0" />
18 |     <arg name="vehicle_width" default="1.895" />
19 |     <arg name="vehicle_wheel_base" default="2.790" />
20 |     <arg name="vehicle_safety_distance" default="0.1" />
21 |     <arg name="max_speed" default="5.0" />
22 |     <node pkg="speed_planner" type="speed_planner" name="speed_planner" output="screen">
23 |         <param name="mass" value="$(arg mass)" />
24 |         <param name="mu" value="$(arg mu)" />
25 |         <param name="ds" value="$(arg ds)" />
26 |         <param name="preview_distance" value="$(arg preview_distance)" />
27 |         <param name="callback_dt" value="$(arg callback_dt)" />
28 |         <param name="time_weight" value="$(arg time_weight)" />
29 |         <param name="smooth_weight" value="$(arg smooth_weight)" />
30 |         <param name="velocity_weight" value="$(arg velocity_weight)" />
31 |         <param name="longitudinal_slack_weight" value="$(arg longitudinal_slack_weight)" />
32 |         <param name="lateral_slack_weight" value="$(arg lateral_slack_weight)" />
33 |         <param name="curvature_weight" value="$(arg curvature_weight)" />
34 |         <param name="decay_factor" value="$(arg decay_factor)" />
35 |         <param name="lateral_g" value="$(arg lateral_g)" />
36 |         <param name="skip_size" value="$(arg skip_size)" />
37 |         <param name="smooth_size" value="$(arg smooth_size)" />
38 |         <param name="vehicle_length" value="$(arg vehicle_length)" />
39 |         <param name="vehicle_width" value="$(arg vehicle_width)" />
40 |         <param name="vehicle_wheel_base" value="$(arg vehicle_wheel_base)" />
41 |         <param name="vehicle_safety_distance" value="$(arg vehicle_safety_distance)" />
42 |         <param name="max_speed" value="$(arg max_speed)" />
43 |     </node>
44 | </launch>
45 | 


--------------------------------------------------------------------------------
/src/obstacle.cpp:
--------------------------------------------------------------------------------
 1 | #include "speed_planner/obstacle.h"
 2 | 
 3 | Obstacle::Obstacle(const double x, 
 4 |                    const double y, 
 5 |                    const double angle, 
 6 |                    const double radius) 
 7 |                    : x_(x), y_(y), angle_(angle), radius_(radius)
 8 | {
 9 | }
10 | 
11 | StaticObstacle::StaticObstacle(const double x,
12 |                                const double y,
13 |                                const double angle,
14 |                                const double radius)
15 |                                : Obstacle(x, y, angle, radius)
16 | {
17 |     type_ = STATIC;
18 | }
19 | 
20 | std::vector<std::pair<double, std::pair<double, double>>> StaticObstacle::getPosition()
21 | {
22 |     std::vector<std::pair<double, std::pair<double, double>>> position;
23 |     position.resize(1);
24 | 
25 |     position[0].first = 0.0;
26 |     position[0].second.first  = x_;
27 |     position[0].second.second = y_;
28 | 
29 |     return position;
30 | }
31 | 
32 | DynamicObstacle::DynamicObstacle(const double x,
33 |                                  const double y,
34 |                                  const double angle,
35 |                                  const double radius,
36 |                                  const double translational_velocity,
37 |                                  const double predicted_time_horizon,
38 |                                  const double dt)
39 |                                 : Obstacle(x, y, angle, radius), 
40 |                                   translationa_velocity_(translational_velocity),
41 |                                   predicted_time_horizon_(predicted_time_horizon),
42 |                                   dt_(dt)
43 | {
44 |     type_ = DYNAMIC;
45 |     setFutureTrajectory();
46 | }
47 | 
48 | void DynamicObstacle::setFutureTrajectory()
49 | {
50 |     int N = int(predicted_time_horizon_/dt_);
51 |     predicted_trajectory_.resize(N);
52 | 
53 |     predicted_trajectory_[0].first = 0.0;
54 |     predicted_trajectory_[0].second.first  = x_;
55 |     predicted_trajectory_[0].second.second = y_;
56 |     for(int i=1; i<N; ++i)
57 |     {
58 |         predicted_trajectory_[i].first = predicted_trajectory_[i-1].first + dt_;
59 |         predicted_trajectory_[i].second.first  = predicted_trajectory_[i-1].second.first  + translationa_velocity_*cos(angle_)*dt_;
60 |         predicted_trajectory_[i].second.second = predicted_trajectory_[i-1].second.second + translationa_velocity_*sin(angle_)*dt_;
61 |     }
62 | }
63 | 
64 | std::vector<std::pair<double, std::pair<double, double>>> DynamicObstacle::getPosition()
65 | {
66 |     return predicted_trajectory_;
67 | }


--------------------------------------------------------------------------------
/include/speed_planner/trajectory.h:
--------------------------------------------------------------------------------
 1 | #ifndef TRAJECTORY_ROS_H
 2 | #define TRAJECTORY_ROS_H 
 3 | 
 4 | #include <vector>
 5 | #include <cassert>
 6 | 
 7 | class Trajectory
 8 | {
 9 | public:
10 |     Trajectory() {};
11 |     Trajectory(const std::vector<double>& current_trajectory_x, 
12 |                const std::vector<double>& current_trajectory_y,
13 |                const std::vector<double>& current_trajectory_yaw,
14 |                const std::vector<double>& current_trajectory_curvature,
15 |                const std::vector<double>& calculated_velocity, 
16 |                const std::vector<double>& calculated_acceleration)
17 |     {
18 |         assert(current_trajectory_x.size() == calculated_velocity.size());
19 |         assert(current_trajectory_y.size() == calculated_velocity.size());
20 |         assert(current_trajectory_yaw.size() == calculated_velocity.size());
21 |         assert(current_trajectory_curvature.size() == calculated_velocity.size());
22 |         assert(calculated_acceleration.size() == calculated_velocity.size());
23 | 
24 |         x_.resize(current_trajectory_x.size());
25 |         y_.resize(current_trajectory_y.size());
26 |         yaw_.resize(current_trajectory_y.size());
27 |         curvature_.resize(current_trajectory_y.size());
28 |         velocity_.resize(calculated_velocity.size());
29 |         acceleration_.resize(calculated_acceleration.size());
30 | 
31 |         for(int i=0; i<current_trajectory_x.size(); ++i)
32 |         {
33 |             x_[i] = current_trajectory_x[i];
34 |             y_[i] = current_trajectory_y[i];
35 |             yaw_[i] = current_trajectory_yaw[i];
36 |             curvature_[i] = current_trajectory_curvature[i];
37 |         }
38 | 
39 |         for(int i=0; i<velocity_.size(); ++i)
40 |         {
41 |             velocity_[i] = calculated_velocity[i];
42 |             acceleration_[i] = calculated_acceleration[i];
43 |         }
44 |     }
45 | 
46 |     Trajectory(const std::vector<double>& current_trajectory_x, 
47 |                const std::vector<double>& current_trajectory_y,
48 |                const std::vector<double>& current_trajectory_yaw,
49 |                const std::vector<double>& current_trajectory_curvature,
50 |                const int nearest_id)
51 |     {
52 |         int required_size = current_trajectory_x.size()-nearest_id;
53 |         x_.reserve(required_size);
54 |         y_.reserve(required_size);
55 |         yaw_.reserve(required_size);
56 |         curvature_.reserve(required_size);
57 | 
58 |         for(int i=nearest_id; i<current_trajectory_x.size(); ++i)
59 |         {
60 |             x_.push_back(current_trajectory_x[i]);
61 |             y_.push_back(current_trajectory_y[i]);
62 |             yaw_.push_back(current_trajectory_yaw[i]);
63 |             curvature_.push_back(current_trajectory_curvature[i]);
64 |         }
65 |     }
66 | 
67 | 
68 |     ~Trajectory() = default;
69 | 
70 |     std::vector<double> x_;
71 |     std::vector<double> y_;
72 |     std::vector<double> yaw_;
73 |     std::vector<double> curvature_;
74 |     std::vector<double> velocity_;
75 |     std::vector<double> acceleration_;
76 | };
77 | 
78 | #endif //TRAJECTORY_ROS_H


--------------------------------------------------------------------------------
/include/speed_planner/speed_planner_node.h:
--------------------------------------------------------------------------------
 1 | #ifndef SPEED_PLANNER_NODE_ROS_H
 2 | #define SPEED_PLANNER_NODE_ROS_H
 3 | 
 4 | #include <autoware_msgs/Lane.h>
 5 | #include <autoware_msgs/VehicleStatus.h>
 6 | #include <autoware_msgs/DetectedObjectArray.h>
 7 | #include <ros/ros.h>
 8 | #include <Eigen/Eigen>
 9 | #include <vector>
10 | #include <iostream>
11 | #include <random>
12 | #include <chrono>
13 | #include <cstring>
14 | #include <memory>
15 | #include <visualization_msgs/MarkerArray.h>
16 | #include <visualization_msgs/Marker.h>
17 | #include <tf/transform_datatypes.h>
18 | #include <tf2_ros/transform_listener.h>
19 | #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
20 | #include <std_msgs/Float64.h>
21 | #include <std_msgs/Float32.h>
22 | #include "speed_planner/convex_speed_optimizer.h"
23 | #include "speed_planner/obstacle.h"
24 | #include "speed_planner/vehicle_info.h"
25 | #include "speed_planner/collision_checker.h"
26 | #include "speed_planner/trajectory.h"
27 | #include "speed_planner/utils.h"
28 | 
29 | class SpeedPlannerNode
30 | {
31 |     public:
32 |         SpeedPlannerNode();
33 |         ~SpeedPlannerNode() = default;
34 | 
35 |     private:
36 |         ros::NodeHandle nh_, private_nh_;
37 |         ros::Publisher optimized_waypoints_pub_;
38 |         ros::Publisher result_velocity_pub_;
39 |         ros::Publisher optimized_waypoints_debug_;
40 |         ros::Publisher desired_velocity_pub_;
41 |         ros::Subscriber current_status_sub_;
42 |         ros::Subscriber current_pose_sub_;
43 |         ros::Subscriber current_velocity_sub_;
44 |         ros::Subscriber final_waypoints_sub_;
45 |         ros::Subscriber objects_sub_;
46 | 
47 |         ros::Timer timer_;
48 | 
49 |         std::unique_ptr<tf2_ros::Buffer> tf2_buffer_ptr_;
50 |         std::unique_ptr<tf2_ros::TransformListener> tf2_listner_ptr_;
51 |   
52 |         std::unique_ptr<autoware_msgs::Lane> in_lane_ptr_;
53 |         std::unique_ptr<autoware_msgs::VehicleStatus> in_status_ptr_;
54 |         std::unique_ptr<geometry_msgs::PoseStamped> in_pose_ptr_;
55 |         std::unique_ptr<geometry_msgs::TwistStamped> in_twist_ptr_;
56 |         std::unique_ptr<geometry_msgs::PoseStamped> in_nav_goal_ptr_;
57 |         std::unique_ptr<autoware_msgs::DetectedObjectArray> in_objects_ptr_;
58 |   
59 |         std::unique_ptr<ConvexSpeedOptimizer> speedOptimizer_;
60 |         std::unique_ptr<VehicleInfo> ego_vehicle_ptr_;
61 |         std::unique_ptr<CollisionChecker> collision_checker_ptr_;
62 |         std::unique_ptr<Trajectory> previous_trajectory_;
63 | 
64 |         void waypointsCallback(const autoware_msgs::Lane& msg);
65 |         void objectsCallback(const autoware_msgs::DetectedObjectArray& msg);
66 |         void currentStatusCallback(const autoware_msgs::VehicleStatus& msg);
67 |         void currentPoseCallback(const geometry_msgs::PoseStamped& msg);
68 |         void currentVelocityCallback(const geometry_msgs::TwistStamped& msg);
69 |         void timerCallback(const ros::TimerEvent &e);
70 | 
71 |         double curvatureWeight_;
72 |         double decayFactor_;
73 |         double previousVelocity_;
74 |         double timer_callback_dt_;
75 |         double lateral_g_;
76 |         double max_speed_;
77 |         int skip_size_;
78 |         int smooth_size_;
79 | };
80 | 
81 | #endif


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>speed_planner</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The speed_planner package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="yutaka@todo.todo">yutaka</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but multiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/speed_planner</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, multiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintainers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35 |   <!--   <depend>roscpp</depend> -->
36 |   <!--   Note that this is equivalent to the following: -->
37 |   <!--   <build_depend>roscpp</build_depend> -->
38 |   <!--   <exec_depend>roscpp</exec_depend> -->
39 |   <!-- Use build_depend for packages you need at compile time: -->
40 |   <!--   <build_depend>message_generation</build_depend> -->
41 |   <!-- Use build_export_depend for packages you need in order to build against this package: -->
42 |   <!--   <build_export_depend>message_generation</build_export_depend> -->
43 |   <!-- Use buildtool_depend for build tool packages: -->
44 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
45 |   <!-- Use exec_depend for packages you need at runtime: -->
46 |   <!--   <exec_depend>message_runtime</exec_depend> -->
47 |   <!-- Use test_depend for packages you need only for testing: -->
48 |   <!--   <test_depend>gtest</test_depend> -->
49 |   <!-- Use doc_depend for packages you need only for building documentation: -->
50 |   <!--   <doc_depend>doxygen</doc_depend> -->
51 |   <buildtool_depend>catkin</buildtool_depend>
52 |   <build_depend>autoware_msgs</build_depend>
53 |   <build_depend>roscpp</build_depend>
54 |   <build_depend>std_msgs</build_depend>
55 |   <build_depend>tf2</build_depend>
56 |   <build_depend>tf2_ros</build_depend>
57 |   <build_depend>vector_map</build_depend>
58 |   <build_export_depend>autoware_msgs</build_export_depend>
59 |   <build_export_depend>roscpp</build_export_depend>
60 |   <build_export_depend>std_msgs</build_export_depend>
61 |   <build_export_depend>tf2</build_export_depend>
62 |   <build_export_depend>tf2_ros</build_export_depend>
63 |   <build_export_depend>vector_map</build_export_depend>
64 |   <exec_depend>autoware_msgs</exec_depend>
65 |   <exec_depend>roscpp</exec_depend>
66 |   <exec_depend>std_msgs</exec_depend>
67 |   <exec_depend>tf2</exec_depend>
68 |   <exec_depend>tf2_ros</exec_depend>
69 |   <exec_depend>vector_map</exec_depend>
70 | 
71 | 
72 |   <!-- The export tag contains other, unspecified, tags -->
73 |   <export>
74 |     <!-- Other tools can request additional information be placed here -->
75 | 
76 |   </export>
77 | </package>
78 | 


--------------------------------------------------------------------------------
/src/collision_checker.cpp:
--------------------------------------------------------------------------------
  1 | #include "speed_planner/collision_checker.h"
  2 | 
  3 | bool CollisionChecker::check(const Trajectory& trajectory, 
  4 |                              const std::vector<std::shared_ptr<Obstacle>>& obstacles,
  5 |                              const std::unique_ptr<VehicleInfo>& ego_vehicle,
  6 |                              std::unique_ptr<CollisionInfo>& result)
  7 | {
  8 |     if(obstacles.empty())
  9 |         return false;
 10 | 
 11 |     bool is_collide = false;
 12 |     for(int i=0; i<obstacles.size(); ++i)
 13 |     {
 14 |         Obstacle::TYPE obstacle_type = obstacles[i]->getType(); 
 15 |         if(obstacle_type == Obstacle::TYPE::STATIC)
 16 |             is_collide = static_obstacle_check(trajectory, obstacles[i], ego_vehicle, result);
 17 |         else if(obstacle_type == Obstacle::TYPE::DYNAMIC)
 18 |             is_collide = dynamic_obstacle_check(trajectory, obstacles[i], ego_vehicle, result);
 19 |     }
 20 | 
 21 |     return is_collide;
 22 | }
 23 | 
 24 | int CollisionChecker::collision_check(const Trajectory& trajectory,
 25 |                                       const std::unique_ptr<VehicleInfo>& ego_vehicle,
 26 |                                       const double obstacle_x,
 27 |                                       const double obstacle_y,
 28 |                                       const double obstacle_radius)
 29 | {
 30 |     int check_interval_size = 1.0/0.1;
 31 |     for(size_t i=0; i<trajectory.x_.size(); i+=check_interval_size)
 32 |     {
 33 |         //step1 check the largest radius around the vehicle
 34 |         double x = trajectory.x_[i];  //vehicle center's x coordinate 
 35 |         double y = trajectory.y_[i];  //vehicle center's y coordinate
 36 |         double yaw = trajectory.yaw_[i];
 37 |         double dist = std::sqrt(std::pow((x - obstacle_x), 2) + std::pow((y - obstacle_y), 2));
 38 |         double clearance_radius = ego_vehicle->circumcircle_radius_+ego_vehicle->safety_distance_; //largest_circle + safety_distance
 39 | 
 40 |         if(dist <= obstacle_radius + clearance_radius)
 41 |         {
 42 |             //step2 check the middle circles
 43 |             double middle_clearance_radius = ego_vehicle->middlecircle_radius_ + ego_vehicle->safety_distance_;
 44 |             for(size_t mcircleId=0; mcircleId<2; ++mcircleId)
 45 |             {
 46 |                 double xmc = x + ego_vehicle->middlecircle_deviation_*cos(yaw+mcircleId*M_PI);
 47 |                 double ymc = y + ego_vehicle->middlecircle_deviation_*sin(yaw+mcircleId*M_PI);
 48 |                 double middleDist = std::sqrt(std::pow(xmc-obstacle_x, 2)+std::pow(ymc-obstacle_y, 2));
 49 | 
 50 |                 if(middleDist <= obstacle_radius + middle_clearance_radius)
 51 |                 {
 52 |                     std::cout << "Collide with Middle Circle" << std::endl;
 53 |                     return i; //collision
 54 |                 }
 55 |             }
 56 | 
 57 |             //step3 check the 4 footprint circles
 58 |             double footprint_clearance_radius = ego_vehicle->footprintcircle_radius_ + ego_vehicle->safety_distance_;
 59 |             for(size_t fcircleId=0; fcircleId<4; ++fcircleId)
 60 |             {
 61 |                 double xfc;
 62 |                 double yfc;
 63 |                 if(fcircleId==0)
 64 |                 {
 65 |                    xfc = x + ego_vehicle->footprintcircle_deviation_*cos(yaw-M_PI/2+ego_vehicle->footprint_deviation_yaw_);
 66 |                    yfc = y + ego_vehicle->footprintcircle_deviation_*sin(yaw-M_PI/2+ego_vehicle->footprint_deviation_yaw_);
 67 |                 }
 68 |                 else if(fcircleId==1)
 69 |                 {
 70 |                    xfc = x + ego_vehicle->footprintcircle_deviation_*cos(yaw+M_PI/2-ego_vehicle->footprint_deviation_yaw_);
 71 |                    yfc = y + ego_vehicle->footprintcircle_deviation_*sin(yaw+M_PI/2-ego_vehicle->footprint_deviation_yaw_);
 72 |                 }
 73 |                 else if(fcircleId==2)
 74 |                 {
 75 |                    xfc = x + ego_vehicle->footprintcircle_deviation_*cos(yaw+M_PI/2+ego_vehicle->footprint_deviation_yaw_);
 76 |                    yfc = y + ego_vehicle->footprintcircle_deviation_*sin(yaw+M_PI/2+ego_vehicle->footprint_deviation_yaw_);
 77 |                 }
 78 |                 else if(fcircleId==3)
 79 |                 {
 80 |                     xfc = x + ego_vehicle->footprintcircle_deviation_*cos(yaw+3*M_PI/2-ego_vehicle->footprint_deviation_yaw_);
 81 |                     yfc = y + ego_vehicle->footprintcircle_deviation_*sin(yaw+3*M_PI/2-ego_vehicle->footprint_deviation_yaw_);
 82 |                 }
 83 | 
 84 |                 double footprintDist = std::sqrt(std::pow(xfc-obstacle_x, 2)+std::pow(yfc-obstacle_y, 2));
 85 | 
 86 |                 if(footprintDist<= obstacle_radius + footprint_clearance_radius)
 87 |                 {
 88 |                     std::cout << "Collide with Footprint Circle" << std::endl;
 89 |                     return i;
 90 |                 }
 91 |             }
 92 |         }
 93 |     }
 94 | 
 95 |     return -1;
 96 | }
 97 | 
 98 | bool CollisionChecker::static_obstacle_check(const Trajectory& trajectory,
 99 |                                              const std::shared_ptr<Obstacle>& obstacle,
100 |                                              const std::unique_ptr<VehicleInfo>& ego_vehicle,
101 |                                              std::unique_ptr<CollisionInfo>& result)
102 | {
103 |     std::cout << "This is Static Obstacle" << std::endl;
104 |     double obstacle_x = obstacle->getPosition().begin()->second.first;
105 |     double obstacle_y = obstacle->getPosition().begin()->second.second;
106 |     double obstacle_radius = obstacle->getRadius();
107 | 
108 |     int collision_id = collision_check(trajectory, ego_vehicle, obstacle_x, obstacle_y, obstacle_radius);
109 | 
110 |     if(collision_id>=0)
111 |     {
112 |         result.reset(new CollisionInfo(Obstacle::TYPE::STATIC, collision_id, 0.0));
113 |         return true;
114 |     }
115 | 
116 |     return false;
117 | }
118 | 
119 | bool CollisionChecker::dynamic_obstacle_check(const Trajectory& trajectory,
120 |                                               const std::shared_ptr<Obstacle>& obstacle,
121 |                                               const std::unique_ptr<VehicleInfo>& ego_vehicle,
122 |                                               std::unique_ptr<CollisionInfo>& result)
123 | {
124 |     std::cout << "This is Dynamic Obstacle" << std::endl;
125 |     double obstacle_radius = obstacle->getRadius();
126 | 
127 |     std::vector<std::pair<double, std::pair<double, double>>> obstacle_trajectory = obstacle->getPosition();
128 |     for(int i=0 ;i<obstacle_trajectory.size(); ++i)
129 |     {
130 |         double obstacle_x = obstacle_trajectory[i].second.first;
131 |         double obstacle_y = obstacle_trajectory[i].second.second;
132 | 
133 |         int collision_id = collision_check(trajectory, ego_vehicle, obstacle_x, obstacle_y, obstacle_radius);
134 |         if(collision_id>=0)
135 |         {
136 |             result.reset(new CollisionInfo(Obstacle::TYPE::DYNAMIC, collision_id, obstacle_trajectory[i].first));
137 |             return true;
138 |         }
139 |     }
140 | 
141 |     return false;
142 | }


--------------------------------------------------------------------------------
/src/convex_speed_optimizer.cpp:
--------------------------------------------------------------------------------
  1 | #include "speed_planner/convex_speed_optimizer.h"
  2 | 
  3 | ConvexSpeedOptimizer::ConvexSpeedOptimizer
  4 |                        (const double previewDistance, 
  5 |                         const double ds, 
  6 |                         const double mass,
  7 |                         const double mu,
  8 |                         std::array<double, 5>& weight)
  9 | : previewDistance_(previewDistance), 
 10 |   ds_(ds), 
 11 |   mass_(mass), 
 12 |   mu_(mu), 
 13 |   gravity_(9.83), 
 14 |   epsilon_(1e-6),
 15 |   weight_(weight)
 16 | {
 17 |     if(ds_<1e-10)
 18 |         ds_ = 0.1;
 19 | }
 20 | 
 21 | bool ConvexSpeedOptimizer::calcOptimizedSpeed(const Trajectory& trajectory,
 22 |                                               std::vector<double>& result_speed, 
 23 |                                               std::vector<double>& result_acceleration, 
 24 |                                               const std::vector<double>& Vr,
 25 |                                               const std::vector<double>& Vd,
 26 |                                               const std::vector<double>& Arlon,
 27 |                                               const std::vector<double>& Arlat,
 28 |                                               const std::vector<double>& Aclon,
 29 |                                               const std::vector<double>& Aclat,
 30 |                                               const double a0,
 31 |                                               const bool is_collide,
 32 |                                               const std::unique_ptr<CollisionInfo>& collision_info,
 33 |                                               const double safeTime)
 34 | {
 35 |     int N = trajectory.x_.size();
 36 |     int variableSize = 8*N-1+2*(N-1);
 37 | 
 38 |     assert(result_speed.size() == N);
 39 |     assert(result_acceleration.size() == N);
 40 | 
 41 |     try
 42 |     {
 43 |         /* Create Environment */
 44 |         GRBEnv env = GRBEnv();
 45 |         GRBModel model = GRBModel(env);
 46 |         model.set(GRB_IntParam_OutputFlag, 0);
 47 | 
 48 |         //Create Variables
 49 |         std::vector<GRBVar> variables;
 50 |         variables.resize(variableSize);
 51 | 
 52 |         variables[0] = model.addVar(Vd[0]*Vd[0], Vd[0]*Vd[0], 0.0, GRB_CONTINUOUS, "b"+std::to_string(0));
 53 |         for(int i=1; i<N; ++i)
 54 |             variables[i] = model.addVar(0.0, (Vr[i])*(Vr[i]), 0.0, GRB_CONTINUOUS, "b"+std::to_string(i));
 55 | 
 56 |         variables[N] = model.addVar(a0, a0, 0.0, GRB_CONTINUOUS, "a0");
 57 |         for(int i=N+1; i<2*N-1; ++i)
 58 |             variables[i] = model.addVar(-Arlon[i-N], Arlon[i-N], 0.0, GRB_CONTINUOUS, "a"+std::to_string(i-N));
 59 |         variables[2*N-1] = model.addVar(0.0, 0.0, 0.0, GRB_CONTINUOUS, "a"+std::to_string(N-1));
 60 | 
 61 |         for(int i=2*N; i<4*N; ++i)
 62 |             variables[i] = model.addVar(0.0, GRB_INFINITY, 0.0, GRB_CONTINUOUS, "slack"+std::to_string(i-2*N));
 63 | 
 64 |         for(int i=4*N; i<6*N; ++i)
 65 |             variables[i] = model.addVar(-GRB_INFINITY, GRB_INFINITY, 0.0, GRB_CONTINUOUS, "u"+std::to_string(i-4*N));
 66 | 
 67 |         for(int i=6*N; i<7*N; ++i)
 68 |             variables[i] = model.addVar(epsilon_, GRB_INFINITY, 0.0, GRB_CONTINUOUS, "cv"+std::to_string(i-6*N));
 69 | 
 70 |         for(int i=7*N; i<8*N-1; ++i)
 71 |             variables[i] = model.addVar(-GRB_INFINITY, GRB_INFINITY, 0.0, GRB_CONTINUOUS, "d"+std::to_string(i-7*N));
 72 | 
 73 |         for(int i=8*N-1; i<8*N-1+(N-1)*2; ++i)
 74 |             variables[i] = model.addVar(0.0, GRB_INFINITY, 0.0, GRB_CONTINUOUS, "spe"+std::to_string(i-(8*N-1)));
 75 | 
 76 |         //set objective
 77 |         GRBLinExpr  Jt = 0.0;
 78 |         GRBQuadExpr Js = 0.0;
 79 |         GRBQuadExpr Jv = 0.0;
 80 |         GRBQuadExpr JLonSlack = 0.0;
 81 |         GRBQuadExpr JLatSlack = 0.0;
 82 |         for(size_t i=0; i<N-1; ++i)
 83 |         {
 84 |             Jt += 2*ds_*variables[i+7*N];
 85 |             Js += (variables[i+N+1]-variables[i+N])*(variables[i+N+1]-variables[i+N])/ds_;
 86 |             Jv += (variables[i] - Vd[i]*Vd[i])*(variables[i]-Vd[i]*Vd[i])*ds_;
 87 |             JLonSlack += variables[i+2*N] * variables[i+2*N];
 88 |             JLatSlack += variables[i+3*N] * variables[i+3*N];
 89 |         }
 90 |         Jv += (variables[N-1] - Vd[N-1]*Vd[N-1])*(variables[N-1] - Vd[N-1]*Vd[N-1])*ds_;
 91 |         JLonSlack += variables[3*N-1] * variables[3*N-1];
 92 |         JLatSlack += variables[4*N-1] * variables[4*N-1];
 93 | 
 94 |         //model.setObjective(weight_[0]*Jt+weight_[1]*Js+weight_[2]*Jv+weight_[3]*JLonSlack+weight_[4]*JLatSlack, GRB_MINIMIZE);
 95 |         model.setObjective(weight_[1]*Js+weight_[2]*Jv+weight_[3]*JLonSlack+weight_[4]*JLatSlack, GRB_MINIMIZE);
 96 | 
 97 |         /* constraint1 speed and acceleration constraints */
 98 |         for(int i=0; i<N-1; ++i)
 99 |             model.addConstr((variables[i+1]-variables[i])/ds_ - 2*variables[i+N] == 0, "c"+std::to_string(i));
100 | 
101 |         // constraints2 longitudinal and lateral acceleration
102 |         // longitudinal comfort constraints
103 |         for(int i=0; i<N; ++i)
104 |         {
105 |             model.addConstr(Aclon[i]+variables[i+2*N] >= variables[i+N], "absconstr"+std::to_string(i));
106 |             model.addConstr(-(Aclon[i]+variables[i+2*N]) <= variables[i+N], "absconstr"+std::to_string(i+N));
107 |         }
108 | 
109 |         // lateral comfort constraints
110 |         for(int i=0; i<N; ++i)
111 |         {
112 |             model.addConstr(mass_*(Aclat[i]+variables[i+3*N]) >= variables[i+5*N], "c"+std::to_string(i+N));
113 |             model.addConstr(-mass_*(Aclat[i]+variables[i+3*N]) <= variables[i+5*N], "c"+std::to_string(i+2*N));
114 |         }
115 | 
116 |         // constraints3 friction circle
117 |         for(int i=0; i<N; ++i)
118 |         {
119 |             model.addQConstr(variables[i+4*N]*variables[i+4*N]+variables[i+5*N]*variables[i+5*N]<=(mu_*mass_*gravity_)*(mu_*mass_*gravity_),
120 |                             "qcfc"+std::to_string(i));
121 |             model.addConstr(variables[i+4*N]<=mass_*Arlon[i], "c"+std::to_string(i+3*N));
122 |         }
123 | 
124 |         // constraint4  dynamics constraints
125 |         for(int i=2; i<N-1; ++i)
126 |         {
127 |             double rx1 = (trajectory.x_[i+1]-trajectory.x_[i])/ds_;
128 |             double rx2 = (trajectory.x_[i-2]-trajectory.x_[i-1]-trajectory.x_[i]+trajectory.x_[i+1])/(2*ds_*ds_);
129 |             model.addConstr(variables[i+4*N]*cos(trajectory.yaw_[i]) - variables[i+5*N]*sin(trajectory.yaw_[i]) ==
130 |                            (rx2 * variables[i] + rx1 * variables[i+N]), "c"+std::to_string(i+4*N));
131 | 
132 |             double ry1 = (trajectory.y_[i+1]-trajectory.y_[i])/ds_;
133 |             double ry2 = (trajectory.y_[i-2]-trajectory.y_[i-1]-trajectory.y_[i]+trajectory.y_[i+1])/(2*ds_*ds_);
134 |             model.addConstr(variables[i+4*N]*sin(trajectory.yaw_[i]) + variables[i+5*N]*cos(trajectory.yaw_[i]) ==
135 |                            (ry2 * variables[i] + ry1 * variables[i+N]), "c"+std::to_string(i+5*N-3));
136 |         }
137 | 
138 |         /* constraints5 time window constraints */
139 |         if(is_collide)
140 |         {
141 |             if(collision_info->getType() == Obstacle::TYPE::DYNAMIC)
142 |             {
143 |                 for(int i=0; i<N-1; ++i)
144 |                 {
145 |                     model.addConstr(variables[i+(8*N-1)]==variables[i+6*N+1]+variables[i+6*N]+epsilon_-variables[i+7*N], "se1"+std::to_string(i));
146 |                     model.addConstr(variables[i+(9*N-2)]==variables[i+6*N+1]+variables[i+6*N]+epsilon_+variables[i+7*N], "se2"+std::to_string(i));
147 |                     model.addQConstr(4+variables[i+(8*N-1)]*variables[i+(8*N-1)]<=variables[i+(9*N-2)]*variables[i+(9*N-2)], "qc_time_window"+std::to_string(i));
148 |                 }
149 | 
150 |                 for(int i=0; i<N; ++i)
151 |                     model.addQConstr(variables[i]>=variables[i+6*N]*variables[i+6*N], "qc"+std::to_string(i));
152 | 
153 |                 GRBLinExpr timeWindowLin=0.0;
154 |                 double collision_time = collision_info->getCollisionTime()-1;
155 |                 if(collision_time<0)
156 |                     collision_time = 0.0;
157 |                 double traversal_time = collision_info->getTraversalTime();
158 |                 for(int i=0; i<collision_info->getId(); ++i)
159 |                     timeWindowLin += 2*ds_*variables[i+7*N];
160 |                 model.addConstr(timeWindowLin<=collision_time, "timeWindow1");
161 |                 model.addConstr(timeWindowLin>=0.0, "timeWindow2");
162 |             }
163 |         }
164 | 
165 |         //Optimization step
166 |         model.optimize();
167 | 
168 |         for(int i=0; i<N; ++i)
169 |         {
170 |             result_speed[i] = std::sqrt(variables[i].get(GRB_DoubleAttr_X));
171 |             result_acceleration[i] = variables[i+N].get(GRB_DoubleAttr_X);
172 |         }
173 |     }
174 |     catch(GRBException e)
175 |     {
176 |         std::cout << "Error code = " << e.getErrorCode() << std::endl;
177 |         std::cout << e.getMessage() << std::endl;
178 | 
179 |         return false;
180 |     }
181 |     catch(...)
182 |     {
183 |         std::cout << "Exception during optimization" << std::endl;
184 |         
185 |         return false;
186 |     }
187 | 
188 |     return true;
189 | }


--------------------------------------------------------------------------------
/src/speed_planner_node.cpp:
--------------------------------------------------------------------------------
  1 | #include "speed_planner/speed_planner_node.h"
  2 | 
  3 | int main(int argc, char** argv)
  4 | {
  5 |     ros::init(argc, argv, "speed_planner");
  6 |     SpeedPlannerNode node;
  7 |     ros::spin();
  8 | 
  9 |     return 0;
 10 | }
 11 | 
 12 | SpeedPlannerNode::SpeedPlannerNode() : nh_(), private_nh_("~"), previous_trajectory_()
 13 | {
 14 |     double mass;
 15 |     double mu;
 16 |     double ds;
 17 |     double previewDistance;
 18 |     double vehicle_length;
 19 |     double vehicle_width;
 20 |     double vehicle_wheel_base;
 21 |     double vehicle_safety_distance;
 22 |     std::array<double, 5> weight{0};
 23 | 
 24 |     private_nh_.param<double>("mass", mass, 1500.0);
 25 |     private_nh_.param<double>("mu", mu, 0.8);
 26 |     private_nh_.param<double>("ds", ds, 0.1);
 27 |     private_nh_.param<double>("preview_distance", previewDistance, 20.0);
 28 |     private_nh_.param<double>("callback_dt", timer_callback_dt_, 0.2);
 29 |     private_nh_.param<double>("curvature_weight", curvatureWeight_, 20.0);
 30 |     private_nh_.param<double>("decay_factor", decayFactor_, 0.8);
 31 |     private_nh_.param<double>("time_weight", weight[0], 0.0);
 32 |     private_nh_.param<double>("smooth_weight", weight[1], 15.0);
 33 |     private_nh_.param<double>("velocity_weight", weight[2], 0.001);
 34 |     private_nh_.param<double>("longitudinal_slack_weight", weight[3], 1.0);
 35 |     private_nh_.param<double>("lateral_slack_weight", weight[4], 10.0);
 36 |     private_nh_.param<double>("lateral_g", lateral_g_, 0.4);
 37 |     private_nh_.param<int>("skip_size", skip_size_, 10);
 38 |     private_nh_.param<int>("smooth_size", smooth_size_, 50);
 39 |     private_nh_.param<double>("vehicle_length", vehicle_length, 5.0);
 40 |     private_nh_.param<double>("vehicle_width", vehicle_width, 1.895);
 41 |     private_nh_.param<double>("vehicle_wheel_base", vehicle_wheel_base, 2.790);
 42 |     private_nh_.param<double>("vehicle_safety_distance", vehicle_safety_distance, 0.1);
 43 |     private_nh_.param<double>("max_speed", max_speed_, 5.0);
 44 | 
 45 |     speedOptimizer_.reset(new ConvexSpeedOptimizer(previewDistance, ds, mass, mu, weight));
 46 |     ego_vehicle_ptr_.reset(new VehicleInfo(vehicle_length, vehicle_width, vehicle_wheel_base,vehicle_safety_distance));
 47 |     collision_checker_ptr_.reset(new CollisionChecker());
 48 |     tf2_buffer_ptr_.reset(new tf2_ros::Buffer());
 49 |     tf2_listner_ptr_.reset(new tf2_ros::TransformListener(*tf2_buffer_ptr_));
 50 | 
 51 |     optimized_waypoints_pub_ = nh_.advertise<autoware_msgs::Lane>("final_waypoints", 1, true);
 52 |     optimized_waypoints_debug_ = nh_.advertise<geometry_msgs::Twist>("optimized_speed_debug", 1, true);
 53 |     result_velocity_pub_ = nh_.advertise<std_msgs::Float32>("result_velocity", 1, true);
 54 |     desired_velocity_pub_ = nh_.advertise<std_msgs::Float32>("desired_velocity", 1, true);
 55 | 
 56 |     final_waypoints_sub_ = nh_.subscribe("safety_waypoints", 1, &SpeedPlannerNode::waypointsCallback, this);
 57 |     current_pose_sub_ = nh_.subscribe("/current_pose", 1, &SpeedPlannerNode::currentPoseCallback, this);
 58 |     current_status_sub_ = nh_.subscribe("/vehicle_status", 1, &SpeedPlannerNode::currentStatusCallback, this);
 59 |     current_velocity_sub_ = nh_.subscribe("/current_velocity", 1, &SpeedPlannerNode::currentVelocityCallback, this);
 60 |     objects_sub_ = nh_.subscribe("/detection/fake_perception/objects", 1, &SpeedPlannerNode::objectsCallback, this);
 61 |     timer_ = nh_.createTimer(ros::Duration(timer_callback_dt_), &SpeedPlannerNode::timerCallback, this);
 62 | }
 63 | 
 64 | void SpeedPlannerNode::waypointsCallback(const autoware_msgs::Lane& msg)
 65 | {
 66 |   in_lane_ptr_.reset(new autoware_msgs::Lane(msg));
 67 | }
 68 | 
 69 | void SpeedPlannerNode::currentPoseCallback(const geometry_msgs::PoseStamped & msg)
 70 | {
 71 |   in_pose_ptr_.reset(new geometry_msgs::PoseStamped(msg));
 72 | }
 73 | 
 74 | void SpeedPlannerNode::currentVelocityCallback(const geometry_msgs::TwistStamped& msg)
 75 | {
 76 |   
 77 |   in_twist_ptr_.reset(new geometry_msgs::TwistStamped(msg));
 78 | }
 79 | 
 80 | void SpeedPlannerNode::currentStatusCallback(const autoware_msgs::VehicleStatus& msg)
 81 | {
 82 |   in_status_ptr_.reset(new autoware_msgs::VehicleStatus(msg));
 83 | }
 84 | 
 85 | void SpeedPlannerNode::objectsCallback(const autoware_msgs::DetectedObjectArray& msg)
 86 | {
 87 |   if(in_lane_ptr_)
 88 |   {
 89 |     if(msg.objects.size() == 0)
 90 |     {
 91 |       std::cerr << "size of objects is 0" << std::endl;
 92 |       return;
 93 |     }
 94 |     geometry_msgs::TransformStamped objects2map_tf;
 95 |     try
 96 |     {
 97 |         objects2map_tf = tf2_buffer_ptr_->lookupTransform(
 98 |           /*target*/  in_lane_ptr_->header.frame_id,  //map
 99 |           /*src*/ msg.objects.begin()->header.frame_id,  //lidar
100 |           ros::Time(0));
101 |     }
102 |     catch (tf2::TransformException &ex)
103 |     {
104 |         ROS_WARN("%s", ex.what());
105 |         return;
106 |     }
107 |     in_objects_ptr_.reset(new autoware_msgs::DetectedObjectArray(msg));
108 |     in_objects_ptr_->header.frame_id = in_lane_ptr_->header.frame_id;
109 |     for(auto& object: in_objects_ptr_->objects)
110 |     {
111 |       object.header.frame_id = in_lane_ptr_->header.frame_id;
112 |       geometry_msgs::PoseStamped current_object_pose;
113 |       current_object_pose.header = object.header;
114 |       current_object_pose.pose = object.pose;
115 |       geometry_msgs::PoseStamped transformed_pose;
116 |       tf2::doTransform(current_object_pose, transformed_pose, objects2map_tf);
117 |       object.pose = transformed_pose.pose;
118 |     }
119 |   }
120 | }
121 | 
122 | void SpeedPlannerNode::timerCallback(const ros::TimerEvent& e)
123 | {
124 |     if(in_lane_ptr_&& in_twist_ptr_ && ego_vehicle_ptr_ && in_pose_ptr_)
125 |     {
126 |         int waypointSize = in_lane_ptr_->waypoints.size();
127 |         std::vector<double> waypoint_x(waypointSize, 0.0);
128 |         std::vector<double> waypoint_y(waypointSize, 0.0);
129 |         std::vector<double> waypoint_yaw(waypointSize, 0.0);
130 |         std::vector<double> waypoint_curvature(waypointSize, 0.0);
131 |         double current_x = in_pose_ptr_->pose.position.x;
132 |         double current_y = in_pose_ptr_->pose.position.y;
133 | 
134 |         for(size_t id=0; id<waypointSize; ++id)
135 |         {
136 |             waypoint_x[id] = in_lane_ptr_->waypoints[id].pose.pose.position.x;
137 |             waypoint_y[id] = in_lane_ptr_->waypoints[id].pose.pose.position.y;
138 |             waypoint_yaw[id] = tf::getYaw(in_lane_ptr_->waypoints[id].pose.pose.orientation);
139 |             waypoint_curvature[id] = in_lane_ptr_->waypoints[id].pose.pose.position.z;
140 |         }
141 |         
142 |         //1. create trajectory
143 |         int nearest_waypoint_id = getNearestId(current_x, current_y, waypoint_x, waypoint_y);
144 |         Trajectory trajectory(waypoint_x, waypoint_y, waypoint_yaw, waypoint_curvature, nearest_waypoint_id);
145 | 
146 |         //2. initial speed and initial acceleration
147 |         //initial velocity
148 |         int nearest_previous_point_id=0;
149 |         double v0 = 0.0;
150 |         double a0 = 0.0;
151 | 
152 |         if(previous_trajectory_==nullptr)
153 |         {
154 |           double v0 = in_twist_ptr_->twist.linear.x;
155 |           previousVelocity_ = v0;
156 |         }
157 |         else
158 |         {
159 |           nearest_previous_point_id = getNearestId(current_x, current_y, previous_trajectory_->x_, previous_trajectory_->y_, 2);
160 |           v0 = previous_trajectory_->velocity_[nearest_previous_point_id];
161 |           a0 = previous_trajectory_->acceleration_[nearest_previous_point_id];
162 |           previousVelocity_ = v0;
163 | 
164 |           ROS_INFO("Nearest id is %d", nearest_previous_point_id);
165 |         }
166 |         
167 |         ROS_INFO("Value of v0: %f", v0);
168 |         ROS_INFO("Value of a0: %f", a0);
169 |         ROS_INFO("Current Velocity: %f", in_twist_ptr_->twist.linear.x);
170 | 
171 |         //3. dyanmic obstacles
172 |         std::unique_ptr<CollisionInfo> collision_info_ptr;
173 |         bool is_collide = false;
174 | 
175 |         if(in_objects_ptr_ && !in_objects_ptr_->objects.empty())
176 |         {
177 |           std::vector<std::shared_ptr<Obstacle>> obstacles;
178 |           obstacles.reserve(in_objects_ptr_->objects.size());
179 | 
180 |           for(int i=0; i<in_objects_ptr_->objects.size(); ++i)
181 |           {
182 |             if(in_objects_ptr_->objects[i].velocity.linear.x>0.1)
183 |             {
184 |               double obstacle_x = in_objects_ptr_->objects[i].pose.position.x;
185 |               double obstacle_y = in_objects_ptr_->objects[i].pose.position.y;
186 |               double obstacle_angle    = tf::getYaw(in_objects_ptr_->objects[i].pose.orientation);
187 |               double obstacle_radius   = std::sqrt(std::pow(in_objects_ptr_->objects[i].dimensions.x, 2) + std::pow(in_objects_ptr_->objects[i].dimensions.y, 2));
188 |               double obstacle_velocity = in_objects_ptr_->objects[i].velocity.linear.x;
189 | 
190 |               obstacles.push_back(std::make_shared<DynamicObstacle>(obstacle_x, obstacle_y, obstacle_angle, obstacle_radius, obstacle_velocity, 5, 0.5));
191 |             }
192 |             else
193 |             {
194 |               double obstacle_x = in_objects_ptr_->objects[i].pose.position.x;
195 |               double obstacle_y = in_objects_ptr_->objects[i].pose.position.y;
196 |               double obstacle_angle    = tf::getYaw(in_objects_ptr_->objects[i].pose.orientation);
197 |               double obstacle_radius   = std::sqrt(std::pow(in_objects_ptr_->objects[i].dimensions.x, 2) + std::pow(in_objects_ptr_->objects[i].dimensions.y, 2));
198 | 
199 |               obstacles.push_back(std::make_shared<StaticObstacle>(obstacle_x, obstacle_y, obstacle_angle, obstacle_radius));
200 |             }
201 |           }
202 | 
203 |           is_collide = collision_checker_ptr_->check(trajectory, obstacles, ego_vehicle_ptr_, collision_info_ptr);
204 |         }
205 | 
206 |         if(is_collide)
207 |         {
208 |           assert(collision_info_ptr!=nullptr);
209 |           ROS_INFO("Collide Position id is %d", collision_info_ptr->getId());
210 |           ROS_INFO("Collision Occured Position is %f", trajectory.x_[collision_info_ptr->getId()]);
211 |         }
212 |         else
213 |           ROS_INFO("Not Collide");
214 | 
215 |         double safeTime = 30.0;
216 | 
217 |         //4. Create Speed Constraints and Acceleration Constraints
218 |         int N = trajectory.x_.size();
219 |         std::vector<double> Vr(N, 0.0);     //restricted speed array
220 |         std::vector<double> Vd(N, 0.0);     //desired speed array
221 |         std::vector<double> Arlon(N, 0.0);  //acceleration longitudinal restriction
222 |         std::vector<double> Arlat(N, 0.0);  //acceleration lateral restriction
223 |         std::vector<double> Aclon(N, 0.0);  //comfort longitudinal acceleration restriction
224 |         std::vector<double> Aclat(N, 0.0);  //comfort lateral acceleration restriction
225 | 
226 |         if(is_collide && collision_info_ptr->getType()==Obstacle::TYPE::STATIC)
227 |         {
228 |           Vr[0] = max_speed_;
229 |           Vd[0] = v0;
230 |           for(int i=1; i<collision_info_ptr->getId()-100; ++i)
231 |           {
232 |             Vr[i] = max_speed_;
233 |             Vd[i] = std::max(std::min(Vr[i]-0.5, std::sqrt(lateral_g_/(std::fabs(trajectory.curvature_[i]+1e-10)))), 1.0);
234 |           }
235 |         }
236 |         else if (is_collide && collision_info_ptr->getType()==Obstacle::TYPE::DYNAMIC)
237 |         {
238 |           Vr[0] = v0+0.1;
239 |           Vd[0] = v0;
240 |           for(size_t i=1; i<Vr.size(); ++i)
241 |           {
242 |             Vr[i] = max_speed_;
243 |             Vd[i] = std::max(std::min(Vr[i]-0.5, std::sqrt(lateral_g_/(std::fabs(trajectory.curvature_[i]+1e-10)))), 1.0);
244 |           }
245 |         }
246 |         else
247 |         {
248 |           Vr[0] = v0+0.1;
249 |           Vd[0] = v0;
250 |           for(size_t i=1; i<Vr.size(); ++i)
251 |           {
252 |             Vr[i] = max_speed_;
253 |             Vd[i] = std::max(std::min(Vr[i]-0.5, std::sqrt(lateral_g_/(std::fabs(trajectory.curvature_[i]+1e-10)))), 1.0);
254 |           }
255 |         }
256 | 
257 |         double mu = speedOptimizer_->mu_;
258 |         for(size_t i=0; i<N; ++i)
259 |         {
260 |             Arlon[i] = 0.5*mu*9.83;
261 |             Arlat[i] = 0.5*mu*9.83;
262 |             Aclon[i] = 0.4*mu*9.83;
263 |             Aclat[i] = 0.4*mu*9.83;
264 |         }
265 | 
266 | 
267 |         //Output the information
268 |         ROS_INFO("Size: %d", N);
269 |         if(collision_info_ptr!=nullptr)
270 |         {
271 |           ROS_INFO("Collision id %d", collision_info_ptr->getId());
272 |           ROS_INFO("Collision time %f", collision_info_ptr->getCollisionTime());
273 |         }
274 | 
275 |         //////////////////////////////////////////////////////////////////////////////////////////
276 |         ////////////////////////////////Calculate Optimized Speed////////////////////////////////
277 |         std::vector<double> result_speed(N, 0.0);
278 |         std::vector<double> result_acceleration(N, 0.0);
279 |         bool is_result = speedOptimizer_->calcOptimizedSpeed(trajectory, 
280 |                                                              result_speed, 
281 |                                                              result_acceleration, 
282 |                                                              Vr, 
283 |                                                              Vd, 
284 |                                                              Arlon, 
285 |                                                              Arlat, 
286 |                                                              Aclon, 
287 |                                                              Aclat, 
288 |                                                              a0, 
289 |                                                              is_collide,
290 |                                                              collision_info_ptr,
291 |                                                              safeTime);
292 | 
293 |         if(is_result)
294 |         {
295 |           ROS_INFO("Gurobi Success");
296 |           //5. set result
297 |           autoware_msgs::Lane speedOptimizedLane;
298 |           speedOptimizedLane.lane_id = in_lane_ptr_->lane_id;
299 |           speedOptimizedLane.lane_index = in_lane_ptr_->lane_index;
300 |           speedOptimizedLane.is_blocked = in_lane_ptr_->is_blocked;
301 |           speedOptimizedLane.increment = in_lane_ptr_->increment;
302 |           speedOptimizedLane.header = in_lane_ptr_->header;
303 |           speedOptimizedLane.cost = in_lane_ptr_->cost;
304 |           speedOptimizedLane.closest_object_distance = in_lane_ptr_->closest_object_distance;
305 |           speedOptimizedLane.closest_object_velocity = in_lane_ptr_->closest_object_velocity;
306 |           speedOptimizedLane.waypoints.reserve(result_speed.size());
307 | 
308 |           for(int i=0; i<N; i++)
309 |           {
310 |             result_speed[i] = std::min(result_speed[i] ,max_speed_-0.1);
311 |             autoware_msgs::Waypoint waypoint;
312 |             waypoint.pose.pose.position.x = trajectory.x_[i];
313 |             waypoint.pose.pose.position.y = trajectory.y_[i];
314 |             waypoint.pose.pose.position.z = in_lane_ptr_->waypoints[0].pose.pose.position.z;
315 |             waypoint.pose.pose.orientation = tf::createQuaternionMsgFromYaw(trajectory.yaw_[i]);
316 |             waypoint.pose.header = in_lane_ptr_->header;
317 |             waypoint.twist.header=in_lane_ptr_->header;
318 |             waypoint.twist.twist.linear.x = result_speed[i];
319 | 
320 |             //std::cout << result_speed[i] << std::endl;
321 | 
322 |             speedOptimizedLane.waypoints.push_back(waypoint);
323 |           }
324 | 
325 |           if(!result_speed.empty())
326 |           {
327 |             std_msgs::Float32 result_velocity;
328 |             result_velocity.data = result_speed[0];
329 |             result_velocity_pub_.publish(result_velocity);
330 |             std_msgs::Float32 desired_velocity;
331 |             desired_velocity.data = Vd[2];
332 |             desired_velocity_pub_.publish(desired_velocity);
333 |           }
334 | 
335 |           optimized_waypoints_pub_.publish(speedOptimizedLane);
336 |           previous_trajectory_.reset(new Trajectory(waypoint_x, waypoint_y, waypoint_yaw, waypoint_curvature, result_speed, result_acceleration));
337 |         }
338 |         else
339 |         {
340 |           ROS_WARN("[Speed Planner]: Gurobi Failed");
341 |           std::cout << "Vr[0]: " << Vr[0] << std::endl;
342 |           std::cout << "Vd[0]: " << Vd[0] << std::endl;
343 |           std::cout << "Vr[1]: " << Vr[1] << std::endl;
344 |           std::cout << "Vd[1]: " << Vd[1] << std::endl;
345 |           std::cout << "Vr[2]: " << Vr[2] << std::endl;
346 |           std::cout << "Vd[2]: " << Vd[2] << std::endl;
347 |           std::cout << "V0: " << v0 << std::endl;
348 |           std::cout << "a0: " << a0 << std::endl;
349 |           //"if gurobi failed calculation"
350 |           if(previous_trajectory_==nullptr)
351 |             return;
352 | 
353 |           //5. set previous result
354 |           autoware_msgs::Lane speedOptimizedLane;
355 |           speedOptimizedLane.lane_id = in_lane_ptr_->lane_id;
356 |           speedOptimizedLane.lane_index = in_lane_ptr_->lane_index;
357 |           speedOptimizedLane.is_blocked = in_lane_ptr_->is_blocked;
358 |           speedOptimizedLane.increment = in_lane_ptr_->increment;
359 |           speedOptimizedLane.header = in_lane_ptr_->header;
360 |           speedOptimizedLane.cost = in_lane_ptr_->cost;
361 |           speedOptimizedLane.closest_object_distance = in_lane_ptr_->closest_object_distance;
362 |           speedOptimizedLane.closest_object_velocity = in_lane_ptr_->closest_object_velocity;
363 |           speedOptimizedLane.waypoints.reserve(trajectory.x_.size());
364 | 
365 |           for(int i=nearest_previous_point_id; i<N; i++)
366 |           {
367 |             autoware_msgs::Waypoint waypoint;
368 |             waypoint.pose.pose.position.x = trajectory.x_[i-nearest_previous_point_id];
369 |             waypoint.pose.pose.position.y = trajectory.y_[i-nearest_previous_point_id];
370 |             waypoint.pose.pose.position.z = in_lane_ptr_->waypoints[0].pose.pose.position.z;
371 |             waypoint.pose.pose.orientation = tf::createQuaternionMsgFromYaw(trajectory.yaw_[i-nearest_previous_point_id]);
372 |             waypoint.pose.header = in_lane_ptr_->header;
373 |             waypoint.twist.header=in_lane_ptr_->header;
374 |             waypoint.twist.twist.linear.x = previous_trajectory_->velocity_[i];
375 | 
376 |             speedOptimizedLane.waypoints.push_back(waypoint);
377 |           }
378 | 
379 |           optimized_waypoints_pub_.publish(speedOptimizedLane);
380 |         }
381 | 
382 |         ROS_INFO("=======================================================");
383 |     }
384 | 
385 | }
386 | 


--------------------------------------------------------------------------------