Fix platooning_control (#2417)

* reject old ctrl commands

* add control cmd timeout param and update control node name

* update base control class

* remove cmd timeout

* fix unit test

carma_guidance_plugins/src/control_plugin.cpp

@@ -31,9 +31,9 @@ namespace carma_guidance_plugins
  return "control/trajectory_control";
  }
 
- uint8_t ControlPlugin::get_type() 
+ uint8_t ControlPlugin::get_type()
  {
- return carma_planning_msgs::msg::Plugin::CONTROL; 
+ return carma_planning_msgs::msg::Plugin::CONTROL;
  }
 
  void ControlPlugin::current_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg)
@@ -75,9 +75,10 @@ namespace carma_guidance_plugins
  if (this->get_activation_status()) // Only trigger when activated
  {
  this->vehicle_cmd_pub_->publish(this->generate_command());
+
  }
  });
- 
+
  return PluginBaseNode::handle_on_configure(prev_state);
  }
 
@@ -107,4 +108,3 @@ namespace carma_guidance_plugins
  }
 
 } // carma_guidance_plugins
-

platooning_control/src/platooning_control.cpp

@@ -359,7 +359,7 @@ namespace platooning_control
  double dx1 = trajectory_points[trajectory_points.size()-1].x - trajectory_points[0].x;
  double dy1 = trajectory_points[trajectory_points.size()-1].y - trajectory_points[0].y;
  double d1 = sqrt(dx1*dx1 + dy1*dy1);
- double t1 = rclcpp::Time((trajectory_points[trajectory_points.size()-1].target_time)).seconds() - rclcpp::Time(trajectory_points[0].target_time).nanoseconds();
+ double t1 = (rclcpp::Time((trajectory_points[trajectory_points.size()-1].target_time)).nanoseconds() - rclcpp::Time(trajectory_points[0].target_time).nanoseconds())/1e9;
 
  double avg_speed = d1/t1;
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "trajectory_points size = " << trajectory_points.size() << ", d1 = " << d1 << ", t1 = " << t1 << ", avg_speed = " << avg_speed);

pure_pursuit_wrapper/src/pure_pursuit_wrapper.cpp

@@ -58,7 +58,7 @@ carma_ros2_utils::CallbackReturn PurePursuitWrapperNode::on_configure_plugin()
  get_parameter<double>("emergency_stop_distance", config_.emergency_stop_distance);
  get_parameter<double>("speed_thres_traveling_direction", config_.speed_thres_traveling_direction);
  get_parameter<double>("dist_front_rear_wheels", config_.dist_front_rear_wheels);
- 
+
  // integrator configs
  get_parameter<double>("dt", config_.dt);
  get_parameter<double>("integrator_max_pp", config_.integrator_max_pp);
@@ -70,7 +70,7 @@ carma_ros2_utils::CallbackReturn PurePursuitWrapperNode::on_configure_plugin()
 
  // Register runtime parameter update callback
  add_on_set_parameters_callback(std::bind(&PurePursuitWrapperNode::parameter_update_callback, this, std_ph::_1));
- 
+
  // create config for pure_pursuit worker
  pure_pursuit::Config cfg{
  config_.minimum_lookahead_distance,
@@ -84,18 +84,18 @@ carma_ros2_utils::CallbackReturn PurePursuitWrapperNode::on_configure_plugin()
  };
 
  pure_pursuit::IntegratorConfig i_cfg;
- i_cfg.dt = config_.dt; 
- i_cfg.integrator_max_pp = config_.integrator_max_pp; 
- i_cfg.integrator_min_pp = config_.integrator_min_pp; 
- i_cfg.Ki_pp = config_.Ki_pp; 
+ i_cfg.dt = config_.dt;
+ i_cfg.integrator_max_pp = config_.integrator_max_pp;
+ i_cfg.integrator_min_pp = config_.integrator_min_pp;
+ i_cfg.Ki_pp = config_.Ki_pp;
  i_cfg.integral = 0.0; // accumulator of integral starts from 0
