modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-03-18 21:50:36 +01:00
Code Issues Packages Projects Releases Wiki Activity

Dynamic strat BackFront (#27)

Browse Source 
Co-authored-by: Modelec <modelec.isen@gmail.com>
This commit is contained in:
Ackimixs
2026-01-28 08:53:14 +01:00
committed by GitHub
parent 083a3032f4
commit e00a040358
30 changed files with 457 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
fastdds_setup.xml
View File

@@ -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>

2
joy.ros2_launch_marcel.desktop
View File

@@ -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;

2
no_lidar.joy.ros2_launch_marcel.desktop
View File

@@ -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;

4
no_lidar.ros2_launch_marcel.desktop
View File

@@ -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;

2
ros2_launch_marcel.desktop
View File

@@ -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;

4
src/modelec_com/src/pcb_action_interface.cpp
View File

@@ -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());

2
src/modelec_strat/data/obstacles.xml
View File

@@ -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"/>

14
src/modelec_strat/include/modelec_strat/action/free_action.hpp
View File

@@ -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_;

14
src/modelec_strat/include/modelec_strat/action/take_action.hpp
View File

@@ -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_;

16
src/modelec_strat/include/modelec_strat/action_executor.hpp
View File

@@ -35,9 +35,9 @@ 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);
@@ -45,6 +45,18 @@ namespace Modelec
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_;

11
src/modelec_strat/include/modelec_strat/missions/free_mission.hpp
View File

@@ -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;
@@ -25,7 +26,9 @@ namespace Modelec {
UP,
GO_BACK,
DONE,
} step_;
};
BaseAction::Front front_;
MissionStatus status_;
std::shared_ptr<NavigationHelper> nav_;
@@ -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_;
};
}

13
src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp
View File

@@ -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_;
@@ -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_;
};
}

3
src/modelec_strat/include/modelec_strat/missions/mission_base.hpp
View File

@@ -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_;
};
}

11
src/modelec_strat/include/modelec_strat/missions/take_mission.hpp
View File

@@ -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;
@@ -25,7 +26,9 @@ namespace Modelec {
TAKE,
UP,
DONE,
} step_;
};
BaseAction::Front front_;
std::shared_ptr<BoxObstacle> closestBox;
MissionStatus status_;
@@ -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_;
};
}

10
src/modelec_strat/include/modelec_strat/navigation_helper.hpp
View File

@@ -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);

1
src/modelec_strat/include/modelec_strat/strat_fms.hpp
View File

@@ -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_;

6
src/modelec_strat/src/action/down_action.cpp
View File

@@ -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;
}

20
src/modelec_strat/src/action/free_action.cpp
View File

@@ -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());
}

20
src/modelec_strat/src/action/take_action.cpp
View File

@@ -6,17 +6,17 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_ex
{
}
Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n) : TakeAction(action_executor)
Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front, int n) : TakeAction(action_executor)
{
AddServo(n, front);
}
Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> servo) : TakeAction(action_executor)
Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, Front> servo) : TakeAction(action_executor)
{
AddServo(servo);
}
Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, bool>> servos) : TakeAction(action_executor)
Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::vector<std::pair<int, Front>> 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<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::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<int, bool> servo)
void Modelec::TakeAction::AddServo(std::pair<int, Front> servo)
{
servos_.emplace_back(servo);
}
void Modelec::TakeAction::AddServos(const std::vector<std::pair<int, bool>>& servos)
void Modelec::TakeAction::AddServos(const std::vector<std::pair<int, Front>>& servos)
{
servos_.insert(servos_.end(), servos.begin(), servos.end());
}

8
src/modelec_strat/src/action/up_action.cpp
View File

@@ -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;
}

38
src/modelec_strat/src/action_executor.cpp
View File

