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

BackFront + Joy + change IHM + some change + some fix (#26)

Browse Source 
Co-authored-by: Modelec <modelec.isen@gmail.com>
This commit is contained in:
Ackimixs
2026-01-22 18:31:39 +01:00
committed by GitHub
parent a4813243a4
commit 083a3032f4
43 changed files with 794 additions and 405 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
View File

@@ -56,6 +56,7 @@ namespace Modelec
rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoints>::SharedPtr odo_add_waypoints_subscriber_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_set_pos_subscriber_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPid>::SharedPtr odo_set_pid_subscriber_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr odo_ask_active_waypoint_subscriber_;
rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_subscriber_;
void AddWaypointCallback(const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg);

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

@@ -263,11 +263,14 @@ namespace Modelec
};*/
servo_value_ = {
{0, 0},
{1, 3},
{2, 1.45},
{3, 1.6},
{4, 0},
{0, 2.95},
{1, 0.93},
{2, 0},
{3, 3},
{4, 0.8},
{5, 0.8},
{6, 0.8},
{7, 0.8},
};
std::string data = "MOV;SERVO;" + std::to_string(servo_value_.size()) + ";";

42
src/modelec_com/src/pcb_odo_interface.cpp
View File

@@ -69,6 +69,13 @@ namespace Modelec
SetPIDCallback(msg);
});
odo_ask_active_waypoint_subscriber_ = this->create_subscription<std_msgs::msg::Empty>(
"odometry/ask_active_waypoint", 10,
[this](const std_msgs::msg::Empty::SharedPtr)
{
GetData("WAYPOINT", {"ACTIVE"});
});
joy_subscriber_ = this->create_subscription<sensor_msgs::msg::Joy>(
"joy", 30,
[this](const sensor_msgs::msg::Joy::SharedPtr msg)
@@ -170,12 +177,38 @@ namespace Modelec
}
else if (tokens[1] == "WAYPOINT")
{
int id = std::stoi(tokens[2]);
if (tokens[2] == "REACH")
{
// RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[3].c_str());
auto message = modelec_interfaces::msg::OdometryWaypoint();
message.id = id;
int id = std::stoi(tokens[3]);
odo_waypoint_reach_publisher_->publish(message);
auto message = modelec_interfaces::msg::OdometryWaypoint();
message.id = id;
odo_waypoint_reach_publisher_->publish(message);
}
else if (tokens.size() >= 7)
{
int id = std::stoi(tokens[3]);
bool is_end = tokens[4] == "1";
long x = std::stol(tokens[5]);
long y = std::stol(tokens[6]);
double theta = std::stod(tokens[7]);
bool reach = tokens.size() > 7 ? tokens[8] == "1" : false;
if (reach)
{
auto message = modelec_interfaces::msg::OdometryWaypoint();
message.id = id;
message.is_end = is_end;
message.x = x;
message.y = y;
message.theta = theta;
odo_waypoint_reach_publisher_->publish(message);
}
}
}
else if (tokens[1] == "PID")
{
@@ -201,6 +234,7 @@ namespace Modelec
}
else if (tokens[1] == "WAYPOINT")
{
// RCLCPP_INFO(this->get_logger(), "Waypoint added successfully.");
}
else if (tokens[1] == "PID")
{

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

@@ -99,7 +99,7 @@ def generate_launch_description():
executable='pcb_odo_interface',
name='pcb_odo_interface',
parameters=[{
'serial_port': "/tmp/USB_ODO",
'serial_port': "/dev/USB_ODO",
'baudrate': 115200,
'name': "pcb_odo",
}]
@@ -109,7 +109,7 @@ def generate_launch_description():
executable='pcb_action_interface',
name='pcb_action_interface',
parameters=[{
'serial_port': "/tmp/USB_ACTION",
'serial_port': "/dev/USB_ACTION",
'baudrate': 115200,
'name': "pcb_action",
}]
@@ -138,11 +138,7 @@ def generate_launch_description():
package='joy',
executable='joy_node',
name='joy_node',
output='screen',
parameters=[{
'dev': '/dev/input/js0', # optional: specify your joystick device
'deadzone': 0.05
}]
output='screen'
)
return [joy]
return []
@@ -172,4 +168,4 @@ def generate_launch_description():
OpaqueFunction(function=launch_joy),
])
# to run in debug : , prefix=['xterm -e gdb -ex run --args']
# to run in debug : , prefix=['xterm -e gdb -ex run --args']

3
src/modelec_gui/include/modelec_gui/pages/home_page.hpp
View File

@@ -40,6 +40,7 @@ namespace ModelecGUI
QSvgRenderer* renderer_;
std::vector<QPushButton*> spawn_buttons_;
QPushButton* yellow_spawn_buttons_;
QPushButton* blue_spawn_buttons_;
};
}

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

