diff --git a/fastdds_setup.xml b/fastdds_setup.xml index de973c3..bc22a6e 100644 --- a/fastdds_setup.xml +++ b/fastdds_setup.xml @@ -23,7 +23,7 @@ -
100.113.219.4
+
100.84.205.108
 diff --git a/joy.ros2_launch_marcel.desktop b/joy.ros2_launch_marcel.desktop index 204eef5..e321000 100644 --- a/joy.ros2_launch_marcel.desktop +++ b/joy.ros2_launch_marcel.desktop @@ -2,7 +2,7 @@ Type=Application Name=Joy Comment=Lance le système ROS 2 avec GUI -Exec=/home/modelec/modelec-marcel-ROS/start_ros2.sh +Exec=/home/modelec/Modelec-ROS2/start_ros2.sh Icon=utilities-terminal Terminal=true Categories=Utility; diff --git a/no_lidar.joy.ros2_launch_marcel.desktop b/no_lidar.joy.ros2_launch_marcel.desktop index 60e6063..e323a0d 100644 --- a/no_lidar.joy.ros2_launch_marcel.desktop +++ b/no_lidar.joy.ros2_launch_marcel.desktop @@ -2,7 +2,7 @@ Type=Application Name=Joy no Lidar Comment=Lance le système ROS 2 avec GUI -Exec=/home/modelec/modelec-marcel-ROS/start_ros2.sh with_rplidar:=false +Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false Icon=utilities-terminal Terminal=true Categories=Utility; diff --git a/no_lidar.ros2_launch_marcel.desktop b/no_lidar.ros2_launch_marcel.desktop index c01e2fc..ff3a9d2 100644 --- a/no_lidar.ros2_launch_marcel.desktop +++ b/no_lidar.ros2_launch_marcel.desktop @@ -2,7 +2,9 @@ Type=Application Name=No Lidar Comment=Lance le système ROS 2 avec GUI -Exec=/home/modelec/modelec-marcel-ROS/start_ros2.sh with_joy:=false with_rplidar:=false + +Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_joy:=false with_rplidar:=false + Icon=utilities-terminal Terminal=true Categories=Utility; diff --git a/ros2_launch_marcel.desktop b/ros2_launch_marcel.desktop index 6eed291..b919b14 100644 --- a/ros2_launch_marcel.desktop +++ b/ros2_launch_marcel.desktop @@ -2,7 +2,7 @@ Type=Application Name=Lancer ROS 2 Comment=Lance le système ROS 2 avec GUI -Exec=/home/modelec/modelec-marcel-ROS/start_ros2.sh with_joy:=false +Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_joy:=false Icon=utilities-terminal Terminal=true Categories=Utility; diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index aa28560..6ee069f 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -446,6 +446,10 @@ namespace Modelec relay_move_res_pub_->publish(relay_msg); } + else if (tokens[1] == "TIR") + { + // Do nothing for now + } else { RCLCPP_WARN(this->get_logger(), "Unknown message format for OK response: '%s'", trim(msg).c_str()); diff --git a/src/modelec_strat/data/obstacles.xml b/src/modelec_strat/data/obstacles.xml index 0d46ad6..151de31 100644 --- a/src/modelec_strat/data/obstacles.xml +++ b/src/modelec_strat/data/obstacles.xml @@ -18,7 +18,7 @@ - + diff --git a/src/modelec_strat/include/modelec_strat/action/free_action.hpp b/src/modelec_strat/include/modelec_strat/action/free_action.hpp index acb123a..3a4532d 100644 --- a/src/modelec_strat/include/modelec_strat/action/free_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/free_action.hpp @@ -9,20 +9,20 @@ namespace Modelec { public: FreeAction(const std::shared_ptr& action_executor); - FreeAction(const std::shared_ptr& action_executor, bool front, int n); - FreeAction(const std::shared_ptr& action_executor, std::pair servo); - FreeAction(const std::shared_ptr& action_executor, std::vector> servos); + FreeAction(const std::shared_ptr& action_executor, Front front, int n); + FreeAction(const std::shared_ptr& action_executor, std::pair servo); + FreeAction(const std::shared_ptr& action_executor, std::vector> servos); void Next() override; void Init(const std::vector& params) override; - void AddServo(int id, bool front); - void AddServo(std::pair servo); - void AddServos(const std::vector>& servos); + void AddServo(int id, Front front); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); inline static const std::string Name = ActionExec::FREE; private: - std::vector> servos_; + std::vector> servos_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action/take_action.hpp b/src/modelec_strat/include/modelec_strat/action/take_action.hpp index fd9f226..68cd7a6 100644 --- a/src/modelec_strat/include/modelec_strat/action/take_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/take_action.hpp @@ -9,20 +9,20 @@ namespace Modelec { public: TakeAction(const std::shared_ptr& action_executor); - TakeAction(const std::shared_ptr& action_executor, bool front, int n); - TakeAction(const std::shared_ptr& action_executor, std::pair servo); - TakeAction(const std::shared_ptr& action_executor, std::vector> servos); + TakeAction(const std::shared_ptr& action_executor, Front front, int n); + TakeAction(const std::shared_ptr& action_executor, std::pair servo); + TakeAction(const std::shared_ptr& action_executor, std::vector> servos); void Next() override; void Init(const std::vector& params) override; - void AddServo(int id, bool front); - void AddServo(std::pair servo); - void AddServos(const std::vector>& servos); + void AddServo(int id, Front front); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); inline static const std::string Name = ActionExec::TAKE; private: - std::vector> servos_; + std::vector> servos_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 795c015..6186b01 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -35,9 +35,9 @@ namespace Modelec void Up(BaseAction::Front front, bool force = false); - void Take(std::vector> servos, bool force = false); + void Take(const std::vector>& servos, bool force = false); - void Free(std::vector> servos, bool force = false); + void Free(const std::vector>& servos, bool force = false); void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg); @@ -45,6 +45,18 @@ namespace Modelec std::array, 2> box_obstacles_; + bool IsEmpty() const; + + bool IsFull() const; + + bool HasBox(BaseAction::Front front) const; + + bool HasFrontBox() const; + + bool HasBackBox() const; + + bool HasOneBox() const; + protected: rclcpp::Publisher::SharedPtr asc_move_pub_; rclcpp::Publisher::SharedPtr servo_move_pub_; diff --git a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp index 34738a0..0bdd862 100644 --- a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp @@ -8,7 +8,8 @@ namespace Modelec { class FreeMission : public Mission { public: FreeMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor); + const std::shared_ptr& action_executor, + BaseAction::Front front = BaseAction::FRONT); void Start(rclcpp::Node::SharedPtr node) override; void Update() override; @@ -25,7 +26,9 @@ namespace Modelec { UP, GO_BACK, DONE, - } step_; + }; + + BaseAction::Front front_; MissionStatus status_; std::shared_ptr nav_; @@ -38,5 +41,9 @@ namespace Modelec { rclcpp::Time go_timeout_; rclcpp::Time deploy_time_; + + std::optional min_time_; + + rclcpp::Time last_ask_waypoint_time_; }; } \ No newline at end of file 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 a61d684..0ba87a9 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 @@ -20,14 +20,11 @@ namespace Modelec private: enum Step { - GO_FRONT, - AWAIT_95S, - GO_HOME, - DONE, - ROTATE_TO_HOME, + GO_HOME, GO_CLOSE, - } step_; + DONE, + }; MissionStatus status_; std::shared_ptr nav_; @@ -38,5 +35,9 @@ namespace Modelec rclcpp::Publisher::SharedPtr score_pub_; int mission_score_ = 0; rclcpp::Time go_timeout_; + + std::optional min_time_; + + rclcpp::Time last_ask_waypoint_time_; }; } 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 21033b5..fa0e360 100644 --- a/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/mission_base.hpp @@ -24,5 +24,8 @@ namespace Modelec virtual void Clear() = 0; virtual MissionStatus GetStatus() const = 0; virtual std::string GetName() const = 0; + + protected: + std::queue steps_; }; } diff --git a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp index 64f7db5..eba1206 100644 --- a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp @@ -8,7 +8,8 @@ namespace Modelec { class TakeMission : public Mission { public: TakeMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor); + const std::shared_ptr& action_executor, + BaseAction::Front front = BaseAction::FRONT); void Start(rclcpp::Node::SharedPtr node) override; void Update() override; @@ -25,7 +26,9 @@ namespace Modelec { TAKE, UP, DONE, - } step_; + }; + + BaseAction::Front front_; std::shared_ptr closestBox; MissionStatus status_; @@ -35,5 +38,9 @@ namespace Modelec { rclcpp::Time go_timeout_; rclcpp::Time deploy_time_; + + std::optional min_time_; + + rclcpp::Time last_ask_waypoint_time_; }; } \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 0e0a209..eabaa3a 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -60,17 +60,17 @@ namespace Modelec bool HasArrived() const; - bool RotateTo(const PosMsg::SharedPtr& pos); - bool RotateTo(const Point& pos); + bool RotateTo(const PosMsg::SharedPtr& pos, bool front = true); + bool RotateTo(const Point& pos, bool front = true); void Rotate(double angle); int GoTo(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); int GoTo(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE); int GoTo(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); - int GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); - int GoToRotateFirst(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE); - int GoToRotateFirst(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); + int GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE, bool front = true); + int GoToRotateFirst(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE, bool front = true); + int GoToRotateFirst(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE, bool front = true); int CanGoTo(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); int CanGoTo(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE); diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index d552887..2069550 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -62,6 +62,7 @@ namespace Modelec int team_id_ = 0; std::queue game_action_sequence_; + bool static_strat_ = false; std::shared_ptr nav_; std::shared_ptr action_executor_; diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 19a6dde..fd843f9 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -35,12 +35,12 @@ void Modelec::DownAction::Next() { msg.items[0].id = 0; msg.items[0].start_angle = 1.95; - msg.items[0].end_angle = 2.95; + msg.items[0].end_angle = 3; msg.items[0].duration_s = 1; msg.items[1].id = 1; msg.items[1].start_angle = 1.9; - msg.items[1].end_angle = 0.9; + msg.items[1].end_angle = 0.85; msg.items[1].duration_s = 1; msg.items[2].id = 2; @@ -50,7 +50,7 @@ void Modelec::DownAction::Next() msg.items[3].id = 3; msg.items[3].start_angle = 2.7; - msg.items[3].end_angle = 3; + msg.items[3].end_angle = 2.9; msg.items[3].duration_s = 1; } diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index fcc18a1..52fe403 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -8,17 +8,17 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr& action_ex steps_.push(ActionExec::DONE_STEP); } -Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, bool front, int n) : FreeAction(action_executor) +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, Front front, int n) : FreeAction(action_executor) { AddServo(n, front); } -Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::pair servo) : FreeAction(action_executor) +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::pair servo) : FreeAction(action_executor) { AddServo(servo.first, servo.second); } -Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::vector> servos) : FreeAction(action_executor) +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::vector> servos) : FreeAction(action_executor) { AddServos(servos); } @@ -44,9 +44,9 @@ void Modelec::FreeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { - msg.items[i].id = servos_[i].first + (servos_[i].second ? 3 : 11); - msg.items[i].start_angle = servos_[i].second ? 0.8 : 0; - msg.items[i].end_angle = servos_[i].second ? 2.5 : 0; + msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); + msg.items[i].start_angle = servos_[i].second ? 3 : 0; + msg.items[i].end_angle = servos_[i].second ? 1 : 0; msg.items[i].duration_s = 0.5; } @@ -71,22 +71,22 @@ void Modelec::FreeAction::Init(const std::vector& params) { int id = std::stoi(params[i]); bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true; - AddServo(id, front); + AddServo(id, front ? FRONT : BACK); } } } -void Modelec::FreeAction::AddServo(int id, bool front) +void Modelec::FreeAction::AddServo(int id, Front front) { servos_.emplace_back(id, front); } -void Modelec::FreeAction::AddServo(std::pair servo) +void Modelec::FreeAction::AddServo(std::pair servo) { servos_.emplace_back(servo); } -void Modelec::FreeAction::AddServos(const std::vector>& servos) +void Modelec::FreeAction::AddServos(const std::vector>& servos) { servos_.insert(servos_.end(), servos.begin(), servos.end()); } diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index 01a1469..5c05624 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -6,17 +6,17 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr& action_ex { } -Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, bool front, int n) : TakeAction(action_executor) +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, Front front, int n) : TakeAction(action_executor) { AddServo(n, front); } -Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::pair servo) : TakeAction(action_executor) +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::pair servo) : TakeAction(action_executor) { AddServo(servo); } -Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::vector> servos) : TakeAction(action_executor) +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::vector> servos) : TakeAction(action_executor) { AddServos(servos); } @@ -42,9 +42,9 @@ void Modelec::TakeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { - msg.items[i].id = servos_[i].first + (servos_[i].second ? 3 : 11); - msg.items[i].start_angle = servos_[i].second ? 0.8 : 0; - msg.items[i].end_angle = servos_[i].second ? 2.7 : 0; + msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); + msg.items[i].start_angle = servos_[i].second ? 1 : 0; + msg.items[i].end_angle = servos_[i].second ? 3 : 0; msg.items[i].duration_s = 0.5; } @@ -69,22 +69,22 @@ void Modelec::TakeAction::Init(const std::vector& params) { int id = std::stoi(params[i]); bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true; - AddServo(id, front); + AddServo(id, front ? FRONT : BACK); } } } -void Modelec::TakeAction::AddServo(int id, bool front) +void Modelec::TakeAction::AddServo(int id, Front front) { servos_.emplace_back(id, front); } -void Modelec::TakeAction::AddServo(std::pair servo) +void Modelec::TakeAction::AddServo(std::pair servo) { servos_.emplace_back(servo); } -void Modelec::TakeAction::AddServos(const std::vector>& servos) +void Modelec::TakeAction::AddServos(const std::vector>& servos) { servos_.insert(servos_.end(), servos.begin(), servos.end()); } diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 1cbeb21..0907bba 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -34,12 +34,12 @@ void Modelec::UPAction::Next() if (front_ == FRONT || front_ == BOTH) { msg.items[0].id = 0; - msg.items[0].start_angle = 2.95; + msg.items[0].start_angle = 3; msg.items[0].end_angle = 1.95; msg.items[0].duration_s = 1; msg.items[1].id = 1; - msg.items[1].start_angle = 0.9; + msg.items[1].start_angle = 0.85; msg.items[1].end_angle = 1.9; msg.items[1].duration_s = 1; @@ -49,8 +49,8 @@ void Modelec::UPAction::Next() msg.items[2].duration_s = 1; msg.items[3].id = 3; - msg.items[3].start_angle = 3; - msg.items[3].end_angle = 2.5; + msg.items[3].start_angle = 2.9; + msg.items[3].end_angle = 2.7; msg.items[3].duration_s = 1; } diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 0d36e83..d41a7cc 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -96,11 +96,11 @@ namespace Modelec } else if (msg->buttons[3] == 1) // X button { - Take({{0, true}, {1, true}, {2, true}, {3, true}}); + Take({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}}); } else if (msg->buttons[4] == 1) // Y button { - Free({{0, true}, {1, true}, {2, true}, {3, true}}); + Free({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}}); } } }); @@ -177,7 +177,7 @@ namespace Modelec } } - void ActionExecutor::Take(std::vector> servos, bool force) { + void ActionExecutor::Take(const std::vector>& servos, bool force) { if (action_done_ || force) { action_ = std::make_shared(shared_from_this(), servos); @@ -191,7 +191,7 @@ namespace Modelec } } - void ActionExecutor::Free(std::vector> servos, bool force) { + void ActionExecutor::Free(const std::vector>& servos, bool force) { if (action_done_ || force) { action_ = std::make_shared(shared_from_this(), servos); @@ -222,4 +222,34 @@ namespace Modelec servo_move_pub_->publish(msg); step_running_ += msg.items.size(); } + + bool ActionExecutor::IsEmpty() const + { + return box_obstacles_[0] == nullptr && box_obstacles_[1] == nullptr; + } + + bool ActionExecutor::IsFull() const + { + return box_obstacles_[0] != nullptr && box_obstacles_[1] != nullptr; + } + + bool ActionExecutor::HasBox(BaseAction::Front front) const + { + return box_obstacles_[front] != nullptr; + } + + bool ActionExecutor::HasFrontBox() const + { + return HasBox(BaseAction::FRONT); + } + + bool ActionExecutor::HasBackBox() const + { + return HasBox(BaseAction::BACK); + } + + bool ActionExecutor::HasOneBox() const + { + return HasFrontBox() != HasBackBox(); + } } diff --git a/src/modelec_strat/src/deposite_zone.cpp b/src/modelec_strat/src/deposite_zone.cpp index 654daf5..cfd388b 100644 --- a/src/modelec_strat/src/deposite_zone.cpp +++ b/src/modelec_strat/src/deposite_zone.cpp @@ -21,8 +21,7 @@ namespace Modelec { for (auto takePos = TakeAngleElem->FirstChildElement("Angle"); takePos; takePos = takePos->NextSiblingElement("Angle")) { - double angle; - takePos->QueryDoubleAttribute("value", &angle); + double angle = takePos->DoubleText(0); take_angle_.push_back(angle); } } @@ -48,8 +47,8 @@ namespace Modelec for (const auto& angle : take_angle_) { Point p = Point( - static_cast(position_.x + dist * std::cos(angle)), - static_cast(position_.y + dist * std::sin(angle)), + position_.x + dist * std::cos(angle), + position_.y + dist * std::sin(angle), angle); double distance = p.distance(currentPos); diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 2162158..f9e0359 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -3,8 +3,11 @@ #include "modelec_strat/action/base_action.hpp" namespace Modelec { - FreeMission::FreeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) - : step_(GO_TO_FREE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { + FreeMission::FreeMission(const std::shared_ptr& nav, + const std::shared_ptr& action_executor, + BaseAction::Front front) + : front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + { } void FreeMission::Start(rclcpp::Node::SharedPtr node) @@ -12,9 +15,20 @@ namespace Modelec { node_ = node; go_timeout_ = node_->now(); + deploy_time_ = node_->now(); + last_ask_waypoint_time_ = node_->now(); status_ = MissionStatus::RUNNING; - step_ = GO_TO_FREE; + + std::queue empty; + std::swap(steps_, empty); + + steps_.push(GO_TO_FREE); + steps_.push(DOWN); + steps_.push(FREE); + steps_.push(UP); + steps_.push(GO_BACK); + steps_.push(DONE); } void FreeMission::Update() @@ -26,10 +40,11 @@ namespace Modelec { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 5) + if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) { - // nav_->AskWaypoint(); - // return; + nav_->AskWaypoint(); + last_ask_waypoint_time_ = node_->now(); + return; } if ((node_->now() - go_timeout_).seconds() < 10) { @@ -37,6 +52,22 @@ namespace Modelec { } } + if (min_time_.has_value()) + { + if ((node_->now() - min_time_.value()).seconds() < 0.1) + { + return; + } + else + { + min_time_.reset(); + } + } + + + auto step_ = steps_.front(); + steps_.pop(); + switch (step_) { case GO_TO_FREE: @@ -50,19 +81,30 @@ namespace Modelec { auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta)); - if (nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist), true, Pathfinding::FREE)) + auto pos = depoPoint.GetTakePosition(dist); + + RCLCPP_INFO( + node_->get_logger(), + "FreeMission: position (%.2d, %.2d) with distance %.2f", + pos.x, pos.y, dist); + + pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; + + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, front_ == BaseAction::FRONT) != Pathfinding::FREE) { - nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist), true, Pathfinding::FREE | Pathfinding::OBSTACLE); + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + return; + } } go_timeout_ = node_->now(); } - - step_ = FREE; break; case DOWN: { - action_executor_->Down(BaseAction::FRONT); + action_executor_->Down(front_); deploy_time_ = node_->now(); } @@ -70,42 +112,50 @@ namespace Modelec { break; case FREE: { - action_executor_->Free({{0, true}, {1, true}, {2, true}, {3, true}}); + action_executor_->Free({{0, front_}, {1, front_}, {2, front_}, {3, front_}}); deploy_time_ = node_->now(); - auto obs = action_executor_->box_obstacles_[0]; - action_executor_->box_obstacles_[0] = nullptr; + auto obs = action_executor_->box_obstacles_[front_]; + action_executor_->box_obstacles_[front_] = nullptr; auto pos = nav_->GetCurrentPos(); obs->SetPosition( - pos->x + 250 * cos(pos->theta), - pos->y + 250 * sin(pos->theta), + pos->x + 200 * cos(pos->theta + (front_ == BaseAction::FRONT ? 0 : M_PI)), + pos->y + 200 * sin(pos->theta + (front_ == BaseAction::FRONT ? 0 : M_PI)), pos->theta); obs->SetAtObjective(true); nav_->GetPathfinding()->AddObstacle(obs); - } - step_ = UP; + min_time_ = node_->now() + rclcpp::Duration::from_seconds(0.5); + } break; case UP: { - action_executor_->Up(BaseAction::FRONT); + action_executor_->Up(front_); deploy_time_ = node_->now(); } - - step_ = GO_BACK; break; case GO_BACK: { - nav_->GoTo(target_deposite_zone_->GetPosition().GetTakePosition(500), true, Pathfinding::FREE | Pathfinding::OBSTACLE); + auto currPos = nav_->GetCurrentPos(); + + auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta)); + + auto pos = depoPoint.GetTakePosition(300); + pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; + + if (nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + return; + } go_timeout_ = node_->now(); } - step_ = DONE; break; case DONE: { diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index 64b670a..8e8026e 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -5,7 +5,7 @@ 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) + status_(MissionStatus::READY), nav_(nav), start_time_(start_time) { } @@ -18,18 +18,27 @@ namespace Modelec score_pub_ = node_->create_publisher("/strat/score", 10); go_timeout_ = node_->now(); + last_ask_waypoint_time_ = node_->now(); status_ = MissionStatus::RUNNING; - step_ = ROTATE_TO_HOME; + + std::queue empty; + std::swap(steps_, empty); + + steps_.push(ROTATE_TO_HOME); + steps_.push(GO_HOME); + steps_.push(GO_CLOSE); + steps_.push(DONE); } void GoHomeMission::Update() { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 2) + if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) { - // nav_->AskWaypoint(); + nav_->AskWaypoint(); + last_ask_waypoint_time_ = node_->now(); return; } if ((node_->now() - go_timeout_).seconds() < 10) @@ -38,6 +47,21 @@ namespace Modelec } } + if (min_time_.has_value()) + { + if ((node_->now() - min_time_.value()).seconds() < 0.1) + { + return; + } + else + { + min_time_.reset(); + } + } + + auto step_ = steps_.front(); + steps_.pop(); + switch (step_) { case ROTATE_TO_HOME: @@ -55,8 +79,6 @@ namespace Modelec nav_->RotateTo(home_point_); go_timeout_ = node_->now(); - - step_ = GO_HOME; } break; case GO_HOME: @@ -71,23 +93,18 @@ namespace Modelec } go_timeout_ = node_->now(); - - step_ = GO_CLOSE; } break; case GO_CLOSE: { - if ((node_->now() - start_time_).seconds() < 94) + /*if ((node_->now() - start_time_).seconds() < 94) { break; - } + }*/ nav_->GoTo(home_point_, true); go_timeout_ = node_->now(); - - step_ = DONE; - } break; case DONE: diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index 6bcf895..47bac9f 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -3,8 +3,11 @@ #include "modelec_strat/action/base_action.hpp" namespace Modelec { - TakeMission::TakeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) - : step_(GO_TO_TAKE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { + TakeMission::TakeMission(const std::shared_ptr& nav, + const std::shared_ptr& action_executor, + BaseAction::Front front) + : front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + { } void TakeMission::Start(rclcpp::Node::SharedPtr node) @@ -12,9 +15,20 @@ namespace Modelec { node_ = node; go_timeout_ = node_->now(); + deploy_time_ = node_->now(); + last_ask_waypoint_time_ = node_->now(); status_ = MissionStatus::RUNNING; - step_ = GO_TO_TAKE; + + std::queue empty; + std::swap(steps_, empty); + + steps_.push(GO_TO_TAKE); + steps_.push(GO_TO_TAKE_CLOSE); + steps_.push(DOWN); + steps_.push(TAKE); + steps_.push(UP); + steps_.push(DONE); } void TakeMission::Update() @@ -26,10 +40,11 @@ namespace Modelec { if (!nav_->HasArrived()) { - if ((node_->now() - go_timeout_).seconds() < 5) + if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) { - // nav_->AskWaypoint(); - // return; + nav_->AskWaypoint(); + last_ask_waypoint_time_ = node_->now(); + return; } if ((node_->now() - go_timeout_).seconds() < 10) { @@ -37,6 +52,21 @@ namespace Modelec { } } + if (min_time_.has_value()) + { + if ((node_->now() - min_time_.value()).seconds() < 0.1) + { + return; + } + else + { + min_time_.reset(); + } + } + + auto step_ = steps_.front(); + steps_.pop(); + switch (step_) { case GO_TO_TAKE: @@ -46,58 +76,76 @@ namespace Modelec { action_executor_->box_obstacles_[0] = closestBox; + action_executor_->box_obstacles_[front_] = closestBox; + auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); + pos.theta += (front_ == BaseAction::FRONT ? 0 : M_PI); - if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL)) + if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE) { - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL)) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE) { - nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + break; + } } } go_timeout_ = node_->now(); } - - step_ = GO_TO_TAKE_CLOSE; break; case GO_TO_TAKE_CLOSE: { - auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); + if (action_executor_->box_obstacles_[front_] == nullptr) + { + status_ = MissionStatus::FAILED; + break; + } - nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); + auto pos = action_executor_->box_obstacles_[front_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); + pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; + + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + break; + } go_timeout_ = node_->now(); } - - step_ = DOWN; break; case DOWN: { - action_executor_->Down(BaseAction::FRONT); + action_executor_->Down(front_); deploy_time_ = node_->now(); } - - step_ = TAKE; break; case TAKE: { - action_executor_->Take({{0, true}, {1, true}, {2, true}, {3, true}}); + action_executor_->Take({{0, front_}, {1, front_}, {2, front_}, {3, front_}}); deploy_time_ = node_->now(); + min_time_ = node_->now() + rclcpp::Duration::from_seconds(0.5); } - - step_ = UP; break; case UP: { - action_executor_->Up(BaseAction::FRONT); + action_executor_->Up(front_); deploy_time_ = node_->now(); } - - step_ = DONE; break; case DONE: - nav_->GetPathfinding()->RemoveObstacle(closestBox->GetId()); + { + if (action_executor_->box_obstacles_[front_] == nullptr) + { + status_ = MissionStatus::FAILED; + break; + } + + nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[front_]->GetId()); + } + status_ = MissionStatus::DONE; break; default: diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 525c249..021fe26 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -286,11 +286,11 @@ namespace Modelec return waypoint_queue_.empty() && waypoints_.back().reached; } - bool NavigationHelper::RotateTo(const PosMsg::SharedPtr& pos) + bool NavigationHelper::RotateTo(const PosMsg::SharedPtr& pos, bool front) { double angle = std::atan2(pos->y - current_pos_->y, pos->x - current_pos_->x); - if (std::abs(angle - current_pos_->theta) > M_PI / 3) + if (std::abs(angle - (current_pos_->theta + (front ? 0 : M_PI))) > M_PI / 4) { Rotate(angle); return true; @@ -298,11 +298,11 @@ namespace Modelec return false; } - bool NavigationHelper::RotateTo(const Point& pos) + bool NavigationHelper::RotateTo(const Point& pos, bool front) { double angle = std::atan2(pos.y - current_pos_->y, pos.x - current_pos_->x); - if (std::abs(angle - current_pos_->theta) > M_PI / 4) + if (std::abs(angle - (current_pos_->theta + (front ? 0 : M_PI))) > M_PI / 4) { Rotate(angle); return true; @@ -364,7 +364,7 @@ namespace Modelec return GoTo(goal.x, goal.y, goal.theta, isClose, collisionMask); } - int NavigationHelper::GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask) + int NavigationHelper::GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask, bool front) { last_go_to_ = {goal, isClose, collisionMask}; @@ -376,7 +376,7 @@ namespace Modelec } auto p = Point(wl[0].x, wl[0].y, wl[0].theta); - if (RotateTo(p)) + if (RotateTo(p, front)) { await_rotate_ = true; @@ -403,18 +403,18 @@ namespace Modelec return res; } - int NavigationHelper::GoToRotateFirst(int x, int y, double theta, bool isClose, int collisionMask) + int NavigationHelper::GoToRotateFirst(int x, int y, double theta, bool isClose, int collisionMask, bool front) { PosMsg::SharedPtr goal = std::make_shared(); goal->x = x; goal->y = y; goal->theta = theta; - return GoToRotateFirst(goal, isClose, collisionMask); + return GoToRotateFirst(goal, isClose, collisionMask, front); } - int NavigationHelper::GoToRotateFirst(const Point& goal, bool isClose, int collisionMask) + int NavigationHelper::GoToRotateFirst(const Point& goal, bool isClose, int collisionMask, bool front) { - return GoToRotateFirst(goal.x, goal.y, goal.theta, isClose, collisionMask); + return GoToRotateFirst(goal.x, goal.y, goal.theta, isClose, collisionMask, front); } int NavigationHelper::CanGoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask) diff --git a/src/modelec_strat/src/obstacle/box.cpp b/src/modelec_strat/src/obstacle/box.cpp index 31a29d3..fdae6b2 100644 --- a/src/modelec_strat/src/obstacle/box.cpp +++ b/src/modelec_strat/src/obstacle/box.cpp @@ -45,7 +45,7 @@ namespace Modelec } } - return Point(x_, y_, optimizedAngle); + return {x_, y_, optimizedAngle}; } std::vector BoxObstacle::GetAllPossiblePositions() const diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index c95c054..be23ccf 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -67,9 +67,10 @@ namespace Modelec } game_action_sequence_.push(State::TAKE_MISSION); - game_action_sequence_.push(State::FREE_MISSION); game_action_sequence_.push(State::TAKE_MISSION); game_action_sequence_.push(State::FREE_MISSION); + game_action_sequence_.push(State::FREE_MISSION); + static_strat_ = true; } void StratFMS::Init() @@ -145,8 +146,8 @@ namespace Modelec action_executor_->Up(BaseAction::Front::BOTH, true); action_executor_->Free({ - {0, true}, {1, true}, {2, true}, {3, true}, - {0, false}, {1, false}, {2, false}, {3, false}, + {0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}, + {0, BaseAction::BACK}, {1, BaseAction::BACK}, {2, BaseAction::BACK}, {3, BaseAction::BACK}, }, true); Transition(State::WAIT_START, "System ready"); @@ -178,7 +179,11 @@ namespace Modelec Transition(State::STOP, "All missions done"); } - else if (elapsed.seconds() < 70) + else if (elapsed.seconds() > 60 && !action_executor_->IsEmpty()) + { + Transition(State::FREE_MISSION, "No Time left, freeing boxes"); + } + else if (elapsed.seconds() < 80) { Transition(State::SELECT_GAME_ACTION, "Selecting a game action"); } @@ -191,15 +196,74 @@ namespace Modelec } case State::SELECT_GAME_ACTION: { - if (game_action_sequence_.empty()) + + if (static_strat_) { + if (game_action_sequence_.empty()) { + Transition(State::DO_GO_HOME, "No more game actions in sequence"); + return; + } + + auto next_action = game_action_sequence_.front(); + game_action_sequence_.pop(); + + Transition(next_action, "Selecting next game action from sequence"); + return; + } + + + // TODO : If close to border, do the side mission (thermometre) + + if (action_executor_->IsFull()) + { + RCLCPP_INFO(get_logger(), "All box are present on robot, selecting FREE mission"); + Transition(State::FREE_MISSION, "Selecting FREE mission"); + } + else if (action_executor_->IsEmpty()) { - Transition(State::DO_GO_HOME, "No more game actions"); + RCLCPP_INFO(get_logger(), "No box present on robot, selecting TAKE mission"); + Transition(State::TAKE_MISSION, "Selecting TAKE mission"); } else { - auto action = game_action_sequence_.front(); - game_action_sequence_.pop(); - Transition(action, "Selected game action"); + auto pos = nav_->GetCurrentPos(); + auto closestBox = nav_->GetClosestObstacle(pos); + auto closestDeposite = nav_->GetClosestDepositeZone(pos); + + if (closestBox && closestDeposite) + { + double distToBox = Point::distance(Point(pos->x, pos->y, pos->theta), + closestBox->GetPosition()); + double distToDeposite = Point::distance(Point(pos->x, pos->y, pos->theta), + closestDeposite->GetPosition()); + + if (distToBox < distToDeposite) + { + RCLCPP_INFO(get_logger(), "Choosing TAKE mission (dist to box: %.2f < dist to deposite: %.2f)", + distToBox, distToDeposite); + Transition(State::TAKE_MISSION, "Selecting TAKE mission"); + } + else + { + RCLCPP_INFO(get_logger(), "Choosing FREE mission (dist to deposite: %.2f < dist to box: %.2f)", + distToDeposite, distToBox); + Transition(State::FREE_MISSION, "Selecting FREE mission"); + } + } + else if (closestBox) + { + RCLCPP_INFO(get_logger(), "Only box available, selecting TAKE mission"); + Transition(State::TAKE_MISSION, "Selecting TAKE mission"); + } + else if (closestDeposite) + { + RCLCPP_INFO(get_logger(), "Only deposite available, selecting FREE mission"); + Transition(State::FREE_MISSION, "Selecting FREE mission"); + } + else + { + RCLCPP_WARN(get_logger(), "No box or deposite available, cannot select mission!"); + Transition(State::STOP, "No missions available"); + } } } @@ -207,28 +271,73 @@ namespace Modelec case State::TAKE_MISSION: if (!current_mission_) { - current_mission_ = std::make_unique(nav_, action_executor_); - current_mission_->Start(shared_from_this()); + if (action_executor_->HasFrontBox()) + { + if (action_executor_->HasBackBox()) + { + RCLCPP_WARN(get_logger(), "Both front and back box obstacles are occupied!"); + current_mission_.reset(); + Transition(State::SELECT_MISSION, "Cannot take box, both sides occupied"); + break; + } + + RCLCPP_INFO(get_logger(), "Front box obstacle is occupied, taking from back"); + current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::BACK); + current_mission_->Start(shared_from_this()); + } + else + { + current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::FRONT); + current_mission_->Start(shared_from_this()); + } } current_mission_->Update(); if (current_mission_->GetStatus() == MissionStatus::DONE) { current_mission_.reset(); Transition(State::SELECT_MISSION, "Take done"); + } + else if (current_mission_->GetStatus() == MissionStatus::FAILED) + { + current_mission_.reset(); + RCLCPP_ERROR(get_logger(), "Take mission failed!"); + Transition(State::SELECT_MISSION, "Take mission failed"); } break; case State::FREE_MISSION: if (!current_mission_) { - current_mission_ = std::make_unique(nav_, action_executor_); - current_mission_->Start(shared_from_this()); + if (!action_executor_->HasFrontBox()) + { + if (!action_executor_->HasBackBox()) + { + RCLCPP_WARN(get_logger(), "Both front and back box obstacles are free!"); + Transition(State::SELECT_MISSION, "Cannot free box, both sides empty"); + break; + } + + RCLCPP_INFO(get_logger(), "Front box obstacle is occupied, taking from back"); + current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::BACK); + current_mission_->Start(shared_from_this()); + } + else + { + current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::FRONT); + current_mission_->Start(shared_from_this()); + } } current_mission_->Update(); if (current_mission_->GetStatus() == MissionStatus::DONE) { current_mission_.reset(); Transition(State::SELECT_MISSION, "Free done"); + } + else if (current_mission_->GetStatus() == MissionStatus::FAILED) + { + current_mission_.reset(); + RCLCPP_ERROR(get_logger(), "Free mission failed!"); + Transition(State::SELECT_MISSION, "Free mission failed"); } break; @@ -243,6 +352,12 @@ namespace Modelec { current_mission_.reset(); Transition(State::STOP, "Cleanup done"); + } + else if (current_mission_->GetStatus() == MissionStatus::FAILED) + { + current_mission_.reset(); + RCLCPP_ERROR(get_logger(), "Go Home mission failed!"); + Transition(State::STOP, "Go Home mission failed"); } break; diff --git a/src/modelec_utils/include/modelec_utils/point.hpp b/src/modelec_utils/include/modelec_utils/point.hpp index 578286f..90850ac 100644 --- a/src/modelec_utils/include/modelec_utils/point.hpp +++ b/src/modelec_utils/include/modelec_utils/point.hpp @@ -1,6 +1,6 @@ #pragma once -#define CLOSE_DISTANCE 150 +#define CLOSE_DISTANCE 165 #define BASE_DISTANCE 290 namespace Modelec diff --git a/start_ros2.sh b/start_ros2.sh index 56c9bba..25ed44d 100755 --- a/start_ros2.sh +++ b/start_ros2.sh @@ -2,11 +2,11 @@ set -e source /opt/ros/jazzy/setup.bash -source /home/modelec/modelec-marcel-ROS/install/setup.bash +source /home/modelec/Modelec-ROS2/install/setup.bash export RCL_LOG_LEVEL=info export RMW_IMPLEMENTATION=rmw_fastrtps_cpp -export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/modelec-marcel-ROS/fastdds_setup.xml +export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/Modelec-ROS2/fastdds_setup.xml export ROS_DOMAIN_ID=128 exec ros2 launch modelec_core modelec.launch.py "$@"