Parsec

The Parsec node reads the multi-zone depth sensor and publishes distance data across a configurable 4×4 or 8×8 zone grid. Each zone returns a distance from 2 cm to 4 m. Data publishes in three formats simultaneously — a raw zone array, a grid image, and a 3D point cloud — select only the outputs your application needs.

Zone value encoding (mm):

• 0 – 4000 — valid distance
• -1 — zone disabled
• -2 — invalid / no return

Configuration

parsec:
  enabled: true
  sensors: ["parsec"]
  parsec:
    transport: can              # "can" or "usb"
    can_id: 5                   # CAN transport only
    serial_port: /dev/ttyACM0   # USB transport only
    frame_id: "parsec_link"
    publish_rate_hz: 15
    publish_outputs: ["zones", "grid", "pointcloud"]

Parameters

Parameter	Description
transport	Interface: can or usb
can_id	CAN bus device ID (CAN transport only)
serial_port	USB serial port path (USB transport only, default /dev/ttyACM0)
frame_id	TF frame ID included in the message header
publish_rate_hz	Output rate in Hz — max 60 Hz at 4×4, max 15 Hz at 8×8
publish_outputs	Topics to publish: any combination of zones, grid, pointcloud

Topics

Published

Topic	Type	Description	Rate
/<name>/zones	studica_control/ParsecZoneMsg	Flat distance array in mm, one value per zone	publish_rate_hz
/<name>/grid	sensor_msgs/Image (16UC1)	Zone grid as an image — each pixel holds a uint16 distance in mm	publish_rate_hz
/<name>/pointcloud	sensor_msgs/PointCloud2	3D points in metres, one per valid zone, x/y estimated from grid position	publish_rate_hz
/<name>/min_range	sensor_msgs/Range	Nearest valid zone distance in metres — always published	publish_rate_hz

note

All four topics carry the same underlying zone grid in different ROS message formats. Enable only the outputs your application consumes.

Decoding the Grid Image

Each pixel in the 16UC1 image holds a uint16 distance in millimetres. Use cv_bridge to convert:

from cv_bridge import CvBridge
import numpy as np

bridge = CvBridge()
img_mm = bridge.imgmsg_to_cv2(msg, desired_encoding='16UC1')  # numpy uint16, shape (H, W)

To decode manually without cv_bridge:

# pixel at index i (0 .. width*height - 1)
mm = msg.data[2 * i] + (msg.data[2 * i + 1] << 8)

Service

Service name: /<name>/parsec_cmd
Type: studica_control/SetData

Command	Description
get_config	Returns device settings as a string
get_zone_distance	Distance of a specific zone in mm — pass zone index in initparams.n_encoder
get_min_distance	Distance of the nearest valid zone in mm

Example

Python

#!/usr/bin/env python3
"""Parsec — subscribe to multi-zone ToF readings (100 frames, then exit).

Run:  ros2 run studica_control parsec_example.py
Requires: studica_launch.py running, parsec enabled in params.yaml
          sensors: ["parsec"]  (name must match SENSOR below)
"""
import math

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Range
from studica_control.msg import ParsecZoneMsg
from studica_control.srv import SetData

SENSOR = 'parsec'
MAX_PRINTS = 100


def _zone_cell(d: int) -> str:
    if d == -1:
        return '    off'
    if d == -2 or d < 1 or d > 4000:
        return '    ---'
    return f'{d:4d} mm'


class ParsecExample(Node):
    def __init__(self):
        super().__init__('parsec_example')
        self.create_subscription(ParsecZoneMsg, f'/{SENSOR}/zones', self.on_zones, 10)
        self.create_subscription(Range, f'/{SENSOR}/min_range', self.on_min_range, 10)
        self.client = self.create_client(SetData, f'/{SENSOR}/parsec_cmd')
        self.create_timer(5.0, self.poll_service)
        self.print_count = 0

    def on_zones(self, msg: ParsecZoneMsg):
        side = 8 if msg.zones == 64 else 4
        center = msg.zones // 2
        center_mm = msg.fdist[center] if center < len(msg.fdist) else -2
        self.get_logger().info(
            f'{self.print_count:3d} | seq={msg.seq} count={msg.zones} center={center_mm} mm'
        )
        for row in range(side):
            row_vals = msg.fdist[row * side:(row + 1) * side]
            if not row_vals:
                break
            self.get_logger().info('  ' + ' '.join(_zone_cell(d) for d in row_vals))

        self.print_count += 1
        if self.print_count >= MAX_PRINTS:
            rclpy.shutdown()

    def on_min_range(self, msg: Range):
        if math.isinf(msg.range):
            self.get_logger().warn('no valid zones in frame')
        else:
            self.get_logger().info(f'nearest valid zone: {msg.range:.3f} m')

    def poll_service(self):
        if not self.client.wait_for_service(timeout_sec=1.0):
            return
        req = SetData.Request()
        req.params = 'get_min_distance'
        future = self.client.call_async(req)
        future.add_done_callback(lambda f: self.get_logger().info(
            f'get_min_distance: {f.result().message} mm'
        ))