@@ -12,52 +12,64 @@ namespace ModelecGUI
{
spawn_pub_ = node_->create_publisher<modelec_interfaces::msg::Spawn>("strat/spawn", 10);
auto w = Modelec::Config::get<int>("config.spawn.width_mm");
auto h = Modelec::Config::get<int>("config.spawn.height_mm");
auto* layout = new QHBoxLayout(this);
layout->setContentsMargins(0, 0, 0, 0);
layout->setSpacing(0);
spawn_sub_ = node_->create_subscription<modelec_interfaces::msg::Spawn>("nav/spawn", 10,
[this, w, h](const modelec_interfaces::msg::Spawn::SharedPtr msg)
{
auto ratioX = 1200 / 3000.0f;
auto ratioY = 800 / 2000.0f;
yellow_spawn_buttons_ = new QPushButton("Yellow", this);
blue_spawn_buttons_ = new QPushButton("Blue", this);
auto* button = new QPushButton(this);
spawn_buttons_.push_back(button);
yellow_spawn_buttons_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
blue_spawn_buttons_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
button->setText(msg->team_id == 0 ? "Yellow" : "Blue");
button->setStyleSheet(
msg->team_id == 0
? "background-color: rgba(255, 255, 0, 128); border: none; color: black; font-size: 24px;"
: "background-color: rgba(0, 0, 255, 128); border: none; color: white; font-size: 24px;"
);
yellow_spawn_buttons_->setStyleSheet(
"QPushButton {"
" background-color: rgba(255, 215, 0, 140);"
" border: none;"
" font-size: 32px;"
" font-weight: bold;"
" color: black;"
"}"
"QPushButton:hover {"
" background-color: rgba(255, 215, 0, 180);"
"}"
);
button->move(
static_cast<int>(msg->x * ratioX - (w * ratioX) / 2),
static_cast<int>(800 - msg->y * ratioY - (h * ratioY) / 2)
);
blue_spawn_buttons_->setStyleSheet(
"QPushButton {"
" background-color: rgba(0, 90, 200, 140);"
" border: none;"
" font-size: 32px;"
" font-weight: bold;"
" color: white;"
"}"
"QPushButton:hover {"
" background-color: rgba(0, 90, 200, 180);"
"}"
);
button->setFixedSize(w * ratioX, h * ratioY);
layout->addWidget(yellow_spawn_buttons_, 1);
layout->addWidget(blue_spawn_buttons_, 1);
button->show();
connect(yellow_spawn_buttons_, &QPushButton::clicked, this, [this]()
{
modelec_interfaces::msg::Spawn msg;
msg.team_id = 0;
msg.name = modelec_interfaces::msg::Spawn::TOP;
spawn_pub_->publish(msg);
emit TeamChoose();
});
connect(button, &QPushButton::clicked, this, [this, msg]()
{
modelec_interfaces::msg::Spawn team_msg;
team_msg.team_id = msg->team_id;
team_msg.name = modelec_interfaces::msg::Spawn::TOP;
spawn_pub_->publish(team_msg);
emit TeamChoose();
});
});
connect(blue_spawn_buttons_, &QPushButton::clicked, this, [this]()
{
modelec_interfaces::msg::Spawn msg;
msg.team_id = 1;
msg.name = modelec_interfaces::msg::Spawn::TOP;
spawn_pub_->publish(msg);
emit TeamChoose();
});
reset_strat_pub_ = node_->create_publisher<std_msgs::msg::Empty>("strat/reset", 10);
ask_spawn_client_ = node_->create_client<std_srvs::srv::Empty>("nav/ask_spawn");
ask_spawn_client_->wait_for_service();
auto ask_spawn_request_ = std::make_shared<std_srvs::srv::Empty::Request>();
auto res = ask_spawn_client_->async_send_request(ask_spawn_request_);
rclcpp::spin_until_future_complete(node_->get_node_base_interface(), res);
}
void HomePage::Init()

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

@@ -167,7 +167,6 @@ namespace ModelecGUI
{
if (auto res = result.get())
{
RCLCPP_INFO(node_->get_logger(), "Map received: %d x %d", res->width, res->height);
map_width_ = res->width;
map_height_ = res->height;
}

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

@@ -118,7 +118,6 @@ namespace ModelecGUI
{
if (auto res = result.get())
{
RCLCPP_INFO(node_->get_logger(), "Map received: %d x %d", res->width, res->height);
map_width_ = res->width;
map_height_ = res->height;
}

2
src/modelec_strat/CMakeLists.txt
View File

@@ -37,7 +37,7 @@ set(strat_fsm_sources
)
add_executable(strat_fsm ${strat_fsm_sources})
ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces ament_index_cpp)
ament_target_dependencies(strat_fsm rclcpp std_msgs sensor_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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

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

@@ -1,32 +1,71 @@
<Map>
<DepositeZone id="0">
<Pos x="1250" y="1450" theta="0" w="200" h="200" />
<TakeAngle>
<Angle>-1.5708</Angle>
</TakeAngle>
</DepositeZone>
<DepositeZone id="1">
<Pos x="1750" y="1450" theta="0" w="200" h="200" />
<TakeAngle>
<Angle>-1.5708</Angle>
</TakeAngle>
</DepositeZone>
<DepositeZone id="2">
<Pos x="100" y="800" theta="0" w="200" h="200" />
<TakeAngle>
<Angle>0</Angle>
</TakeAngle>
</DepositeZone>
<DepositeZone id="3">
<Pos x="800" y="800" theta="0" w="200" h="200" />
<TakeAngle>
<Angle>0</Angle>
<Angle>1.5708</Angle>
<Angle>-1.5708</Angle>
<Angle>3.1415</Angle>
</TakeAngle>
</DepositeZone>
<DepositeZone id="4">
<Pos x="1500" y="800" theta="0" w="200" h="200" />
<TakeAngle>
<Angle>0</Angle>
<Angle>1.5708</Angle>
<Angle>-1.5708</Angle>
<Angle>3.1415</Angle>
</TakeAngle>
</DepositeZone>
<DepositeZone id="5">
<Pos x="2200" y="800" theta="0" w="200" h="200" />
<TakeAngle>
<Angle>0</Angle>
<Angle>1.5708</Angle>
<Angle>-1.5708</Angle>
<Angle>3.1415</Angle>
</TakeAngle>
</DepositeZone>
<DepositeZone id="6">
<Pos x="2900" y="800" theta="0" w="200" h="200" />
<TakeAngle>
<Angle>3.1415</Angle>
</TakeAngle>
</DepositeZone>
<DepositeZone id="7">
<Pos x="700" y="100" theta="0" w="200" h="200" />
<TakeAngle>
<Angle>1.5708</Angle>
</TakeAngle>
</DepositeZone>
<DepositeZone id="8">
<Pos x="1500" y="100" theta="0" w="200" h="200" />
<TakeAngle>
<Angle>1.5708</Angle>
</TakeAngle>
</DepositeZone>
<DepositeZone id="9">
<Pos x="2300" y="100" theta="0" w="200" h="200" />
<TakeAngle>
<Angle>1.5708</Angle>
</TakeAngle>
</DepositeZone>
</Map>

11
src/modelec_strat/include/modelec_strat/action/base_action.hpp
View File

@@ -25,6 +25,13 @@ namespace Modelec
class BaseAction
{
public:
enum Front
{
FRONT = 1,
BACK = 0,
BOTH = -1,
};
using Ptr = std::shared_ptr<BaseAction>;
using FactoryFn =
std::function<Ptr(const std::shared_ptr<ActionExecutor>&)>;
@@ -41,13 +48,9 @@ namespace Modelec
{
}
virtual ~BaseAction() = default;
virtual void Execute() = 0;
virtual void Next() = 0;
virtual bool IsDone() const { return done_; }
virtual void Init(const std::vector<std::string>& params) = 0;
// static constexpr std::string_view Name = "BaseAction";
static Ptr CreateAction(
const std::string& action_name,
const std::shared_ptr<ActionExecutor>& action_executor);

7
src/modelec_strat/include/modelec_strat/action/down_action.hpp
View File

@@ -9,17 +9,16 @@ namespace Modelec
{
public:
DownAction(const std::shared_ptr<ActionExecutor>& action_executor);
DownAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front);
DownAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front);
void Execute() override;
void Next() override;
void Init(const std::vector<std::string>& params) override;
void SetFront(bool front);
void SetFront(Front front);
inline static const std::string Name = ActionExec::DOWN;
private:
bool front_;
Front front_;
std::queue<int> steps_;

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

@@ -10,18 +10,19 @@ 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);
void Execute() override;
void Next() override;
void Init(const std::vector<std::string>& params) override;
void SetFront(bool front);
void SetN(int n);
void AddServo(int id, bool front);
void AddServo(std::pair<int, bool> servo);
void AddServos(const std::vector<std::pair<int, bool>>& servos);
inline static const std::string Name = ActionExec::FREE;
private:
bool front_;
int n_;
std::vector<std::pair<int, bool>> servos_;
std::queue<int> steps_;

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

@@ -10,18 +10,19 @@ 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);
void Execute() override;
void Next() override;
void Init(const std::vector<std::string>& params) override;
void SetFront(bool front);
void SetN(int n);
void AddServo(int id, bool front);
void AddServo(std::pair<int, bool> servo);
void AddServos(const std::vector<std::pair<int, bool>>& servos);
inline static const std::string Name = ActionExec::TAKE;
private:
bool front_;
int n_;
std::vector<std::pair<int, bool>> servos_;
std::queue<int> steps_;

