diff --git a/crosbot_explore/include/crosbot_explore/basicClient/actionParameters.hpp b/crosbot_explore/include/crosbot_explore/basicClient/actionParameters.hpp index 4f359495a815ba287fd8dd5e709aac6a63da3a91..d1f508260b564d8cb8be08b9304aa18c8f78c9d8 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 679c85c8b0bc391f627ea2a1378652f25b14db56..997cf81be688d54360ab92cf21554e1310a13d0c 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 9223fefff93dd6a4eb18ab493787c42a688cc429..045e8323ce6d4ab3248ea5e4efc3c2200dcbf5d0 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 6d90d77481586d9460a55a83e7a21ce6673e3fa0..54b937982c5b26f1a765ac236aa1394139163aa0 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 04f1c8ba97720e48e236b4b7f55ed02626402503..b23733e1bd606eb48f5232532e5c2c9ee2c8ec13 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 3a5985ca5bd75471e4cdb4db04eefc8645d18896..6d28bc423bf90bbb37c061360da080f4e80f2266 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 fcd98be5ccc696ced4cd049319ef999648078d13..b39ecb1bfac11294779d0981ba1a29d77c5fa9be 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 603081d5e28eb36db6c08ad0d55f17080bc80048..5324b68a6b19787ae2f6573c7dc272f0c6c6207d 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;