#include <pure_pursuit_wrapper.hpp>

Inheritance diagram for pure_pursuit_wrapper::PurePursuitWrapperNode:

Collaboration diagram for pure_pursuit_wrapper::PurePursuitWrapperNode:

Public Member Functions
	PurePursuitWrapperNode (const rclcpp::NodeOptions &options)
	Constructor. More...

autoware_msgs::msg::ControlCommandStamped	generate_command () override
	Extending class provided method which should generate a command message which will be published to the required topic by the base class. More...

rcl_interfaces::msg::SetParametersResult	parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
	Example callback for dynamic parameter updates. More...

carma_ros2_utils::CallbackReturn	on_configure_plugin () override
	This method should be used to load parameters and will be called on the configure state transition. More...

bool	get_availability () override
	Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...

std::string	get_version_id () override
	Returns the version id of this plugin. More...

std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint >	remove_repeated_timestamps (const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &traj_points)
	Drops any points that sequentially have same target_time and return new trajectory_points in order to avoid divide by zero situation. More...

motion::motion_common::State	convert_state (geometry_msgs::msg::PoseStamped pose, geometry_msgs::msg::TwistStamped twist)

autoware_msgs::msg::ControlCommandStamped	convert_cmd (motion::motion_common::Command cmd)

Public Member Functions inherited from carma_guidance_plugins::ControlPlugin
	ControlPlugin (const rclcpp::NodeOptions &)
	ControlPlugin constructor. More...

virtual	~ControlPlugin ()=default
	Virtual destructor for safe deletion. More...

virtual autoware_msgs::msg::ControlCommandStamped	generate_command ()=0
	Extending class provided method which should generate a command message which will be published to the required topic by the base class. More...

std::string	get_capability () override
	Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...

