From e8aad7984430fb46b608dacfd22aad297bfa6ba6 Mon Sep 17 00:00:00 2001
From: Michael Ferguson <mfergs7@gmail.com>
Date: Fri, 7 Apr 2023 15:50:41 -0400
Subject: [PATCH] tablebot: onsite debugging
* deal with laser noise by filtering noisy points
* deal with laser noise by looking only away from robot for goal
* add version number to make it easier to know if upload worked
---
projects/tablebot/behaviors.hpp | 6 +++---
projects/tablebot/main.cpp | 1 +
projects/tablebot/segmentation.hpp | 24 +++++++++++++++++++++---
projects/tablebot/tablebot.hpp | 1 +
projects/tablebot/tablebot.py | 7 +++++++
5 files changed, 33 insertions(+), 6 deletions(-)
diff --git a/projects/tablebot/behaviors.hpp b/projects/tablebot/behaviors.hpp
index 2d70c6d..3283285 100644
--- a/projects/tablebot/behaviors.hpp
+++ b/projects/tablebot/behaviors.hpp
@@ -302,7 +302,7 @@ bool verify_neck(uint32_t * last_stable_stamp, uint32_t stop_time = 250)
int is_table_present()
{
// Inspect only points right in front of us
- int points_ct = project_points(line_points, MAX_POINTS, true, 0.1f, -0.1f, 0.2f);
+ int points_ct = project_points(line_points, MAX_POINTS, true, 0.0f, 3.0f, 0.1f, -0.1f, 0.2f);
if (points_ct < 5)
{
// Not enough - call table missing
@@ -522,7 +522,7 @@ void run_behavior(uint16_t id, uint32_t stamp)
}
// Find all points on top of the table, less than 0.5m to either side of the robot
- int points_ct = project_points(line_points, MAX_POINTS, true, 0.5f, 0.0f, 0.2f);
+ int points_ct = project_points(line_points, MAX_POINTS, true, 0.0f, 3.0f, 0.5f, 0.0f, 0.2f);
// Send those points for debugging
udp_send_packet((unsigned char *) &line_points, points_ct * 12, return_port, PACKET_PROJECTED_POINTS);
@@ -710,7 +710,7 @@ void run_behavior(uint16_t id, uint32_t stamp)
}
// Find all points on top of the table, less than 0.5m to either side of the robot
- int points_ct = project_points(line_points, MAX_POINTS, true, 0.5f, 0.0f, 0.2f);
+ int points_ct = project_points(line_points, MAX_POINTS, true, 0.15f, 3.0f, 0.5f, 0.0f, 0.2f);
// Send those points for debugging
udp_send_packet((unsigned char *) &line_points, points_ct * 12, return_port, PACKET_PROJECTED_POINTS);
diff --git a/projects/tablebot/main.cpp b/projects/tablebot/main.cpp
index 3f7d8da..2ff5908 100644
--- a/projects/tablebot/main.cpp
+++ b/projects/tablebot/main.cpp
@@ -177,6 +177,7 @@ int main(void)
system_state.pose_y = 0.0f;
system_state.pose_th = 0.0f;
system_state.last_motor_command = 0;
+ system_state.version = 100;
// Setup drive motors
m1_pid.set_max_step(10);
diff --git a/projects/tablebot/segmentation.hpp b/projects/tablebot/segmentation.hpp
index 3a505e0..9ceb0bf 100644
--- a/projects/tablebot/segmentation.hpp
+++ b/projects/tablebot/segmentation.hpp
@@ -50,7 +50,7 @@ typedef struct
// Projects the points from the assembler
// Returns the number of points projected
int project_points(point_t * points, int max_points, bool project_neck,
- float limit_y, float min_z, float max_z)
+ float min_x, float max_x, float limit_y, float min_z, float max_z)
{
float cos_neck = 1.0f, sin_neck = 0.0f;
if (project_neck)
@@ -86,8 +86,9 @@ int project_points(point_t * points, int max_points, bool project_neck,
float xx = cos_neck * x;
float zz = -sin_neck * x + 0.127f; // Neck is 5" off ground
- if (zz < max_z && zz > min_z &&
- y > -limit_y && y < limit_y && x > 0.0f)
+ if (xx < max_x && xx > min_x &&
+ zz < max_z && zz > min_z &&
+ y > -limit_y && y < limit_y)
{
points[points_idx].x = xx;
points[points_idx].y = y;
@@ -133,6 +134,23 @@ int extract_segments(point_t * points, int num_points,
}
else
{
+ // If we skip this point (as noise) does the segment continue?
+ int next = i + 1;
+ if (next < num_points)
+ {
+ dx = segments[segment_idx].end.x - points[next].x;
+ dy = segments[segment_idx].end.y - points[next].y;
+ d = dx * dx + dy * dy;
+ if (d < jump_distance_squared)
+ {
+ // Add to current segment
+ segments[segment_idx].end = points[i];
+ ++segments[segment_idx].points;
+ continue;
+ }
+ }
+
+ // Can't make the segment continue
if (++segment_idx >= max_segments)
{
// Out of segments to fill
diff --git a/projects/tablebot/tablebot.hpp b/projects/tablebot/tablebot.hpp
index 5d392ea..fc60a01 100644
--- a/projects/tablebot/tablebot.hpp
+++ b/projects/tablebot/tablebot.hpp
@@ -202,6 +202,7 @@ typedef struct
float target_dist;
float target_yaw;
+ uint32_t version;
uint32_t last_motor_command;
} system_state_t;
diff --git a/projects/tablebot/tablebot.py b/projects/tablebot/tablebot.py
index 6303883..dc3fe5f 100755
--- a/projects/tablebot/tablebot.py
+++ b/projects/tablebot/tablebot.py
@@ -57,6 +57,8 @@ class TableBotGUI:
goal_pose_y = 0
goal_pose_z = 0
+ version = None
+
# Table size in meters
# TODO: read this from firmware
TABLE_LENGTH = 1.2192
@@ -228,6 +230,11 @@ def handle_packets(self):
self.target_pose = struct.unpack_from("<f", packet, 96)[0]
self.target_yaw = struct.unpack_from("<f", packet, 100)[0]
+ version = struct.unpack_from("<L", packet, 104)[0]
+ if self.version != version:
+ self.version = version
+ print("Version: %i" % version)
+
if len(self.pose_x) == 0 or \
abs(pose_x - self.pose_x[-1]) > 0.01 or \
abs(pose_y - self.pose_y[-1]) > 0.01 or \