7
src/modelec_strat/include/modelec_strat/action/up_action.hpp
View File

@@ -9,17 +9,16 @@ namespace Modelec
{
public:
UPAction(const std::shared_ptr<ActionExecutor>& action_executor);
UPAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front);
UPAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front);
void Execute() override;
void Next() override;
void Init(const std::vector<std::string>& params) override;
void SetFront(bool front);
void SetFront(Front front);
inline static const std::string Name = ActionExec::UP;
private:
bool front_;
Front front_;
std::queue<int> steps_;

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

@@ -7,6 +7,11 @@
#include <modelec_interfaces/msg/action_servo_timed_array.hpp>
#include <modelec_interfaces/msg/action_exec.hpp>
#include <sensor_msgs/msg/joy.hpp>
#include "action/base_action.hpp"
#include "obstacle/box.hpp"
namespace Modelec
{
class BaseAction;
@@ -14,16 +19,6 @@ namespace Modelec
class ActionExecutor : public std::enable_shared_from_this<ActionExecutor>
{
public:
enum Action
{
NONE,
UP,
DOWN,
TAKE,
FREE
};
ActionExecutor();
ActionExecutor(const rclcpp::Node::SharedPtr& node);
@@ -36,18 +31,20 @@ namespace Modelec
void ReInit();
void Down(bool front = true);
void Down(BaseAction::Front front, bool force = false);
void Up(bool front = true);
void Up(BaseAction::Front front, bool force = false);
void Take(bool front = true, int n = 0);
void Free(bool front = true, int n = 0);
void Take(std::vector<std::pair<int, bool>> servos, bool force = false);
void Free(std::vector<std::pair<int, bool>> servos, bool force = false);
void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg);
void MoveServo(const modelec_interfaces::msg::ActionServoPosArray& msg);
std::array<std::shared_ptr<BoxObstacle>, 2> box_obstacles_;
protected:
rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPosArray>::SharedPtr servo_move_pub_;
@@ -60,6 +57,7 @@ namespace Modelec
rclcpp::Subscription<modelec_interfaces::msg::ActionServoTimedArray>::SharedPtr servo_timed_move_res_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionExec>::SharedPtr action_exec_sub_;
rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
std::shared_ptr<BaseAction> action_;

5
src/modelec_strat/include/modelec_strat/deposite_zone.hpp
View File

@@ -1,6 +1,5 @@
#pragma once
#include <limits>
#include <queue>
#include <tinyxml2.h>
#include <modelec_utils/point.hpp>
@@ -14,6 +13,8 @@ namespace Modelec
Point GetPosition() const;
Point GetBestTakePosition(const Point& currentPos, int dist = CLOSE_DISTANCE) const;
void Validate(bool valid);
bool Validate() const;
@@ -29,6 +30,8 @@ namespace Modelec
int w_, h_;
Point position_;
std::vector<double> take_angle_;
bool has_box_ = false;
};
}

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

