diff --git a/pure_pursuit_core/include/pure_pursuit_core/heading_control/AckermannSteeringController.hpp b/pure_pursuit_core/include/pure_pursuit_core/heading_control/AckermannSteeringController.hpp index 2d9d0e4..0bd61a0 100644 --- a/pure_pursuit_core/include/pure_pursuit_core/heading_control/AckermannSteeringController.hpp +++ b/pure_pursuit_core/include/pure_pursuit_core/heading_control/AckermannSteeringController.hpp @@ -22,8 +22,6 @@ struct AckermannSteeringCtrlParameters : public HeadingControllerParameters { std::string asString() const override; }; - - class AckermannSteeringController : public HeadingController { public: AckermannSteeringController() = default; diff --git a/pure_pursuit_core/src/AckermannSteeringController.cpp b/pure_pursuit_core/src/AckermannSteeringController.cpp index 77bf981..2516000 100644 --- a/pure_pursuit_core/src/AckermannSteeringController.cpp +++ b/pure_pursuit_core/src/AckermannSteeringController.cpp @@ -14,20 +14,18 @@ namespace pure_pursuit { namespace { - const double kRadToDeg = 180.0 / M_PI; +const double kRadToDeg = 180.0 / M_PI; } std::string AckermannSteeringCtrlParameters::asString() const { - std::string ret = HeadingControllerParameters::asString() + "\n"; + std::string ret = HeadingControllerParameters::asString() + "\n"; - ret += "wheel base (m): " + std::to_string(wheelBase_) + "\n"; - ret += "max steering angle magnitued (deg): " - + std::to_string(kRadToDeg * maxSteeringAngleMagnitude_) + "\n"; - ret += "max steering rate of change (deg/s): " - + std::to_string(kRadToDeg * maxSteeringRateOfChange_) + "\n"; - ret += "dt (sec): " + std::to_string(dt_) + "\n"; + ret += "wheel base (m): " + std::to_string(wheelBase_) + "\n"; + ret += "max steering angle magnitued (deg): " + std::to_string(kRadToDeg * maxSteeringAngleMagnitude_) + "\n"; + ret += "max steering rate of change (deg/s): " + std::to_string(kRadToDeg * maxSteeringRateOfChange_) + "\n"; + ret += "dt (sec): " + std::to_string(dt_) + "\n"; - return ret; + return ret; } bool AckermannSteeringController::advanceImpl() { diff --git a/pure_pursuit_core/src/AdaptiveVelocityController.cpp b/pure_pursuit_core/src/AdaptiveVelocityController.cpp index b411346..69ab911 100644 --- a/pure_pursuit_core/src/AdaptiveVelocityController.cpp +++ b/pure_pursuit_core/src/AdaptiveVelocityController.cpp @@ -11,16 +11,13 @@ namespace pure_pursuit { - -std::string AdaptiveVelocityControllerParameters::asString() const{ - std::string ret; - ret += "desired velocity (m/s): " + std::to_string(desiredVelocity_) + "\n"; - ret += "distance to goal when braking starts (m): " - + std::to_string(distanceToGoalWhenBrakingStarts_) + "\n"; - ret += "max velocity rate of change (m/s2): " - + std::to_string(maxVelocityRateOfChange_) + "\n"; - ret += "timestep (sec): " + std::to_string(timestep_); - return ret; +std::string AdaptiveVelocityControllerParameters::asString() const { + std::string ret; + ret += "desired velocity (m/s): " + std::to_string(desiredVelocity_) + "\n"; + ret += "distance to goal when braking starts (m): " + std::to_string(distanceToGoalWhenBrakingStarts_) + "\n"; + ret += "max velocity rate of change (m/s2): " + std::to_string(maxVelocityRateOfChange_) + "\n"; + ret += "timestep (sec): " + std::to_string(timestep_); + return ret; } bool AdaptiveVelocityController::computeVelocity() { diff --git a/pure_pursuit_core/src/ConstantVelocityController.cpp b/pure_pursuit_core/src/ConstantVelocityController.cpp index d6b900d..219a38f 100644 --- a/pure_pursuit_core/src/ConstantVelocityController.cpp +++ b/pure_pursuit_core/src/ConstantVelocityController.cpp @@ -9,15 +9,14 @@ namespace pure_pursuit { -void ConstantVelocityController::updateDrivingDirection(DrivingDirection drivingDirection){ - if (drivingDirection != drivingDirection_){ - rateLimiter_.reset(0.0); // assumption is that the robot is at zero velocity when changing directions - std::cout << "reseting the velocity rate limiter" << std::endl; - } - drivingDirection_ = drivingDirection; +void ConstantVelocityController::updateDrivingDirection(DrivingDirection drivingDirection) { + if (drivingDirection != drivingDirection_) { + rateLimiter_.reset(0.0); // assumption is that the robot is at zero velocity when changing directions + std::cout << "reseting the velocity rate limiter" << std::endl; + } + drivingDirection_ = drivingDirection; } - bool ConstantVelocityController::computeVelocity() { switch (drivingDirection_) { case DrivingDirection::FWD: { diff --git a/pure_pursuit_core/src/HeadingController.cpp b/pure_pursuit_core/src/HeadingController.cpp index 65b82a2..0827ae4 100644 --- a/pure_pursuit_core/src/HeadingController.cpp +++ b/pure_pursuit_core/src/HeadingController.cpp @@ -14,17 +14,14 @@ namespace pure_pursuit { std::string HeadingControllerParameters::asString() const { - std::string ret; - ret += "Lookahead distance fwd: " + std::to_string(lookaheadDistanceFwd_) - + "\n"; - ret += "Lookahead distance bck: " + std::to_string(lookaheadDistanceBck_) - + "\n"; - ret += "Anchor distance fwd: " + std::to_string(anchorDistanceFwd_) + "\n"; - ret += "Anchor distance bck: " + std::to_string(anchorDistanceBck_) + "\n"; - ret += "dead zone width: " + std::to_string(deadZoneWidth_) + "\n"; - ret += "avg filter current sample weight: " - + std::to_string(avgFilgerCurrentSampleWeight_); - return ret; + std::string ret; + ret += "Lookahead distance fwd: " + std::to_string(lookaheadDistanceFwd_) + "\n"; + ret += "Lookahead distance bck: " + std::to_string(lookaheadDistanceBck_) + "\n"; + ret += "Anchor distance fwd: " + std::to_string(anchorDistanceFwd_) + "\n"; + ret += "Anchor distance bck: " + std::to_string(anchorDistanceBck_) + "\n"; + ret += "dead zone width: " + std::to_string(deadZoneWidth_) + "\n"; + ret += "avg filter current sample weight: " + std::to_string(avgFilgerCurrentSampleWeight_); + return ret; } bool HeadingController::initialize() { diff --git a/pure_pursuit_ros/include/pure_pursuit_ros/AckermannSteeringControllerRos.hpp b/pure_pursuit_ros/include/pure_pursuit_ros/AckermannSteeringControllerRos.hpp index d191941..c89f303 100644 --- a/pure_pursuit_ros/include/pure_pursuit_ros/AckermannSteeringControllerRos.hpp +++ b/pure_pursuit_ros/include/pure_pursuit_ros/AckermannSteeringControllerRos.hpp @@ -7,11 +7,10 @@ #pragma once -#include -#include "pure_pursuit_core/heading_control/AckermannSteeringController.hpp" #include #include - +#include +#include "pure_pursuit_core/heading_control/AckermannSteeringController.hpp" namespace pure_pursuit { @@ -23,8 +22,7 @@ class AckermannSteeringControllerRos : public AckermannSteeringController { void setParameters(const AckermannSteeringCtrlParameters& parameters) override; private: - - void ddCallback(pure_pursuit_ros::PurePursuitConfig &config, uint32_t level); + void ddCallback(pure_pursuit_ros::PurePursuitConfig& config, uint32_t level); void initRos(); bool advanceImpl() override; @@ -47,11 +45,10 @@ class AckermannSteeringControllerRos : public AckermannSteeringController { std::unique_ptr> ddServer_; dynamic_reconfigure::Server::CallbackType ddCalback_; pure_pursuit_ros::PurePursuitConfig ddConfig_; - }; -void updateFromDD(const pure_pursuit_ros::PurePursuitConfig &config, AckermannSteeringCtrlParameters *param); -void updateDD(const AckermannSteeringCtrlParameters ¶m, pure_pursuit_ros::PurePursuitConfig *config); +void updateFromDD(const pure_pursuit_ros::PurePursuitConfig& config, AckermannSteeringCtrlParameters* param); +void updateDD(const AckermannSteeringCtrlParameters& param, pure_pursuit_ros::PurePursuitConfig* config); std::unique_ptr createAckermannSteeringControllerRos(const AckermannSteeringCtrlParameters& parameters, ros::NodeHandle* nh); diff --git a/pure_pursuit_ros/include/pure_pursuit_ros/AdaptiveVelocityControllerRos.hpp b/pure_pursuit_ros/include/pure_pursuit_ros/AdaptiveVelocityControllerRos.hpp index 4850860..55a3874 100644 --- a/pure_pursuit_ros/include/pure_pursuit_ros/AdaptiveVelocityControllerRos.hpp +++ b/pure_pursuit_ros/include/pure_pursuit_ros/AdaptiveVelocityControllerRos.hpp @@ -7,47 +7,37 @@ #pragma once -#include -#include "pure_pursuit_core/velocity_control/AdaptiveVelocityController.hpp" #include #include - +#include +#include "pure_pursuit_core/velocity_control/AdaptiveVelocityController.hpp" namespace pure_pursuit { -class AdaptiveVelocityControllerRos: public AdaptiveVelocityController { - using BASE = AdaptiveVelocityController; - -public: - explicit AdaptiveVelocityControllerRos(ros::NodeHandle *nh); - void setParameters(const AdaptiveVelocityControllerParameters ¶meters) - override; +class AdaptiveVelocityControllerRos : public AdaptiveVelocityController { + using BASE = AdaptiveVelocityController; -private: + public: + explicit AdaptiveVelocityControllerRos(ros::NodeHandle* nh); + void setParameters(const AdaptiveVelocityControllerParameters& parameters) override; - void ddCallback(pure_pursuit_ros::PurePursuitAdaptiveVelConfig &config, - uint32_t level); - void initRos(); + private: + void ddCallback(pure_pursuit_ros::PurePursuitAdaptiveVelConfig& config, uint32_t level); + void initRos(); - ros::NodeHandle *nh_; - ros::NodeHandle ddnh_; - - boost::recursive_mutex ddMutex_; - std::unique_ptr< - dynamic_reconfigure::Server< - pure_pursuit_ros::PurePursuitAdaptiveVelConfig>> ddServer_; - dynamic_reconfigure::Server::CallbackType ddCalback_; - pure_pursuit_ros::PurePursuitAdaptiveVelConfig ddConfig_; + ros::NodeHandle* nh_; + ros::NodeHandle ddnh_; + boost::recursive_mutex ddMutex_; + std::unique_ptr> ddServer_; + dynamic_reconfigure::Server::CallbackType ddCalback_; + pure_pursuit_ros::PurePursuitAdaptiveVelConfig ddConfig_; }; -void updateFromDD(const pure_pursuit_ros::PurePursuitAdaptiveVelConfig &config, - AdaptiveVelocityControllerParameters *param); -void updateDD(const AdaptiveVelocityControllerParameters ¶m, - pure_pursuit_ros::PurePursuitAdaptiveVelConfig *config); - +void updateFromDD(const pure_pursuit_ros::PurePursuitAdaptiveVelConfig& config, AdaptiveVelocityControllerParameters* param); +void updateDD(const AdaptiveVelocityControllerParameters& param, pure_pursuit_ros::PurePursuitAdaptiveVelConfig* config); std::unique_ptr createAdaptiveVelocityControllerRos(const AdaptiveVelocityControllerParameters& parameters, - ros::NodeHandle* nh); + ros::NodeHandle* nh); } /* namespace pure_pursuit*/ diff --git a/pure_pursuit_ros/src/AckermannSteeringControllerRos.cpp b/pure_pursuit_ros/src/AckermannSteeringControllerRos.cpp index 3c6ad48..af58cfb 100644 --- a/pure_pursuit_ros/src/AckermannSteeringControllerRos.cpp +++ b/pure_pursuit_ros/src/AckermannSteeringControllerRos.cpp @@ -14,32 +14,29 @@ namespace pure_pursuit { namespace { - const double kDegToRad = M_PI / 180.0; - const double kRadToDeg = 180.0 / M_PI; -} +const double kDegToRad = M_PI / 180.0; +const double kRadToDeg = 180.0 / M_PI; +} // namespace AckermannSteeringControllerRos::AckermannSteeringControllerRos(ros::NodeHandle* nh) : BASE(), nh_(nh), ddnh_("ackerman_steering") { - ddServer_ = std::make_unique>(ddMutex_,ddnh_); + ddServer_ = std::make_unique>(ddMutex_, ddnh_); initRos(); } - -void AckermannSteeringControllerRos::ddCallback(pure_pursuit_ros::PurePursuitConfig &config, uint32_t level){ - ddConfig_ = config; - updateFromDD(ddConfig_, ¶meters_); - ROS_INFO("Reconfigure Request: \n %s", - parameters_.asString().c_str()); +void AckermannSteeringControllerRos::ddCallback(pure_pursuit_ros::PurePursuitConfig& config, uint32_t level) { + ddConfig_ = config; + updateFromDD(ddConfig_, ¶meters_); + ROS_INFO("Reconfigure Request: \n %s", parameters_.asString().c_str()); } -void AckermannSteeringControllerRos::setParameters(const AckermannSteeringCtrlParameters& parameters){ - BASE::setParameters(parameters); - updateDD(parameters,&ddConfig_); - ddServer_->updateConfig(ddConfig_); +void AckermannSteeringControllerRos::setParameters(const AckermannSteeringCtrlParameters& parameters) { + BASE::setParameters(parameters); + updateDD(parameters, &ddConfig_); + ddServer_->updateConfig(ddConfig_); } void AckermannSteeringControllerRos::initRos() { - - ddCalback_ = boost::bind(&AckermannSteeringControllerRos::ddCallback,this, _1, _2); + ddCalback_ = boost::bind(&AckermannSteeringControllerRos::ddCallback, this, _1, _2); ddServer_->setCallback(ddCalback_); lookaheadPointPub_ = nh_->advertise("pure_pursuit_heading_control/lookahead_point", 1, true); @@ -149,33 +146,28 @@ std::unique_ptr createAckermannSteeringControllerRos(const Ac return std::move(ctrl); } -void updateFromDD(const pure_pursuit_ros::PurePursuitConfig &config, AckermannSteeringCtrlParameters *param){ - param->lookaheadDistanceFwd_ = config.lookahead_fwd; - param->lookaheadDistanceBck_ = config.lookahead_bck; - param->anchorDistanceFwd_ = config.anchor_dist_fwd; - param->anchorDistanceBck_ = config.anchor_dist_bck; - param->deadZoneWidth_ = config.dead_zone_width; - param->avgFilgerCurrentSampleWeight_ = config.avg_filter_current_sample_weight; - param->maxSteeringAngleMagnitude_ = config.max_steering_angle_magnitude_in_deg * kDegToRad; - param->maxSteeringRateOfChange_ = config.max_steering_rate_of_change_in_deg_per_sec * kDegToRad; - param->wheelBase_ = config.wheel_base; - - +void updateFromDD(const pure_pursuit_ros::PurePursuitConfig& config, AckermannSteeringCtrlParameters* param) { + param->lookaheadDistanceFwd_ = config.lookahead_fwd; + param->lookaheadDistanceBck_ = config.lookahead_bck; + param->anchorDistanceFwd_ = config.anchor_dist_fwd; + param->anchorDistanceBck_ = config.anchor_dist_bck; + param->deadZoneWidth_ = config.dead_zone_width; + param->avgFilgerCurrentSampleWeight_ = config.avg_filter_current_sample_weight; + param->maxSteeringAngleMagnitude_ = config.max_steering_angle_magnitude_in_deg * kDegToRad; + param->maxSteeringRateOfChange_ = config.max_steering_rate_of_change_in_deg_per_sec * kDegToRad; + param->wheelBase_ = config.wheel_base; } -void updateDD(const AckermannSteeringCtrlParameters ¶m, - pure_pursuit_ros::PurePursuitConfig *config) { - config->lookahead_fwd = param.lookaheadDistanceFwd_; - config->lookahead_fwd = param.lookaheadDistanceFwd_; - config->lookahead_bck = param.lookaheadDistanceBck_; - config->anchor_dist_fwd = param.anchorDistanceFwd_; - config->anchor_dist_bck = param.anchorDistanceBck_; - config->dead_zone_width = param.deadZoneWidth_; - config->avg_filter_current_sample_weight = - param.avgFilgerCurrentSampleWeight_; - config->max_steering_angle_magnitude_in_deg =kRadToDeg * param.maxSteeringAngleMagnitude_; - config->max_steering_rate_of_change_in_deg_per_sec = kRadToDeg * param.maxSteeringRateOfChange_; - config->wheel_base = param.wheelBase_; - +void updateDD(const AckermannSteeringCtrlParameters& param, pure_pursuit_ros::PurePursuitConfig* config) { + config->lookahead_fwd = param.lookaheadDistanceFwd_; + config->lookahead_fwd = param.lookaheadDistanceFwd_; + config->lookahead_bck = param.lookaheadDistanceBck_; + config->anchor_dist_fwd = param.anchorDistanceFwd_; + config->anchor_dist_bck = param.anchorDistanceBck_; + config->dead_zone_width = param.deadZoneWidth_; + config->avg_filter_current_sample_weight = param.avgFilgerCurrentSampleWeight_; + config->max_steering_angle_magnitude_in_deg = kRadToDeg * param.maxSteeringAngleMagnitude_; + config->max_steering_rate_of_change_in_deg_per_sec = kRadToDeg * param.maxSteeringRateOfChange_; + config->wheel_base = param.wheelBase_; } } /* namespace pure_pursuit */ diff --git a/pure_pursuit_ros/src/AdaptiveVelocityControllerRos.cpp b/pure_pursuit_ros/src/AdaptiveVelocityControllerRos.cpp index c8dabaf..b3124c3 100644 --- a/pure_pursuit_ros/src/AdaptiveVelocityControllerRos.cpp +++ b/pure_pursuit_ros/src/AdaptiveVelocityControllerRos.cpp @@ -9,52 +9,42 @@ #include "pure_pursuit_core/math.hpp" #include "se2_visualization_ros/visualization_helpers.hpp" - - namespace pure_pursuit { AdaptiveVelocityControllerRos::AdaptiveVelocityControllerRos(ros::NodeHandle* nh) : BASE(), nh_(nh), ddnh_("adaptive_vel_controller") { - ddServer_ = std::make_unique>(ddMutex_,ddnh_); + ddServer_ = std::make_unique>(ddMutex_, ddnh_); initRos(); } - -void AdaptiveVelocityControllerRos::ddCallback(pure_pursuit_ros::PurePursuitAdaptiveVelConfig &config, uint32_t level){ - ddConfig_ = config; - updateFromDD(ddConfig_, ¶meters_); - ROS_INFO("Reconfigure Request: \n %s", - parameters_.asString().c_str()); +void AdaptiveVelocityControllerRos::ddCallback(pure_pursuit_ros::PurePursuitAdaptiveVelConfig& config, uint32_t level) { + ddConfig_ = config; + updateFromDD(ddConfig_, ¶meters_); + ROS_INFO("Reconfigure Request: \n %s", parameters_.asString().c_str()); } -void AdaptiveVelocityControllerRos::setParameters(const AdaptiveVelocityControllerParameters& parameters){ - BASE::setParameters(parameters); - updateDD(parameters,&ddConfig_); - ddServer_->updateConfig(ddConfig_); +void AdaptiveVelocityControllerRos::setParameters(const AdaptiveVelocityControllerParameters& parameters) { + BASE::setParameters(parameters); + updateDD(parameters, &ddConfig_); + ddServer_->updateConfig(ddConfig_); } void AdaptiveVelocityControllerRos::initRos() { - - ddCalback_ = boost::bind(&AdaptiveVelocityControllerRos::ddCallback,this, _1, _2); + ddCalback_ = boost::bind(&AdaptiveVelocityControllerRos::ddCallback, this, _1, _2); ddServer_->setCallback(ddCalback_); - } -void updateFromDD(const pure_pursuit_ros::PurePursuitAdaptiveVelConfig &config, AdaptiveVelocityControllerParameters *param){ - -param->desiredVelocity_ = config.nominal_velocity; -param->maxVelocityRateOfChange_ = config.max_vel_rate_of_change; -param->distanceToGoalWhenBrakingStarts_ = config.distance_when_braking_starts; - +void updateFromDD(const pure_pursuit_ros::PurePursuitAdaptiveVelConfig& config, AdaptiveVelocityControllerParameters* param) { + param->desiredVelocity_ = config.nominal_velocity; + param->maxVelocityRateOfChange_ = config.max_vel_rate_of_change; + param->distanceToGoalWhenBrakingStarts_ = config.distance_when_braking_starts; } -void updateDD(const AdaptiveVelocityControllerParameters ¶m, - pure_pursuit_ros::PurePursuitAdaptiveVelConfig *config) { - config->distance_when_braking_starts = param.distanceToGoalWhenBrakingStarts_; - config->max_vel_rate_of_change = param.maxVelocityRateOfChange_; - config->nominal_velocity = param.desiredVelocity_; - +void updateDD(const AdaptiveVelocityControllerParameters& param, pure_pursuit_ros::PurePursuitAdaptiveVelConfig* config) { + config->distance_when_braking_starts = param.distanceToGoalWhenBrakingStarts_; + config->max_vel_rate_of_change = param.maxVelocityRateOfChange_; + config->nominal_velocity = param.desiredVelocity_; } std::unique_ptr createAdaptiveVelocityControllerRos(const AdaptiveVelocityControllerParameters& parameters, - ros::NodeHandle* nh) { + ros::NodeHandle* nh) { auto ctrl = std::make_unique(nh); ctrl->setParameters(parameters); return std::move(ctrl); diff --git a/pure_pursuit_ros/src/dd_standalone_example.cpp b/pure_pursuit_ros/src/dd_standalone_example.cpp index 74e9a15..31e7061 100644 --- a/pure_pursuit_ros/src/dd_standalone_example.cpp +++ b/pure_pursuit_ros/src/dd_standalone_example.cpp @@ -5,19 +5,16 @@ * Author: jelavice */ - - #include #include #include -void callback(pure_pursuit_ros::PurePursuitConfig &config, uint32_t level) { - ROS_INFO("Reconfigure Request: \n LookaheadDistance: %f ", - config.lookahead_fwd); +void callback(pure_pursuit_ros::PurePursuitConfig& config, uint32_t level) { + ROS_INFO("Reconfigure Request: \n LookaheadDistance: %f ", config.lookahead_fwd); } -int main(int argc, char **argv) { +int main(int argc, char** argv) { ros::init(argc, argv, "dd_standalone_example"); boost::recursive_mutex ddMutex; @@ -35,7 +32,6 @@ int main(int argc, char **argv) { ros::spinOnce(); ROS_INFO("Now the lookahead_fwd should be %f in the rqt console", dummyCfg.lookahead_fwd); - ROS_INFO("Spinning node"); ros::spin(); return 0;