diff --git a/crosbot_explore/CMakeLists.txt b/crosbot_explore/CMakeLists.txt index ddb1431e6217d7ba75273f65c9dbe008927b9db9..0821a9477910e135e479fa8cfbbef2e5ccaab09f 100644 --- a/crosbot_explore/CMakeLists.txt +++ b/crosbot_explore/CMakeLists.txt @@ -6,14 +6,14 @@ project(crosbot_explore) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp - geometry_msgs + geometry_msgs nav_msgs dynamic_reconfigure message_generation - genmsg + genmsg actionlib_msgs actionlib - crosbot + crosbot crosbot_map crosbot_msgs ) @@ -74,11 +74,11 @@ catkin_package( INCLUDE_DIRS include LIBRARIES crosbot_explore CATKIN_DEPENDS - roscpp - geometry_msgs + roscpp + geometry_msgs nav_msgs actionlib_msgs - crosbot + crosbot crosbot_map crosbot_msgs # DEPENDS system_lib @@ -100,6 +100,7 @@ add_library(crosbot_explore src/astar.cpp src/explorer.cpp src/explorerROS.cpp + src/simplePID.cpp ) ## Declare a cpp executable @@ -114,20 +115,20 @@ add_dependencies(crosbot_explore ${PROJECT_NAME}_gencfg ${PROJECT_NAME}_generate_messages ) -add_dependencies(astar_explorer - ${PROJECT_NAME} +add_dependencies(astar_explorer + ${PROJECT_NAME} crosbot_explore ) add_dependencies(astar_planner ${PROJECT_NAME} crosbot_explore ) -add_dependencies(explorer +add_dependencies(explorer ${PROJECT_NAME} crosbot_explore ) -add_dependencies(basic_client - ${PROJECT_NAME} +add_dependencies(basic_client + ${PROJECT_NAME} crosbot_explore ) diff --git a/crosbot_explore/cfg/CrosbotExploreReconfig.cfg b/crosbot_explore/cfg/CrosbotExploreReconfig.cfg index 502c58a59279d85c5bb67d369e27b74234a20fe2..63155e39f4cab9b18c6e396303d5be5d56cfd207 100755 --- a/crosbot_explore/cfg/CrosbotExploreReconfig.cfg +++ b/crosbot_explore/cfg/CrosbotExploreReconfig.cfg @@ -22,6 +22,11 @@ gen.add("map_sub", str_t, 0, "Map Sub Topic", "map") gen.add("drive_max_vel", double_t, 0, "Voronoi Constraint: Expand", 0.3, 0, 2) gen.add("drive_max_turn", double_t, 0, "Voronoi Constraint: Expand", 0.3, 0, 2) +gen.add("drive_min_vel", double_t, 0, "Voronoi Constraint: Expand", 0.01, 0, 2) +gen.add("drive_smooth_vel", bool_t, 0, "Voronoi Constraint: Expand", False) +gen.add("drive_vel_p", double_t, 0, "Voronoi Constraint: Expand", 1, 0, 10) +gen.add("drive_vel_i", double_t, 0, "Voronoi Constraint: Expand", 0, 0, 10) +gen.add("drive_vel_d", double_t, 0, "Voronoi Constraint: Expand", 0, 0, 10) # This file name must match the same name as the 3rd parameter, or catkin will keep rebuilding the header file -exit(gen.generate(PACKAGE, "crosbot_explore_reconfig", "CrosbotExploreReconfig")) \ No newline at end of file +exit(gen.generate(PACKAGE, "crosbot_explore_reconfig", "CrosbotExploreReconfig")) diff --git a/crosbot_explore/include/crosbot_explore/driveParameters.hpp b/crosbot_explore/include/crosbot_explore/driveParameters.hpp index e3dd9db10f888954f805abc65d3958c3a43b0c93..de53e879c31365683f16ebc9b3d508c49c571afc 100644 --- a/crosbot_explore/include/crosbot_explore/driveParameters.hpp +++ b/crosbot_explore/include/crosbot_explore/driveParameters.hpp @@ -20,6 +20,10 @@ public: double minVel; double minTurn; double turnOnly; + bool smoothVel; + float velP; + float velI; + float velD; ReadWriteLock driveLock; Pose driveTarget; @@ -27,7 +31,8 @@ public: DriveParameters() : maxVel(0.5), maxTurn(DEG2RAD(15)), minVel(0.1), minTurn(0.1), - turnOnly(DEG2RAD(30)), driveTarget(INFINITY, INFINITY, INFINITY) + turnOnly(DEG2RAD(30)), driveTarget(INFINITY, INFINITY, INFINITY), + smoothVel(false), velP(1), velI(0), velD(0) {} inline Pose getDriveTarget() { diff --git a/crosbot_explore/include/crosbot_explore/explorerROS.hpp b/crosbot_explore/include/crosbot_explore/explorerROS.hpp index 045e8323ce6d4ab3248ea5e4efc3c2200dcbf5d0..e92d14ae62816799dd4ed2193831fe60ec3dcf24 100644 --- a/crosbot_explore/include/crosbot_explore/explorerROS.hpp +++ b/crosbot_explore/include/crosbot_explore/explorerROS.hpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -126,6 +127,9 @@ private: ReconfigServer::CallbackType reconfig_callbackType; boost::recursive_mutex mutexReconfig; bool gotReconfig; + + // PID to smooth velocity + SimplePID pidControl; }; } // namespace crosbot diff --git a/crosbot_explore/include/crosbot_explore/simplePID.hpp b/crosbot_explore/include/crosbot_explore/simplePID.hpp new file mode 100644 index 0000000000000000000000000000000000000000..2cb50978faf742f1c5eb8a6fa6d4960e3b37dddc --- /dev/null +++ b/crosbot_explore/include/crosbot_explore/simplePID.hpp @@ -0,0 +1,41 @@ +/** + * @brief Simple PID control + * + * Simple PID control which assumes constant rate + * + * @file simplePID.cpp + * @author German Castro, UNSW Australia + * @date 2018-04-19 + */ + +#ifndef SIMPLE_PID_H +#define SIMPLE_PID_H + +namespace crosbot { + +class SimplePID { + public: + SimplePID(); + SimplePID(float p, float i, float d, float rate); + virtual ~SimplePID(); + void setP(float p); + void setI(float i); + void setD(float d); + void setRate(float rate); + double pid(double error); + double newTarget(double target); + void reset(); + private: + double lastError; + double sumError; + double lastValue; + + double kP; + double kI; + double kD; + + float rate; + +}; +} +#endif // SIMPLE_PID_H \ No newline at end of file diff --git a/crosbot_explore/src/explorerROS.cpp b/crosbot_explore/src/explorerROS.cpp index b2ba5941d52a8498b79948fb67f394d2d28cf222..ad6c3aeaf80c4b0fa8fd7bd48f44f576fcc490eb 100644 --- a/crosbot_explore/src/explorerROS.cpp +++ b/crosbot_explore/src/explorerROS.cpp @@ -74,6 +74,16 @@ void ExplorerROSNode::callback_reconfigure(crosbot_explore::CrosbotExploreReconf driveParams.maxTurn = config.drive_max_turn; driveParams.maxVel = config.drive_max_vel; + driveParams.minVel = config.drive_min_vel; + driveParams.smoothVel = config.drive_smooth_vel; + driveParams.velP = config.drive_vel_p; + driveParams.velI = config.drive_vel_i; + driveParams.velD = config.drive_vel_d; + + pidControl.setP(driveParams.velP); + pidControl.setI(driveParams.velI); + pidControl.setD(driveParams.velD); + ROS_INFO("%s maxVel: %.3lf, maxTurn: %.3lf, minVel: %.3lf, smoothVel: %s, velP: %.3lf, velI: %.3lf, velD: %.3lf", LOG_START, driveParams.maxVel, driveParams.maxTurn, driveParams.minVel, driveParams.smoothVel?"true":"false", driveParams.velP, driveParams.velI, driveParams.velD); currentGoalId = -1; @@ -215,7 +225,12 @@ void ExplorerROSNode::configure() { driveParams.maxTurn = cfg->getParamAsDouble("maxTurn", driveParams.maxTurn); driveParams.minVel = cfg->getParamAsDouble("minVel", driveParams.minVel); driveParams.minTurn = cfg->getParamAsDouble("minTurn", driveParams.minTurn); - ROS_INFO("%s maxVel: %.2lf, maxTurn: %.2lf", LOG_START, driveParams.maxVel, driveParams.maxTurn); + driveParams.turnOnly = cfg->getParamAsDouble("turnOnly", driveParams.turnOnly); + driveParams.smoothVel = cfg->getParamAsBool("smoothVel", driveParams.smoothVel); + driveParams.velP = cfg->getParamAsDouble("velP", driveParams.velP); + driveParams.velI = cfg->getParamAsDouble("velI", driveParams.velI); + driveParams.velD = cfg->getParamAsDouble("velD", driveParams.velD); + ROS_INFO("%s maxVel: %.2lf, maxTurn: %.2lf, smoothVel: %s, velP: %.2lf, velI: %.2lf, velD: %.2lf", LOG_START, driveParams.maxVel, driveParams.maxTurn, driveParams.smoothVel?"true":"false", driveParams.velP, driveParams.velI, driveParams.velD); } ConfigElementPtr hysteresisCfg = cfg->getChild("hysteresis"); if (hysteresisCfg != NULL) { @@ -236,6 +251,12 @@ void ExplorerROSNode::configure() { voronoiImagePub = nh.advertise(imagePub_name, 1); velPub = nh.advertise(velPub_name, 1); + //Set pidControl to smooth velocity + pidControl.setP(driveParams.velP); + pidControl.setI(driveParams.velI); + pidControl.setD(driveParams.velD); + pidControl.setRate(rate_driveThread); + // Dynamic reconfiguration gotReconfig = false; reconfig_server = new ReconfigServer(mutexReconfig); @@ -383,8 +404,28 @@ bool ExplorerROSNode::stopMotors() { ROS_INFO("%s stopMotors()", LOG_START); geometry_msgs::Twist twist; - twist.linear.x = twist.linear.y = twist.linear.z = 0; twist.angular.x = twist.angular.y = twist.angular.z = 0; + twist.linear.y = twist.linear.z = 0; + + // We just set any non zero value + twist.linear.x = 1; + + // Smooth stop speed + if(driveParams.smoothVel){ + ros::Rate rate(rate_driveThread); + while(twist.linear.x >= driveParams.minVel || twist.linear.x <= -driveParams.minVel ){ + twist.linear.x = pidControl.newTarget(0.0); + + velPub.publish(twist); + + rate.sleep(); + } + } + + twist.linear.x = twist.linear.y = twist.linear.z = 0; + + // Be sure that the velocity smoother sets speed to 0 + pidControl.reset(); velPub.publish(twist); @@ -435,6 +476,12 @@ void ExplorerROSNode::driveTo(const Pose& relativePosition) { twist.linear.x = (d / searchParams.wallSearchDistance) * driveParams.maxVel * (1 - (fabs(a) / driveParams.turnOnly)); twist.linear.x = std::min(twist.linear.x, driveParams.maxVel); twist.linear.x = std::max(twist.linear.x, -driveParams.maxVel); + + // If we need to smooth velocity we apply a basic PID control + if(driveParams.smoothVel){ + twist.linear.x = pidControl.newTarget(twist.linear.x); + } + if (twist.linear.x < driveParams.minVel && twist.linear.x > -driveParams.minVel) { if (twist.linear.x > 0) { twist.linear.x = driveParams.minVel; @@ -442,6 +489,7 @@ void ExplorerROSNode::driveTo(const Pose& relativePosition) { twist.linear.x = -driveParams.minVel; } } + //ROS_INFO("%s twist command: linear.x: %.4lf, angular.z: %.4lf", LOG_START, twist.linear.x, twist.angular.z); } @@ -517,6 +565,11 @@ void ExplorerROSNode::publishReconfig(VoronoiGrid::Constraints& constraints) { config.drive_max_turn = driveParams.maxTurn; config.drive_max_vel = driveParams.maxVel; + config.drive_min_vel = driveParams.minVel; + config.drive_smooth_vel = driveParams.smoothVel; + config.drive_vel_p = driveParams.velP; + config.drive_vel_i = driveParams.velI; + config.drive_vel_d = driveParams.velD; // Call service reconfig_server->updateConfig(config); diff --git a/crosbot_explore/src/simplePID.cpp b/crosbot_explore/src/simplePID.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c2566b295e746a1e7823792ed243e92caa8c44a7 --- /dev/null +++ b/crosbot_explore/src/simplePID.cpp @@ -0,0 +1,114 @@ +/** + * @brief Simple PID control + * + * Simple PID control which assumes constant rate + * + * @file simplePID.cpp + * @author German Castro, UNSW Australia + * @date 2018-04-19 + */ + +#include + +#include + + +namespace crosbot { + +SimplePID::SimplePID() +{ + this->sumError = 0; + this->lastError = NAN; + this->lastValue = 0; + + this->rate = 1; +} + +SimplePID::SimplePID(float p, float i, float d, float rate) +{ + this->kP = p; + this->kI = i; + this->kD = d; + this->rate = rate; + + this->sumError = 0; + this->lastError = NAN; + this->lastValue = 0; +} + +SimplePID::~SimplePID() +{ + +} + +void SimplePID::setP(float p) +{ + this->kP = p; +} + +void SimplePID::setI(float i) +{ + this->kI = i; +} + +void SimplePID::setD(float d) +{ + this->kD = d; +} + +void SimplePID::setRate(float rate) +{ + this->rate = rate; +} + +void SimplePID::reset() +{ + this->sumError = 0; + this->lastError = NAN; + this->lastValue = 0; + +} + +/** + * @brief Calculates PID signal + * + * @param error + * @return double + */ +double SimplePID::pid(double error){ + // generalized PID formula + //correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors) + + // calculate a motor speed for the current conditions + if (std::isnan(error)) { + return 0; + } + + double pid = this->kP * error; + + pid += this->kI * (sumError) / rate; + if(!std::isnan(lastError)){ + pid += this->kD * (error - lastError) * rate; + } + // set the last and sumerrors for next loop iteration + lastError = error; + sumError += error; + + return pid; +} + +/** + * @brief Sets a new target assuming that current state is last value returned and constant time 1 / rate + * + * @param target + * @return double + */ +double SimplePID::newTarget(double target){ + double error = (target - lastValue); + double pid = this->pid(error); + lastValue = lastValue + pid; + return lastValue; +} + + +} \ No newline at end of file