@@ -20,7 +20,10 @@ namespace Modelec {
enum Step
{
GO_TO_FREE,
DOWN,
FREE,
UP,
GO_BACK,
DONE,
} step_;
@@ -31,6 +34,8 @@ namespace Modelec {
std::shared_ptr<DepositeZone> target_deposite_zone_;
double angle_;
rclcpp::Time go_timeout_;
rclcpp::Time deploy_time_;
};

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

@@ -20,10 +20,14 @@ namespace Modelec {
enum Step
{
GO_TO_TAKE,
GO_TO_TAKE_CLOSE,
DOWN,
TAKE,
UP,
DONE,
} step_;
std::shared_ptr<BoxObstacle> closestBox;
MissionStatus status_;
std::shared_ptr<NavigationHelper> nav_;
std::shared_ptr<ActionExecutor> action_executor_;

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

@@ -113,6 +113,8 @@ namespace Modelec
Point GetSpawn() const;
void AskWaypoint();
protected:
void OnWaypointReach(const WaypointMsg::SharedPtr msg);
@@ -171,8 +173,7 @@ namespace Modelec
rclcpp::Publisher<modelec_interfaces::msg::Spawn>::SharedPtr spawn_pub_;
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr ask_spawn_srv_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr odo_get_pos_pub_;
rclcpp::Time last_odo_get_pos_time_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr odo_ask_waypoint_pub_;
};
@@ -187,11 +188,14 @@ namespace Modelec
{
if (auto obs = std::dynamic_pointer_cast<T>(obstacle.second))
{
auto dist = Point::distance(robotPos, obs->GetPosition());
if (dist < distance)
if (!obs->IsAtObjective())
{
distance = dist;
closest_obstacle = obs;
auto dist = Point::distance(robotPos, obs->GetPosition());
if (dist < distance)
{
distance = dist;
closest_obstacle = obs;
}
}
}
}

4
src/modelec_strat/include/modelec_strat/obstacle/box.hpp
View File

@@ -15,16 +15,12 @@ namespace Modelec
BoxObstacle(tinyxml2::XMLElement* obstacleElem);
BoxObstacle(const modelec_interfaces::msg::Obstacle& msg);
bool IsAtObjective() const;
void SetAtObjective(bool atObjective);
Point GetOptimizedGetPos(const modelec_interfaces::msg::OdometryPos::SharedPtr& msg) const;
Point GetOptimizedGetPos(const Point& currentPos) const;
std::vector<Point> GetAllPossiblePositions() const;
protected:
bool isAtObjective = false;
std::vector<double> possible_angles_;
};

55
src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp
View File

@@ -26,46 +26,37 @@ namespace Modelec
virtual modelec_interfaces::msg::Obstacle toMsg() const;
int GetId() const { return id_; }
int GetX() const { return x_; }
int GetY() const { return y_; }
double GetTheta() const { return theta_; }
int GetWidth() const { return w_; }
int GetHeight() const { return h_; }
const std::string& Type() const { return type_; }
Point GetPosition() const { return Point(x_, y_, theta_); }
int GetId() const;
int GetX() const;
int GetY() const;
double GetTheta() const;
int GetWidth() const;
int GetHeight() const;
const std::string& Type() const;
Point GetPosition() const;
void SetId(int id) { id_ = id; }
void SetX(int x) { x_ = x; }
void SetY(int y) { y_ = y; }
void SetTheta(double theta) { theta_ = theta; }
void SetWidth(int w) { w_ = w; }
void SetHeight(int h) { h_ = h; }
void SetType(const std::string& type) { type_ = type; }
void SetId(int id);
void SetX(int x);
void SetY(int y);
void SetTheta(double theta);
void SetWidth(int w);
void SetHeight(int h);
void SetType(const std::string& type);
void SetPosition(int x, int y, double theta)
{
x_ = x;
y_ = y;
theta_ = theta;
}
void SetPosition(int x, int y, double theta);
void SetPosition(const Point& p)
{
x_ = p.x;
y_ = p.y;
theta_ = p.theta;
}
void SetPosition(const Point& p);
void SetSize(int w, int h)
{
w_ = w;
h_ = h;
}
void SetSize(int w, int h);
bool IsAtObjective() const;
void SetAtObjective(bool atObjective);
protected:
int id_, x_, y_, w_, h_;
double theta_;
std::string type_;
bool isAtObjective = false;
};
}

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

@@ -35,7 +35,7 @@ namespace Modelec
int score_free_zone_ = 0;
rclcpp::TimerBase::SharedPtr timer_add_;
rclcpp::TimerBase::SharedPtr timer_remove_;
// rclcpp::TimerBase::SharedPtr timer_remove_;
rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr start_time_sub_;
rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr add_obs_pub_;

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

@@ -8,15 +8,11 @@ Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_ex
steps_.push(ActionExec::DONE_STEP);
}
Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front) : DownAction(action_executor)
Modelec::DownAction::DownAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front) : DownAction(action_executor)
{
front_ = front;
}
void Modelec::DownAction::Execute()
{
}
void Modelec::DownAction::Next()
{
if (steps_.empty())
@@ -33,27 +29,54 @@ void Modelec::DownAction::Next()
case ActionExec::DOWN_STEP:
{
ActionServoTimedArray msg;
msg.items.resize(4);
msg.items.resize(front_ == BOTH ? 8 : 4);
msg.items[0].id = front_ ? 0 : 8;
msg.items[0].start_angle = front_ ? 1.49 : 0;
msg.items[0].end_angle = front_ ? 0 : 0;
msg.items[0].duration_s = 1;
if (front_ == FRONT || front_ == BOTH)
{
msg.items[0].id = 0;
msg.items[0].start_angle = 1.95;
msg.items[0].end_angle = 2.95;
msg.items[0].duration_s = 1;
msg.items[1].id = front_ ? 1 : 9;
msg.items[1].start_angle = front_ ? 1.5 : 0;
msg.items[1].end_angle = front_ ? 3 : 0;
msg.items[1].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].duration_s = 1;
msg.items[2].id = front_ ? 2 : 10;
msg.items[2].start_angle = front_ ? 3 : 0;
msg.items[2].end_angle = front_ ? 1.45 : 0;
msg.items[2].duration_s = 1;
msg.items[2].id = 2;
msg.items[2].start_angle = 0.3;
msg.items[2].end_angle = 0;
msg.items[2].duration_s = 1;
msg.items[3].id = front_ ? 3 : 11;
msg.items[3].start_angle = front_ ? 0 : 0;
msg.items[3].end_angle = front_ ? 1.6 : 0;
msg.items[3].duration_s = 1;
msg.items[3].id = 3;
msg.items[3].start_angle = 2.7;
msg.items[3].end_angle = 3;
msg.items[3].duration_s = 1;
}
if (front_ == BACK || front_ == BOTH)
{
int i = front_ == BOTH ? 4 : 0;
msg.items[i].id = 8;
msg.items[i].start_angle = 0;
msg.items[i].end_angle = 0;
msg.items[i].duration_s = 1;
msg.items[i+1].id = 9;
msg.items[i+1].start_angle = 0;
msg.items[i+1].end_angle = 0;
msg.items[i+1].duration_s = 1;
msg.items[i+2].id = 10;
msg.items[i+2].start_angle = 0;
msg.items[i+2].end_angle = 0;
msg.items[i+2].duration_s = 1;
msg.items[i+3].id = 11;
msg.items[i+3].start_angle = 0;
msg.items[i+3].end_angle = 0;
msg.items[i+3].duration_s = 1;
}
action_executor_->MoveServoTimed(msg);
}
@@ -72,11 +95,11 @@ void Modelec::DownAction::Init(const std::vector<std::string>& params)
{
if (!params.empty())
{
SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front");
SetFront(static_cast<Front>(std::stoi(params[1])));
}
}
void Modelec::DownAction::SetFront(bool front)
void Modelec::DownAction::SetFront(Front front)
{
front_ = front;
}

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