- i_cfg.is_integrator_enabled = config_.is_integrator_enabled; 
- 
+ i_cfg.is_integrator_enabled = config_.is_integrator_enabled;
+
  pp_ = std::make_shared<pure_pursuit::PurePursuit>(cfg, i_cfg);
 
  // Return success if everything initialized successfully
  return CallbackReturn::SUCCESS;
-} 
+}
 
 motion::motion_common::State PurePursuitWrapperNode::convert_state(geometry_msgs::msg::PoseStamped pose, geometry_msgs::msg::TwistStamped twist)
 {
@@ -148,7 +148,7 @@ autoware_msgs::msg::ControlCommandStamped PurePursuitWrapperNode::generate_comma
  pp_->set_trajectory(autoware_traj_plan);
 
  const auto cmd{pp_->compute_command(state_tf)};
- 
+
  converted_cmd = convert_cmd(cmd);
 
 
@@ -167,9 +167,9 @@ rcl_interfaces::msg::SetParametersResult PurePursuitWrapperNode::parameter_updat
  {"dist_front_rear_wheels", config_.dist_front_rear_wheels},
  {"integrator_max_pp", config_.integrator_max_pp},
  {"integrator_min_pp", config_.integrator_min_pp},
- {"Ki_pp", config_.Ki_pp}
+ {"Ki_pp", config_.Ki_pp},
  }, parameters);
- 
+
  auto error_bool = update_params<bool>({
  {"is_interpolate_lookahead_point", config_.is_interpolate_lookahead_point},
  {"is_delay_compensation", config_.is_delay_compensation},
@@ -184,19 +184,19 @@ rcl_interfaces::msg::SetParametersResult PurePursuitWrapperNode::parameter_updat
 }
 
 
-bool PurePursuitWrapperNode::get_availability() 
+bool PurePursuitWrapperNode::get_availability()
 {
  return true;
 }
 
-std::string PurePursuitWrapperNode::get_version_id() 
+std::string PurePursuitWrapperNode::get_version_id()
 {
  return "v4.0";
 }
 
 std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> PurePursuitWrapperNode::remove_repeated_timestamps(const std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>& traj_points)
 {
- 
+
  std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> new_traj_points;
 
  carma_planning_msgs::msg::TrajectoryPlanPoint prev_point;

trajectory_executor/src/trajectory_executor_node.cpp

@@ -55,8 +55,8 @@ namespace trajectory_executor
  out[config_.default_control_plugin] = config_.default_control_plugin_topic;
 
  //Hardcoding platoon control plugins
- std::string control_plugin2 = "platoon_control";
- std::string control_plugin_topic2 = "/guidance/plugins/platoon_control/plan_trajectory";
+ std::string control_plugin2 = "platooning_control";
+ std::string control_plugin_topic2 = "/guidance/plugins/platooning_control/plan_trajectory";
  out[control_plugin2] = control_plugin_topic2;
 
  return out;
@@ -112,7 +112,7 @@ namespace trajectory_executor
 
  timer_ = create_timer(
  get_clock(),
- std::chrono::milliseconds(timer_period_ms), 
+ std::chrono::milliseconds(timer_period_ms),
  std::bind(&TrajectoryExecutor::onTrajEmitTick, this));
 
  return CallbackReturn::SUCCESS;
@@ -136,13 +136,13 @@ namespace trajectory_executor
  it->second->publish(*cur_traj_);
  } else {
  std::ostringstream description_builder;
- description_builder << "No match found for control plugin " 
- << control_plugin << " at point " 
+ description_builder << "No match found for control plugin "
+ << control_plugin << " at point "
  << timesteps_since_last_traj_ << " in current trajectory!";
 
  throw std::invalid_argument(description_builder.str());
  }
- timesteps_since_last_traj_++; 
+ timesteps_since_last_traj_++;
  } else {
  RCLCPP_DEBUG_STREAM(get_logger(), "Awaiting initial trajectory publication...");
  }
@@ -159,7 +159,7 @@ namespace trajectory_executor
 
  cur_traj_ = std::unique_ptr<carma_planning_msgs::msg::TrajectoryPlan>(move(msg));
  timesteps_since_last_traj_ = 0;
