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

adjust new obstacle

Browse Source
This commit is contained in:
acki
2025-10-07 13:41:49 +02:00
parent a42ea84833
commit e88e5d8d8e
15 changed files with 18 additions and 633 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -86,7 +86,7 @@ namespace ModelecGUI {
});
connect(map_action_, &QAction::triggered, this, [this]() {
stackedWidget->setCurrentWidget(map_page_);
stackedWidget->setCurrentWidget(test_map_page_);
});
connect(playmat_map_, &QAction::triggered, this, [this]() {

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

@@ -44,7 +44,7 @@ namespace ModelecGUI
{
modelec_interfaces::msg::Spawn team_msg;
team_msg.team_id = msg->team_id;
team_msg.name = modelec_interfaces::msg::Spawn::BOTTOM;
team_msg.name = modelec_interfaces::msg::Spawn::TOP;
spawn_pub_->publish(team_msg);
emit TeamChoose();

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/2026_BETA.svg"),
QString(":/img/playmat/2026_FINAL.svg"),
this)), node_(node)
{
ratioBetweenMapAndWidgetX_ = width() / 3000.0f;

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

@@ -248,8 +248,7 @@ namespace ModelecGUI
msg.close = true;
if (show_obstacle_)
{
msg.mask = modelec_interfaces::msg::OdometryGoTo::FREE | modelec_interfaces::msg::OdometryGoTo::WALL |
modelec_interfaces::msg::OdometryGoTo::OBSTACLE | modelec_interfaces::msg::OdometryGoTo::ENEMY;
msg.mask = modelec_interfaces::msg::OdometryGoTo::FREE | modelec_interfaces::msg::OdometryGoTo::WALL;
}
else
{

2
src/modelec_interfaces/msg/Map/Obstacle.msg
View File

@@ -1,5 +1,5 @@
string ESTRADE="estrade"
string GRADIN="gradin"
string GRADIN="box"
string PAMI_START="pami-start"
int32 id

2
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/banner_mission.cpp src/missions/go_home_mission.cpp src/obstacle/obstacle.cpp src/obstacle/box.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/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

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

@@ -1,26 +1,24 @@
<Obstacles>
<!-- TOP -->
<Obstacle id="1" x="850" y="1900" theta="1.5708" width="400" height="200" type="estrade"/>
<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"/>
<Obstacle id="1" x="1500" y="1800" theta="1.5708" width="1800" height="400" type="estrade"/>
<!--<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"/>-->
<!-- 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" >
<Box id="10" x="175" y="1200" theta="0" width="200" height="150" type="box" />
<Box id="11" x="1150" y="800" theta="1.5708" width="200" height="150" type="box" >
<possible-angle theta="-1.5708" />
</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"/>
<Box id="12" x="175" y="400" theta="0" width="200" height="150" type="box" />
<Box id="13" x="1100" y="175" theta="1.5708" width="200" height="150" type="box" />
<!-- 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">
<Box id="20" x="2825" y="1200" theta="3.1415" width="200" height="150" type="box" />
<Box id="21" x="1850" y="800" theta="1.5708" width="200" height="150" type="box">
<possible-angle theta="-1.5708" />
</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"/>
<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"/>
<!-- PAMI Start -->
<Obstacle id="30" x="75" y="1750" theta="1.5708" width="150" height="450" type="pami-start"/>

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

@@ -14,37 +14,10 @@ namespace Modelec
enum Action
{
NONE,
DEPLOY_BANNER,
UNDEPLOY_BANNER,
TAKE_POT,
PLACE_POT,
DEPLOY_MAX_SIZE,
};
enum Step
{
// Banner
DEPLOY_BANNER_STEP,
UNDEPLOY_BANNER_STEP,
// Take Pot
ASC_GO_DOWN,
STICK_TO_STRUCT,
ASC_GO_UP,
RETRACT_BOTTOM_PLATE,
ASC_GO_DOWN_TO_TAKE_POT,
STICK_POT,
ASC_GO_UP_TO_TAKE_POT,
PLACE_FIRST_PLATE,
// Take One floor
STICK_ALL,
// Place Pot
ASC_FINAL,
FREE_ALL,
REMOVE_ACTION_STEP,
};
ActionExecutor();
@@ -57,32 +30,9 @@ namespace Modelec
void Update();
void DeployBanner();
void UndeployBanner();
void TakePot(bool two_floor = true);
void PlacePot(bool two_floor = true);
void ReInit();
protected:
/*
rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_get_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_get_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_get_pub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_get_res_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_get_res_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_get_res_sub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_set_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_set_pub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_set_res_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_set_res_sub_;
*/
rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_move_pub_;

47
src/modelec_strat/include/modelec_strat/missions/banner_mission.hpp
View File

@@ -1,47 +0,0 @@
#pragma once
#include <modelec_strat/action_executor.hpp>
#include <modelec_strat/missions/mission_base.hpp>
#include <modelec_strat/navigation_helper.hpp>
#include <std_msgs/msg/int64.hpp>
namespace Modelec
{
/*
* Banderole mission
*
*/
class BannerMission : public Mission
{
public:
BannerMission(const std::shared_ptr<NavigationHelper>& nav,
const std::shared_ptr<ActionExecutor>& action_executor);
void Start(rclcpp::Node::SharedPtr node) override;
void Update() override;
void Clear() override;
MissionStatus GetStatus() const override;
std::string GetName() const override { return "Promotion"; }
enum Step
{
GO_TO_FRONT,
DEPLOY_BANNER,
WAIT_2_SECONDS,
UNDEPLOY_BANNER,
GO_FORWARD,
DONE
} step_;
MissionStatus status_;
std::shared_ptr<NavigationHelper> nav_;
std::shared_ptr<ActionExecutor> action_executor_;
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr score_pub_;
int mission_score_ = 0;
Point spawn_;
rclcpp::Time deploy_time_;
rclcpp::Time go_timeout_;
};
}

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

@@ -83,13 +83,6 @@ namespace Modelec
void AddObstacle(const std::shared_ptr<Obstacle>& obstacle);
template <typename T,
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<BoxObstacle> GetClosestColumn(const PosMsg::SharedPtr &pos,
const std::vector<int> &blacklistedId = {});
void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
void OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
@@ -139,27 +132,4 @@ namespace Modelec
rclcpp::Service<modelec_interfaces::srv::MapSize>::SharedPtr map_size_srv_;
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr ask_obstacle_srv_;
};
template <typename T, typename>
std::shared_ptr<T> Pathfinding::GetClosestObstacle(const PosMsg::SharedPtr& pos) const
{
std::shared_ptr<T> closest_obstacle = nullptr;
auto robotPos = Point(pos->x, pos->y, pos->theta);
float distance = std::numeric_limits<float>::max();
for (const auto& obstacle : obstacle_map_)
{
if (auto obs = std::dynamic_pointer_cast<T>(obstacle.second))
{
auto dist = Point::distance(robotPos, obs->position());
if (dist < distance)
{
distance = dist;
closest_obstacle = obs;
}
}
}
return closest_obstacle;
}
}

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

@@ -5,7 +5,6 @@
#include "action_executor.hpp"
#include "navigation_helper.hpp"
#include "missions/mission_base.hpp"
#include "missions/banner_mission.hpp"
#include "missions/go_home_mission.hpp"
#include <std_msgs/msg/bool.hpp>
@@ -27,8 +26,6 @@ namespace Modelec
WAIT_START,
SELECT_MISSION,
DO_PROMOTION,
DO_GO_HOME,
STOP
};
@@ -60,7 +57,6 @@ namespace Modelec
bool team_selected_ = false;
std::unique_ptr<Mission> current_mission_;
int team_id_ = 0;
bool is_banner_done_ = false;
std::shared_ptr<NavigationHelper> nav_;
std::shared_ptr<ActionExecutor> action_executor_;

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