@@ -10,12 +10,17 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_ex
Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n) : FreeAction(action_executor)
{
front_ = front;
n_ = n;
AddServo(n, front);
}
void Modelec::FreeAction::Execute()
Modelec::FreeAction::FreeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> 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)
{
AddServos(servos);
}
void Modelec::FreeAction::Next()
@@ -35,12 +40,16 @@ void Modelec::FreeAction::Next()
{
modelec_interfaces::msg::ActionServoTimedArray msg;
msg.items.resize(1);
msg.items.resize(servos_.size());
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].duration_s = 0.5;
}
msg.items[0].id = n_ + (front_ ? 3 : 11);
msg.items[0].start_angle = front_ ? 3 : 0;
msg.items[0].end_angle = front_ ? 0 : 0;
msg.items[0].duration_s = 0.5;
action_executor_->MoveServoTimed(msg);
}
break;
@@ -58,17 +67,26 @@ void Modelec::FreeAction::Init(const std::vector<std::string>& params)
{
if (params.size() >= 2)
{
SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front");
SetN(std::stoi(params[2]));
for (size_t i = 1; i < params.size(); i += 2)
{
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);
}
}
}
void Modelec::FreeAction::SetFront(bool front)
void Modelec::FreeAction::AddServo(int id, bool front)
{
front_ = front;
servos_.emplace_back(id, front);
}
void Modelec::FreeAction::SetN(int n)
void Modelec::FreeAction::AddServo(std::pair<int, bool> servo)
{
n_ = n;
servos_.emplace_back(servo);
}
void Modelec::FreeAction::AddServos(const std::vector<std::pair<int, bool>>& servos)
{
servos_.insert(servos_.end(), servos.begin(), servos.end());
}

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

@@ -4,18 +4,21 @@
Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor) : BaseAction(action_executor)
{
steps_.push(ActionExec::TAKE_STEP);
steps_.push(ActionExec::DONE_STEP);
}
Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front, int n) : TakeAction(action_executor)
{
front_ = front;
n_ = n;
AddServo(n, front);
}
void Modelec::TakeAction::Execute()
Modelec::TakeAction::TakeAction(const std::shared_ptr<ActionExecutor>& action_executor, std::pair<int, bool> 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)
{
AddServos(servos);
}
void Modelec::TakeAction::Next()
@@ -35,12 +38,16 @@ void Modelec::TakeAction::Next()
{
modelec_interfaces::msg::ActionServoTimedArray msg;
msg.items.resize(1);
msg.items.resize(servos_.size());
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].duration_s = 0.5;
}
msg.items[0].id = n_ + (front_ ? 3 : 11);
msg.items[0].start_angle = front_ ? 0 : 0;
msg.items[0].end_angle = front_ ? 3 : 0;
msg.items[0].duration_s = 0.5;
action_executor_->MoveServoTimed(msg);
}
break;
@@ -58,17 +65,26 @@ void Modelec::TakeAction::Init(const std::vector<std::string>& params)
{
if (params.size() >= 2)
{
SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front");
SetN(std::stoi(params[2]));
for (size_t i = 1; i < params.size(); i += 2)
{
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);
}
}
}
void Modelec::TakeAction::SetFront(bool front)
void Modelec::TakeAction::AddServo(int id, bool front)
{
front_ = front;
servos_.emplace_back(id, front);
}
void Modelec::TakeAction::SetN(int n)
void Modelec::TakeAction::AddServo(std::pair<int, bool> servo)
{
n_ = n;
servos_.emplace_back(servo);
}
void Modelec::TakeAction::AddServos(const std::vector<std::pair<int, bool>>& servos)
{
servos_.insert(servos_.end(), servos.begin(), servos.end());
}

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

