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

start to be very nice :)

Browse Source
This commit is contained in:
acki
2025-05-04 01:47:49 -04:00
parent e22b745b02
commit 5526e1694c
16 changed files with 179 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
src/modelec_core/launch/test.no_strat.modelec.launch.yml Normal file
View File

@@ -0,0 +1,26 @@
launch:
- node:
pkg: 'modelec_com'
exec: "serial_listener"
name: "serial_listener"
- node:
pkg: 'modelec_com'
exec: 'pcb_odo_interface'
name: 'pcb_odo_interface'
- node:
pkg: 'modelec_com'
exec: 'pcb_alim_interface'
name: 'pcb_alim_interface'
- node:
pkg: 'modelec_gui'
exec: "modelec_gui"
name: "modelec_gui"
- node:
pkg: 'modelec_core'
exec: 'speed_result'
name: 'speed_result'

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

@@ -28,4 +28,9 @@
<map_height_mm>2000</map_height_mm>
</size>
</map>
<spawn>
<blue x="2650" y="1600" theta="-1.5708" />
<yellow x="350" y="1600" theta="-1.5708" />
</spawn>
</config>

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

@@ -1,24 +1,33 @@
#pragma once
#include <modelec_strat/missions/mission_base.hpp>
#include <modelec_strat/navigation_helper.hpp>
#include "mission_base.hpp"
namespace Modelec
{
class GoHomeMission : public Mission
{
public:
GoHomeMission(const std::shared_ptr<NavigationHelper>& nav);
GoHomeMission(const std::shared_ptr<NavigationHelper>& nav, const rclcpp::Time& start_time);
void start(rclcpp::Node::SharedPtr node) override;
void update() override;
void clear() override;
MissionStatus getStatus() const override;
std::string name() const override { return "GoHome"; }
private:
enum Step {
GO_HOME,
GO_CLOSE,
DONE
} step_;
MissionStatus status_;
std::shared_ptr<NavigationHelper> nav_;
rclcpp::Node::SharedPtr node_;
rclcpp::Time start_time_;
Point home_point_;
};
}

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

@@ -3,6 +3,7 @@
#include <rclcpp/rclcpp.hpp>
#include <string>
namespace Modelec
{
enum class MissionStatus
@@ -10,6 +11,7 @@ namespace Modelec
READY,
RUNNING,
DONE,
FINISH_ALL,
FAILED
};
@@ -19,6 +21,7 @@ namespace Modelec
virtual ~Mission() = default;
virtual void start(rclcpp::Node::SharedPtr node) = 0;
virtual void update() = 0;
virtual void clear() = 0;
virtual MissionStatus getStatus() const = 0;
virtual std::string name() const = 0;
};

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

