This example demonstrates how to use the Magicbot-Z1 SDK for initialization, robot connection, robot sensor control (lidar, RGBD camera, binocular camera) and other basic operations.

C++

Example file: sensor_example.cpp

Reference documentation: sensor_reference.md

#include "magic_robot.h"
#include "magic_type.h"

#include <unistd.h>
#include <csignal>

#include <iostream>

using namespace magic::z1;

magic::z1::MagicRobot robot;

void signalHandler(int signum) {
  std::cout << "Interrupt signal (" << signum << ") received.\n";

  robot.Shutdown();
  // Exit process
  exit(signum);
}

int main() {
  // Bind SIGINT (Ctrl+C)
  signal(SIGINT, signalHandler);

  std::string local_ip = "192.168.54.111";
  // Configure local IP address for direct network connection and initialize SDK
  if (!robot.Initialize(local_ip)) {
    std::cerr << "robot sdk initialize failed." << std::endl;
    robot.Shutdown();
    return -1;
  }

  // Set RPC timeout to 10 seconds
  robot.SetTimeout(10000);

  // Connect to robot
  auto status = robot.Connect();
  if (status.code != ErrorCode::OK) {
    std::cerr << "connect robot failed"
              << ", code: " << status.code
              << ", message: " << status.message << std::endl;
    robot.Shutdown();
    return -1;
  }

  auto& controller = robot.GetSensorController();

  // Open LiDAR
  status = controller.OpenLidar();
  if (status.code != ErrorCode::OK) {
    std::cerr << "open lidar failed"
              << ", code: " << status.code
              << ", message: " << status.message << std::endl;
    robot.Shutdown();
    return -1;
  }

  // Subscribe to LiDAR IMU data
  controller.SubscribeLidarImu([](const std::shared_ptr<Imu> msg) {
    std::cout << "++++++++++ receive lidar imu." << std::endl;

    std::cout << "lidar imu timestamp: " << msg->timestamp << std::endl;
    std::cout << "lidar imu orientation: " << msg->orientation[0] << ", " << msg->orientation[1] << ", " << msg->orientation[2] << ", " << msg->orientation[3] << std::endl;
    std::cout << "lidar imu angular_velocity: " << msg->angular_velocity[0] << ", " << msg->angular_velocity[1] << ", " << msg->angular_velocity[2] << std::endl;
    std::cout << "lidar imu linear_acceleration: " << msg->linear_acceleration[0] << ", " << msg->linear_acceleration[1] << ", " << msg->linear_acceleration[2] << std::endl;
    std::cout << "lidar imu temperature: " << msg->temperature << std::endl;
  });

  // Subscribe to LiDAR PointCloud data
  controller.SubscribeLidarPointCloud([](const std::shared_ptr<PointCloud2> msg) {
    std::cout << "++++++++++ receive lidar point cloud." << std::endl;
    // TODO: handle lidar point cloud data
    std::cout << "lidar point cloud size: " << msg->data.size() << std::endl;
    std::cout << "lidar point cloud width: " << msg->width << std::endl;
    std::cout << "lidar point cloud height: " << msg->height << std::endl;
    std::cout << "lidar point cloud is_dense: " << msg->is_dense << std::endl;
    std::cout << "lidar point cloud is_bigendian: " << msg->is_bigendian << std::endl;
    std::cout << "lidar point cloud point_step: " << msg->point_step << std::endl;
    std::cout << "lidar point cloud row_step: " << msg->row_step << std::endl;
    std::cout << "lidar point cloud fields: " << msg->fields.size() << std::endl;
    std::cout << "lidar point cloud fields name: " << msg->fields[0].name << std::endl;
    std::cout << "lidar point cloud fields offset: " << msg->fields[0].offset << std::endl;
    std::cout << "lidar point cloud fields datatype: " << msg->fields[0].datatype << std::endl;
    std::cout << "lidar point cloud fields count: " << msg->fields[0].count << std::endl;
  });

  usleep(20000000);

  // Close LiDAR
  status = controller.CloseLidar();
  if (status.code != ErrorCode::OK) {
    std::cerr << "close lidar failed"
              << ", code: " << status.code
              << ", message: " << status.message << std::endl;
    robot.Shutdown();
    return -1;
  }

  // Disconnect from robot
  status = robot.Disconnect();
  if (status.code != ErrorCode::OK) {
    std::cerr << "disconnect robot failed"
              << ", code: " << status.code
              << ", message: " << status.message << std::endl;
    robot.Shutdown();
    return -1;
  }

  robot.Shutdown();

  return 0;
}

Python

Example file: sensor_example.py

Reference documentation: sensor_reference.md

Code Example

#!/usr/bin/env python3

import sys
import time
import signal
import logging
from typing import Optional

import magicbot_z1_python as magicbot