def main():
    rclpy.init()
    rclpy.spin(ParsecExample())
    rclpy.shutdown()

C++

/*
 * parsec_example.cpp
 *
 * Subscribe to multi-zone ToF readings published by the Parsec component.
 * Prints nearest valid distance and a compact zone grid for 100 frames, then exits.
 * Polls get_min_distance via service every 5 seconds.
 *
 * Run:      ros2 run studica_control parsec_example
 * Requires: studica_launch.py running, parsec enabled in params.yaml
 */

#include <cmath>
#include <cstdio>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/range.hpp"
#include "studica_control/msg/parsec_zone_msg.hpp"
#include "studica_control/srv/set_data.hpp"

using namespace std::chrono_literals;

static const std::string SENSOR = "parsec";
static constexpr int kMaxPrints = 100;

class ParsecExample : public rclcpp::Node {
public:
    ParsecExample() : Node("parsec_example") {
        zones_sub_ = create_subscription<studica_control::msg::ParsecZoneMsg>(
            "/" + SENSOR + "/zones", 10,
            std::bind(&ParsecExample::on_zones, this, std::placeholders::_1));

        range_sub_ = create_subscription<sensor_msgs::msg::Range>(
            "/" + SENSOR + "/min_range", 10,
            std::bind(&ParsecExample::on_min_range, this, std::placeholders::_1));

        client_ = create_client<studica_control::srv::SetData>("/" + SENSOR + "/parsec_cmd");
        poll_timer_ = create_wall_timer(5s, std::bind(&ParsecExample::poll_service, this));
    }

private:
    static bool is_valid_mm(int16_t d) { return d >= 1 && d <= 4000; }

    void on_zones(const studica_control::msg::ParsecZoneMsg::SharedPtr msg) {
        const int side = (msg->zones == 64) ? 8 : 4;
        const int center = msg->zones / 2;
        const int16_t center_mm = (center < (int)msg->fdist.size()) ? msg->fdist[center] : -2;

        RCLCPP_INFO(get_logger(), "%3d | seq=%u zones=%u center=%d mm",
                    print_count_, msg->seq, msg->zones, center_mm);

        for (int row = 0; row < side; ++row) {
            std::string line = "  ";
            for (int col = 0; col < side; ++col) {
                const int16_t d = msg->fdist[row * side + col];
                char cell[16];
                if (d == -1)       std::snprintf(cell, sizeof(cell), "%7s", "off");
                else if (!is_valid_mm(d)) std::snprintf(cell, sizeof(cell), "%7s", "---");
                else               std::snprintf(cell, sizeof(cell), "%4d mm", d);
                line += cell;
                if (col + 1 < side) line += ' ';
            }
            RCLCPP_INFO(get_logger(), "%s", line.c_str());
        }

        if (++print_count_ >= kMaxPrints) rclcpp::shutdown();
    }

    void on_min_range(const sensor_msgs::msg::Range::SharedPtr msg) {
        if (std::isinf(msg->range))
            RCLCPP_WARN(get_logger(), "no valid zones in frame");
        else
            RCLCPP_INFO(get_logger(), "nearest valid zone: %.3f m", msg->range);
    }

    void poll_service() {
        if (!client_->wait_for_service(1s)) return;
        auto req = std::make_shared<studica_control::srv::SetData::Request>();
        req->params = "get_min_distance";
        client_->async_send_request(req,
            [this](rclcpp::Client<studica_control::srv::SetData>::SharedFuture f) {
                RCLCPP_INFO(get_logger(), "get_min_distance: %s mm", f.get()->message.c_str());
            });
    }

    rclcpp::Subscription<studica_control::msg::ParsecZoneMsg>::SharedPtr zones_sub_;
    rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr range_sub_;
    rclcpp::Client<studica_control::srv::SetData>::SharedPtr client_;
    rclcpp::TimerBase::SharedPtr poll_timer_;
    int print_count_{0};
};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ParsecExample>());
    rclcpp::shutdown();
    return 0;
}