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

add new playmat

Browse Source
This commit is contained in:
acki
2025-10-06 17:50:02 +02:00
parent 037e3c841a
commit 06091b7d65
22 changed files with 270010 additions and 555 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/modelec_core/CMakeLists.txt
View File

@@ -17,14 +17,6 @@ find_package(Boost REQUIRED COMPONENTS system)
find_package(modelec_interfaces REQUIRED)
find_package(modelec_utils REQUIRED)
add_executable(speed_result src/speed_result.cpp)
ament_target_dependencies(speed_result rclcpp std_msgs sensor_msgs modelec_interfaces)
target_link_libraries(speed_result modelec_utils::utils)
target_include_directories(speed_result PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# Enable testing and linting
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
@@ -35,7 +27,6 @@ endif()
# Install targets
install(TARGETS
speed_result
DESTINATION lib/${PROJECT_NAME}
)

26
src/modelec_core/include/modelec/speed_result.hpp
View File

@@ -1,26 +0,0 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <modelec_interfaces/msg/odometry_speed.hpp>
#include <fstream>
namespace Modelec
{
class SpeedResultNode : public rclcpp::Node {
public:
SpeedResultNode();
~SpeedResultNode() override;
private:
rclcpp::Time start_time_;
std::string fileName_;
std::ofstream file_;
rclcpp::Subscription<modelec_interfaces::msg::OdometrySpeed>::SharedPtr sub_speed_;
void SpeedCallback(const modelec_interfaces::msg::OdometrySpeed::SharedPtr msg);
};
}

37
src/modelec_core/src/speed_result.cpp
View File

@@ -1,37 +0,0 @@
#include <modelec/speed_result.hpp>
namespace Modelec {
SpeedResultNode::SpeedResultNode() : Node("SpeedResult"), fileName_("speed_data.csv"), file_(fileName_)
{
if (!file_.is_open()) {
std::cerr << "Error opening file: " << fileName_ << std::endl;
}
sub_speed_ = this->create_subscription<modelec_interfaces::msg::OdometrySpeed>(
"odometry/speed", 10,
std::bind(&SpeedResultNode::SpeedCallback, this, std::placeholders::_1));
start_time_ = this->now();
file_ << "time,x,y,theta" << std::endl; // Write header to the file
}
SpeedResultNode::~SpeedResultNode()
{
file_.close();
}
void SpeedResultNode::SpeedCallback(const modelec_interfaces::msg::OdometrySpeed::SharedPtr msg)
{
std::string time = std::to_string(this->now().nanoseconds() - start_time_.nanoseconds());
file_ << time << ","
<< msg->x << "," << msg->y << "," << msg->theta << std::endl;
}
}
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Modelec::SpeedResultNode>());
rclcpp::shutdown();
return 0;
}

123234
src/modelec_gui/img/playmat/2026_BETA.svg Normal file
View File

File diff suppressed because one or more lines are too long
Side by Side

After

Width:  |  Height:  |  Size: 41 MiB

146701
src/modelec_gui/img/playmat/2026_FINAL.svg Normal file
View File

File diff suppressed because one or more lines are too long
Side by Side

After

Width:  |  Height:  |  Size: 17 MiB

2
src/modelec_gui/resource.qrc
View File

@@ -2,6 +2,8 @@
<RCC version="1.0">
<qresource prefix="/">
<file>img/playmat/2025_FINAL.svg</file>
<file>img/playmat/2026_BETA.svg</file>
<file>img/playmat/2026_FINAL.svg</file>
<file>img/playmat/grid_v1.svg</file>
<file>img/logo/ISEN-Nantes.png</file>
<file>img/logo/ISEN-Nantes-fond-blanc.svg</file>

2
src/modelec_gui/src/pages/home_page.cpp
View File

