Dynamic strat BackFront #27

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged

Ackimixs merged 80 commits into develop from develop-strat-5

Jan 28, 2026

fastdds_setup.xml

-Original file line number
+Diff line change
@@ Expand Up / @@ -23,7 +23,7 @@ @@
                         <!-- End repeat -->
                         <locator>
                             <udpv4>
-                                <address>100.113.219.4</address> <!-- ackimixs -->
+                                <address>100.84.205.108</address> <!-- ackimixs -->
                             </udpv4>
                         </locator>
                         <locator>
@@ Expand Down @@

joy.ros2_launch_marcel.desktop

-Original file line number
+Diff line change
@@ Expand Up / @@ -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;

no_lidar.joy.ros2_launch_marcel.desktop

-Original file line number
+Diff line change
@@ Expand Up / @@ -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;

no_lidar.ros2_launch_marcel.desktop

-Original file line number
+Diff line change
@@ Expand Up / @@ -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;

ros2_launch_marcel.desktop

-Original file line number
+Diff line change
@@ Expand Up / @@ -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;

src/modelec_com/src/pcb_action_interface.cpp

-Original file line number
+Diff line change
@@ Expand Up / @@ -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());
@@ Expand Down @@

src/modelec_strat/data/obstacles.xml

-Original file line number
+Diff line change
@@ Expand Up / @@ -18,7 +18,7 @@ @@
             <possible-angle theta="-1.5708" />
         </Box>
         <Box id="22" x="2825" y="400" theta="3.1415" width="200" height="150" type="box"/>
-        <Box id="23" x="1900" y="175" theta="-1.5708" width="200" height="150" type="box"/>
+        <Box id="23" x="1900" y="175" theta="1.5708" width="200" height="150" type="box"/>
         <!-- PAMI Start -->
         <Obstacle id="30" x="75" y="1750" theta="1.5708" width="150" height="450" type="pami-start"/>
@@ Expand Down @@

src/modelec_strat/include/modelec_strat/action/free_action.hpp

            
                      Original file line number
                      Diff line number
                      Diff line change
                  
    @@ -9,20 +9,20 @@ namespace Modelec
  
        {

        public:

            FreeAction(const std::shared_ptr<ActionExecutor>& action_executor);

            FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n);

            FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> servo);

            FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, bool>> servos);

            FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front, int n);

            FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Front> servo);

            FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Front>> servos);

            void Next() override;

            void Init(const std::vector<std::string>& params) override;

            void AddServo(int id, bool front);

            void AddServo(std::pair<int, bool> servo);

            void AddServos(const std::vector<std::pair<int, bool>>& servos);

            void AddServo(int id, Front front);

            void AddServo(std::pair<int, Front> servo);

            void AddServos(const std::vector<std::pair<int, Front>>& servos);

            inline static const std::string Name = ActionExec::FREE;

        private:

            std::vector<std::pair<int, bool>> servos_;

            std::vector<std::pair<int, Front>> servos_;

            std::queue<int> steps_;

src/modelec_strat/include/modelec_strat/action/take_action.hpp

            
                      Original file line number
                      Diff line number
                      Diff line change
                  
    @@ -9,20 +9,20 @@ namespace Modelec
  
        {

        public:

            TakeAction(const std::shared_ptr<ActionExecutor>& action_executor);

            TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n);

            TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> servo);

            TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, bool>> servos);

            TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front, int n);

            TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Front> servo);

            TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Front>> servos);

            void Next() override;

            void Init(const std::vector<std::string>& params) override;

            void AddServo(int id, bool front);

            void AddServo(std::pair<int, bool> servo);

            void AddServos(const std::vector<std::pair<int, bool>>& servos);

            void AddServo(int id, Front front);

            void AddServo(std::pair<int, Front> servo);

            void AddServos(const std::vector<std::pair<int, Front>>& servos);

            inline static const std::string Name = ActionExec::TAKE;

        private:

            std::vector<std::pair<int, bool>> servos_;

            std::vector<std::pair<int, Front>> servos_;

            std::queue<int> steps_;

src/modelec_strat/include/modelec_strat/action_executor.hpp

            
                      Original file line number
                      Diff line number
                      Diff line change
                  
    @@ -35,16 +35,28 @@ namespace Modelec
  
            void Up(BaseAction::Front front, bool force = false);

            void Take(std::vector<std::pair<int, bool>> servos, bool force = false);

            void Take(const std::vector<std::pair<int, BaseAction::Front>>& servos, bool force = false);

            void Free(std::vector<std::pair<int, bool>> servos, bool force = false);

            void Free(const std::vector<std::pair<int, BaseAction::Front>>& servos, bool force = false);

            void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg);

            void MoveServo(const modelec_interfaces::msg::ActionServoPosArray& msg);

            std::array<std::shared_ptr<BoxObstacle>, 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<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_pub_;

            rclcpp::Publisher<modelec_interfaces::msg::ActionServoPosArray>::SharedPtr servo_move_pub_;

src/modelec_strat/include/modelec_strat/missions/free_mission.hpp