uint8_t	get_type () override final
	Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_deactivate (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_cleanup (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_shutdown (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override final

Public Member Functions inherited from carma_guidance_plugins::PluginBaseNode
	PluginBaseNode (const rclcpp::NodeOptions &)
	PluginBaseNode constructor. More...

virtual	~PluginBaseNode ()=default
	Virtual destructor for safe deletion. More...

virtual std::shared_ptr< carma_wm::WMListener >	get_world_model_listener () final
	Method to return the default world model listener provided as a convience by this base class If this method or get_world_model() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...

virtual carma_wm::WorldModelConstPtr	get_world_model () final
	Method to return the default world model provided as a convience by this base class If this method or get_world_model_listener() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...

virtual bool	get_activation_status () final
	Returns the activation status of this plugin. The plugins API callbacks will only be triggered when this method returns true. More...

virtual uint8_t	get_type ()
	Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...

std::string	get_plugin_name_and_ns () const
	Return the name of this plugin with namespace. NOTE: If only the name of the plugin is required, use get_plugin_name() More...

std::string	get_plugin_name () const
	Return the name of this plugin. More...

virtual bool	get_availability ()=0
	Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...

virtual std::string	get_capability ()=0
	Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...

virtual std::string	get_version_id ()=0
	Returns the version id of this plugin. More...

virtual carma_ros2_utils::CallbackReturn	on_configure_plugin ()=0
	Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. This method should be used to load parameters and is required to be implemented. More...

virtual carma_ros2_utils::CallbackReturn	on_activate_plugin ()
	Method which is triggered when this plugin is moved from the INACTIVE to ACTIVE states. This method should be used to prepare for future callbacks for plugin's capabilites. More...

virtual carma_ros2_utils::CallbackReturn	on_deactivate_plugin ()
	Method which is triggered when this plugin is moved from the ACTIVE to INACTIVE states. This method should be used to disable any functionality which should cease execution when plugin is inactive. More...

virtual carma_ros2_utils::CallbackReturn	on_cleanup_plugin ()
	Method which is triggered when this plugin is moved from the INACTIVE to UNCONFIGURED states. This method should be used to fully reset the plugin such that a future call to on_configure_plugin would leave the plugin in a fresh state as though just launched. More...

virtual carma_ros2_utils::CallbackReturn	on_shutdown_plugin ()
	Method which is triggered when this plugin is moved from any state to FINALIZED This method should be used to generate any shutdown logs or final cleanup. More...

virtual carma_ros2_utils::CallbackReturn	on_error_plugin (const std::string &exception_string)
	Method which is triggered when an unhandled exception occurs in this plugin This method should be used to cleanup such that the plugin could be moved to UNCONFIGURED state if possible. More...

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_deactivate (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_cleanup (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_shutdown (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override

	FRIEND_TEST (carma_guidance_plugins_test, connections_test)

Private Member Functions
std::shared_ptr< pure_pursuit::PurePursuit >	get_pure_pursuit_worker ()

	FRIEND_TEST (PurePursuitTest, sanity_check)

Private Attributes
PurePursuitWrapperConfig	config_

std::shared_ptr< pure_pursuit::PurePursuit >	pp_

Additional Inherited Members
Protected Member Functions inherited from carma_guidance_plugins::ControlPlugin
void	current_pose_callback (geometry_msgs::msg::PoseStamped::UniquePtr msg)

void	current_twist_callback (geometry_msgs::msg::TwistStamped::UniquePtr msg)

virtual void	current_trajectory_callback (carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg)
	Extending class provided method which can optionally handle trajectory plan callbacks. More...

Protected Attributes inherited from carma_guidance_plugins::ControlPlugin
boost::optional< geometry_msgs::msg::PoseStamped >	current_pose_
	The most recent pose message received by this node. More...

boost::optional< geometry_msgs::msg::TwistStamped >	current_twist_
	The most recent velocity message received by this node. More...

boost::optional< carma_planning_msgs::msg::TrajectoryPlan >	current_trajectory_
	The most recent trajectory received by this plugin. More...

Detailed Description

Definition at line 37 of file pure_pursuit_wrapper.hpp.

Constructor & Destructor Documentation

◆ PurePursuitWrapperNode()

pure_pursuit_wrapper::PurePursuitWrapperNode::PurePursuitWrapperNode ( const rclcpp::NodeOptions & options )

explicit

Constructor.

Definition at line 28 of file pure_pursuit_wrapper.cpp.

  : carma_guidance_plugins::ControlPlugin(options)
{
  basic_autonomy::set_logger(get_logger().get_child("basic_autonomy"));
  config_ = PurePursuitWrapperConfig();
  config_.vehicle_response_lag = declare_parameter<double>("vehicle_response_lag", config_.vehicle_response_lag);
  config_.minimum_lookahead_distance = declare_parameter<double>("minimum_lookahead_distance", config_.minimum_lookahead_distance);
  config_.maximum_lookahead_distance = declare_parameter<double>("maximum_lookahead_distance", config_.maximum_lookahead_distance);
  config_.speed_to_lookahead_ratio = declare_parameter<double>("speed_to_lookahead_ratio", config_.speed_to_lookahead_ratio);
  config_.is_interpolate_lookahead_point = declare_parameter<bool>("is_interpolate_lookahead_point", config_.is_interpolate_lookahead_point);
  config_.is_delay_compensation = declare_parameter<bool>("is_delay_compensation", config_.is_delay_compensation);
  config_.emergency_stop_distance = declare_parameter<double>("emergency_stop_distance", config_.emergency_stop_distance);
  config_.speed_thres_traveling_direction = declare_parameter<double>("speed_thres_traveling_direction", config_.speed_thres_traveling_direction);
  config_.dist_front_rear_wheels = declare_parameter<double>("dist_front_rear_wheels", config_.dist_front_rear_wheels);
 
  // integrator part
  config_.dt = declare_parameter<double>("dt", config_.dt);
  config_.integrator_max_pp = declare_parameter<double>("integrator_max_pp", config_.integrator_max_pp);
  config_.integrator_min_pp = declare_parameter<double>("integrator_min_pp", config_.integrator_min_pp);
  config_.Ki_pp = declare_parameter<double>("Ki_pp", config_.Ki_pp);
  config_.is_integrator_enabled = declare_parameter<bool>("is_integrator_enabled", config_.is_integrator_enabled);
}

Here is the call graph for this function:

Member Function Documentation

◆ convert_cmd()

autoware_msgs::msg::ControlCommandStamped pure_pursuit_wrapper::PurePursuitWrapperNode::convert_cmd ( motion::motion_common::Command cmd )

Definition at line 116 of file pure_pursuit_wrapper.cpp.

{
  autoware_msgs::msg::ControlCommandStamped return_cmd;
  return_cmd.header.stamp = cmd.stamp;
 
  return_cmd.cmd.linear_acceleration = cmd.long_accel_mps2;
  return_cmd.cmd.linear_velocity = cmd.velocity_mps;
  return_cmd.cmd.steering_angle = cmd.front_wheel_angle_rad;
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("pure_pursuit_wrapper"), "generate_command() cmd.stamp: " << std::to_string(rclcpp::Time(cmd.stamp).seconds()));
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("pure_pursuit_wrapper"), "generate_command() cmd.long_accel_mps2: " << cmd.long_accel_mps2);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("pure_pursuit_wrapper"), "generate_command() cmd.velocity_mps: " << cmd.velocity_mps);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("pure_pursuit_wrapper"), "generate_command() cmd.rear_wheel_angle_rad: " << cmd.rear_wheel_angle_rad);
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("pure_pursuit_wrapper"), "generate_command() cmd.front_wheel_angle_rad: " << cmd.front_wheel_angle_rad);
 
  return return_cmd;
}

References basic_autonomy::get_logger(), and carma_cooperative_perception::to_string().

Referenced by generate_command().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ convert_state()

motion::motion_common::State pure_pursuit_wrapper::PurePursuitWrapperNode::convert_state	(	geometry_msgs::msg::PoseStamped	pose,
		geometry_msgs::msg::TwistStamped	twist
	)

Definition at line 102 of file pure_pursuit_wrapper.cpp.

{
  motion::motion_common::State state;
  state.header = pose.header;
  state.state.x = pose.pose.position.x;
  state.state.y = pose.pose.position.y;
  state.state.z = pose.pose.position.z;
  state.state.heading.real = pose.pose.orientation.w;
  state.state.heading.imag = pose.pose.orientation.z;
 
  state.state.longitudinal_velocity_mps = twist.twist.linear.x;
  return state;
}

Referenced by generate_command().

Here is the caller graph for this function:

◆ FRIEND_TEST()

pure_pursuit_wrapper::PurePursuitWrapperNode::FRIEND_TEST	(	PurePursuitTest	,
		sanity_check
	)

private

◆ generate_command()

autoware_msgs::msg::ControlCommandStamped pure_pursuit_wrapper::PurePursuitWrapperNode::generate_command ( )

overridevirtual

Extending class provided method which should generate a command message which will be published to the required topic by the base class.

NOTE: Implementer can determine if trajectory has changed based on current_trajectory_->trajectory_id

Returns: The command message to publish

Implements carma_guidance_plugins::ControlPlugin.

Definition at line 134 of file pure_pursuit_wrapper.cpp.

{
  // process and save the trajectory inside pure_pursuit
  autoware_msgs::msg::ControlCommandStamped converted_cmd;
 
  if (!current_trajectory_ || !current_pose_ || !current_twist_)
    return converted_cmd;
 
  motion::control::controller_common::State state_tf = convert_state(current_pose_.get(), current_twist_.get());
 
  RCLCPP_DEBUG_STREAM(rclcpp::get_logger("pure_pursuit_wrapper"), "Forced from frame_id: " << state_tf.header.frame_id << ", into: " << current_trajectory_.get().header.frame_id);
 
  current_trajectory_.get().header.frame_id = state_tf.header.frame_id;
 
  auto autoware_traj_plan = basic_autonomy::waypoint_generation::process_trajectory_plan(current_trajectory_.get(), config_.vehicle_response_lag);
 
  pp_->set_trajectory(autoware_traj_plan);
 
  const auto cmd{pp_->compute_command(state_tf)};
 
  converted_cmd = convert_cmd(cmd);
 
  return converted_cmd;
}

References config_, convert_cmd(), convert_state(), carma_guidance_plugins::ControlPlugin::current_pose_, carma_guidance_plugins::ControlPlugin::current_trajectory_, carma_guidance_plugins::ControlPlugin::current_twist_, basic_autonomy::get_logger(), pp_, basic_autonomy::waypoint_generation::process_trajectory_plan(), and pure_pursuit_wrapper::PurePursuitWrapperConfig::vehicle_response_lag.

Here is the call graph for this function:

◆ get_availability()

bool pure_pursuit_wrapper::PurePursuitWrapperNode::get_availability ( )

overridevirtual

Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes.

Returns: This method should return true if the plugin's current understanding of the world means it would be capable of planning or executing its capability.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 189 of file pure_pursuit_wrapper.cpp.

{
  return true;
}

◆ get_pure_pursuit_worker()

std::shared_ptr< pure_pursuit::PurePursuit > pure_pursuit_wrapper::PurePursuitWrapperNode::get_pure_pursuit_worker ( )

inlineprivate

Definition at line 82 of file pure_pursuit_wrapper.hpp.

    {
        return pp_;
    }

References pp_.

◆ get_version_id()

std::string pure_pursuit_wrapper::PurePursuitWrapperNode::get_version_id ( )

overridevirtual

Returns the version id of this plugin.

Returns: The version id represented as a string

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 194 of file pure_pursuit_wrapper.cpp.

{
  return "v4.0";
}

◆ on_configure_plugin()

carma_ros2_utils::CallbackReturn pure_pursuit_wrapper::PurePursuitWrapperNode::on_configure_plugin ( )

overridevirtual

This method should be used to load parameters and will be called on the configure state transition.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 51 of file pure_pursuit_wrapper.cpp.

{
  config_ = PurePursuitWrapperConfig();
  get_parameter<double>("vehicle_response_lag", config_.vehicle_response_lag);
  get_parameter<double>("minimum_lookahead_distance", config_.minimum_lookahead_distance);
  get_parameter<double>("maximum_lookahead_distance", config_.maximum_lookahead_distance);
  get_parameter<double>("speed_to_lookahead_ratio", config_.speed_to_lookahead_ratio);
  get_parameter<bool>("is_interpolate_lookahead_point", config_.is_interpolate_lookahead_point);
  get_parameter<bool>("is_delay_compensation", config_.is_delay_compensation);
  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);
  get_parameter<double>("integrator_min_pp", config_.integrator_min_pp);
  get_parameter<double>("Ki_pp", config_.Ki_pp);
  get_parameter<bool>("is_integrator_enabled", config_.is_integrator_enabled);
 
  RCLCPP_INFO_STREAM(rclcpp::get_logger("pure_pursuit_wrapper"), "Loaded Params: " << config_);
 
  // 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,
    config_.maximum_lookahead_distance,
    config_.speed_to_lookahead_ratio,
    config_.is_interpolate_lookahead_point,
    config_.is_delay_compensation,
    config_.emergency_stop_distance,
    config_.speed_thres_traveling_direction,
    config_.dist_front_rear_wheels,
  };
 
  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.integral = 0.0; // accumulator of integral starts from 0
  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;
}