@@ -8,15 +8,11 @@ Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_execut
steps_.push(ActionExec::DONE_STEP);
}
Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_executor, bool front) : UPAction(action_executor)
Modelec::UPAction::UPAction(const std::shared_ptr<ActionExecutor>& action_executor, Front front) : UPAction(action_executor)
{
front_ = front;
}
void Modelec::UPAction::Execute()
{
}
void Modelec::UPAction::Next()
{
if (steps_.empty())
@@ -33,27 +29,54 @@ void Modelec::UPAction::Next()
case ActionExec::UP_STEP:
{
ActionServoTimedArray msg;
msg.items.resize(4);
msg.items.resize(front_ == BOTH ? 8 : 4);
msg.items[0].id = front_ ? 0 : 8;
msg.items[0].start_angle = front_ ? 0 : 0;
msg.items[0].end_angle = front_ ? 1.49 : 0;
msg.items[0].duration_s = 1;
if (front_ == FRONT || front_ == BOTH)
{
msg.items[0].id = 0;
msg.items[0].start_angle = 2.95;
msg.items[0].end_angle = 1.95;
msg.items[0].duration_s = 1;
msg.items[1].id = front_ ? 1 : 9;
msg.items[1].start_angle = front_ ? 3 : 0;
msg.items[1].end_angle = front_ ? 1.5 : 0;
msg.items[1].duration_s = 1;
msg.items[1].id = 1;
msg.items[1].start_angle = 0.9;
msg.items[1].end_angle = 1.9;
msg.items[1].duration_s = 1;
msg.items[2].id = front_ ? 2 : 10;
msg.items[2].start_angle = front_ ? 1.45 : 0;
msg.items[2].end_angle = front_ ? 3 : 0;
msg.items[2].duration_s = 1;
msg.items[2].id = 2;
msg.items[2].start_angle = 0;
msg.items[2].end_angle = 0.3;
msg.items[2].duration_s = 1;
msg.items[3].id = front_ ? 3 : 11;
msg.items[3].start_angle = front_ ? 1.6 : 0;
msg.items[3].end_angle = front_ ? 0 : 0;
msg.items[3].duration_s = 1;
msg.items[3].id = 3;
msg.items[3].start_angle = 3;
msg.items[3].end_angle = 2.5;
msg.items[3].duration_s = 1;
}
if (front_ == BACK || front_ == BOTH) {
int i = front_ == BOTH ? 4 : 0;
msg.items[i].id = 8;
msg.items[i].start_angle = 0;
msg.items[i].end_angle = 0;
msg.items[i].duration_s = 1;
msg.items[i+1].id = 9;
msg.items[i+1].start_angle = 0;
msg.items[i+1].end_angle = 0;
msg.items[i+1].duration_s = 1;
msg.items[i+2].id = 10;
msg.items[i+2].start_angle = 0;
msg.items[i+2].end_angle = 0;
msg.items[i+2].duration_s = 1;
msg.items[i+3].id = 11;
msg.items[i+3].start_angle = 0;
msg.items[i+3].end_angle = 0;
msg.items[i+3].duration_s = 1;
}
action_executor_->MoveServoTimed(msg);
}
@@ -72,11 +95,11 @@ void Modelec::UPAction::Init(const std::vector<std::string>& params)
{
if (!params.empty())
{
SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front");
SetFront(static_cast<Front>(std::stoi(params[1])));
}
}
void Modelec::UPAction::SetFront(bool front)
void Modelec::UPAction::SetFront(Front front)
{
front_ = front;
}

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

@@ -79,6 +79,31 @@ namespace Modelec
step_running_ -= msg->items.size();
Update();
});
joy_sub_ = node_->create_subscription<sensor_msgs::msg::Joy>(
"/joy", 10, [this](const sensor_msgs::msg::Joy::SharedPtr msg)
{
// use game controller to manually control all the action. make it carefully
if (msg->buttons.size() >= 5)
{
if (msg->buttons[0] == 1) // A button
{
Down(BaseAction::BOTH);
}
else if (msg->buttons[1] == 1) // B button
{
Up(BaseAction::BOTH);
}
else if (msg->buttons[3] == 1) // X button
{
Take({{0, true}, {1, true}, {2, true}, {3, true}});
}
else if (msg->buttons[4] == 1) // Y button
{
Free({{0, true}, {1, true}, {2, true}, {3, true}});
}
}
});
}
rclcpp::Node::SharedPtr ActionExecutor::GetNode() const
@@ -124,45 +149,57 @@ namespace Modelec
action_ = nullptr;
}
void ActionExecutor::Down(bool front) {
if (action_done_)
void ActionExecutor::Down(BaseAction::Front front, bool force) {
if (action_done_ || force)
{
action_ = std::make_shared<DownAction>(shared_from_this(), front);
if (action_done_)
{
step_running_ = 0;
}
action_done_ = false;
step_running_ = 0;
Update();
}
}
void ActionExecutor::Up(bool front) {
if (action_done_)
void ActionExecutor::Up(BaseAction::Front front, bool force) {
if (action_done_ || force)
{
action_ = std::make_shared<UPAction>(shared_from_this(), front);
if (action_done_)
{
step_running_ = 0;
}
action_done_ = false;
step_running_ = 0;
Update();
}
}
void ActionExecutor::Take(bool front, int n) {
if (action_done_)
void ActionExecutor::Take(std::vector<std::pair<int, bool>> servos, bool force) {
if (action_done_ || force)
{
action_ = std::make_shared<TakeAction>(shared_from_this(), front, n);
action_ = std::make_shared<TakeAction>(shared_from_this(), servos);
if (action_done_)
{
step_running_ = 0;
}
action_done_ = false;
step_running_ = 0;
Update();
}
}
void ActionExecutor::Free(bool front, int n) {
if (action_done_)
void ActionExecutor::Free(std::vector<std::pair<int, bool>> servos, bool force) {
if (action_done_ || force)
{
action_ = std::make_shared<FreeAction>(shared_from_this(), front, n);
action_ = std::make_shared<FreeAction>(shared_from_this(), servos);
if (action_done_)
{
step_running_ = 0;
}
action_done_ = false;
step_running_ = 0;
Update();
}

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

@@ -1,3 +1,4 @@
#include <cmath>
#include <modelec_strat/deposite_zone.hpp>
#include <rclcpp/logging.hpp>
@@ -15,6 +16,16 @@ namespace Modelec
posElem->QueryDoubleAttribute("theta", &position_.theta);
posElem->QueryIntAttribute("w", &w_);
posElem->QueryIntAttribute("h", &h_);
if (auto TakeAngleElem = obstacleElem->FirstChildElement("TakeAngle"))
{
for (auto takePos = TakeAngleElem->FirstChildElement("Angle"); takePos; takePos = takePos->NextSiblingElement("Angle"))
{
double angle;
takePos->QueryDoubleAttribute("value", &angle);
take_angle_.push_back(angle);
}
}
}
}
@@ -23,6 +34,38 @@ namespace Modelec
return position_;
}
Point DepositeZone::GetBestTakePosition(const Point& currentPos, int dist) const
{
if (take_angle_.empty())
{
RCLCPP_WARN(rclcpp::get_logger("DepositeZone"), "No take angles defined for DepositeZone id=%d", id_);
return position_;
}
double min_distance = std::numeric_limits<double>::max();
double best_angle = take_angle_.front();
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)),
angle);
double distance = p.distance(currentPos);
if (distance < min_distance)
{
min_distance = distance;
best_angle = angle;
}
}
Point best_position = position_;
best_position.theta = best_angle;
return best_position;
}
void DepositeZone::Validate(bool valid)
{
has_box_ = valid;

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

@@ -1,5 +1,7 @@
#include <modelec_strat/missions/free_mission.hpp>
#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) {
@@ -24,6 +26,11 @@ namespace Modelec {
if (!nav_->HasArrived())
{
if ((node_->now() - go_timeout_).seconds() < 5)
{
// nav_->AskWaypoint();
// return;
}
if ((node_->now() - go_timeout_).seconds() < 10)
{
return;
@@ -37,30 +44,68 @@ namespace Modelec {
auto currPos = nav_->GetCurrentPos();
auto dist = std::clamp(Point::distance(Point(currPos->x, currPos->y, currPos->theta),
nav_->GetClosestDepositeZone(nav_->GetCurrentPos())->GetPosition()), 0.0, 200.0);
nav_->GetClosestDepositeZone(nav_->GetCurrentPos())->GetPosition()), 0.0, 300.0);
target_deposite_zone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), {}, true);
auto depoPoint = target_deposite_zone_->GetPosition();
auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta));
auto angle = atan2(depoPoint.y - currPos->y,
depoPoint.x - currPos->x);
nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist,
angle + M_PI), true, Pathfinding::FREE);
if (nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist), true, Pathfinding::FREE))
{
nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist), true, Pathfinding::FREE | Pathfinding::OBSTACLE);
}
go_timeout_ = node_->now();
step_ = FREE;
}
step_ = FREE;
break;
case DOWN:
{
action_executor_->Down(BaseAction::FRONT);
deploy_time_ = node_->now();
}
step_ = FREE;
break;
case FREE:
{
action_executor_->Down();
action_executor_->Free({{0, true}, {1, true}, {2, true}, {3, true}});
deploy_time_ = node_->now();
step_ = DONE;
auto obs = action_executor_->box_obstacles_[0];
action_executor_->box_obstacles_[0] = nullptr;
auto pos = nav_->GetCurrentPos();
obs->SetPosition(
pos->x + 250 * cos(pos->theta),
pos->y + 250 * sin(pos->theta),
pos->theta);
obs->SetAtObjective(true);
nav_->GetPathfinding()->AddObstacle(obs);
}
step_ = UP;
break;
case UP:
{
action_executor_->Up(BaseAction::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);
go_timeout_ = node_->now();
}
step_ = DONE;
break;
case DONE:
{
@@ -83,4 +128,4 @@ namespace Modelec {
return status_;
}
}
}

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