-Original file line number
+Diff line change
@@ Expand Up / @@ -8,7 +8,8 @@ namespace Modelec { @@
         class FreeMission : public Mission {
         public:
             FreeMission(const std::shared_ptr<NavigationHelper>& nav,
-                          const std::shared_ptr<ActionExecutor>& action_executor);
+                          const std::shared_ptr<ActionExecutor>& action_executor,
+                          BaseAction::Front front = BaseAction::FRONT);
             void Start(rclcpp::Node::SharedPtr node) override;
             void Update() override;
@@ Expand All / @@ -25,7 +26,9 @@ namespace Modelec { @@
                 UP,
                 GO_BACK,
                 DONE,
-            } step_;
+            };
+            BaseAction::Front front_;
             MissionStatus status_;
             std::shared_ptr<NavigationHelper> nav_;
@@ Expand All / @@ -38,5 +41,9 @@ namespace Modelec { @@
             rclcpp::Time go_timeout_;
             rclcpp::Time deploy_time_;
+            std::optional<rclcpp::Time> min_time_;
+            rclcpp::Time last_ask_waypoint_time_;
         };
     }

src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp

-Original file line number
+Diff line change
@@ Expand Up / @@ -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<NavigationHelper> nav_;
@@ Expand All / @@ -38,5 +35,9 @@ namespace Modelec @@
             rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr score_pub_;
             int mission_score_ = 0;
             rclcpp::Time go_timeout_;
+            std::optional<rclcpp::Time> min_time_;
+            rclcpp::Time last_ask_waypoint_time_;
         };
     }

src/modelec_strat/include/modelec_strat/missions/mission_base.hpp

-Original file line number
+Diff line change
@@ Expand Up / @@ -24,5 +24,8 @@ namespace Modelec @@
             virtual void Clear() = 0;
             virtual MissionStatus GetStatus() const = 0;
             virtual std::string GetName() const = 0;
+        protected:
+            std::queue<int> steps_;
         };
     }

src/modelec_strat/include/modelec_strat/missions/take_mission.hpp

-Original file line number
+Diff line change
@@ Expand Up / @@ -8,7 +8,8 @@ namespace Modelec { @@
         class TakeMission : public Mission {
         public:
             TakeMission(const std::shared_ptr<NavigationHelper>& nav,
-                          const std::shared_ptr<ActionExecutor>& action_executor);
+                          const std::shared_ptr<ActionExecutor>& action_executor,
+                          BaseAction::Front front = BaseAction::FRONT);
             void Start(rclcpp::Node::SharedPtr node) override;
             void Update() override;
@@ Expand All / @@ -25,7 +26,9 @@ namespace Modelec { @@
                 TAKE,
                 UP,
                 DONE,
-            } step_;
+            };
+            BaseAction::Front front_;
             std::shared_ptr<BoxObstacle> closestBox;
             MissionStatus status_;
@@ Expand All / @@ -35,5 +38,9 @@ namespace Modelec { @@
             rclcpp::Time go_timeout_;
             rclcpp::Time deploy_time_;
+            std::optional<rclcpp::Time> min_time_;
+            rclcpp::Time last_ask_waypoint_time_;
         };
     }

src/modelec_strat/include/modelec_strat/navigation_helper.hpp

            
                      Original file line number
                      Diff line number
                      Diff line change
                  
    @@ -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);

src/modelec_strat/include/modelec_strat/strat_fms.hpp

-Original file line number
+Diff line change
@@ Expand Up / @@ -62,6 +62,7 @@ namespace Modelec @@
             int team_id_ = 0;
             std::queue<State> game_action_sequence_;
+            bool static_strat_ = false;
             std::shared_ptr<NavigationHelper> nav_;
             std::shared_ptr<ActionExecutor> action_executor_;
@@ Expand Down @@

src/modelec_strat/src/action/down_action.cpp

            
                      Original file line number
                      Diff line number
                      Diff line change
                  
    @@ -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;

                }

src/modelec_strat/src/action/free_action.cpp

            
                      Original file line number
                      Diff line number
                      Diff line change
                  
    @@ -8,17 +8,17 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_ex
  
        steps_.push(ActionExec::DONE_STEP);

    }

    Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n) : FreeAction(action_executor)

    Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front, int n) : FreeAction(action_executor)

    {

        AddServo(n, front);

    }

    Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> servo) : FreeAction(action_executor)

    Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Front> servo) : FreeAction(action_executor)

    {

        AddServo(servo.first, servo.second);

    }

    Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, bool>> servos) : FreeAction(action_executor)

    Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Front>> 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<std::string>& 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<int, bool> servo)

    void Modelec::FreeAction::AddServo(std::pair<int, Front> servo)

    {

        servos_.emplace_back(servo);

    }

    void Modelec::FreeAction::AddServos(const std::vector<std::pair<int, bool>>& servos)

    void Modelec::FreeAction::AddServos(const std::vector<std::pair<int, Front>>& servos)

    {

        servos_.insert(servos_.end(), servos.begin(), servos.end());

    }

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Dynamic strat BackFront #27

Uh oh!

Diff view

Diff view

There are no files selected for viewing

Uh oh!