@@ -8,7 +8,7 @@ namespace ModelecGUI
{
HomePage::HomePage(rclcpp::Node::SharedPtr node, QWidget* parent)
: QWidget(parent), node_(node),
renderer_(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this))
renderer_(new QSvgRenderer(QString(":/img/playmat/2026_FINAL.svg"), this))
{
spawn_pub_ = node_->create_publisher<modelec_interfaces::msg::Spawn>("strat/spawn", 10);

2
src/modelec_gui/src/pages/map_page.cpp
View File

@@ -10,7 +10,7 @@ namespace ModelecGUI
{
MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent),
renderer(new QSvgRenderer(
QString(":/img/playmat/2025_FINAL.svg"),
QString(":/img/playmat/2026_BETA.svg"),
this)), node_(node)
{
ratioBetweenMapAndWidgetX_ = width() / 3000.0f;

4
src/modelec_gui/src/pages/test_map_page.cpp
View File

@@ -11,7 +11,7 @@ namespace ModelecGUI
TestMapPage::TestMapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent),
renderer(new QSvgRenderer(
QString(
":/img/playmat/2025_FINAL.svg"),
":/img/playmat/2026_FINAL.svg"),
this)), node_(node)
{
ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
@@ -147,7 +147,7 @@ namespace ModelecGUI
void TestMapPage::setPlaymatMap()
{
renderer->load(QString(":/img/playmat/2025_FINAL.svg"));
renderer->load(QString(":/img/playmat/2026_FINAL.svg"));
update();
}

7
src/modelec_strat/CMakeLists.txt
View File

@@ -15,7 +15,7 @@ find_package(ament_index_cpp REQUIRED)
find_package(modelec_interfaces REQUIRED)
find_package(modelec_utils REQUIRED)
add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/missions/prepare_concert_mission.cpp src/missions/banner_mission.cpp src/missions/go_home_mission.cpp src/obstacle/obstacle.cpp src/obstacle/column.cpp src/deposite_zone.cpp)
add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/missions/banner_mission.cpp src/missions/go_home_mission.cpp src/obstacle/obstacle.cpp src/obstacle/box.cpp src/deposite_zone.cpp)
ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces ament_index_cpp)
target_link_libraries(strat_fsm modelec_utils::utils modelec_utils::config)
target_include_directories(strat_fsm PUBLIC
@@ -60,4 +60,9 @@ install(
FILES_MATCHING PATTERN "*.xml"
)
# Install include files
install(DIRECTORY include/
DESTINATION include/
)
ament_package()

24
src/modelec_strat/data/config.xml
View File

@@ -40,38 +40,14 @@
<spawn>
<yellow>
<top x="375" y="1800" theta="-1.5708" />
<side x="2800" y="875" theta="3.1415" />
<bottom x="1225" y="200" theta="1.5708" />
</yellow>
<blue>
<top x="2625" y="1800" theta="-1.5708" />
<side x="200" y="875" theta="0" />
<bottom x="1775" y="200" theta="1.5708" />
</blue>
<width_mm>450</width_mm>
<height_mm>450</height_mm>
</spawn>
<usb>
<pcb>
<pcb_odo>
<name>pcb_odo</name>
<port>/dev/USB_ODO</port>
<!--<port>/tmp/USB_ODO</port>-->
<baud_rate>115200</baud_rate>
</pcb_odo>
<pcb_alim>
<name>pcb_alim</name>
<port>/dev/USB_ALIM</port>
<!--<port>/tmp/USB_ALIM</port>-->
<baud_rate>115200</baud_rate>
</pcb_alim>
<pcb_action>
<name>pcb_action</name>
<port>/dev/USB_ACTION</port>
<!--<port>/tmp/USB_ACTION</port>-->
<baud_rate>115200</baud_rate>
</pcb_action>
</pcb>
<timeout_ms>300</timeout_ms>
<attempt>5</attempt>
</usb>

79
src/modelec_strat/data/deposite_zone.xml
View File

@@ -1,67 +1,32 @@
<Map>
<!-- YELLOW ZONE -->
<DepositeZone id="100" team="0" max_pot="1">
<Pos x="775" y="75" theta="1.5708" w="450" h="150" />
<PotPos>
<Pos x="775" y="75" theta="1.5708" />
</PotPos>
<DepositeZone id="0">
<Pos x="1150" y="1350" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="101" team="0" max_pot="2">
<Pos x="1225" y="225" theta="1.5708" w="450" h="450" />
<PotPos>
<!--<Pos x="1225" y="125" theta="1.5708" />-->
<Pos x="1225" y="200" theta="1.5708" />
<Pos x="1225" y="400" theta="1.5708" />
</PotPos>
<DepositeZone id="1">
<Pos x="1650" y="1350" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="102" team="0" max_pot="1">
<Pos x="2775" y="75" theta="1.5708" w="450" h="150" />
<PotPos>
<Pos x="2775" y="75" theta="1.5708" />
</PotPos>
<DepositeZone id="2">
<Pos x="0" y="700" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="103" team="0" max_pot="3">
<Pos x="2775" y="875" theta="3.1415" w="450" h="450" />
<PotPos>
<Pos x="2925" y="875" theta="3.1415" />
<Pos x="2775" y="875" theta="3.1415" />
<Pos x="2600" y="875" theta="3.1415" />
</PotPos>
<DepositeZone id="3">
<Pos x="700" y="700" theta="0" w="200" h="200" />
</DepositeZone>
<!-- BLUE ZONE -->
<DepositeZone id="110" team="1" max_pot="1">
<Pos x="2225" y="75" theta="1.5708" w="450" h="150" />
<PotPos>
<Pos x="2225" y="75" theta="1.5708" />
</PotPos>
<DepositeZone id="4">
<Pos x="1400" y="700" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="111" team="1" max_pot="2">
<Pos x="1775" y="225" theta="1.5708" w="450" h="450" />
<PotPos>
<!--<Pos x="1775" y="75" theta="1.5708" />-->
<Pos x="1775" y="200" theta="1.5708" />
<Pos x="1775" y="400" theta="1.5708" />
</PotPos>
<DepositeZone id="5">
<Pos x="2100" y="700" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="112" team="1" max_pot="1">
<Pos x="225" y="75" theta="1.5708" w="450" h="150" />
<PotPos>
<Pos x="225" y="75" theta="1.5708" />
</PotPos>
<DepositeZone id="6">
<Pos x="2800" y="700" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="113" team="1" max_pot="3">
<Pos x="225" y="875" theta="0" w="450" h="450" />
<PotPos>
<Pos x="75" y="875" theta="0" />
<Pos x="225" y="875" theta="0" />
<Pos x="400" y="875" theta="0" />
</PotPos>
<DepositeZone id="7">
<Pos x="600" y="0" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="8">
<Pos x="1400" y="0" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="9">
<Pos x="2200" y="0" theta="0" w="200" h="200" />
</DepositeZone>
</Map>

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

@@ -4,23 +4,23 @@
<Obstacle id="2" x="1500" y="1775" theta="1.5708" width="900" height="450" type="estrade"/>
<Obstacle id="3" x="2150" y="1900" theta="1.5708" width="400" height="200" type="estrade"/>
<!-- Gradin TOP TO BOTTOM LEFT -->
<Gradin id="10" x="825" y="1725" theta="-1.5708" width="400" height="100" type="gradin" />
<Gradin id="11" x="75" y="1325" theta="0" width="400" height="100" type="gradin"/>
<Gradin id="12" x="1100" y="950" theta="1.5708" width="400" height="100" type="gradin" >
<!-- Box TOP TO BOTTOM LEFT -->
<Box id="10" x="825" y="1725" theta="-1.5708" width="400" height="100" type="box" />
<Box id="11" x="75" y="1325" theta="0" width="400" height="100" type="box"/>
<Box id="12" x="1100" y="950" theta="1.5708" width="400" height="100" type="box" >
<possible-angle theta="-1.5708" />
</Gradin>
<Gradin id="13" x="75" y="400" theta="0" width="400" height="100" type="gradin"/>
<Gradin id="14" x="775" y="250" theta="1.5708" width="400" height="100" type="gradin"/>
</Box>
<Box id="13" x="75" y="400" theta="0" width="400" height="100" type="box"/>
<Box id="14" x="775" y="250" theta="1.5708" width="400" height="100" type="box"/>
<!-- Gradin TOP TO BOTTOM RIGHT -->
<Gradin id="20" x="2175" y="1725" theta="-1.5708" width="400" height="100" type="gradin" />
<Gradin id="21" x="2925" y="1325" theta="3.1415" width="400" height="100" type="gradin"/>
<Gradin id="22" x="1900" y="950" theta="1.5708" width="400" height="100" type="gradin">
<!-- Box TOP TO BOTTOM RIGHT -->
<Box id="20" x="2175" y="1725" theta="-1.5708" width="400" height="100" type="box" />
<Box id="21" x="2925" y="1325" theta="3.1415" width="400" height="100" type="box"/>
<Box id="22" x="1900" y="950" theta="1.5708" width="400" height="100" type="box">
<possible-angle theta="-1.5708" />
</Gradin>
<Gradin id="23" x="2925" y="400" theta="3.1415" width="400" height="100" type="gradin"/>
<Gradin id="24" x="2225" y="250" theta="1.5708" width="400" height="100" type="gradin"/>
</Box>
<Box id="23" x="2925" y="400" theta="3.1415" width="400" height="100" type="box"/>
<Box id="24" x="2225" y="250" theta="1.5708" width="400" height="100" type="box"/>
<!-- PAMI Start -->
<Obstacle id="30" x="75" y="1750" theta="1.5708" width="150" height="450" type="pami-start"/>

49
src/modelec_strat/include/modelec_strat/missions/prepare_concert_mission.hpp
View File

@@ -1,49 +0,0 @@
#pragma once
#include <modelec_strat/action_executor.hpp>
#include <modelec_strat/missions/mission_base.hpp>
#include <modelec_strat/navigation_helper.hpp>
#include <modelec_strat/obstacle/column.hpp>
#include <std_msgs/msg/int64.hpp>
namespace Modelec
{
class PrepareConcertMission : public Mission
{
public:
PrepareConcertMission(const std::shared_ptr<NavigationHelper>& nav,
const std::shared_ptr<ActionExecutor>& action_executor, bool two_floor = true);
void Start(rclcpp::Node::SharedPtr node) override;
void Update() override;
void Clear() override;
MissionStatus GetStatus() const override;
std::string GetName() const override { return "PrepareConcert"; }
private:
enum Step
{
GO_TO_COLUMN,
GO_CLOSE_TO_COLUMN,
TAKE_COLUMN,
GO_BACK,
GO_TO_PLATFORM,
GO_CLOSE_TO_PLATFORM,
PLACE_PLATFORM,
GO_BACK_2,
DONE
} step_;
MissionStatus status_;
std::shared_ptr<NavigationHelper> nav_;
std::shared_ptr<ActionExecutor> action_executor_;
rclcpp::Node::SharedPtr node_;
bool two_floor_;
std::shared_ptr<ColumnObstacle> column_;
std::shared_ptr<DepositeZone> closestDepoZone_;
Point closestDepoZonePoint_;
rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr score_pub_;
int mission_score_ = 0;
};
}

12
src/modelec_strat/include/modelec_strat/obstacle/column.hpp → src/modelec_strat/include/modelec_strat/obstacle/box.hpp
View File

@@ -5,15 +5,15 @@
namespace Modelec
{
class ColumnObstacle : public Obstacle
class BoxObstacle : public Obstacle
{
public:
ColumnObstacle() = default;
ColumnObstacle(const ColumnObstacle&) = default;
BoxObstacle() = default;
BoxObstacle(const BoxObstacle&) = default;
ColumnObstacle(int id, int x, int y, double theta, int w, int h, const std::string& type);
ColumnObstacle(tinyxml2::XMLElement* obstacleElem);
ColumnObstacle(const modelec_interfaces::msg::Obstacle& msg);
BoxObstacle(int id, int x, int y, double theta, int w, int h, const std::string& type);
BoxObstacle(tinyxml2::XMLElement* obstacleElem);
BoxObstacle(const modelec_interfaces::msg::Obstacle& msg);
bool IsAtObjective() const;
void SetAtObjective(bool atObjective);

6
src/modelec_strat/include/modelec_strat/pathfinding.hpp
View File

@@ -16,7 +16,7 @@
#include <modelec_strat/obstacle/obstacle.hpp>
#include "obstacle/column.hpp"
#include "obstacle/box.hpp"
namespace Modelec
@@ -87,8 +87,8 @@ namespace Modelec
typename = std::enable_if_t<std::is_base_of<Obstacle, T>::value>>
std::shared_ptr<T> GetClosestObstacle(const PosMsg::SharedPtr& pos) const;
std::shared_ptr<ColumnObstacle> GetClosestColumn(const PosMsg::SharedPtr& pos,
const std::vector<int>& blacklistedId = {});
std::shared_ptr<BoxObstacle> GetClosestColumn(const PosMsg::SharedPtr &pos,
const std::vector<int> &blacklistedId = {});
void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);

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

@@ -6,7 +6,6 @@
#include "navigation_helper.hpp"
#include "missions/mission_base.hpp"
#include "missions/banner_mission.hpp"
#include "missions/prepare_concert_mission.hpp"
#include "missions/go_home_mission.hpp"
#include <std_msgs/msg/bool.hpp>
@@ -28,7 +27,6 @@ namespace Modelec
WAIT_START,
SELECT_MISSION,
DO_PREPARE_CONCERT,
DO_PROMOTION,
DO_GO_HOME,

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

@@ -8,38 +8,6 @@ namespace Modelec
ActionExecutor::ActionExecutor(const rclcpp::Node::SharedPtr& node) : node_(node)
{
/*asc_get_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionAscPos>("/action/get/asc", 10);
servo_get_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionServoPos>("/action/get/servo", 10);
relay_get_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionRelayState>("/action/get/relay", 10);
asc_get_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionAscPos>(
"/action/get/asc/res", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr)
{
});
servo_get_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionServoPos>(
"/action/get/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr)
{
});
relay_get_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionRelayState>(
"/action/get/relay/res", 10, [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr)
{
});
asc_set_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionAscPos>("/action/set/asc", 10);
servo_set_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionServoPos>("/action/set/servo", 10);
asc_set_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionAscPos>(
"/action/set/asc/res", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr)
{
});
servo_set_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionServoPos>(
"/action/set/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr)
{
});*/
asc_move_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionAscPos>("/action/move/asc", 10);
servo_move_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionServoPos>("/action/move/servo", 10);
relay_move_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionRelayState>("/action/move/relay", 10);

244
src/modelec_strat/src/missions/prepare_concert_mission.cpp
View File

@@ -1,244 +0,0 @@
#include <modelec_strat/missions/prepare_concert_mission.hpp>
#include <modelec_strat/obstacle/column.hpp>
#include <modelec_utils/config.hpp>
namespace Modelec
{
PrepareConcertMission::PrepareConcertMission(const std::shared_ptr<NavigationHelper>& nav,
const std::shared_ptr<ActionExecutor>& action_executor,
bool two_floor) :
step_(GO_TO_COLUMN),
status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor), two_floor_(two_floor)
{
}
void PrepareConcertMission::Start(rclcpp::Node::SharedPtr node)
{
node_ = node;
if (two_floor_)
{
mission_score_ = Config::get<int>("config.mission_score.concert.niv_2", 0);
}
else
{
mission_score_ = Config::get<int>("config.mission_score.concert.niv_1", 0);
}
score_pub_ = node_->create_publisher<std_msgs::msg::Int64>("/strat/score", 10);
if (!nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId()))
{
status_ = MissionStatus::FINISH_ALL;
return;
}
int timeout = 0;
std::vector<int> blacklistId = {};
bool canGo = false;
while (!canGo && timeout < 10)
{
if (auto col = nav_->GetPathfinding()->GetClosestColumn(nav_->GetCurrentPos(), blacklistId))
{
column_ = col;
auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakePosition(400);
auto res = nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL);
if (res != Pathfinding::FREE)
{
RCLCPP_WARN(node_->get_logger(), "Cannot go to column %d, trying another one", column_->GetId());
blacklistId.push_back(column_->GetId());
}
else
{
canGo = true;
}
}
timeout++;
}
if (!canGo)
{
status_ = MissionStatus::FINISH_ALL;
return;
}
status_ = MissionStatus::RUNNING;
}
void PrepareConcertMission::Update()
{
if (status_ != MissionStatus::RUNNING)
{
return;
}
if (!nav_->HasArrived())
{
return;
}
if (!action_executor_->IsActionDone())
{
return;
}
switch (step_)
{
case GO_TO_COLUMN:
{
auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition();
nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
}
step_ = GO_CLOSE_TO_COLUMN;
break;
case GO_CLOSE_TO_COLUMN:
action_executor_->TakePot(two_floor_);
nav_->GetPathfinding()->RemoveObstacle(column_->GetId());
step_ = TAKE_COLUMN;
break;
case TAKE_COLUMN:
{
closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId());
if (!closestDepoZone_)
{
status_ = MissionStatus::FAILED;
return;
}
closestDepoZonePoint_ = closestDepoZone_->GetNextPotPos();
auto p = closestDepoZonePoint_.GetTakeBasePosition();
auto res = nav_->CanGoTo(p, false, Pathfinding::FREE | Pathfinding::WALL);
if (res != Pathfinding::FREE)
{
auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
step_ = GO_BACK;
}
else
{
nav_->GoToRotateFirst(p, false, Pathfinding::FREE | Pathfinding::WALL);
closestDepoZone_->ValidNextPotPos();
step_ = GO_TO_PLATFORM;
}
}
break;
case GO_BACK:
{
bool canGo = false;
std::vector<int> blacklistId = {};
int timeout = 0;
while (!canGo && timeout < 3)
{
if (auto zone = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId(), blacklistId))
{
closestDepoZone_ = zone;
closestDepoZonePoint_ = closestDepoZone_->GetNextPotPos();
auto p = closestDepoZonePoint_.GetTakeBasePosition();
if (nav_->CanGoTo(p, false, Pathfinding::FREE | Pathfinding::WALL) != Pathfinding::FREE)
{
if (nav_->GoToRotateFirst(p, true, Pathfinding::FREE | Pathfinding::WALL) !=
Pathfinding::FREE)
{
blacklistId.push_back(closestDepoZone_->GetId());
}
else
{
canGo = true;
}
}
else
{
nav_->GoToRotateFirst(p, false, Pathfinding::FREE | Pathfinding::WALL);
canGo = true;
}
}
else
{
status_ = MissionStatus::FINISH_ALL;
return;
}
timeout++;
}
if (!canGo)
{
status_ = MissionStatus::FINISH_ALL;
return;
}
closestDepoZonePoint_ = closestDepoZone_->ValidNextPotPos();
}
step_ = GO_TO_PLATFORM;
break;
case GO_TO_PLATFORM:
{
auto p = closestDepoZonePoint_.GetTakeClosePosition();
nav_->GoTo(p.x, p.y, p.theta, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
}
step_ = GO_CLOSE_TO_PLATFORM;
break;
case GO_CLOSE_TO_PLATFORM:
{
action_executor_->PlacePot(two_floor_);
column_->SetX(closestDepoZonePoint_.x);
column_->SetY(closestDepoZonePoint_.y);
column_->SetTheta(closestDepoZonePoint_.theta);
column_->SetAtObjective(true);
nav_->GetPathfinding()->AddObstacle(column_);
}
step_ = PLACE_PLATFORM;
break;
case PLACE_PLATFORM:
{
auto p = closestDepoZonePoint_.GetTakeBasePosition();
if (nav_->CanGoTo(p.x, p.y, p.theta) != Pathfinding::FREE)
{
nav_->GoTo(p.x, p.y, p.theta, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
}
}
step_ = GO_BACK_2;
break;
case GO_BACK_2:
{
std_msgs::msg::Int64 msg;
msg.data = mission_score_;
score_pub_->publish(msg);
step_ = DONE;
status_ = MissionStatus::DONE;
break;
}
default:
break;
}
}
void PrepareConcertMission::Clear()
{
// TODO : if is doing construction, stop everything and release everything
}
MissionStatus PrepareConcertMission::GetStatus() const
{
return status_;
}
}