@@ -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<std::pair<int, bool>> servos, bool force) {
void ActionExecutor::Take(const std::vector<std::pair<int, BaseAction::Front>>& servos, bool force) {
if (action_done_ || force)
{
action_ = std::make_shared<TakeAction>(shared_from_this(), servos);
@@ -191,7 +191,7 @@ namespace Modelec
}
}
void ActionExecutor::Free(std::vector<std::pair<int, bool>> servos, bool force) {
void ActionExecutor::Free(const std::vector<std::pair<int, BaseAction::Front>>& servos, bool force) {
if (action_done_ || force)
{
action_ = std::make_shared<FreeAction>(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();
}
}

7
src/modelec_strat/src/deposite_zone.cpp
View File

@@ -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<int>(position_.x + dist * std::cos(angle)),
static_cast<int>(position_.y + dist * std::sin(angle)),
position_.x + dist * std::cos(angle),
position_.y + dist * std::sin(angle),
angle);
double distance = p.distance(currentPos);

96
src/modelec_strat/src/missions/free_mission.cpp
View File

@@ -3,8 +3,11 @@
#include "modelec_strat/action/base_action.hpp"
namespace Modelec {
FreeMission::FreeMission(const std::shared_ptr<NavigationHelper>& nav, const std::shared_ptr<ActionExecutor>& action_executor)
: step_(GO_TO_FREE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) {
FreeMission::FreeMission(const std::shared_ptr<NavigationHelper>& nav,
const std::shared_ptr<ActionExecutor>& 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<int> 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:
{

43
src/modelec_strat/src/missions/go_home_mission.cpp
View File

@@ -5,7 +5,7 @@
namespace Modelec
{
GoHomeMission::GoHomeMission(const std::shared_ptr<NavigationHelper>& 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<std_msgs::msg::Int64>("/strat/score", 10);
go_timeout_ = node_->now();
last_ask_waypoint_time_ = node_->now();
status_ = MissionStatus::RUNNING;
step_ = ROTATE_TO_HOME;
std::queue<int> 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:

100
src/modelec_strat/src/missions/take_mission.cpp
View File

@@ -3,8 +3,11 @@
#include "modelec_strat/action/base_action.hpp"
namespace Modelec {
TakeMission::TakeMission(const std::shared_ptr<NavigationHelper>& nav, const std::shared_ptr<ActionExecutor>& action_executor)
: step_(GO_TO_TAKE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) {
TakeMission::TakeMission(const std::shared_ptr<NavigationHelper>& nav,
const std::shared_ptr<ActionExecutor>& 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<int> 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;
auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
action_executor_->box_obstacles_[front_] = closestBox;
if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL))
auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
pos.theta += (front_ == BaseAction::FRONT ? 0 : M_PI);
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:

20
src/modelec_strat/src/navigation_helper.cpp
View File

@@ -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<PosMsg>();
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)

2
src/modelec_strat/src/obstacle/box.cpp
View File

@@ -45,7 +45,7 @@ namespace Modelec
}
}
return Point(x_, y_, optimizedAngle);
return {x_, y_, optimizedAngle};
}
std::vector<Point> BoxObstacle::GetAllPossiblePositions() const

141
src/modelec_strat/src/strat_fms.cpp
View File

@@ -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())
{
Transition(State::DO_GO_HOME, "No more game actions");
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())
{
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<BoxObstacle>(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<TakeMission>(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<TakeMission>(nav_, action_executor_, BaseAction::BACK);
current_mission_->Start(shared_from_this());
}
else
{
current_mission_ = std::make_unique<TakeMission>(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<FreeMission>(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<FreeMission>(nav_, action_executor_, BaseAction::BACK);
current_mission_->Start(shared_from_this());
}
else
{
current_mission_ = std::make_unique<FreeMission>(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;

2
src/modelec_utils/include/modelec_utils/point.hpp
View File

@@ -1,6 +1,6 @@
#pragma once
#define CLOSE_DISTANCE 150
#define CLOSE_DISTANCE 165
#define BASE_DISTANCE 290
namespace Modelec

4
start_ros2.sh
View File

@@ -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 "$@"