diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index a17c3de..cc45c6f 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -7,15 +7,14 @@ namespace Modelec { - class ActionExecutor { public: - enum Action { NONE, DEPLOY_BANNER, + RETRACT_BANNER, TAKE_POT, PLACE_POT, }; @@ -36,6 +35,8 @@ namespace Modelec void DeployBanner(); + void RetractBanner(); + void TakePot(); void PlacePot(); @@ -75,5 +76,4 @@ namespace Modelec private: rclcpp::Node::SharedPtr node_; }; - -} \ No newline at end of file +} diff --git a/src/modelec_strat/include/modelec_strat/enemy_manager.hpp b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp index a86a725..760870a 100644 --- a/src/modelec_strat/include/modelec_strat/enemy_manager.hpp +++ b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp @@ -21,7 +21,6 @@ namespace Modelec void TimerCallback(); private: - rclcpp::Subscription::SharedPtr current_pos_sub_; rclcpp::Subscription::SharedPtr laser_scan_sub_; rclcpp::Subscription::SharedPtr state_sub_; diff --git a/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp index 2d00b61..7e3d836 100644 --- a/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp @@ -15,15 +15,16 @@ namespace Modelec { public: BannerMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor); + const std::shared_ptr& action_executor); - void start(rclcpp::Node::SharedPtr node) override; - void update() override; - void clear() override; - MissionStatus getStatus() const override; - std::string name() const override { return "Promotion"; } + void Start(rclcpp::Node::SharedPtr node) override; + void Update() override; + void Clear() override; + MissionStatus GetStatus() const override; + std::string GetName() const override { return "Promotion"; } - enum Step { + enum Step + { GO_TO_FRONT, DEPLOY_BANNER, GO_FORWARD, diff --git a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp index 218fafc..797bae0 100644 --- a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp @@ -11,14 +11,15 @@ namespace Modelec public: GoHomeMission(const std::shared_ptr& nav, const rclcpp::Time& start_time); - void start(rclcpp::Node::SharedPtr node) override; - void update() override; - void clear() override; - MissionStatus getStatus() const override; - std::string name() const override { return "GoHome"; } + void Start(rclcpp::Node::SharedPtr node) override; + void Update() override; + void Clear() override; + MissionStatus GetStatus() const override; + std::string GetName() const override { return "GoHome"; } private: - enum Step { + enum Step + { ROTATE_TO_HOME, GO_HOME, GO_CLOSE, diff --git a/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp b/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp index a0ac3e7..21033b5 100644 --- a/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp @@ -19,10 +19,10 @@ namespace Modelec { public: virtual ~Mission() = default; - virtual void start(rclcpp::Node::SharedPtr node) = 0; - virtual void update() = 0; - virtual void clear() = 0; - virtual MissionStatus getStatus() const = 0; - virtual std::string name() const = 0; + virtual void Start(rclcpp::Node::SharedPtr node) = 0; + virtual void Update() = 0; + virtual void Clear() = 0; + virtual MissionStatus GetStatus() const = 0; + virtual std::string GetName() const = 0; }; } diff --git a/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp index 464aa02..9c58283 100644 --- a/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp @@ -6,21 +6,23 @@ #include #include -namespace Modelec { - - class PrepareConcertMission : public Mission { +namespace Modelec +{ + class PrepareConcertMission : public Mission + { public: PrepareConcertMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor); - void start(rclcpp::Node::SharedPtr node) override; - void update() override; - void clear() override; - MissionStatus getStatus() const override; - std::string name() const override { return "PrepareConcert"; } + void Start(rclcpp::Node::SharedPtr node) override; + void Update() override; + void Clear() override; + MissionStatus GetStatus() const override; + std::string GetName() const override { return "PrepareConcert"; } private: - enum Step { + enum Step + { GO_TO_COLUMN, GO_CLOSE_TO_COLUMN, TAKE_COLUMN, @@ -43,5 +45,4 @@ namespace Modelec { rclcpp::Publisher::SharedPtr score_pub_; int mission_score_ = 0; }; - } diff --git a/src/modelec_strat/include/modelec_strat/obstacle/column.hpp b/src/modelec_strat/include/modelec_strat/obstacle/column.hpp index 4b833e5..58f41c1 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/column.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/column.hpp @@ -12,7 +12,7 @@ namespace Modelec ColumnObstacle(const ColumnObstacle&) = default; ColumnObstacle(int id, int x, int y, double theta, int w, int h, const std::string& type); - ColumnObstacle(tinyxml2::XMLElement * obstacleElem); + ColumnObstacle(tinyxml2::XMLElement* obstacleElem); ColumnObstacle(const modelec_interfaces::msg::Obstacle& msg); bool IsAtObjective() const; diff --git a/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp b/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp index 7391edd..3676e1d 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp @@ -8,12 +8,17 @@ #include -namespace Modelec { - class Obstacle { +namespace Modelec +{ + class Obstacle + { public: - Obstacle() : id_(-1), x_(0), y_(0), w_(0), h_(0), theta_(0), type_("unknown") {} + Obstacle() : id_(-1), x_(0), y_(0), w_(0), h_(0), theta_(0), type_("unknown") + { + } + Obstacle(int id, int x, int y, double theta, int w, int h, const std::string& type); - Obstacle(tinyxml2::XMLElement * obstacleElem); + Obstacle(tinyxml2::XMLElement* obstacleElem); Obstacle(const modelec_interfaces::msg::Obstacle& msg); Obstacle(const Obstacle& other) = default; @@ -21,39 +26,43 @@ namespace Modelec { virtual modelec_interfaces::msg::Obstacle toMsg() const; - int id() const { return id_; } - int x() const { return x_; } - int y() const { return y_; } - double theta() const { return theta_; } - int width() const { return w_; } - int height() const { return h_; } - const std::string& type() const { return type_; } + int GetId() const { return id_; } + int GetX() const { return x_; } + int GetY() const { return y_; } + double GetTheta() const { return theta_; } + int GetWidth() const { return w_; } + int GetHeight() const { return h_; } + const std::string& Type() const { return type_; } Point GetPosition() const { return Point(x_, y_, theta_); } - void setId(int id) { id_ = id; } - void setX(int x) { x_ = x; } - void setY(int y) { y_ = y; } - void setTheta(double theta) { theta_ = theta; } - void setWidth(int w) { w_ = w; } - void setHeight(int h) { h_ = h; } - void setType(const std::string& type) { type_ = type; } + void SetId(int id) { id_ = id; } + void SetX(int x) { x_ = x; } + void SetY(int y) { y_ = y; } + void SetTheta(double theta) { theta_ = theta; } + void SetWidth(int w) { w_ = w; } + void SetHeight(int h) { h_ = h; } + void SetType(const std::string& type) { type_ = type; } - void setPosition(int x, int y, double theta) { + void SetPosition(int x, int y, double theta) + { x_ = x; y_ = y; theta_ = theta; } - void setPosition(const Point& p) { + void SetPosition(const Point& p) + { x_ = p.x; y_ = p.y; theta_ = p.theta; } - void setSize(int w, int h) { + void SetSize(int w, int h) + { w_ = w; h_ = h; } + protected: int id_, x_, y_, w_, h_; double theta_; diff --git a/src/modelec_strat/include/modelec_strat/pami_manager.hpp b/src/modelec_strat/include/modelec_strat/pami_manager.hpp index ae9bb69..c3aecdb 100644 --- a/src/modelec_strat/include/modelec_strat/pami_manager.hpp +++ b/src/modelec_strat/include/modelec_strat/pami_manager.hpp @@ -12,8 +12,8 @@ #include #include -namespace Modelec { - +namespace Modelec +{ class PamiManger : public rclcpp::Node { public: @@ -43,5 +43,4 @@ namespace Modelec { rclcpp::Publisher::SharedPtr score_pub_; rclcpp::Subscription::SharedPtr reset_strat_sub_; }; - -} \ No newline at end of file +} diff --git a/src/modelec_strat/include/modelec_strat/pathfinding.hpp b/src/modelec_strat/include/modelec_strat/pathfinding.hpp index 303b067..e350775 100644 --- a/src/modelec_strat/include/modelec_strat/pathfinding.hpp +++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp @@ -35,7 +35,7 @@ namespace Modelec explicit Waypoint(const WaypointMsg& waypoint); - WaypointMsg toMsg() const; + WaypointMsg ToMsg() const; }; class Pathfinding @@ -65,7 +65,7 @@ namespace Modelec Pathfinding(const rclcpp::Node::SharedPtr& node); - rclcpp::Node::SharedPtr getNode() const; + rclcpp::Node::SharedPtr GetNode() const; std::pair FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal, bool isClose = false, diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index 6102638..f8f3148 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -44,9 +44,9 @@ namespace Modelec void Reset(); protected: - void transition(State next, const std::string& reason); + void Transition(State next, const std::string& reason); - void update(); + void Update(); private: State state_; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 6df92df..f5a1c68 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -85,31 +85,58 @@ namespace Modelec return; } - step_.pop(); - step_done_ = false; + if (step_done_) + { + step_.pop(); + step_done_ = false; + + switch (step_.front()) + { + } + } } void ActionExecutor::DeployBanner() { - action_ = DEPLOY_BANNER; - action_done_ = false; + if (action_done_) + { + action_ = DEPLOY_BANNER; + action_done_ = false; - Update(); + Update(); + } + } + + void ActionExecutor::RetractBanner() + { + if (action_done_) + { + action_ = RETRACT_BANNER; + action_done_ = false; + + Update(); + } } void ActionExecutor::TakePot() { - action_ = TAKE_POT; - action_done_ = false; + if (action_done_) + { + action_ = TAKE_POT; + action_done_ = false; - Update(); + Update(); + } } void ActionExecutor::PlacePot() { - action_ = PLACE_POT; - action_done_ = false; + if (action_done_) + { + action_ = PLACE_POT; + action_done_ = false; - Update(); + Update(); + } } } diff --git a/src/modelec_strat/src/deposite_zone.cpp b/src/modelec_strat/src/deposite_zone.cpp index e724539..27be957 100644 --- a/src/modelec_strat/src/deposite_zone.cpp +++ b/src/modelec_strat/src/deposite_zone.cpp @@ -22,7 +22,8 @@ namespace Modelec auto posPotList = obstacleElem->FirstChildElement("PotPos"); if (posPotList) { - for (auto elemPos = posPotList->FirstChildElement("Pos"); elemPos; elemPos = elemPos->NextSiblingElement("Pos")) + for (auto elemPos = posPotList->FirstChildElement("Pos"); elemPos; elemPos = elemPos-> + NextSiblingElement("Pos")) { Point pos; elemPos->QueryIntAttribute("x", &pos.x); diff --git a/src/modelec_strat/src/missions/banner_mission.cpp b/src/modelec_strat/src/missions/banner_mission.cpp index 7a77a3b..2c8cc16 100644 --- a/src/modelec_strat/src/missions/banner_mission.cpp +++ b/src/modelec_strat/src/missions/banner_mission.cpp @@ -4,12 +4,13 @@ namespace Modelec { - BannerMission::BannerMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) : step_(GO_TO_FRONT), + BannerMission::BannerMission(const std::shared_ptr& nav, + const std::shared_ptr& action_executor) : step_(GO_TO_FRONT), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { } - void BannerMission::start(rclcpp::Node::SharedPtr node) + void BannerMission::Start(rclcpp::Node::SharedPtr node) { node_ = node; @@ -24,7 +25,7 @@ namespace Modelec status_ = MissionStatus::RUNNING; } - void BannerMission::update() + void BannerMission::Update() { if (status_ != MissionStatus::RUNNING) return; @@ -65,11 +66,11 @@ namespace Modelec } } - void BannerMission::clear() + void BannerMission::Clear() { } - MissionStatus BannerMission::getStatus() const + MissionStatus BannerMission::GetStatus() const { return status_; } diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index f6948c1..ae82e52 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -4,11 +4,12 @@ namespace Modelec { - GoHomeMission::GoHomeMission(const std::shared_ptr& nav, const rclcpp::Time& start_time) : step_(ROTATE_TO_HOME), status_(MissionStatus::READY), nav_(nav), start_time_(start_time) + GoHomeMission::GoHomeMission(const std::shared_ptr& nav, const rclcpp::Time& start_time) : + step_(ROTATE_TO_HOME), status_(MissionStatus::READY), nav_(nav), start_time_(start_time) { } - void GoHomeMission::start(rclcpp::Node::SharedPtr node) + void GoHomeMission::Start(rclcpp::Node::SharedPtr node) { node_ = node; @@ -31,58 +32,58 @@ namespace Modelec status_ = MissionStatus::RUNNING; } - void GoHomeMission::update() + void GoHomeMission::Update() { if (!nav_->HasArrived()) return; switch (step_) { - case ROTATE_TO_HOME: + case ROTATE_TO_HOME: + { + if (nav_->GoTo(home_point_.GetTakeBasePosition()) != Pathfinding::FREE) { - if (nav_->GoTo(home_point_.GetTakeBasePosition()) != Pathfinding::FREE) + if (nav_->GoTo(home_point_.GetTakeBasePosition(), true) != Pathfinding::FREE) { - if (nav_->GoTo(home_point_.GetTakeBasePosition(), true) != Pathfinding::FREE) - { - status_ = MissionStatus::FAILED; - return; - } + status_ = MissionStatus::FAILED; + return; } } + } - step_ = GO_HOME; + step_ = GO_HOME; + break; + case GO_HOME: + if ((node_->now() - start_time_).seconds() < 94) + { break; - case GO_HOME: - if ((node_->now() - start_time_).seconds() < 94) - { - break; - } + } - nav_->GoTo(home_point_.GetTakePosition(0), true); + nav_->GoTo(home_point_.GetTakePosition(0), true); + + step_ = GO_CLOSE; + break; + + case GO_CLOSE: + { + std_msgs::msg::Int64 msg; + msg.data = mission_score_; + score_pub_->publish(msg); + + step_ = DONE; + status_ = MissionStatus::DONE; - step_ = GO_CLOSE; - break; - - case GO_CLOSE: - { - std_msgs::msg::Int64 msg; - msg.data = mission_score_; - score_pub_->publish(msg); - - step_ = DONE; - status_ = MissionStatus::DONE; - - break; - } - default: break; + } + default: + break; } } - void GoHomeMission::clear() + void GoHomeMission::Clear() { } - MissionStatus GoHomeMission::getStatus() const + MissionStatus GoHomeMission::GetStatus() const { return status_; } diff --git a/src/modelec_strat/src/missions/prepare_concert_mission.cpp b/src/modelec_strat/src/missions/prepare_concert_mission.cpp index 258327f..60578c2 100644 --- a/src/modelec_strat/src/missions/prepare_concert_mission.cpp +++ b/src/modelec_strat/src/missions/prepare_concert_mission.cpp @@ -4,12 +4,14 @@ namespace Modelec { - PrepareConcertMission::PrepareConcertMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) : step_(GO_TO_COLUMN), + PrepareConcertMission::PrepareConcertMission(const std::shared_ptr& nav, + const std::shared_ptr& action_executor) : + step_(GO_TO_COLUMN), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { } - void PrepareConcertMission::start(rclcpp::Node::SharedPtr node) + void PrepareConcertMission::Start(rclcpp::Node::SharedPtr node) { node_ = node; @@ -38,7 +40,7 @@ namespace Modelec auto res = nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL); if (res != Pathfinding::FREE) { - blacklistId.push_back(column_->id()); + blacklistId.push_back(column_->GetId()); } else { @@ -58,7 +60,7 @@ namespace Modelec status_ = MissionStatus::RUNNING; } - void PrepareConcertMission::update() + void PrepareConcertMission::Update() { if (status_ != MissionStatus::RUNNING) return; @@ -78,7 +80,7 @@ namespace Modelec break; case GO_CLOSE_TO_COLUMN: action_executor_->TakePot(); - nav_->GetPathfinding()->RemoveObstacle(column_->id()); + nav_->GetPathfinding()->RemoveObstacle(column_->GetId()); step_ = TAKE_COLUMN; break; @@ -112,7 +114,6 @@ namespace Modelec break; case GO_BACK: { - bool canGo = false; std::vector blacklistId = {}; int timeout = 0; @@ -127,7 +128,8 @@ namespace Modelec if (nav_->CanGoTo(p, false, Pathfinding::FREE | Pathfinding::WALL) != Pathfinding::FREE) { - if (nav_->GoToRotateFirst(p, true, Pathfinding::FREE | Pathfinding::WALL) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(p, true, Pathfinding::FREE | Pathfinding::WALL) != + Pathfinding::FREE) { blacklistId.push_back(closestDepoZone_->GetId()); } @@ -175,9 +177,9 @@ namespace Modelec { action_executor_->PlacePot(); - column_->setX(closestDepoZonePoint_.x); - column_->setY(closestDepoZonePoint_.y); - column_->setTheta(closestDepoZonePoint_.theta); + column_->SetX(closestDepoZonePoint_.x); + column_->SetY(closestDepoZonePoint_.y); + column_->SetTheta(closestDepoZonePoint_.theta); column_->SetAtObjective(true); nav_->GetPathfinding()->AddObstacle(column_); } @@ -213,12 +215,12 @@ namespace Modelec } } - void PrepareConcertMission::clear() + void PrepareConcertMission::Clear() { // TODO : if is doing construction, stop everything and release everything } - MissionStatus PrepareConcertMission::getStatus() const + MissionStatus PrepareConcertMission::GetStatus() const { return status_; } diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 91120bc..00ae4a2 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -89,7 +89,7 @@ namespace Modelec { for (auto& w : waypoints_) { - waypoint_pub_->publish(w.toMsg()); + waypoint_pub_->publish(w.ToMsg()); } } diff --git a/src/modelec_strat/src/obstacle/column.cpp b/src/modelec_strat/src/obstacle/column.cpp index b6ea17a..c4b559c 100644 --- a/src/modelec_strat/src/obstacle/column.cpp +++ b/src/modelec_strat/src/obstacle/column.cpp @@ -12,8 +12,8 @@ namespace Modelec { possible_angles_.push_back(theta_); for (auto elem = obstacleElem->FirstChildElement("possible-angle"); - elem; - elem = elem->NextSiblingElement("possible-angle")) + elem; + elem = elem->NextSiblingElement("possible-angle")) { possible_angles_.push_back(elem->DoubleAttribute("theta")); } @@ -61,7 +61,7 @@ namespace Modelec std::vector ColumnObstacle::GetAllPossiblePositions() const { std::vector positions; - for (const auto & angle : possible_angles_) + for (const auto& angle : possible_angles_) { positions.push_back(Point(x_, y_, angle)); } diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index a39ba9e..9336312 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -107,7 +107,7 @@ namespace Modelec "odometry/add_waypoint", 100); } - rclcpp::Node::SharedPtr Pathfinding::getNode() const + rclcpp::Node::SharedPtr Pathfinding::GetNode() const { return node_; } @@ -224,11 +224,11 @@ namespace Modelec // TODO some bug exist with the inflate for (const auto& [id, obstacle] : obstacle_map_) { - float cx = obstacle->x(); - float cy = obstacle->y(); - float width = obstacle->width() + inflate_x * 2 * cell_size_mm_x; - float height = obstacle->height() + inflate_y * 2 * cell_size_mm_y; - float theta = M_PI_2 - obstacle->theta(); + float cx = obstacle->GetX(); + float cy = obstacle->GetY(); + float width = obstacle->GetWidth() + inflate_x * 2 * cell_size_mm_x; + float height = obstacle->GetHeight() + inflate_y * 2 * cell_size_mm_y; + float theta = M_PI_2 - obstacle->GetTheta(); float dx = width / 2.0f; float dy = height / 2.0f; @@ -521,7 +521,7 @@ namespace Modelec void Pathfinding::AddObstacle(const std::shared_ptr& obstacle) { - obstacle_map_[obstacle->id()] = obstacle; + obstacle_map_[obstacle->GetId()] = obstacle; modelec_interfaces::msg::Obstacle msg = obstacle->toMsg(); obstacle_add_pub_->publish(msg); } @@ -539,7 +539,7 @@ namespace Modelec { if (auto obs = std::dynamic_pointer_cast(obstacle)) { - if (!obs->IsAtObjective() && std::find(blacklistedId.begin(), blacklistedId.end(), obs->id()) == + if (!obs->IsAtObjective() && std::find(blacklistedId.begin(), blacklistedId.end(), obs->GetId()) == blacklistedId.end()) { for (auto possiblePos : obs->GetAllPossiblePositions()) @@ -606,10 +606,10 @@ namespace Modelec { if (auto column = std::dynamic_pointer_cast(obs)) { - if (Point::distance(enemyPos, column->GetPosition()) < enemy_width_mm_ + (column->width() / 2) + + if (Point::distance(enemyPos, column->GetPosition()) < enemy_width_mm_ + (column->GetWidth() / 2) + enemy_margin_mm_) { - RemoveObstacle(column->id()); + RemoveObstacle(column->GetId()); } } } @@ -648,7 +648,7 @@ namespace Modelec obstacleElem = obstacleElem->NextSiblingElement("Obstacle")) { std::shared_ptr obs = std::make_shared(obstacleElem); - obstacle_map_[obs->id()] = obs; + obstacle_map_[obs->GetId()] = obs; } for (tinyxml2::XMLElement* obstacleElem = root->FirstChildElement("Gradin"); @@ -656,7 +656,7 @@ namespace Modelec obstacleElem = obstacleElem->NextSiblingElement("Gradin")) { std::shared_ptr obs = std::make_shared(obstacleElem); - obstacle_map_[obs->id()] = obs; + obstacle_map_[obs->GetId()] = obs; } RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", obstacle_map_.size()); @@ -683,7 +683,7 @@ namespace Modelec reached = false; } - WaypointMsg Waypoint::toMsg() const + WaypointMsg Waypoint::ToMsg() const { return static_cast>>(*this); } diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 2f9f06c..6f5ba9b 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -76,7 +76,7 @@ namespace Modelec timer_ = create_wall_timer(std::chrono::milliseconds(100), [this] { - update(); + Update(); }); } @@ -87,7 +87,7 @@ namespace Modelec Init(); } - void StratFMS::transition(State next, const std::string& reason) + void StratFMS::Transition(State next, const std::string& reason) { RCLCPP_INFO(get_logger(), "Transition %d -> %d: %s", static_cast(state_), static_cast(next), reason.c_str()); @@ -98,7 +98,7 @@ namespace Modelec state_pub_->publish(msg); } - void StratFMS::update() + void StratFMS::Update() { auto now = this->now(); @@ -106,7 +106,7 @@ namespace Modelec { case State::INIT: RCLCPP_INFO_ONCE(get_logger(), "State: INIT"); - transition(State::WAIT_START, "System ready"); + Transition(State::WAIT_START, "System ready"); break; case State::WAIT_START: if (started_) @@ -119,7 +119,7 @@ namespace Modelec .count(); start_time_pub_->publish(msg); - transition(State::SELECT_MISSION, "Match started"); + Transition(State::SELECT_MISSION, "Match started"); } break; @@ -129,15 +129,15 @@ namespace Modelec // select mission in a good way there. if (elapsed.seconds() < 2) { - transition(State::DO_PROMOTION, "Start promotion"); + Transition(State::DO_PROMOTION, "Start promotion"); } else if (elapsed.seconds() < 80) { - transition(State::DO_PREPARE_CONCERT, "Proceed to concert"); + Transition(State::DO_PREPARE_CONCERT, "Proceed to concert"); } else { - transition(State::DO_GO_HOME, "Cleanup and finish match"); + Transition(State::DO_GO_HOME, "Cleanup and finish match"); } break; } @@ -146,24 +146,24 @@ namespace Modelec if (!current_mission_) { current_mission_ = std::make_unique(nav_, action_executor_); - current_mission_->start(shared_from_this()); + current_mission_->Start(shared_from_this()); } - current_mission_->update(); - if (current_mission_->getStatus() == MissionStatus::DONE) + current_mission_->Update(); + if (current_mission_->GetStatus() == MissionStatus::DONE) { current_mission_.reset(); - transition(State::SELECT_MISSION, "PrepareConcert finished"); + Transition(State::SELECT_MISSION, "PrepareConcert finished"); } - else if (current_mission_->getStatus() == MissionStatus::FAILED) + else if (current_mission_->GetStatus() == MissionStatus::FAILED) { - current_mission_->clear(); + current_mission_->Clear(); current_mission_.reset(); - transition(State::SELECT_MISSION, "PrepareConcert failed"); + Transition(State::SELECT_MISSION, "PrepareConcert failed"); } - else if (current_mission_->getStatus() == MissionStatus::FINISH_ALL) + else if (current_mission_->GetStatus() == MissionStatus::FINISH_ALL) { current_mission_.reset(); - transition(State::DO_GO_HOME, "Finish all finished"); + Transition(State::DO_GO_HOME, "Finish all finished"); } break; @@ -171,18 +171,18 @@ namespace Modelec if (!current_mission_) { current_mission_ = std::make_unique(nav_, action_executor_); - current_mission_->start(shared_from_this()); + current_mission_->Start(shared_from_this()); } - current_mission_->update(); - if (current_mission_->getStatus() == MissionStatus::DONE) + current_mission_->Update(); + if (current_mission_->GetStatus() == MissionStatus::DONE) { current_mission_.reset(); - transition(State::SELECT_MISSION, "Promotion finished"); + Transition(State::SELECT_MISSION, "Promotion finished"); } - else if (current_mission_->getStatus() == MissionStatus::FAILED) + else if (current_mission_->GetStatus() == MissionStatus::FAILED) { current_mission_.reset(); - transition(State::SELECT_MISSION, "Promotion failed"); + Transition(State::SELECT_MISSION, "Promotion failed"); } break; @@ -190,13 +190,13 @@ namespace Modelec if (!current_mission_) { current_mission_ = std::make_unique(nav_, match_start_time_); - current_mission_->start(shared_from_this()); + current_mission_->Start(shared_from_this()); } - current_mission_->update(); - if (current_mission_->getStatus() == MissionStatus::DONE) + current_mission_->Update(); + if (current_mission_->GetStatus() == MissionStatus::DONE) { current_mission_.reset(); - transition(State::STOP, "Cleanup done"); + Transition(State::STOP, "Cleanup done"); } break;