18
src/modelec_strat/src/obstacle/column.cpp → src/modelec_strat/src/obstacle/box.cpp
View File

@@ -1,13 +1,13 @@
#include <modelec_strat/obstacle/column.hpp>
#include <modelec_strat/obstacle/box.hpp>
namespace Modelec
{
ColumnObstacle::ColumnObstacle(int id, int x, int y, double theta, int w, int h, const std::string& type)
BoxObstacle::BoxObstacle(int id, int x, int y, double theta, int w, int h, const std::string& type)
: Obstacle(id, x, y, theta, w, h, type)
{
}
ColumnObstacle::ColumnObstacle(tinyxml2::XMLElement* obstacleElem)
BoxObstacle::BoxObstacle(tinyxml2::XMLElement* obstacleElem)
: Obstacle(obstacleElem)
{
possible_angles_.push_back(theta_);
@@ -19,28 +19,28 @@ namespace Modelec
}
}
ColumnObstacle::ColumnObstacle(const modelec_interfaces::msg::Obstacle& msg) :
BoxObstacle::BoxObstacle(const modelec_interfaces::msg::Obstacle& msg) :
Obstacle(msg)
{
}
bool ColumnObstacle::IsAtObjective() const
bool BoxObstacle::IsAtObjective() const
{
return isAtObjective;
}
void ColumnObstacle::SetAtObjective(bool atObjective)
void BoxObstacle::SetAtObjective(bool atObjective)
{
isAtObjective = atObjective;
}
Point ColumnObstacle::GetOptimizedGetPos(const modelec_interfaces::msg::OdometryPos::SharedPtr& msg) const
Point BoxObstacle::GetOptimizedGetPos(const modelec_interfaces::msg::OdometryPos::SharedPtr& msg) const
{
Point p = Point(msg->x, msg->y, msg->theta);
return GetOptimizedGetPos(p);
}
Point ColumnObstacle::GetOptimizedGetPos(const Point& currentPos) const
Point BoxObstacle::GetOptimizedGetPos(const Point& currentPos) const
{
auto distance = std::numeric_limits<double>::max();
double optimizedAngle = 0;
@@ -58,7 +58,7 @@ namespace Modelec
return Point(x_, y_, optimizedAngle);
}
std::vector<Point> ColumnObstacle::GetAllPossiblePositions() const
std::vector<Point> BoxObstacle::GetAllPossiblePositions() const
{
std::vector<Point> positions;
for (const auto& angle : possible_angles_)

17
src/modelec_strat/src/pathfinding.cpp
View File

@@ -1,6 +1,5 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_strat/pathfinding.hpp>
#include <modelec_strat/obstacle/column.hpp>
#include <modelec_utils/config.hpp>
namespace Modelec {
@@ -509,15 +508,15 @@ namespace Modelec {
obstacle_add_pub_->publish(msg);
}
std::shared_ptr<ColumnObstacle> Pathfinding::GetClosestColumn(const PosMsg::SharedPtr &pos,
const std::vector<int> &blacklistedId) {
std::shared_ptr<ColumnObstacle> closest_obstacle = nullptr;
std::shared_ptr<BoxObstacle> Pathfinding::GetClosestColumn(const PosMsg::SharedPtr &pos,
const std::vector<int> &blacklistedId) {
std::shared_ptr<BoxObstacle> closest_obstacle = nullptr;
auto robotPos = Point(pos->x, pos->y, pos->theta);
auto enemyPos = Point(last_enemy_pos_.x, last_enemy_pos_.y, last_enemy_pos_.theta);
float score = std::numeric_limits<float>::max();
for (const auto &[id, obstacle]: obstacle_map_) {
if (auto obs = std::dynamic_pointer_cast<ColumnObstacle>(obstacle)) {
if (auto obs = std::dynamic_pointer_cast<BoxObstacle>(obstacle)) {
if (!obs->IsAtObjective() && std::find(blacklistedId.begin(), blacklistedId.end(), obs->GetId()) ==
blacklistedId.end()) {
for (auto possiblePos: obs->GetAllPossiblePositions()) {
@@ -583,7 +582,7 @@ namespace Modelec {
void Pathfinding::OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
Point enemyPos(msg->x, msg->y, msg->theta);
for (auto &[index, obs]: obstacle_map_) {
if (auto column = std::dynamic_pointer_cast<ColumnObstacle>(obs)) {
if (auto column = std::dynamic_pointer_cast<BoxObstacle>(obs)) {
if (Point::distance(enemyPos, column->GetPosition()) < enemy_width_mm_ + (column->GetWidth() / 2) +
enemy_margin_mm_) {
RemoveObstacle(column->GetId());
@@ -622,10 +621,10 @@ namespace Modelec {
obstacle_map_[obs->GetId()] = obs;
}
for (tinyxml2::XMLElement *obstacleElem = root->FirstChildElement("Gradin");
for (tinyxml2::XMLElement *obstacleElem = root->FirstChildElement("Box");
obstacleElem != nullptr;
obstacleElem = obstacleElem->NextSiblingElement("Gradin")) {
std::shared_ptr<ColumnObstacle> obs = std::make_shared<ColumnObstacle>(obstacleElem);
obstacleElem = obstacleElem->NextSiblingElement("Box")) {
std::shared_ptr<BoxObstacle> obs = std::make_shared<BoxObstacle>(obstacleElem);
obstacle_map_[obs->GetId()] = obs;
}

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

@@ -165,10 +165,7 @@ namespace Modelec
Transition(State::DO_GO_HOME, "Go Home");
}*/
/*else if (elapsed.seconds() < 70)
{
Transition(State::DO_PREPARE_CONCERT, "Proceed to concert");
}
/*
else
{
Transition(State::DO_GO_HOME, "Cleanup and finish match");
@@ -176,31 +173,6 @@ namespace Modelec
break;
}
case State::DO_PREPARE_CONCERT:
if (!current_mission_)
{
current_mission_ = std::make_unique<PrepareConcertMission>(nav_, action_executor_, (now - match_start_time_).seconds() < 65);
current_mission_->Start(shared_from_this());
}
current_mission_->Update();
if (current_mission_->GetStatus() == MissionStatus::DONE)
{
current_mission_.reset();
Transition(State::SELECT_MISSION, "PrepareConcert finished");
}
else if (current_mission_->GetStatus() == MissionStatus::FAILED)
{
current_mission_->Clear();
current_mission_.reset();
Transition(State::SELECT_MISSION, "PrepareConcert failed");
}
else if (current_mission_->GetStatus() == MissionStatus::FINISH_ALL)
{
current_mission_.reset();
Transition(State::DO_GO_HOME, "Finish all finished");
}
break;
case State::DO_PROMOTION:
if (!current_mission_)
{