# Configure logging format and level
logging.basicConfig(
    level=logging.INFO,  # Minimum log level
    format="%(asctime)s [%(levelname)s] %(message)s",
    datefmt="%Y-%m-%d %H:%M:%S",
)


# Global variables
robot: Optional[magicbot.MagicRobot] = None


def signal_handler(signum, frame):
    """Signal handler function for graceful exit"""
    global robot
    logging.info(f"Received interrupt signal ({signum}), exiting...")
    if robot:
        robot.disconnect()
        logging.info("Robot disconnected")
        robot.shutdown()
        logging.info("Robot shutdown")
    exit(-1)


imu_counter = 0
def lidar_imu_callback(imu_data):
    """LiDAR IMU data callback function"""
    global imu_counter
    imu_counter += 1
    if imu_counter % 100 == 0:
        logging.info("+++++++++++++ Received LiDAR IMU data")
        logging.info(
            "Received LiDAR IMU data, counter: %d, timestamp: %d",
            imu_counter,
            imu_data.timestamp,
        )
        logging.info("Received LiDAR IMU data, orientation: %s", imu_data.orientation)
        logging.info(
            "Received LiDAR IMU data, angular_velocity: %s", imu_data.angular_velocity
        )
        logging.info(
            "Received LiDAR IMU data, linear_acceleration: %s",
            imu_data.linear_acceleration,
        )
        logging.info("Received LiDAR IMU data, temperature: %s", imu_data.temperature)


point_cloud_counter = 0
def lidar_point_cloud_callback(point_cloud_data):
    """LiDAR point cloud data callback function"""
    global point_cloud_counter
    point_cloud_counter += 1
    logging.info("Received LiDAR point cloud data, counter: %d", point_cloud_counter)


def main():
    """Main function"""
    global robot

    # Bind signal handler
    signal.signal(signal.SIGINT, signal_handler)

    logging.info("Robot model: %s", magicbot.get_robot_model())

    # Create robot instance
    robot = magicbot.MagicRobot()

    try:
        # Configure local IP address for direct network connection and initialize SDK
        local_ip = "192.168.54.111"
        if not robot.initialize(local_ip):
            logging.error("Failed to initialize robot SDK")
            robot.shutdown()
            return -1

        # Set RPC timeout to 10 seconds
        robot.set_timeout(10000)
        
        # Connect to robot
        status = robot.connect()
        if status.code != magicbot.ErrorCode.OK:
            logging.error(
                "Failed to connect to robot, code: %s, message: %s",
                status.code,
                status.message,
            )
            robot.shutdown()
            return -1

        logging.info("Successfully connected to robot")

        # Get sensor controller
        controller = robot.get_sensor_controller()

        # Open LiDAR
        status = controller.open_lidar()
        if status.code != magicbot.ErrorCode.OK:
            logging.error(
                "Failed to open LiDAR, code: %s, message: %s",
                status.code,
                status.message,
            )
            robot.shutdown()
            return -1

        logging.info("LiDAR opened successfully")

        # Subscribe to LiDAR IMU data
        controller.subscribe_lidar_imu(lidar_imu_callback)
        logging.info("Subscribed to LiDAR IMU data")

        # Subscribe to LiDAR point cloud data
        controller.subscribe_lidar_point_cloud(lidar_point_cloud_callback)
        logging.info("Subscribed to LiDAR point cloud data")

        # Wait 20 seconds to receive data
        logging.info("Waiting 20 seconds to receive sensor data...")
        time.sleep(20)

        # Close LiDAR
        status = controller.close_lidar()
        if status.code != magicbot.ErrorCode.OK:
            logging.error(
                "Failed to close LiDAR, code: %s, message: %s",
                status.code,
                status.message,
            )
            robot.shutdown()
            return -1

        logging.info("LiDAR closed successfully")

    except Exception as e:
        logging.error("Exception occurred during program execution: %s", e)
        return -1

    finally:
        # Clean up resources
        try:
            logging.info("Clean up resources")
            # Close sensor controller
            sensor_controller = robot.get_sensor_controller()
            sensor_controller.shutdown()
            logging.info("Sensor controller closed")

            # Disconnect
            robot.disconnect()
            logging.info("Robot connection disconnected")

            # Shutdown robot
            robot.shutdown()
            logging.info("Robot shutdown")

        except Exception as e:
            logging.error("Exception occurred while cleaning up resources: %s", e)


if __name__ == "__main__":
    sys.exit(main())

Running Instructions

1. Environment Setup:

# Set environment variables
export PYTHONPATH=/opt/magic_robotics/magicbot_z1_sdk/lib:$PYTHONPATH
export LD_LIBRARY_PATH=/opt/magic_robotics/magicbot_z1_sdk/lib:$LD_LIBRARY_PATH

2. Run Examples:

# C++
./sensor_example
# Python
python3 sensor_example.py

3. Stop Program:

• Press Ctrl+C to safely stop the program
• The program will automatically clean up all resources