From 74c2a75b76f908ca7562e4336953ce69b3990cb8 Mon Sep 17 00:00:00 2001 From: Timothy Wiley Date: Tue, 18 Jul 2017 13:17:42 +1000 Subject: [PATCH] crosbot_explore: convert explore status to int/enum --- .../basicClient/actionParameters.hpp | 23 +++++++++++-------- .../include/crosbot_explore/explorer.hpp | 9 ++------ .../include/crosbot_explore/explorerROS.hpp | 2 +- crosbot_explore/msg/ExplorerFeedback.msg | 8 ++++++- .../src/basicClient/actionParameters.cpp | 21 +++++++++++++---- .../src/basicClient/basicClient.cpp | 3 ++- crosbot_explore/src/explorer.cpp | 8 +++---- crosbot_explore/src/explorerROS.cpp | 6 +---- 8 files changed, 48 insertions(+), 32 deletions(-) diff --git a/crosbot_explore/include/crosbot_explore/basicClient/actionParameters.hpp b/crosbot_explore/include/crosbot_explore/basicClient/actionParameters.hpp index 4f35949..d1f5082 100644 --- a/crosbot_explore/include/crosbot_explore/basicClient/actionParameters.hpp +++ b/crosbot_explore/include/crosbot_explore/basicClient/actionParameters.hpp @@ -18,11 +18,11 @@ namespace crosbot_explore { class ExploreMode { public: enum { - MODE_STOP = 0, - MODE_RESUME = 1, - MODE_LEFT_WALL = 2, + MODE_STOP = 0, + MODE_RESUME = 1, + MODE_LEFT_WALL = 2, MODE_RIGHT_WALL = 3, - MODE_WAYPOINT = 4 + MODE_WAYPOINT = 4 }; }; @@ -30,12 +30,17 @@ public: * Common exploration status feedback for crosbot explorer - placed in header files to allow extensible status messages * which cannot be achieved through the explore.action class */ -class ExploreStatus { +class ExplorerStatus { public: - static const std::string STATUS_ARRIVED; - static const std::string STATUS_BLOCKED; - static const std::string STATUS_PAUSED; - static const std::string STATUS_RUNNNING; + enum Status { + STATUS_PAUSED = 0, + STATUS_RUNNING = 1, + STATUS_ARRIVED = 2, + STATUS_BLOCKED = 3 + }; + + static crosbot_explore::ExplorerStatus::Status statusFromInt(int status); + static std::string statusToString(crosbot_explore::ExplorerStatus::Status status); }; } // namespace crosbot_explore diff --git a/crosbot_explore/include/crosbot_explore/explorer.hpp b/crosbot_explore/include/crosbot_explore/explorer.hpp index 679c85c..997cf81 100644 --- a/crosbot_explore/include/crosbot_explore/explorer.hpp +++ b/crosbot_explore/include/crosbot_explore/explorer.hpp @@ -147,15 +147,10 @@ public: virtual void pause(); virtual void resume(); - virtual void statusChanged(const std::string& status) {} -protected: - // StatusChanged strings - std::string STATUS_ARRIVED; - std::string STATUS_BLOCKED; - std::string STATUS_RUNNING; - std::string STATUS_PAUSED; + virtual void statusChanged(crosbot_explore::ExplorerStatus::Status status) {} + }; } // namespace crosbot diff --git a/crosbot_explore/include/crosbot_explore/explorerROS.hpp b/crosbot_explore/include/crosbot_explore/explorerROS.hpp index 9223fef..045e832 100644 --- a/crosbot_explore/include/crosbot_explore/explorerROS.hpp +++ b/crosbot_explore/include/crosbot_explore/explorerROS.hpp @@ -77,7 +77,7 @@ public: virtual Pose findDriveTarget(const VoronoiGrid& voronoi, const Pose& robot); virtual bool stopMotors(); virtual void driveTo(const Pose& relativePosition); - virtual void statusChanged(const std::string& status); + virtual void statusChanged(crosbot_explore::ExplorerStatus::Status status); // ROS callbacks void callbackOccGrid(const nav_msgs::OccupancyGridConstPtr& latestMap); diff --git a/crosbot_explore/msg/ExplorerFeedback.msg b/crosbot_explore/msg/ExplorerFeedback.msg index 6d90d77..54b9379 100644 --- a/crosbot_explore/msg/ExplorerFeedback.msg +++ b/crosbot_explore/msg/ExplorerFeedback.msg @@ -1,8 +1,14 @@ # Corresponding Action id int32 id +# Status (as listed in crosbot_explore/explorer.hpp) +int32 STATUS_PAUSED = 0 +int32 STATUS_RUNNING = 1 +int32 STATUS_ARRIVED = 2 +int32 STATUS_BLOCKED = 3 +int32 status + # Keep updated on continuing state of exploration # Search strategy uses mode numbers -string status bool operating int32 searchStrategy diff --git a/crosbot_explore/src/basicClient/actionParameters.cpp b/crosbot_explore/src/basicClient/actionParameters.cpp index 04f1c8b..b23733e 100644 --- a/crosbot_explore/src/basicClient/actionParameters.cpp +++ b/crosbot_explore/src/basicClient/actionParameters.cpp @@ -9,9 +9,22 @@ namespace crosbot_explore { -const std::string ExploreStatus::STATUS_ARRIVED = "Arrived"; -const std::string ExploreStatus::STATUS_BLOCKED = "Blocked"; -const std::string ExploreStatus::STATUS_PAUSED = "Paused"; -const std::string ExploreStatus::STATUS_RUNNNING = "Running"; +crosbot_explore::ExplorerStatus::Status ExplorerStatus::statusFromInt(int status) { + return static_cast(status); +} + +std::string ExplorerStatus::statusToString(crosbot_explore::ExplorerStatus::Status status) { + std::string statusStr = ""; + if (status == crosbot_explore::ExplorerStatus::Status::STATUS_ARRIVED) { + statusStr = "Arrived"; + } else if (status == crosbot_explore::ExplorerStatus::Status::STATUS_BLOCKED) { + statusStr = "Blocked"; + } else if (status == crosbot_explore::ExplorerStatus::Status::STATUS_PAUSED) { + statusStr = "Paused"; + } else if (status == crosbot_explore::ExplorerStatus::Status::STATUS_RUNNING) { + statusStr = "Running"; + } + return statusStr; +} } // namespace crosbot_explore diff --git a/crosbot_explore/src/basicClient/basicClient.cpp b/crosbot_explore/src/basicClient/basicClient.cpp index 3a5985c..6d28bc4 100644 --- a/crosbot_explore/src/basicClient/basicClient.cpp +++ b/crosbot_explore/src/basicClient/basicClient.cpp @@ -135,7 +135,8 @@ void BasicExplorerClient::callback_explorerFeedback(const crosbot_explore::Explo searchStrategy = "waypoint"; } - STATUS_INFO(crosbotStatus, "Explorer feedback: %s (%s)", feedback->status.c_str(), searchStrategy.c_str()); + crosbot_explore::ExplorerStatus::Status status = crosbot_explore::ExplorerStatus::statusFromInt(feedback->status); + STATUS_INFO(crosbotStatus, "Explorer feedback: %s (%s)", ExplorerStatus::statusToString(status).c_str(), searchStrategy.c_str()); } crosbot::Pose3D BasicExplorerClient::getCommandPoint(const crosbot_msgs::ControlCommandPtr command) { diff --git a/crosbot_explore/src/explorer.cpp b/crosbot_explore/src/explorer.cpp index fcd98be..b39ecb1 100644 --- a/crosbot_explore/src/explorer.cpp +++ b/crosbot_explore/src/explorer.cpp @@ -159,7 +159,7 @@ void Explorer::shutdown() { void Explorer::pause() { paused = true; - statusChanged(STATUS_PAUSED); + statusChanged(crosbot_explore::ExplorerStatus::Status::STATUS_PAUSED); hysteresis.reset(); } @@ -171,7 +171,7 @@ void Explorer::resume() { paused = false; stopSent = false; - statusChanged(STATUS_RUNNING); + statusChanged(crosbot_explore::ExplorerStatus::Status::STATUS_RUNNING); } Pose Explorer::findDriveTarget(const VoronoiGrid& voronoi, const Pose& robot) { @@ -764,7 +764,7 @@ Pose Explorer::findWaypointTarget(const VoronoiGrid& voronoi, const Pose& robot) searchParams.atDestination = true; searchParams.rotateToPose = false; ROS_INFO("Explorer: Arrived at destination (%.2lf, %.2lf, %.2lf)", destination.x, destination.y, destination.z); - statusChanged(STATUS_ARRIVED); + statusChanged(crosbot_explore::ExplorerStatus::Status::STATUS_ARRIVED); } searchParams.waypoint = Pose(INFINITY, INFINITY, INFINITY); driveToTarget = Pose(INFINITY, INFINITY, INFINITY); @@ -840,7 +840,7 @@ Pose Explorer::findWaypointTarget(const VoronoiGrid& voronoi, const Pose& robot) searchParams.waypoint.position.x, searchParams.waypoint.position.y, searchParams.waypoint.position.z); } searchParams.pathBlocked = true; - statusChanged(STATUS_BLOCKED); + statusChanged(crosbot_explore::ExplorerStatus::Status::STATUS_BLOCKED); driveToTarget = Pose(INFINITY, INFINITY, INFINITY); } } diff --git a/crosbot_explore/src/explorerROS.cpp b/crosbot_explore/src/explorerROS.cpp index 603081d..5324b68 100644 --- a/crosbot_explore/src/explorerROS.cpp +++ b/crosbot_explore/src/explorerROS.cpp @@ -29,10 +29,6 @@ ExplorerROSNode::ExplorerROSNode() : currentGoalId(-1), latestPose(Pose(INFINITY, INFINITY, INFINITY)) { - STATUS_ARRIVED = crosbot_explore::ExploreStatus::STATUS_ARRIVED; - STATUS_BLOCKED = crosbot_explore::ExploreStatus::STATUS_BLOCKED; - STATUS_RUNNING = crosbot_explore::ExploreStatus::STATUS_RUNNNING; - STATUS_PAUSED = crosbot_explore::ExploreStatus::STATUS_PAUSED; } ExplorerROSNode::~ExplorerROSNode() { @@ -514,7 +510,7 @@ void ExplorerROSNode::publishReconfig(VoronoiGrid::Constraints& constraints) { reconfig_server->updateConfig(config); } -void ExplorerROSNode::statusChanged(const std::string& status) { +void ExplorerROSNode::statusChanged(crosbot_explore::ExplorerStatus::Status status) { crosbot_explore::ExplorerFeedback feedback; feedback.id = currentGoalId; feedback.status = status; -- GitLab