diff --git a/install.sh b/install.sh index f516425..3e7cbef 100755 --- a/install.sh +++ b/install.sh @@ -27,9 +27,9 @@ git submodule init git submodule update echo "source /opt/ros/jazzy/setup.bash -source ~/modelec-marcel-ROS/install/setup.bash +source ~/Modelec-ROS/install/setup.bash export RMW_IMPLEMENTATION=rmw_fastrtps_cpp -export FASTRTPS_DEFAULT_PROFILES_FILE=~/modelec-marcel-ROS/fastdds_setup.xml +export FASTRTPS_DEFAULT_PROFILES_FILE=~/Modelec-ROS/fastdds_setup.xml export ROS_DOMAIN_ID=128" >> ~/.bashrc source ~/.bashrc diff --git a/no_lidar.ros2_launch_marcel.desktop b/no_lidar.ros2_launch_marcel.desktop index b59f89d..c01e2fc 100644 --- a/no_lidar.ros2_launch_marcel.desktop +++ b/no_lidar.ros2_launch_marcel.desktop @@ -2,7 +2,7 @@ Type=Application Name=No Lidar Comment=Lance le système ROS 2 avec GUI -Exec=/home/modelec/modelec-marcel-ROS/start_ros2.sh with_joy:=false with_lidar:=false +Exec=/home/modelec/modelec-marcel-ROS/start_ros2.sh with_joy:=false with_rplidar:=false Icon=utilities-terminal Terminal=true Categories=Utility; diff --git a/simulated_pcb/action.py b/simulated_pcb/action.py index cab53ea..edc6ae2 100644 --- a/simulated_pcb/action.py +++ b/simulated_pcb/action.py @@ -210,7 +210,7 @@ def stop(self): sim.stop() print("Stopped.") -# socat -d -d pty,raw,echo=0,link=/tmp/ACTION_SIM pty,raw,echo=0,link=/tmp/ACTION_USB -# python3 simulated_pcb/action.py --port /tmp/ACTION_SIM -# socat -d -d pty,raw,echo=0,link=/tmp/ODO_SIM pty,raw,echo=0,link=/tmp/ODO_USB -# python3 simulated_pcb/action.py --port /tmp/ODO_SIM +# socat -d -d pty,raw,echo=0,link=/tmp/SIM_ACTION pty,raw,echo=0,link=/tmp/USB_ACTION +# python3 simulated_pcb/action.py --port /tmp/SIM_ACTION +# socat -d -d pty,raw,echo=0,link=/tmp/SIM_ODO pty,raw,echo=0,link=/tmp/USB_ODO +# python3 simulated_pcb/action.py --port /tmp/SIM_ODO diff --git a/simulated_pcb/odo.py b/simulated_pcb/odo.py index 3361c37..0ede088 100644 --- a/simulated_pcb/odo.py +++ b/simulated_pcb/odo.py @@ -14,16 +14,20 @@ def __init__(self, port='/dev/pts/6', baud=115200): self.x = 1225.0 self.y = 300.0 self.theta = math.pi / 2 + self.vx = 0.0 self.vy = 0.0 self.vtheta = 0.0 + self.last_update = time.time() self.last_send = time.time() self.pid = [0, 0, 0] self.tof = {0: 1000, 1: 1000, 2: 1000} - self.waypoints = {} # waypoints by id - self.waypoint_order = [] # ordered list of ids + + # --- waypoint system --- + self.waypoints = {} # id -> waypoint + self.waypoint_queue = [] # FIFO execution self.current_wp_id = None self.thread = threading.Thread(target=self.run) @@ -37,52 +41,53 @@ def update_position(self): dt = now - self.last_update self.last_update = now - if self.start: - if self.waypoint_order: - wp = self.waypoints[self.waypoint_order[0]] - dx = wp['x'] - self.x - dy = wp['y'] - self.y - distance = math.hypot(dx, dy) - angle = math.atan2(dy, dx) - angle_diff = self.normalize_angle(wp['theta'] - self.theta) - - if wp['type'] == 1: - if distance < 5.0 and abs(angle_diff) < 0.15: - self.vx = 0.0 - self.vy = 0.0 - self.vtheta = 0.0 - self.ser.write(f'SET;WAYPOINT;{wp["id"]}\n'.encode('utf-8')) - self.current_wp_id = wp['id'] - self.waypoint_order.pop(0) - del self.waypoints[wp['id']] - else: - speed = min(100.0, distance / 2) - self.vx = speed * math.cos(angle) - self.vy = speed * math.sin(angle) - self.vtheta = max(-9.0, min(9.0, angle_diff * 2)) - else: - speed = min(150.0, distance / 2) - self.vx = speed * math.cos(angle) - self.vy = speed * math.sin(angle) - self.vtheta = 0.0 - if distance < 100: - self.ser.write(f'SET;WAYPOINT;{wp["id"]}\n'.encode('utf-8')) - self.current_wp_id = wp['id'] - self.waypoint_order.pop(0) - del self.waypoints[wp['id']] - else: - self.vx = 0.0 - self.vy = 0.0 + if self.start and self.waypoint_queue: + wp_id = self.waypoint_queue[0] + wp = self.waypoints[wp_id] + + dx = wp['x'] - self.x + dy = wp['y'] - self.y + distance = math.hypot(dx, dy) + + target_angle = math.atan2(dy, dx) + theta_error = self.normalize_angle(wp['theta'] - self.theta) + + # --- motion --- + if wp['type'] == 1: # precise waypoint + speed = min(120.0, distance * 1) + self.vtheta = max(-6.0, min(6.0, theta_error * 2)) + else: # transit waypoint + speed = min(180.0, distance * 2) self.vtheta = 0.0 - self.x += self.vx * dt - self.y += self.vy * dt - self.theta += self.vtheta * dt - self.theta = self.normalize_angle(self.theta) + self.vx = speed * math.cos(target_angle) + self.vy = speed * math.sin(target_angle) + + reached_pos = distance < 5.0 + reached_theta = abs(theta_error) < 0.15 or wp['type'] == 0 + + if reached_pos and reached_theta: + self.vx = self.vy = self.vtheta = 0.0 + self.current_wp_id = wp_id + + self.ser.write(f"SET;WAYPOINT;REACH;{wp_id}\n".encode()) - if now - self.last_send > 0.1 and self.start: - print(f'[TX] SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}') - self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}\n'.encode()) + self.waypoint_queue.pop(0) + del self.waypoints[wp_id] + + else: + self.vx = self.vy = self.vtheta = 0.0 + + # --- integrate --- + self.x += self.vx * dt + self.y += self.vy * dt + self.theta = self.normalize_angle(self.theta + self.vtheta * dt) + + # --- periodic position --- + if self.start and now - self.last_send > 0.1: + self.ser.write( + f"SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}\n".encode() + ) self.last_send = now def run(self): @@ -98,73 +103,62 @@ def run(self): def handle_message(self, msg): if msg == "GET;POS": - print(f'[TX] SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}') - self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}\n'.encode()) + self.ser.write( + f"SET;POS;{int(self.x)};{int(self.y)};{self.theta:.3f}\n".encode() + ) elif msg == "GET;SPEED": - print(f'[TX] SET;SPEED;{int(self.vx)};{int(self.vy)};{int(self.vtheta)}') - self.ser.write(f'SET;SPEED;{int(self.vx)};{int(self.vy)};{int(self.vtheta)}\n'.encode()) + self.ser.write( + f"SET;SPEED;{int(self.vx)};{int(self.vy)};{int(self.vtheta)}\n".encode() + ) elif msg.startswith("GET;DIST;"): n = int(msg.split(';')[2]) dist = self.tof.get(n, 0) - print(f'[TX] SET;DIST;{n};{dist}') - self.ser.write(f'SET;DIST;{n};{dist}\n'.encode()) + self.ser.write(f"SET;DIST;{n};{dist}\n".encode()) elif msg == "GET;PID": - print(f'[TX] SET;PID;{self.pid[0]};{self.pid[1]};{self.pid[2]}') - self.ser.write(f'SET;PID;{self.pid[0]};{self.pid[1]};{self.pid[2]}\n'.encode()) + self.ser.write(f"SET;PID;{self.pid[0]};{self.pid[1]};{self.pid[2]}\n".encode()) elif msg.startswith("SET;PID"): - print(f'[TX] OK;PID;1') - self.ser.write(f'OK;PID;1\n'.encode()) + self.ser.write(b"OK;PID;1\n") elif msg.startswith("SET;START;"): self.start = msg.split(';')[2] == '1' - print(f'[TX] OK;START;1') - self.ser.write(f'OK;START;1\n'.encode()) + self.ser.write(b"OK;START;1\n") + # --- MULTI WAYPOINT (clears previous) --- elif msg.startswith("SET;WAYPOINT;"): - parts = msg.split(';') - if len(parts) >= 7: + parts = msg.split(';')[2:] + + if len(parts) % 5 != 0: + self.ser.write(b"ERR;WAYPOINT;0\n") + return + + # clear existing waypoints + self.waypoints.clear() + self.waypoint_queue.clear() + + for i in range(0, len(parts), 5): wp = { - 'id': int(parts[2]), - 'type': int(parts[3]), - 'x': float(parts[4]), - 'y': float(parts[5]), - 'theta': float(parts[6]) + 'id': int(parts[i]), + 'type': int(parts[i + 1]), + 'x': float(parts[i + 2]), + 'y': float(parts[i + 3]), + 'theta': float(parts[i + 4]), } + self.waypoints[wp['id']] = wp - if wp['id'] not in self.waypoint_order: - self.waypoint_order.append(wp['id']) - self.ser.write(f'OK;WAYPOINT;{wp["id"]}\n'.encode()) + self.waypoint_queue.append(wp['id']) - elif msg.startswith("SET;WAYPOINTS;"): - parts = msg.split(';') - if len(parts) / 7 >= 1: - self.waypoints.clear() - self.waypoint_order.clear() - for i in range(2, len(parts), 5): - wp = { - 'id': int(parts[i]), - 'type': int(parts[i + 1]), - 'x': float(parts[i + 2]), - 'y': float(parts[i + 3]), - 'theta': float(parts[i + 4]) - } - self.waypoints[wp['id']] = wp - if wp['id'] not in self.waypoint_order: - self.waypoint_order.append(wp['id']) - self.ser.write(f'OK;WAYPOINT;{wp["id"]}\n'.encode()) - self.ser.write(b'OK;WAYPOINTS;1\n') + self.ser.write(f"OK;WAYPOINT;{wp['id']}\n".encode()) elif msg.startswith("SET;POS"): parts = msg.split(';') self.x = float(parts[2]) self.y = float(parts[3]) self.theta = float(parts[4]) - print(f'[TX] OK;POS;1') - self.ser.write(b'OK;POS;1\n') + self.ser.write(b"OK;POS;1\n") def stop(self): self.running = False @@ -174,13 +168,15 @@ def stop(self): if __name__ == '__main__': parser = argparse.ArgumentParser(description="Simulated PCB") - parser.add_argument('--port', type=str, default='/dev/pts/6', help='Serial port to use') + parser.add_argument('--port', type=str, default='/dev/pts/6') args = parser.parse_args() sim = None try: sim = SimulatedPCB(port=args.port) + while True: + time.sleep(1) except KeyboardInterrupt: if sim: sim.stop() - print("Simulation stopped") \ No newline at end of file + print("Simulation stopped") diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp index f5a5b09..4e5ab2e 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp @@ -56,6 +56,7 @@ namespace Modelec rclcpp::Subscription::SharedPtr odo_add_waypoints_subscriber_; rclcpp::Subscription::SharedPtr odo_set_pos_subscriber_; rclcpp::Subscription::SharedPtr odo_set_pid_subscriber_; + rclcpp::Subscription::SharedPtr odo_ask_active_waypoint_subscriber_; rclcpp::Subscription::SharedPtr joy_subscriber_; void AddWaypointCallback(const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg); diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 00054b0..aa28560 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -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()) + ";"; diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index d535304..8c79f74 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -69,6 +69,13 @@ namespace Modelec SetPIDCallback(msg); }); + odo_ask_active_waypoint_subscriber_ = this->create_subscription( + "odometry/ask_active_waypoint", 10, + [this](const std_msgs::msg::Empty::SharedPtr) + { + GetData("WAYPOINT", {"ACTIVE"}); + }); + joy_subscriber_ = this->create_subscription( "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") { diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index bc34bf9..62a073d 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -99,7 +99,7 @@ def launch_com(context, *args, **kwargs): 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 launch_com(context, *args, **kwargs): 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 launch_joy(context, *args, **kwargs): 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 launch_joy(context, *args, **kwargs): OpaqueFunction(function=launch_joy), ]) -# to run in debug : , prefix=['xterm -e gdb -ex run --args'] \ No newline at end of file +# to run in debug : , prefix=['xterm -e gdb -ex run --args'] diff --git a/src/modelec_gui/include/modelec_gui/pages/home_page.hpp b/src/modelec_gui/include/modelec_gui/pages/home_page.hpp index 1f423dd..7de1f4e 100644 --- a/src/modelec_gui/include/modelec_gui/pages/home_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/home_page.hpp @@ -40,6 +40,7 @@ namespace ModelecGUI QSvgRenderer* renderer_; - std::vector spawn_buttons_; + QPushButton* yellow_spawn_buttons_; + QPushButton* blue_spawn_buttons_; }; } diff --git a/src/modelec_gui/src/pages/home_page.cpp b/src/modelec_gui/src/pages/home_page.cpp index a8c6e45..5bc00ac 100644 --- a/src/modelec_gui/src/pages/home_page.cpp +++ b/src/modelec_gui/src/pages/home_page.cpp @@ -12,52 +12,64 @@ namespace ModelecGUI { spawn_pub_ = node_->create_publisher("strat/spawn", 10); - auto w = Modelec::Config::get("config.spawn.width_mm"); - auto h = Modelec::Config::get("config.spawn.height_mm"); - - spawn_sub_ = node_->create_subscription("nav/spawn", 10, - [this, w, h](const modelec_interfaces::msg::Spawn::SharedPtr msg) - { - auto ratioX = 1200 / 3000.0f; - auto ratioY = 800 / 2000.0f; - - auto* button = new QPushButton(this); - spawn_buttons_.push_back(button); - - 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;" - ); - - button->move( - static_cast(msg->x * ratioX - (w * ratioX) / 2), - static_cast(800 - msg->y * ratioY - (h * ratioY) / 2) - ); - - button->setFixedSize(w * ratioX, h * ratioY); - - button->show(); - - 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(); - }); - }); + auto* layout = new QHBoxLayout(this); + layout->setContentsMargins(0, 0, 0, 0); + layout->setSpacing(0); + + yellow_spawn_buttons_ = new QPushButton("Yellow", this); + blue_spawn_buttons_ = new QPushButton("Blue", this); + + yellow_spawn_buttons_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + blue_spawn_buttons_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + + 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);" + "}" + ); + + 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);" + "}" + ); + + layout->addWidget(yellow_spawn_buttons_, 1); + layout->addWidget(blue_spawn_buttons_, 1); + + 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(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("strat/reset", 10); - - ask_spawn_client_ = node_->create_client("nav/ask_spawn"); - ask_spawn_client_->wait_for_service(); - auto ask_spawn_request_ = std::make_shared(); - 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() diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index c32d62f..9a2fe1f 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -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; } diff --git a/src/modelec_gui/src/pages/test_map_page.cpp b/src/modelec_gui/src/pages/test_map_page.cpp index b4c1f56..8d09f26 100644 --- a/src/modelec_gui/src/pages/test_map_page.cpp +++ b/src/modelec_gui/src/pages/test_map_page.cpp @@ -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; } diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index 8c8fc9f..b51d79a 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -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 $ diff --git a/src/modelec_strat/data/deposite_zone.xml b/src/modelec_strat/data/deposite_zone.xml index 99fd522..5decfea 100644 --- a/src/modelec_strat/data/deposite_zone.xml +++ b/src/modelec_strat/data/deposite_zone.xml @@ -1,32 +1,71 @@ + + -1.5708 + + + -1.5708 + + + 0 + + + 0 + 1.5708 + -1.5708 + 3.1415 + + + 0 + 1.5708 + -1.5708 + 3.1415 + + + 0 + 1.5708 + -1.5708 + 3.1415 + + + 3.1415 + + + 1.5708 + + + 1.5708 + + + 1.5708 + diff --git a/src/modelec_strat/include/modelec_strat/action/base_action.hpp b/src/modelec_strat/include/modelec_strat/action/base_action.hpp index ee727df..cb4b103 100644 --- a/src/modelec_strat/include/modelec_strat/action/base_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/base_action.hpp @@ -25,6 +25,13 @@ namespace Modelec class BaseAction { public: + enum Front + { + FRONT = 1, + BACK = 0, + BOTH = -1, + }; + using Ptr = std::shared_ptr; using FactoryFn = std::function&)>; @@ -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& params) = 0; - - // static constexpr std::string_view Name = "BaseAction"; - static Ptr CreateAction( const std::string& action_name, const std::shared_ptr& action_executor); diff --git a/src/modelec_strat/include/modelec_strat/action/down_action.hpp b/src/modelec_strat/include/modelec_strat/action/down_action.hpp index ad784e5..814e01e 100644 --- a/src/modelec_strat/include/modelec_strat/action/down_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/down_action.hpp @@ -9,17 +9,16 @@ namespace Modelec { public: DownAction(const std::shared_ptr& action_executor); - DownAction(const std::shared_ptr& action_executor, bool front); + DownAction(const std::shared_ptr& action_executor, Front front); - void Execute() override; void Next() override; void Init(const std::vector& 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 steps_; diff --git a/src/modelec_strat/include/modelec_strat/action/free_action.hpp b/src/modelec_strat/include/modelec_strat/action/free_action.hpp index 20326c2..3a4532d 100644 --- a/src/modelec_strat/include/modelec_strat/action/free_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/free_action.hpp @@ -9,19 +9,20 @@ namespace Modelec { public: FreeAction(const std::shared_ptr& action_executor); - FreeAction(const std::shared_ptr& action_executor, bool front, int n); + FreeAction(const std::shared_ptr& action_executor, Front front, int n); + FreeAction(const std::shared_ptr& action_executor, std::pair servo); + FreeAction(const std::shared_ptr& action_executor, std::vector> servos); - void Execute() override; void Next() override; void Init(const std::vector& params) override; - void SetFront(bool front); - void SetN(int n); + void AddServo(int id, Front front); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); inline static const std::string Name = ActionExec::FREE; private: - bool front_; - int n_; + std::vector> servos_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action/take_action.hpp b/src/modelec_strat/include/modelec_strat/action/take_action.hpp index 9808919..68cd7a6 100644 --- a/src/modelec_strat/include/modelec_strat/action/take_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/take_action.hpp @@ -9,19 +9,20 @@ namespace Modelec { public: TakeAction(const std::shared_ptr& action_executor); - TakeAction(const std::shared_ptr& action_executor, bool front, int n); + TakeAction(const std::shared_ptr& action_executor, Front front, int n); + TakeAction(const std::shared_ptr& action_executor, std::pair servo); + TakeAction(const std::shared_ptr& action_executor, std::vector> servos); - void Execute() override; void Next() override; void Init(const std::vector& params) override; - void SetFront(bool front); - void SetN(int n); + void AddServo(int id, Front front); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); inline static const std::string Name = ActionExec::TAKE; private: - bool front_; - int n_; + std::vector> servos_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action/up_action.hpp b/src/modelec_strat/include/modelec_strat/action/up_action.hpp index 174b21d..95b8ea8 100644 --- a/src/modelec_strat/include/modelec_strat/action/up_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/up_action.hpp @@ -9,17 +9,16 @@ namespace Modelec { public: UPAction(const std::shared_ptr& action_executor); - UPAction(const std::shared_ptr& action_executor, bool front); + UPAction(const std::shared_ptr& action_executor, Front front); - void Execute() override; void Next() override; void Init(const std::vector& 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 steps_; diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 50cd0f7..c8185f2 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -7,6 +7,11 @@ #include #include +#include + +#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 { 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> servos, bool force = false); + void Free(std::vector> servos, bool force = false); void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg); void MoveServo(const modelec_interfaces::msg::ActionServoPosArray& msg); + + std::array, 2> box_obstacles_; + protected: rclcpp::Publisher::SharedPtr asc_move_pub_; rclcpp::Publisher::SharedPtr servo_move_pub_; @@ -60,6 +57,7 @@ namespace Modelec rclcpp::Subscription::SharedPtr servo_timed_move_res_sub_; rclcpp::Subscription::SharedPtr action_exec_sub_; + rclcpp::Subscription::SharedPtr joy_sub_; std::shared_ptr action_; diff --git a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp index 5179646..1876e40 100644 --- a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp +++ b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp @@ -1,6 +1,5 @@ #pragma once -#include #include #include #include @@ -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 take_angle_; + bool has_box_ = false; }; } diff --git a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp index d56a4a3..bb741ad 100644 --- a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp @@ -8,7 +8,8 @@ namespace Modelec { class FreeMission : public Mission { public: FreeMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor); + const std::shared_ptr& action_executor, + BaseAction::Front front = BaseAction::FRONT); void Start(rclcpp::Node::SharedPtr node) override; void Update() override; @@ -20,10 +21,15 @@ namespace Modelec { enum Step { GO_TO_FREE, + DOWN, FREE, + UP, + GO_BACK, DONE, } step_; + BaseAction::Front front_; + MissionStatus status_; std::shared_ptr nav_; std::shared_ptr action_executor_; @@ -31,6 +37,8 @@ namespace Modelec { std::shared_ptr target_deposite_zone_; + double angle_; + rclcpp::Time go_timeout_; rclcpp::Time deploy_time_; }; diff --git a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp index d194339..a27b4fe 100644 --- a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp @@ -8,7 +8,8 @@ namespace Modelec { class TakeMission : public Mission { public: TakeMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor); + const std::shared_ptr& action_executor, + BaseAction::Front front = BaseAction::FRONT); void Start(rclcpp::Node::SharedPtr node) override; void Update() override; @@ -20,10 +21,15 @@ namespace Modelec { enum Step { GO_TO_TAKE, + GO_TO_TAKE_CLOSE, + DOWN, TAKE, + UP, DONE, } step_; + BaseAction::Front front_; + MissionStatus status_; std::shared_ptr nav_; std::shared_ptr action_executor_; diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 1932dab..0e0a209 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -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::SharedPtr spawn_pub_; rclcpp::Service::SharedPtr ask_spawn_srv_; - rclcpp::Publisher::SharedPtr odo_get_pos_pub_; - rclcpp::Time last_odo_get_pos_time_; + rclcpp::Publisher::SharedPtr odo_ask_waypoint_pub_; }; @@ -187,11 +188,14 @@ namespace Modelec { if (auto obs = std::dynamic_pointer_cast(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; + } } } } diff --git a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp index 4124eb8..18f5f8a 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp @@ -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 GetAllPossiblePositions() const; protected: - bool isAtObjective = false; std::vector possible_angles_; }; diff --git a/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp b/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp index 3676e1d..130c3ed 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/obstacle.hpp @@ -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; }; } diff --git a/src/modelec_strat/include/modelec_strat/pami_manager.hpp b/src/modelec_strat/include/modelec_strat/pami_manager.hpp index c3aecdb..2dee336 100644 --- a/src/modelec_strat/include/modelec_strat/pami_manager.hpp +++ b/src/modelec_strat/include/modelec_strat/pami_manager.hpp @@ -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::SharedPtr start_time_sub_; rclcpp::Publisher::SharedPtr add_obs_pub_; diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 9e9c7d8..19a6dde 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -8,15 +8,11 @@ Modelec::DownAction::DownAction(const std::shared_ptr& action_ex steps_.push(ActionExec::DONE_STEP); } -Modelec::DownAction::DownAction(const std::shared_ptr& action_executor, bool front) : DownAction(action_executor) +Modelec::DownAction::DownAction(const std::shared_ptr& 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); + + 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 = 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 = 2; + msg.items[2].start_angle = 0.3; + msg.items[2].end_angle = 0; + msg.items[2].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; + } - 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_ == 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[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[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[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[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[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[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& params) { if (!params.empty()) { - SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front"); + SetFront(static_cast(std::stoi(params[1]))); } } -void Modelec::DownAction::SetFront(bool front) +void Modelec::DownAction::SetFront(Front front) { front_ = front; } diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index ca1b78d..912194a 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -8,14 +8,19 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr& action_ex steps_.push(ActionExec::DONE_STEP); } -Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, bool front, int n) : FreeAction(action_executor) +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, Front front, int n) : FreeAction(action_executor) { - front_ = front; - n_ = n; + AddServo(n, front); } -void Modelec::FreeAction::Execute() +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::pair servo) : FreeAction(action_executor) { + AddServo(servo.first, servo.second); +} + +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::vector> 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& 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 ? FRONT : BACK); + } } } -void Modelec::FreeAction::SetFront(bool front) +void Modelec::FreeAction::AddServo(int id, Front front) +{ + servos_.emplace_back(id, front); +} + +void Modelec::FreeAction::AddServo(std::pair servo) { - front_ = front; + servos_.emplace_back(servo); } -void Modelec::FreeAction::SetN(int n) +void Modelec::FreeAction::AddServos(const std::vector>& servos) { - n_ = n; + servos_.insert(servos_.end(), servos.begin(), servos.end()); } diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index 2b36272..014e8d6 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -4,18 +4,21 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor) : BaseAction(action_executor) { - steps_.push(ActionExec::TAKE_STEP); - steps_.push(ActionExec::DONE_STEP); } -Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, bool front, int n) : TakeAction(action_executor) +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, Front front, int n) : TakeAction(action_executor) { - front_ = front; - n_ = n; + AddServo(n, front); } -void Modelec::TakeAction::Execute() +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::pair servo) : TakeAction(action_executor) { + AddServo(servo); +} + +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::vector> 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& 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 ? FRONT : BACK); + } } } -void Modelec::TakeAction::SetFront(bool front) +void Modelec::TakeAction::AddServo(int id, Front front) +{ + servos_.emplace_back(id, front); +} + +void Modelec::TakeAction::AddServo(std::pair servo) { - front_ = front; + servos_.emplace_back(servo); } -void Modelec::TakeAction::SetN(int n) +void Modelec::TakeAction::AddServos(const std::vector>& servos) { - n_ = n; + servos_.insert(servos_.end(), servos.begin(), servos.end()); } diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 6d8ca66..1cbeb21 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -8,15 +8,11 @@ Modelec::UPAction::UPAction(const std::shared_ptr& action_execut steps_.push(ActionExec::DONE_STEP); } -Modelec::UPAction::UPAction(const std::shared_ptr& action_executor, bool front) : UPAction(action_executor) +Modelec::UPAction::UPAction(const std::shared_ptr& 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); + + 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 = 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 = 2; + msg.items[2].start_angle = 0; + msg.items[2].end_angle = 0.3; + msg.items[2].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[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; + 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[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[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[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[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[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[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& params) { if (!params.empty()) { - SetFront(params[1] == "1" || params[1] == "true" || params[1] == "front"); + SetFront(static_cast(std::stoi(params[1]))); } } -void Modelec::UPAction::SetFront(bool front) +void Modelec::UPAction::SetFront(Front front) { front_ = front; } diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index ec266f8..eb2760a 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -79,6 +79,31 @@ namespace Modelec step_running_ -= msg->items.size(); Update(); }); + + joy_sub_ = node_->create_subscription( + "/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, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}}); + } + else if (msg->buttons[4] == 1) // Y button + { + Free({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}}); + } + } + }); } 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(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(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> servos, bool force) { + if (action_done_ || force) { - action_ = std::make_shared(shared_from_this(), front, n); + action_ = std::make_shared(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> servos, bool force) { + if (action_done_ || force) { - action_ = std::make_shared(shared_from_this(), front, n); + action_ = std::make_shared(shared_from_this(), servos); + if (action_done_) + { + step_running_ = 0; + } action_done_ = false; - step_running_ = 0; Update(); } diff --git a/src/modelec_strat/src/deposite_zone.cpp b/src/modelec_strat/src/deposite_zone.cpp index 64159ec..654daf5 100644 --- a/src/modelec_strat/src/deposite_zone.cpp +++ b/src/modelec_strat/src/deposite_zone.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -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::max(); + double best_angle = take_angle_.front(); + + for (const auto& angle : take_angle_) + { + Point p = Point( + static_cast(position_.x + dist * std::cos(angle)), + static_cast(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; diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index ac1d60e..a72783a 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -1,8 +1,13 @@ #include +#include "modelec_strat/action/base_action.hpp" + namespace Modelec { - FreeMission::FreeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) - : step_(GO_TO_FREE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { + FreeMission::FreeMission(const std::shared_ptr& nav, + const std::shared_ptr& action_executor, + BaseAction::Front front) + : step_(GO_TO_FREE), front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + { } void FreeMission::Start(rclcpp::Node::SharedPtr node) @@ -24,6 +29,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 +47,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, front_}, {1, front_}, {2, front_}, {3, front_}}); + deploy_time_ = node_->now(); + + auto obs = action_executor_->box_obstacles_[0]; + action_executor_->box_obstacles_[front_] = 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); - step_ = DONE; + go_timeout_ = node_->now(); } + + step_ = DONE; break; case DONE: { @@ -83,4 +131,4 @@ namespace Modelec { return status_; } -} \ No newline at end of file +} diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index 7693465..64b670a 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -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; diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index 8d9eccb..6d4d968 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -1,8 +1,13 @@ #include +#include "modelec_strat/action/base_action.hpp" + namespace Modelec { - TakeMission::TakeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) - : step_(GO_TO_TAKE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { + TakeMission::TakeMission(const std::shared_ptr& nav, + const std::shared_ptr& action_executor, + BaseAction::Front front) + : step_(GO_TO_TAKE), front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + { } void TakeMission::Start(rclcpp::Node::SharedPtr node) @@ -24,7 +29,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; } @@ -37,26 +47,75 @@ namespace Modelec { auto closestBox = nav_->GetClosestObstacle(nav_->GetCurrentPos()); + action_executor_->box_obstacles_[front_] = closestBox; + auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); - nav_->GetPathfinding()->RemoveObstacle(closestBox->GetId()); + 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); + } + } - nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL); + go_timeout_ = node_->now(); + } + + step_ = GO_TO_TAKE_CLOSE; + break; + case GO_TO_TAKE_CLOSE: + { + if (action_executor_->box_obstacles_[front_] == nullptr) + { + status_ = MissionStatus::FAILED; + break; + } + + auto pos = action_executor_->box_obstacles_[0]->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(front_); + deploy_time_ = node_->now(); + } + step_ = TAKE; break; case TAKE: { - action_executor_->Up(); + action_executor_->Take({{0, front_}, {1, front_}, {2, front_}, {3, front_}}); + deploy_time_ = node_->now(); + } + + step_ = UP; + break; + case UP: + { + action_executor_->Up(front_); deploy_time_ = node_->now(); } step_ = DONE; break; case DONE: + { + if (action_executor_->box_obstacles_[front_] == nullptr) + { + status_ = MissionStatus::FAILED; + break; + } + + nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[front_]->GetId()); + } + status_ = MissionStatus::DONE; break; default: @@ -73,4 +132,4 @@ namespace Modelec { return status_; } -} \ No newline at end of file +} diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index f61ae0d..525c249 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -97,10 +97,8 @@ namespace Modelec } }); - odo_get_pos_pub_ = node_->create_publisher( - "odometry/get/pos", 30); - - last_odo_get_pos_time_ = node_->now(); + odo_ask_waypoint_pub_ = node_->create_publisher( + "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("config.spawn.yellow.top@theta") ); - spawn_yellow_["side"] = Point( - Config::get("config.spawn.yellow.side@x"), - Config::get("config.spawn.yellow.side@y"), - Config::get("config.spawn.yellow.side@theta") - ); - - spawn_yellow_["bottom"] = Point( - Config::get("config.spawn.yellow.bottom@x"), - Config::get("config.spawn.yellow.bottom@y"), - Config::get("config.spawn.yellow.bottom@theta") - ); - spawn_blue_["top"] = Point( Config::get("config.spawn.blue.top@x"), Config::get("config.spawn.blue.top@y"), Config::get("config.spawn.blue.top@theta") ); - - spawn_blue_["side"] = Point( - Config::get("config.spawn.blue.side@x"), - Config::get("config.spawn.blue.side@y"), - Config::get("config.spawn.blue.side@theta") - ); - - spawn_blue_["bottom"] = Point( - Config::get("config.spawn.blue.bottom@x"), - Config::get("config.spawn.blue.bottom@y"), - Config::get("config.spawn.blue.bottom@theta") - ); } } \ No newline at end of file diff --git a/src/modelec_strat/src/obstacle/box.cpp b/src/modelec_strat/src/obstacle/box.cpp index b3dee90..31a29d3 100644 --- a/src/modelec_strat/src/obstacle/box.cpp +++ b/src/modelec_strat/src/obstacle/box.cpp @@ -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); diff --git a/src/modelec_strat/src/obstacle/obstacle.cpp b/src/modelec_strat/src/obstacle/obstacle.cpp index 5fbdb1f..575e524 100644 --- a/src/modelec_strat/src/obstacle/obstacle.cpp +++ b/src/modelec_strat/src/obstacle/obstacle.cpp @@ -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; + } } diff --git a/src/modelec_strat/src/pami_manager.cpp b/src/modelec_strat/src/pami_manager.cpp index adfdb6b..7b13442 100644 --- a/src/modelec_strat/src/pami_manager.cpp +++ b/src/modelec_strat/src/pami_manager.cpp @@ -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(); - } + }*/ }); } diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 316587b..b6e615e 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -7,6 +7,8 @@ #include #include +#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, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}, + {0, BaseAction::BACK}, {1, BaseAction::BACK}, {2, BaseAction::BACK}, {3, BaseAction::BACK}, + }, true); + Transition(State::WAIT_START, "System ready"); } break; @@ -199,7 +207,23 @@ namespace Modelec case State::TAKE_MISSION: if (!current_mission_) { - current_mission_ = std::make_unique(nav_, action_executor_); + if (action_executor_->box_obstacles_[BaseAction::FRONT] != nullptr) + { + if (action_executor_->box_obstacles_[BaseAction::BACK] != nullptr) + { + RCLCPP_WARN(get_logger(), "Both front and back box obstacles are occupied!"); + current_mission_.reset(); + Transition(State::SELECT_MISSION, "Cannot take box, both sides occupied"); + break; + } + + RCLCPP_INFO(get_logger(), "Front box obstacle is occupied, taking from back"); + current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::BACK); + current_mission_->Start(shared_from_this()); + break; + } + + current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::FRONT); current_mission_->Start(shared_from_this()); } current_mission_->Update(); @@ -213,7 +237,22 @@ namespace Modelec case State::FREE_MISSION: if (!current_mission_) { - current_mission_ = std::make_unique(nav_, action_executor_); + if (action_executor_->box_obstacles_[BaseAction::FRONT] == nullptr) + { + if (action_executor_->box_obstacles_[BaseAction::BACK] == nullptr) + { + RCLCPP_WARN(get_logger(), "Both front and back box obstacles are free!"); + Transition(State::SELECT_MISSION, "Cannot free box, both sides empty"); + break; + } + + RCLCPP_INFO(get_logger(), "Front box obstacle is occupied, taking from back"); + current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::BACK); + current_mission_->Start(shared_from_this()); + break; + } + + current_mission_ = std::make_unique(nav_, action_executor_, BaseAction::FRONT); current_mission_->Start(shared_from_this()); } current_mission_->Update(); diff --git a/src/modelec_utils/include/modelec_utils/point.hpp b/src/modelec_utils/include/modelec_utils/point.hpp index aa6b17c..578286f 100644 --- a/src/modelec_utils/include/modelec_utils/point.hpp +++ b/src/modelec_utils/include/modelec_utils/point.hpp @@ -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; diff --git a/src/modelec_utils/src/point.cpp b/src/modelec_utils/src/point.cpp index bfa8888..9580774 100644 --- a/src/modelec_utils/src/point.cpp +++ b/src/modelec_utils/src/point.cpp @@ -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); } }