@@ -12,6 +12,7 @@ namespace Modelec {
void start(rclcpp::Node::SharedPtr node) override;
void update() override;
void clear() override;
MissionStatus getStatus() const override;
std::string name() const override { return "PrepareConcert"; }

4
src/modelec_strat/include/modelec_strat/missions/promotion_mission.hpp
View File

@@ -1,9 +1,8 @@
#pragma once
#include <modelec_strat/missions/mission_base.hpp>
#include <modelec_strat/navigation_helper.hpp>
#include "mission_base.hpp"
namespace Modelec
{
/*
@@ -17,6 +16,7 @@ namespace Modelec
void start(rclcpp::Node::SharedPtr node) override;
void update() override;
void clear() override;
MissionStatus getStatus() const override;
std::string name() const override { return "Promotion"; }

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

@@ -12,6 +12,12 @@ namespace Modelec {
class NavigationHelper {
public:
enum
{
YELLOW = 0,
BLUE = 1,
};
NavigationHelper();
NavigationHelper(const rclcpp::Node::SharedPtr& node);
@@ -20,6 +26,8 @@ namespace Modelec {
std::shared_ptr<Pathfinding> GetPathfinding() const;
int GetTeamId() const;
void SendWaypoint() const;
void SendWaypoint(const std::vector<WaypointMsg> &waypoints) const;
@@ -35,9 +43,11 @@ namespace Modelec {
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 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);
int CanGoTo(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE);
std::pair<int, WaypointListMsg> FindPath(const PosMsg::SharedPtr& goal, bool isClose = false,
int collisionMask = Pathfinding::FREE);
@@ -48,6 +58,8 @@ namespace Modelec {
std::shared_ptr<DepositeZone> GetClosestDepositeZone(const PosMsg::SharedPtr &pos, int teamId, const std::vector<int>& blacklistedId = {});
PosMsg::SharedPtr GetHomePosition();
protected:
void OnWaypointReach(const WaypointReachMsg::SharedPtr msg);
@@ -58,6 +70,8 @@ namespace Modelec {
std::shared_ptr<Pathfinding> pathfinding_;
int team_id_ = YELLOW;
std::list<Waypoint> waypoints_;
PosMsg::SharedPtr current_pos_;

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

@@ -56,6 +56,7 @@ namespace Modelec
rclcpp::Time match_start_time_;
bool started_ = false;
std::unique_ptr<Mission> current_mission_;
int team_id_ = 0;
std::shared_ptr<NavigationHelper> nav_;
std::unique_ptr<MissionManager> mission_manager_;

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

@@ -2,7 +2,7 @@
namespace Modelec
{
GoHomeMission::GoHomeMission(const std::shared_ptr<NavigationHelper>& nav) : status_(MissionStatus::READY), nav_(nav)
GoHomeMission::GoHomeMission(const std::shared_ptr<NavigationHelper>& nav, const rclcpp::Time& start_time) : step_(GO_HOME), status_(MissionStatus::READY), nav_(nav), start_time_(start_time)
{
}
@@ -10,19 +10,44 @@ namespace Modelec
{
node_ = node;
nav_->GoTo(400, 1600.0, -M_PI_2);
auto pos = nav_->GetHomePosition();
home_point_ = Point(pos->x, pos->y, pos->theta);
nav_->GoTo(home_point_.GetTakeBasePosition());
status_ = MissionStatus::RUNNING;
}
void GoHomeMission::update()
{
if (nav_->HasArrived())
if (!nav_->HasArrived()) return;
switch (step_)
{
status_ = MissionStatus::DONE;
case GO_HOME:
if ((node_->now() - start_time_).seconds() < 94)
{
break;
}
nav_->GoTo(home_point_.GetTakePosition(0), true);
step_ = GO_CLOSE;
break;
case GO_CLOSE:
step_ = DONE;
status_ = MissionStatus::DONE;
break;
default:
break;
}
}
void GoHomeMission::clear()
{
}
MissionStatus GoHomeMission::getStatus() const
{
return status_;

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

@@ -10,12 +10,19 @@ namespace Modelec
void PrepareConcertMission::start(rclcpp::Node::SharedPtr node)
{
if (!nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId()))
{
status_ = MissionStatus::FINISH_ALL;
return;
}
if (auto col = nav_->GetPathfinding()->GetClosestColumn(nav_->GetCurrentPos()))
{
column_ = col;
} else
{
status_ = MissionStatus::FAILED;
status_ = MissionStatus::FINISH_ALL;
return;
}
@@ -24,6 +31,12 @@ namespace Modelec
auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
auto res = nav_->GoTo(pos.x, pos.y, pos.theta, false, Pathfinding::FREE | Pathfinding::WALL);
if (res != Pathfinding::FREE)
{
RCLCPP_WARN(node_->get_logger(), "Can't go to column %d", column_->id());
status_ = MissionStatus::FAILED;
return;
}
status_ = MissionStatus::RUNNING;
}
@@ -52,7 +65,7 @@ namespace Modelec
case TAKE_COLUMN:
{
// TODO
closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), 0);
closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId());
if (!closestDepoZone_)
{
status_ = MissionStatus::FAILED;
@@ -79,7 +92,7 @@ namespace Modelec
break;
case GO_BACK:
{
closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), 0);
closestDepoZone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId());
if (!closestDepoZone_)
{
status_ = MissionStatus::FAILED;
@@ -143,6 +156,11 @@ namespace Modelec
}
}
void PrepareConcertMission::clear()
{
// TODO : if is doing construction, stop everything and release everything
}
MissionStatus PrepareConcertMission::getStatus() const
{
return status_;

4
src/modelec_strat/src/missions/promotion_mission.cpp
View File

@@ -45,6 +45,10 @@ namespace Modelec
}
}
void PromotionMission::clear()
{
}
MissionStatus PromotionMission::getStatus() const
{
return status_;

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

@@ -2,6 +2,8 @@
#include <utility>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include "../../modelec_utils/include/modelec_utils/config.hpp"
namespace Modelec {
NavigationHelper::NavigationHelper()
{
@@ -47,6 +49,11 @@ namespace Modelec {
return pathfinding_;
}
int NavigationHelper::GetTeamId() const
{
return team_id_;
}
void NavigationHelper::SendWaypoint() const
{
for (auto & w : waypoints_)
@@ -196,6 +203,11 @@ namespace Modelec {
return GoTo(goal, isClose, collisionMask);
}
int NavigationHelper::GoTo(const Point& goal, bool isClose, int collisionMask)
{
return GoTo(goal.x, goal.y, goal.theta, isClose, collisionMask);
}
int NavigationHelper::CanGoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask)
{
return FindPath(goal, isClose, collisionMask).first;
@@ -210,6 +222,11 @@ namespace Modelec {
return CanGoTo(goal, isClose, collisionMask);
}
int NavigationHelper::CanGoTo(const Point& goal, bool isClose, int collisionMask)
{
return CanGoTo(goal.x, goal.y, goal.theta, isClose, collisionMask);
}
std::pair<int, WaypointListMsg> NavigationHelper::FindPath(const PosMsg::SharedPtr& goal, bool isClose,
int collisionMask)
{
@@ -270,6 +287,25 @@ namespace Modelec {
return closest_zone;
}
PosMsg::SharedPtr NavigationHelper::GetHomePosition()
{
PosMsg::SharedPtr home = std::make_shared<PosMsg>();
// TODO : handle Team Id
if (team_id_ == YELLOW)
{
home->x = Config::get<int>("config.spawn.yellow@x", 0);
home->y = Config::get<int>("config.spawn.yellow@y", 0);
home->theta = Config::get<double>("config.spawn.yellow@theta", 0);
}
else
{
home->x = Config::get<int>("config.spawn.blue@x", 0);
home->y = Config::get<int>("config.spawn.blue@y", 0);
home->theta = Config::get<double>("config.spawn.blue@theta", 0);
}
return home;
}
void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr msg)
{
for (auto & waypoint : waypoints_)

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

@@ -161,8 +161,19 @@ namespace Modelec
const float cell_size_mm_y = map_height_mm_ / grid_height_;
// Robot dimensions
const int inflate_x = isClose ? 0 : std::ceil(((robot_width_mm_ + margin_mm_) / 2.0f) / cell_size_mm_x);
const int inflate_y = isClose ? 0 : std::ceil(((robot_length_mm_ + margin_mm_) / 2.0f) / cell_size_mm_y);
int inflate_x;
int inflate_y;
if (isClose)
{
inflate_x = std::ceil(((robot_width_mm_) / 2.0f) / cell_size_mm_x);
inflate_y = std::ceil(((robot_length_mm_) / 2.0f) / cell_size_mm_y);
}
else
{
inflate_x = std::ceil(((robot_width_mm_ + margin_mm_) / 2.0f) / cell_size_mm_x);
inflate_y = std::ceil(((robot_length_mm_ + margin_mm_) / 2.0f) / cell_size_mm_y);
}
// 1. Build fresh empty grid
grid_.clear();

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

@@ -111,9 +111,15 @@ namespace Modelec
}
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:
@@ -138,7 +144,7 @@ namespace Modelec
case State::DO_GO_HOME:
if (!current_mission_)
{
current_mission_ = std::make_unique<GoHomeMission>(nav_);
current_mission_ = std::make_unique<GoHomeMission>(nav_, match_start_time_);
current_mission_->start(shared_from_this());
}
current_mission_->update();

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

@@ -13,6 +13,7 @@ namespace Modelec
static double distance(const Point& p1, const Point& p2);
Point GetTakePosition(int distance, double angle) const;
Point GetTakePosition(int distance) const;
Point GetTakeBasePosition() const;
Point GetTakeClosePosition() const;

5
src/modelec_utils/src/point.cpp
View File

@@ -17,6 +17,11 @@ namespace Modelec
return pos;
}
Point Point::GetTakePosition(int distance) const
{
return GetTakePosition(distance, theta);
}
Point Point::GetTakeBasePosition() const
{
return GetTakePosition(300, theta);