@@ -27,6 +27,11 @@ namespace Modelec
{
if (!nav_->HasArrived())
{
if ((node_->now() - go_timeout_).seconds() < 2)
{
// nav_->AskWaypoint();
return;
}
if ((node_->now() - go_timeout_).seconds() < 10)
{
return;

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

@@ -1,5 +1,7 @@
#include <modelec_strat/missions/take_mission.hpp>
#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) {
@@ -24,7 +26,12 @@ namespace Modelec {
if (!nav_->HasArrived())
{
if ((node_->now() - go_timeout_).seconds() < 20)
if ((node_->now() - go_timeout_).seconds() < 5)
{
// nav_->AskWaypoint();
// return;
}
if ((node_->now() - go_timeout_).seconds() < 10)
{
return;
}
@@ -35,28 +42,62 @@ namespace Modelec {
case GO_TO_TAKE:
{
auto closestBox = nav_->GetClosestObstacle<BoxObstacle>(nav_->GetCurrentPos());
closestBox = nav_->GetClosestObstacle<BoxObstacle>(nav_->GetCurrentPos());
action_executor_->box_obstacles_[0] = closestBox;
auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
nav_->GetPathfinding()->RemoveObstacle(closestBox->GetId());
nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL);
if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL))
{
if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL))
{
nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
}
}
go_timeout_ = node_->now();
}
step_ = GO_TO_TAKE_CLOSE;
break;
case GO_TO_TAKE_CLOSE:
{
auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition();
nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE);
go_timeout_ = node_->now();
}
step_ = DOWN;
break;
case DOWN:
{
action_executor_->Down(BaseAction::FRONT);
deploy_time_ = node_->now();
}
step_ = TAKE;
break;
case TAKE:
{
action_executor_->Up();
action_executor_->Take({{0, true}, {1, true}, {2, true}, {3, true}});
deploy_time_ = node_->now();
}
step_ = UP;
break;
case UP:
{
action_executor_->Up(BaseAction::FRONT);
deploy_time_ = node_->now();
}
step_ = DONE;
break;
case DONE:
nav_->GetPathfinding()->RemoveObstacle(closestBox->GetId());
status_ = MissionStatus::DONE;
break;
default:
@@ -73,4 +114,4 @@ namespace Modelec {
return status_;
}
}
}

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

