nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Optimize path handling

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-02-09 10:42:40 +02:00
parent 6323c9eb3b
commit 5c1e41cd1c
1 changed files with 20 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
src/axidraw_controller/src/cpp/axidraw_controller.cpp
View File

@@ -143,6 +143,8 @@ class AxidrawController : public RobotController
auto p = points[i]; auto p = points[i];
long unsigned int count = 0;
// Ensure axidraw is not busy // Ensure axidraw is not busy
while (!is_ready()) while (!is_ready())
{ {
@@ -154,7 +156,14 @@ class AxidrawController : public RobotController
if (p.z > 0) if (p.z > 0)
this->penup_pub->publish(std_msgs::msg::Empty()); this->penup_pub->publish(std_msgs::msg::Empty());
else else
{
this->pendown_pub->publish(std_msgs::msg::Empty()); this->pendown_pub->publish(std_msgs::msg::Empty());
while (i + 1 < points.size() && points[i + 1].z <= 0)
{
count++;
i++;
}
}
// Wait for pen movement to complete // Wait for pen movement to complete
while (!is_ready()) while (!is_ready())
@@ -164,7 +173,17 @@ class AxidrawController : public RobotController
loop_rate.sleep(); loop_rate.sleep();
} }
if (count == 0)
this->move_pub->publish(p); this->move_pub->publish(p);
else
{
std::vector<geometry_msgs::msg::Point> ps(&points[i],&points[i+count]);
robot_interfaces::msg::Points msg;
msg.points = ps;
std::string log = "Publishing path with " + std::to_string(ps.size()) + " points";
RCLCPP_INFO(this->get_logger(), log.c_str());
this->path_pub->publish(msg);
}
// Update status // Update status
feedback->status = std::to_string(i+1) + "/" + std::to_string(points.size()) + " complete"; feedback->status = std::to_string(i+1) + "/" + std::to_string(points.size()) + " complete";