@@ -70,226 +70,6 @@ namespace Modelec
{
switch (step_.front())
{
case DEPLOY_BANNER_STEP:
{
modelec_interfaces::msg::ActionServoPos msg;
msg.id = 5;
msg.pos = 1;
servo_move_pub_->publish(msg);
step_running_ = 1;
}
break;
case UNDEPLOY_BANNER_STEP:
{
modelec_interfaces::msg::ActionServoPos msg;
msg.id = 5;
msg.pos = 0;
servo_move_pub_->publish(msg);
step_running_ = 1;
}
break;
case ASC_GO_DOWN:
{
modelec_interfaces::msg::ActionAscPos asc_msg;
asc_msg.pos = 1;
asc_move_pub_->publish(asc_msg);
modelec_interfaces::msg::ActionServoPos servo_action_bottom_msg;
servo_action_bottom_msg.id = 4;
servo_action_bottom_msg.pos = 1;
servo_move_pub_->publish(servo_action_bottom_msg);
step_running_ = 2;
}
break;
case STICK_TO_STRUCT:
{
modelec_interfaces::msg::ActionRelayState relay_top_msg;
relay_top_msg.state = true;
relay_top_msg.id = 2;
relay_move_pub_->publish(relay_top_msg);
modelec_interfaces::msg::ActionRelayState relay_bottom_msg;
relay_bottom_msg.state = true;
relay_bottom_msg.id = 1;
relay_move_pub_->publish(relay_bottom_msg);
modelec_interfaces::msg::ActionServoPos first_pot_msg;
first_pot_msg.id = 0;
first_pot_msg.pos = 0;
servo_move_pub_->publish(first_pot_msg);
modelec_interfaces::msg::ActionServoPos fourth_pot_msg;
fourth_pot_msg.id = 1;
fourth_pot_msg.pos = 0;
servo_move_pub_->publish(fourth_pot_msg);
step_running_ = 4;
}
break;
case ASC_GO_UP:
{
modelec_interfaces::msg::ActionAscPos asc_msg;
asc_msg.pos = 3;
asc_move_pub_->publish(asc_msg);
step_running_ = 1;
}
break;
case RETRACT_BOTTOM_PLATE:
{
modelec_interfaces::msg::ActionServoPos servo_action_bottom_msg;
servo_action_bottom_msg.id = 2;
servo_action_bottom_msg.pos = 2;
servo_move_pub_->publish(servo_action_bottom_msg);
step_running_ = 1;
}
break;
case ASC_GO_DOWN_TO_TAKE_POT:
{
modelec_interfaces::msg::ActionAscPos asc_msg;
asc_msg.pos = 0;
asc_move_pub_->publish(asc_msg);
step_running_ = 1;
}
break;
case STICK_POT:
{
modelec_interfaces::msg::ActionServoPos top_pot_msg;
top_pot_msg.id = 4;
top_pot_msg.pos = 0;
servo_move_pub_->publish(top_pot_msg);
step_running_ = 1;
}
break;
case ASC_GO_UP_TO_TAKE_POT:
{
modelec_interfaces::msg::ActionAscPos asc_msg;
asc_msg.pos = 3;
asc_move_pub_->publish(asc_msg);
modelec_interfaces::msg::ActionServoPos top_pot_msg;
top_pot_msg.id = 4;
top_pot_msg.pos = 1;
servo_move_pub_->publish(top_pot_msg);
step_running_ = 2;
}
break;
case PLACE_FIRST_PLATE:
{
modelec_interfaces::msg::ActionServoPos action_bottom_msg;
action_bottom_msg.id = 2;
action_bottom_msg.pos = 0;
servo_move_pub_->publish(action_bottom_msg);
step_running_ = 1;
}
break;
case STICK_ALL:
{
modelec_interfaces::msg::ActionRelayState relay_top_msg;
relay_top_msg.state = true;
relay_top_msg.id = 2;
relay_move_pub_->publish(relay_top_msg);
modelec_interfaces::msg::ActionRelayState relay_bottom_msg;
relay_bottom_msg.state = true;
relay_bottom_msg.id = 1;
relay_move_pub_->publish(relay_bottom_msg);
modelec_interfaces::msg::ActionServoPos first_pot_msg;
first_pot_msg.id = 0;
first_pot_msg.pos = 0;
servo_move_pub_->publish(first_pot_msg);
modelec_interfaces::msg::ActionServoPos fourth_pot_msg;
fourth_pot_msg.id = 1;
fourth_pot_msg.pos = 0;
servo_move_pub_->publish(fourth_pot_msg);
modelec_interfaces::msg::ActionServoPos top_pot_msg;
top_pot_msg.id = 4;
top_pot_msg.pos = 0;
servo_move_pub_->publish(top_pot_msg);
step_running_ = 5;
}
break;
case ASC_FINAL:
{
modelec_interfaces::msg::ActionAscPos asc_msg;
asc_msg.pos = 2;
asc_move_pub_->publish(asc_msg);
step_running_ = 1;
}
break;
case FREE_ALL:
{
modelec_interfaces::msg::ActionRelayState relay_top_msg;
relay_top_msg.state = false;
relay_top_msg.id = 2;
relay_move_pub_->publish(relay_top_msg);
modelec_interfaces::msg::ActionRelayState relay_bottom_msg;
relay_bottom_msg.state = false;
relay_bottom_msg.id = 1;
relay_move_pub_->publish(relay_bottom_msg);
modelec_interfaces::msg::ActionServoPos first_pot_msg;
first_pot_msg.id = 0;
first_pot_msg.pos = 1;
servo_move_pub_->publish(first_pot_msg);
modelec_interfaces::msg::ActionServoPos second_pot_msg;
second_pot_msg.id = 1;
second_pot_msg.pos = 1;
servo_move_pub_->publish(second_pot_msg);
modelec_interfaces::msg::ActionServoPos top_pot_msg;
top_pot_msg.id = 4;
top_pot_msg.pos = 0;
servo_move_pub_->publish(top_pot_msg);
step_running_ = 5;
}
break;
case REMOVE_ACTION_STEP:
{
modelec_interfaces::msg::ActionAscPos asc_msg;
asc_msg.pos = 3;
asc_move_pub_->publish(asc_msg);
modelec_interfaces::msg::ActionServoPos servo_action_bottom_msg;
servo_action_bottom_msg.id = 3;
servo_action_bottom_msg.pos = 0;
servo_move_pub_->publish(servo_action_bottom_msg);
step_running_ = 2;
}
break;
default:
return;
}
@@ -298,87 +78,6 @@ namespace Modelec
}
}
void ActionExecutor::DeployBanner()
{
if (action_done_)
{
action_ = DEPLOY_BANNER;
action_done_ = false;
step_running_ = 0;
step_.push(DEPLOY_BANNER_STEP);
Update();
}
}
void ActionExecutor::UndeployBanner()
{
if (action_done_)
{
action_ = UNDEPLOY_BANNER;
action_done_ = false;
step_running_ = 0;
step_.push(UNDEPLOY_BANNER_STEP);
Update();
}
}
void ActionExecutor::TakePot(bool two_floor)
{
if (action_done_)
{
action_ = TAKE_POT;
action_done_ = false;
step_running_ = 0;
if (two_floor)
{
step_.push(ASC_GO_DOWN);
step_.push(STICK_TO_STRUCT);
step_.push(ASC_GO_UP);
step_.push(RETRACT_BOTTOM_PLATE);
step_.push(ASC_GO_DOWN_TO_TAKE_POT);
step_.push(STICK_POT);
step_.push(ASC_GO_UP_TO_TAKE_POT);
step_.push(PLACE_FIRST_PLATE);
}
else
{
step_.push(ASC_GO_DOWN);
step_.push(STICK_ALL);
}
Update();
}
}
void ActionExecutor::PlacePot(bool two_floor)
{
if (action_done_)
{
action_ = PLACE_POT;
action_done_ = false;
step_running_ = 0;
if (two_floor)
{
step_.push(ASC_FINAL);
step_.push(FREE_ALL);
step_.push(REMOVE_ACTION_STEP);
}
else
{
step_.push(FREE_ALL);
step_.push(REMOVE_ACTION_STEP);
}
Update();
}
}
void ActionExecutor::ReInit()
{
action_done_ = true;

114
src/modelec_strat/src/missions/banner_mission.cpp
View File

@@ -1,114 +0,0 @@
#include <modelec_strat/missions/banner_mission.hpp>
#include <modelec_utils/config.hpp>
namespace Modelec
{
BannerMission::BannerMission(const std::shared_ptr<NavigationHelper>& nav,
const std::shared_ptr<ActionExecutor>& action_executor) : step_(GO_TO_FRONT),
status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor)
{
}
void BannerMission::Start(rclcpp::Node::SharedPtr node)
{
node_ = node;
mission_score_ = Config::get<int>("config.mission_score.banner", 0);
score_pub_ = node_->create_publisher<std_msgs::msg::Int64>("/strat/score", 10);
spawn_ = nav_->GetSpawn();
nav_->GoTo(spawn_.x, 100, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
go_timeout_ = node_->now();
status_ = MissionStatus::RUNNING;
}
void BannerMission::Update()
{
if (status_ != MissionStatus::RUNNING)
{
return;
}
if (!action_executor_->IsActionDone())
{
return;
}
if (!nav_->HasArrived())
{
if ((node_->now() - go_timeout_).seconds() < 4)
{
return;
}
}
switch (step_)
{
case GO_TO_FRONT:
{
action_executor_->DeployBanner();
deploy_time_ = node_->now();
step_ = DEPLOY_BANNER;
}
break;
case DEPLOY_BANNER:
{
if ((node_->now() - deploy_time_).seconds() >= 2)
{
step_ = WAIT_2_SECONDS;
}
}
break;
case WAIT_2_SECONDS:
{
action_executor_->UndeployBanner();
step_ = UNDEPLOY_BANNER;
}
break;
case UNDEPLOY_BANNER:
{
go_timeout_ = node_->now();
nav_->GoTo(spawn_.x, 300, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
step_ = GO_FORWARD;
}
break;
case GO_FORWARD:
{
std_msgs::msg::Int64 msg;
msg.data = mission_score_;
score_pub_->publish(msg);
step_ = DONE;
status_ = MissionStatus::DONE;
break;
}
default:
break;
}
}
void BannerMission::Clear()
{
}
MissionStatus BannerMission::GetStatus() const
{
return status_;
}
}

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

@@ -508,47 +508,6 @@ namespace Modelec {
obstacle_add_pub_->publish(msg);
}
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<BoxObstacle>(obstacle)) {
if (!obs->IsAtObjective() && std::find(blacklistedId.begin(), blacklistedId.end(), obs->GetId()) ==
blacklistedId.end()) {
for (auto possiblePos: obs->GetAllPossiblePositions()) {
auto dist = Point::distance(robotPos, possiblePos);
auto distEnemy = Point::distance(enemyPos, possiblePos);
auto takePoint = possiblePos;
takePoint.theta += M_PI;
double theta = std::abs(Point::angleDiff(robotPos, takePoint));
auto dist_score = dist;
auto distEnemy_score = distEnemy * factor_close_enemy_ * has_enemy_pos_;
auto theta_score = (theta / M_PI * 250);
auto s = dist_score + distEnemy_score + theta_score;
if (s < score) {
/*RCLCPP_INFO(node_->get_logger(),
"Column %d at (%d, %d) with dist_s=%f, distEnemy_s=%f, theta=%f, theta_s=%f, score=%f",
obs->GetId(), possiblePos.x, possiblePos.y, dist_score, distEnemy_score, theta,
theta_score, s);*/
score = s;
closest_obstacle = obs;
}
}
}
}
}
return closest_obstacle;
}
void Pathfinding::HandleMapRequest(const std::shared_ptr<modelec_interfaces::srv::Map::Request>,
const std::shared_ptr<modelec_interfaces::srv::Map::Response> response) {
response->width = grid_[0].size();

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

@@ -151,11 +151,7 @@ namespace Modelec
{
auto elapsed = now - match_start_time_;
if (!is_banner_done_)
{
Transition(State::DO_PROMOTION, "Start promotion");
}
else if (elapsed.seconds() >= 100)
if (elapsed.seconds() >= 100)
{
Transition(State::STOP, "All missions done");
}
@@ -173,27 +169,6 @@ namespace Modelec
break;
}
case State::DO_PROMOTION:
if (!current_mission_)
{
current_mission_ = std::make_unique<BannerMission>(nav_, action_executor_);
current_mission_->Start(shared_from_this());
}
current_mission_->Update();
if (current_mission_->GetStatus() == MissionStatus::DONE)
{
current_mission_.reset();
is_banner_done_ = true;
Transition(State::SELECT_MISSION, "Promotion finished");
}
else if (current_mission_->GetStatus() == MissionStatus::FAILED)
{
current_mission_.reset();
is_banner_done_ = true;
Transition(State::SELECT_MISSION, "Promotion failed");
}
break;
case State::DO_GO_HOME:
if (!current_mission_)
{