@@ -97,10 +97,8 @@ namespace Modelec
}
});
odo_get_pos_pub_ = node_->create_publisher<std_msgs::msg::Empty>(
"odometry/get/pos", 30);
last_odo_get_pos_time_ = node_->now();
odo_ask_waypoint_pub_ = node_->create_publisher<std_msgs::msg::Empty>(
"odometry/ask_active_waypoint", 30);
}
void NavigationHelper::ReInit()
@@ -720,6 +718,12 @@ namespace Modelec
return spawn_;
}
void NavigationHelper::AskWaypoint()
{
std_msgs::msg::Empty msg;
odo_ask_waypoint_pub_->publish(msg);
}
void NavigationHelper::OnWaypointReach(const WaypointMsg::SharedPtr msg)
{
for (auto& waypoint : waypoints_)
@@ -757,34 +761,10 @@ namespace Modelec
Config::get<double>("config.spawn.yellow.top@theta")
);
spawn_yellow_["side"] = Point(
Config::get<int>("config.spawn.yellow.side@x"),
Config::get<int>("config.spawn.yellow.side@y"),
Config::get<double>("config.spawn.yellow.side@theta")
);
spawn_yellow_["bottom"] = Point(
Config::get<int>("config.spawn.yellow.bottom@x"),
Config::get<int>("config.spawn.yellow.bottom@y"),
Config::get<double>("config.spawn.yellow.bottom@theta")
);
spawn_blue_["top"] = Point(
Config::get<int>("config.spawn.blue.top@x"),
Config::get<int>("config.spawn.blue.top@y"),
Config::get<double>("config.spawn.blue.top@theta")
);
spawn_blue_["side"] = Point(
Config::get<int>("config.spawn.blue.side@x"),
Config::get<int>("config.spawn.blue.side@y"),
Config::get<double>("config.spawn.blue.side@theta")
);
spawn_blue_["bottom"] = Point(
Config::get<int>("config.spawn.blue.bottom@x"),
Config::get<int>("config.spawn.blue.bottom@y"),
Config::get<double>("config.spawn.blue.bottom@theta")
);
}
}

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

@@ -24,16 +24,6 @@ namespace Modelec
{
}
bool BoxObstacle::IsAtObjective() const
{
return isAtObjective;
}
void BoxObstacle::SetAtObjective(bool atObjective)
{
isAtObjective = atObjective;
}
Point BoxObstacle::GetOptimizedGetPos(const modelec_interfaces::msg::OdometryPos::SharedPtr& msg) const
{
Point p = Point(msg->x, msg->y, msg->theta);

75
src/modelec_strat/src/obstacle/obstacle.cpp
View File

@@ -43,4 +43,79 @@ namespace Modelec
return msg;
}
int Obstacle::GetId() const
{ return id_; }
int Obstacle::GetX() const
{ return x_; }
int Obstacle::GetY() const
{ return y_; }
double Obstacle::GetTheta() const
{ return theta_; }
int Obstacle::GetWidth() const
{ return w_; }
int Obstacle::GetHeight() const
{ return h_; }
const std::string& Obstacle::Type() const
{ return type_; }
Point Obstacle::GetPosition() const
{ return Point(x_, y_, theta_); }
void Obstacle::SetId(int id)
{ id_ = id; }
void Obstacle::SetX(int x)
{ x_ = x; }
void Obstacle::SetY(int y)
{ y_ = y; }
void Obstacle::SetTheta(double theta)
{ theta_ = theta; }
void Obstacle::SetWidth(int w)
{ w_ = w; }
void Obstacle::SetHeight(int h)
{ h_ = h; }
void Obstacle::SetType(const std::string& type)
{ type_ = type; }
void Obstacle::SetPosition(int x, int y, double theta)
{
x_ = x;
y_ = y;
theta_ = theta;
}
void Obstacle::SetPosition(const Point& p)
{
x_ = p.x;
y_ = p.y;
theta_ = p.theta;
}
void Obstacle::SetSize(int w, int h)
{
w_ = w;
h_ = h;
}
bool Obstacle::IsAtObjective() const
{
return isAtObjective;
}
void Obstacle::SetAtObjective(bool atObjective)
{
isAtObjective = atObjective;
}
}

19
src/modelec_strat/src/pami_manager.cpp
View File

@@ -37,21 +37,6 @@ namespace Modelec
auto goal = std::chrono::nanoseconds(msg->data) + std::chrono::seconds(time_to_remove_top_pot_);
auto second_goal = std::chrono::nanoseconds(msg->data) + std::chrono::seconds(time_to_put_zone_);
timer_remove_ = create_wall_timer(
goal - now,
[this]()
{
modelec_interfaces::msg::Obstacle topLeft;
topLeft.id = 10;
remove_obs_pub_->publish(topLeft);
modelec_interfaces::msg::Obstacle topRight;
topRight.id = 20;
remove_obs_pub_->publish(topRight);
timer_remove_->cancel();
});
timer_add_ = create_wall_timer(
second_goal - now,
[this]()
@@ -84,10 +69,10 @@ namespace Modelec
{
timer_add_->cancel();
}
if (timer_remove_)
/*if (timer_remove_)
{
timer_remove_->cancel();
}
}*/
});
}

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

@@ -7,6 +7,8 @@
#include <modelec_strat/missions/take_mission.hpp>
#include <modelec_strat/missions/free_mission.hpp>
#include "modelec_strat/action/base_action.hpp"
namespace Modelec
{
@@ -141,6 +143,12 @@ namespace Modelec
arm_msg.data = true;
tir_arm_set_pub_->publish(arm_msg);
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},
}, true);
Transition(State::WAIT_START, "System ready");
}
break;

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

@@ -1,5 +1,8 @@
#pragma once
#define CLOSE_DISTANCE 150
#define BASE_DISTANCE 290
namespace Modelec
{
struct Point {
@@ -13,6 +16,9 @@ namespace Modelec
static double distance(const Point& p1, const Point& p2);
static double angleDiff(const Point& p1, const Point& p2);
double distance(const Point& p2) const;
double angleDiff(const Point& p2) const;
[[nodiscard]] Point GetTakePosition(int distance, double angle) const;
[[nodiscard]] Point GetTakePosition(int distance) const;

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

@@ -15,6 +15,16 @@ namespace Modelec
return diff - M_PI;
}
double Point::distance(const Point& p2) const
{
return distance(*this, p2);
}
double Point::angleDiff(const Point& p2) const
{
return angleDiff(*this, p2);
}
Point Point::GetTakePosition(int distance, double angle) const
{
Point pos;
@@ -31,11 +41,11 @@ namespace Modelec
Point Point::GetTakeBasePosition() const
{
return GetTakePosition(290, theta);
return GetTakePosition(BASE_DISTANCE, theta);
}
Point Point::GetTakeClosePosition() const
{
return GetTakePosition(150, theta);
return GetTakePosition(CLOSE_DISTANCE, theta);
}
}