Here is the call graph for this function:

◆ parameter_update_callback()

rcl_interfaces::msg::SetParametersResult pure_pursuit_wrapper::PurePursuitWrapperNode::parameter_update_callback ( const std::vector< rclcpp::Parameter > & parameters )

Example callback for dynamic parameter updates.

Definition at line 160 of file pure_pursuit_wrapper.cpp.

{
  auto error_double = update_params<double>({
    {"vehicle_response_lag", config_.vehicle_response_lag},
    {"minimum_lookahead_distance", config_.minimum_lookahead_distance},
    {"maximum_lookahead_distance", config_.maximum_lookahead_distance},
    {"speed_to_lookahead_ratio", config_.speed_to_lookahead_ratio},
    {"emergency_stop_distance", config_.emergency_stop_distance},
    {"speed_thres_traveling_direction", config_.speed_thres_traveling_direction},
    {"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},
    }, parameters);
 
  auto error_bool = update_params<bool>({
    {"is_interpolate_lookahead_point", config_.is_interpolate_lookahead_point},
    {"is_delay_compensation", config_.is_delay_compensation},
    {"is_integrator_enabled", config_.is_integrator_enabled}
    }, parameters);
 
  rcl_interfaces::msg::SetParametersResult result;
 
  result.successful = !error_double && !error_bool;
 
  return result;
}

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ remove_repeated_timestamps()

std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > pure_pursuit_wrapper::PurePursuitWrapperNode::remove_repeated_timestamps ( const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > & traj_points )

Drops any points that sequentially have same target_time and return new trajectory_points in order to avoid divide by zero situation.

Parameters

traj_points Velocity profile to shift. The first point should be the current vehicle speed

NOTE: This function assumes the target_time will not go backwards. In other words, it only removes "sequential" points that have same target_time

Returns: A new trajectory without any repeated time_stamps

Definition at line 199 of file pure_pursuit_wrapper.cpp.

{
 
  std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> new_traj_points;
 
  carma_planning_msgs::msg::TrajectoryPlanPoint prev_point;
  bool first = true;
 
  for(auto point : traj_points){
 
    if(first){
      first = false;
      prev_point = point;
      new_traj_points.push_back(point);
      continue;
    }
 
    if(point.target_time != prev_point.target_time){
      new_traj_points.push_back(point);
      prev_point = point;
    }
    else{
      RCLCPP_DEBUG_STREAM(rclcpp::get_logger("pure_pursuit_wrapper"), "Duplicate point found");
    }
  }
 
  return new_traj_points;
 
}

References basic_autonomy::get_logger(), and process_traj_logs::point.

Here is the call graph for this function:

Member Data Documentation

◆ config_

PurePursuitWrapperConfig pure_pursuit_wrapper::PurePursuitWrapperNode::config_

private

Definition at line 78 of file pure_pursuit_wrapper.hpp.

Referenced by PurePursuitWrapperNode(), generate_command(), on_configure_plugin(), and parameter_update_callback().

◆ pp_

std::shared_ptr<pure_pursuit::PurePursuit> pure_pursuit_wrapper::PurePursuitWrapperNode::pp_

private

Definition at line 80 of file pure_pursuit_wrapper.hpp.

Referenced by generate_command(), get_pure_pursuit_worker(), and on_configure_plugin().

The documentation for this class was generated from the following files:

pure_pursuit_wrapper/include/pure_pursuit_wrapper/pure_pursuit_wrapper.hpp
pure_pursuit_wrapper/src/pure_pursuit_wrapper.cpp

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ PurePursuitWrapperNode()

Member Function Documentation

◆ convert_cmd()

◆ convert_state()

◆ FRIEND_TEST()

◆ generate_command()

◆ get_availability()

◆ get_pure_pursuit_worker()

◆ get_version_id()

◆ on_configure_plugin()

◆ parameter_update_callback()

◆ remove_repeated_timestamps()

Member Data Documentation

◆ config_

◆ pp_