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

default mission to test

Browse Source
This commit is contained in:
acki
2025-11-19 18:26:30 +01:00
parent 19b6eb736a
commit f11360a62a
8 changed files with 201 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/modelec_core/launch/modelec.launch.py
View File

@@ -109,7 +109,7 @@ def generate_launch_description():
executable='pcb_action_interface',
name='pcb_action_interface',
parameters=[{
'serial_port': "/dev/USB_ACTION",
'serial_port': "/tmp/USB_ACTION",
'baudrate': 115200,
'name': "pcb_action",
}]

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/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/missions/take_send_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

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

@@ -14,10 +14,14 @@ namespace Modelec
enum Action
{
NONE,
TAKE,
SEND,
};
enum Step
{
TAKE_STEP,
SEND_STEP
};
ActionExecutor();
@@ -32,6 +36,10 @@ namespace Modelec
void ReInit();
void Send();
void Take();
protected:
rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_pub_;

39
src/modelec_strat/include/modelec_strat/missions/take_send_mission.hpp Normal file
View File

@@ -0,0 +1,39 @@
#pragma once
#include <modelec_strat/missions/mission_base.hpp>
#include <modelec_strat/navigation_helper.hpp>
#include <modelec_strat/action_executor.hpp>
namespace Modelec {
class TakeSendMission : public Mission {
public:
TakeSendMission(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 "GoHome"; }
private:
enum Step
{
GO_TO_TAKE,
TAKE,
WAIT_5S,
GO_TO_SEND,
SEND,
DONE,
} step_;
MissionStatus status_;
std::shared_ptr<NavigationHelper> nav_;
std::shared_ptr<ActionExecutor> action_executor_;
rclcpp::Time go_home_time_;
rclcpp::Node::SharedPtr node_;
rclcpp::Time go_timeout_;
rclcpp::Time deploy_time_;
};
}

3
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/go_home_mission.hpp"
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/int64.hpp>
@@ -26,6 +25,8 @@ namespace Modelec
WAIT_START,
SELECT_MISSION,
TAKE_SEND_MISSION,
DO_GO_HOME,
STOP
};

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

@@ -70,6 +70,26 @@ namespace Modelec
{
switch (step_.front())
{
case SEND_STEP:
{
modelec_interfaces::msg::ActionServoPos msg;
msg.id = 5;
msg.pos = 1;
servo_move_pub_->publish(msg);
step_running_ = 1;
}
break;
case TAKE_STEP:
{
modelec_interfaces::msg::ActionServoPos msg;
msg.id = 6;
msg.pos = 1;
servo_move_pub_->publish(msg);
step_running_ = 1;
}
break;
default:
return;
}
@@ -88,4 +108,30 @@ namespace Modelec
}
action_ = NONE;
}
void ActionExecutor::Send() {
if (action_done_)
{
action_ = SEND;
action_done_ = false;
step_running_ = 0;
step_.push(SEND_STEP);
Update();
}
}
void ActionExecutor::Take() {
if (action_done_)
{
action_ = TAKE;
action_done_ = false;
step_running_ = 0;
step_.push(TAKE_STEP);
Update();
}
}
}

78
src/modelec_strat/src/missions/take_send_mission.cpp Normal file
View File

@@ -0,0 +1,78 @@
#include <modelec_strat/missions/take_send_mission.hpp>
namespace Modelec {
TakeSendMission::TakeSendMission(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) {
}
void TakeSendMission::Start(rclcpp::Node::SharedPtr node)
{
node_ = node;
nav_->GoToRotateFirst(500, 1200, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
go_timeout_ = node_->now();
status_ = MissionStatus::RUNNING;
}
void TakeSendMission::Update()
{
if (!action_executor_->IsActionDone())
{
return;
}
if (!nav_->HasArrived())
{
if ((node_->now() - go_timeout_).seconds() < 4)
{
return;
}
}
switch (step_)
{
case GO_TO_TAKE:
action_executor_->Take();
step_ = TAKE;
break;
case TAKE:
if ((node_->now() - deploy_time_).seconds() >= 5)
{
step_ = WAIT_5S;
}
break;
case WAIT_5S:
nav_->GoToRotateFirst(1100, 500, 0, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
step_ = GO_TO_SEND;
break;
case GO_TO_SEND:
action_executor_->Send();
step_ = SEND;
break;
case SEND:
step_ = DONE;
status_ = MissionStatus::DONE;
break;
default:
break;
}
}
void TakeSendMission::Clear()
{
}
MissionStatus TakeSendMission::GetStatus() const
{
return status_;
}
}

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

@@ -2,6 +2,9 @@
#include <modelec_utils/config.hpp>
#include <modelec_strat/strat_fms.hpp>
#include <modelec_strat/missions/go_home_mission.hpp>
#include <modelec_strat/missions/take_send_mission.hpp>
namespace Modelec
{
@@ -156,6 +159,15 @@ namespace Modelec
Transition(State::STOP, "All missions done");
}
if (elapsed.seconds() < 70)
{
Transition(State::TAKE_SEND_MISSION, "Proceed to concert");
}
else
{
Transition(State::DO_GO_HOME, "Cleanup and finish match");
}
/*if (elapsed.seconds() >= 2)
{
Transition(State::DO_GO_HOME, "Go Home");
@@ -169,6 +181,20 @@ namespace Modelec
break;
}
case State::TAKE_SEND_MISSION:
if (!current_mission_)
{
current_mission_ = std::make_unique<TakeSendMission>(nav_, action_executor_);
current_mission_->Start(shared_from_this());
}
current_mission_->Update();
if (current_mission_->GetStatus() == MissionStatus::DONE)
{
current_mission_.reset();
Transition(State::SELECT_MISSION, "Take and Send done");
}
break;
case State::DO_GO_HOME:
if (!current_mission_)
{