- RCLCPP_INFO_STREAM(get_logger(), "Successfully swapped trajectories!"); 
+ RCLCPP_INFO_STREAM(get_logger(), "Successfully swapped trajectories!");
  }
 
  void TrajectoryExecutor::guidanceStateMonitor(carma_planning_msgs::msg::GuidanceState::UniquePtr msg)

trajectory_executor/test/trajectory_executor_test_suite.cpp

@@ -39,11 +39,11 @@ namespace trajectory_executor_test_suite
  /*!
  * \brief Constructor for TrajectoryExecutorTestSuite
  */
- explicit TrajectoryExecutorTestSuite(const rclcpp::NodeOptions &options) 
+ explicit TrajectoryExecutorTestSuite(const rclcpp::NodeOptions &options)
  : carma_ros2_utils::CarmaLifecycleNode(options) {}
 
  // Publisher
- carma_ros2_utils::PubPtr<carma_planning_msgs::msg::TrajectoryPlan> traj_pub_; 
+ carma_ros2_utils::PubPtr<carma_planning_msgs::msg::TrajectoryPlan> traj_pub_;
 
  // Subscribers
  carma_ros2_utils::SubPtr<carma_planning_msgs::msg::TrajectoryPlan> traj_sub_;
@@ -65,18 +65,18 @@ namespace trajectory_executor_test_suite
  // Setup Subscribers
  traj_sub_ = create_subscription<carma_planning_msgs::msg::TrajectoryPlan>("/guidance/plugins/pure_pursuit/plan_trajectory", 100,
  std::bind(&TrajectoryExecutorTestSuite::trajEmitCallback, this, std_ph::_1));
- traj_sub2_ = create_subscription<carma_planning_msgs::msg::TrajectoryPlan>("/guidance/plugins/platoon_control/plan_trajectory", 100,
+ traj_sub2_ = create_subscription<carma_planning_msgs::msg::TrajectoryPlan>("/guidance/plugins/platooning_control/plan_trajectory", 100,
  std::bind(&TrajectoryExecutorTestSuite::trajEmitCallback, this, std_ph::_1));
- 
+
  return CallbackReturn::SUCCESS;
  }
 
  };
 
  /*!
- * \brief Helper Function: Builds a small sample TrajectoryPlan message for the Pure Pursuit 
+ * \brief Helper Function: Builds a small sample TrajectoryPlan message for the Pure Pursuit
  * controller plugin
- * 
+ *
  * \return A 10-point TrajectoryPlan message containing sample data
  */
  carma_planning_msgs::msg::TrajectoryPlan buildSampleTraj() {
@@ -102,9 +102,9 @@ namespace trajectory_executor_test_suite
  }
 
  /*!
- * \brief Helper Function: Builds a small sample TrajectoryPlan message for the PlatooningControlPlugin 
+ * \brief Helper Function: Builds a small sample TrajectoryPlan message for the PlatooningControlPlugin
  * controller plugin
- * 
+ *
  * \return A 10-point TrajectoryPlan message containing sample data
  */
  carma_planning_msgs::msg::TrajectoryPlan buildSampleTraj2() {
@@ -116,7 +116,7 @@ namespace trajectory_executor_test_suite
  rclcpp::Time cur_time = rclcpp::Time(0,0);
  for (int i = 0; i < 10; i++) {
  carma_planning_msgs::msg::TrajectoryPlanPoint p;
- p.controller_plugin_name = "platoon_control";
+ p.controller_plugin_name = "platooning_control";
  p.lane_id = "0";
  p.planner_plugin_name = "cruising";
  rclcpp::Duration dur((i * 0.13)*1e9); // Convert seconds to nanoseconds

trajectory_follower_wrapper/config/parameters.yaml

@@ -1,2 +1,2 @@
 # acceptable time difference from autoware's control command to still use the command in sec
-incoming_cmd_time_threshold: 1.0
+incoming_cmd_time_threshold: 1.0