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

Co-authored-by: Modelec <modelec.isen@gmail.com>

Author: Ackimixs

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

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;

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

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")
+        print("Simulation stopped")

src/modelec_com/include/modelec_com/pcb_odo_interface.hpp

@@ -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);

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()) + ";";

src/modelec_com/src/pcb_odo_interface.cpp

@@ -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")
             {