Skip to main content

Quick Start: Your First Robot Program

Connect to a real Neuromeka Indy7 robot and run your first hardware control program with PLEM.

Prerequisites:

  • W-RC with PLEM installed
  • Neuromeka Indy7 robot powered ON
  • EtherCAT cable connected between control PC and robot base

Estimated time: 15 minutes


Safety Precautions

Working with industrial manipulators requires strict safety protocols.

Required Safety Requirements
  1. Emergency Stop Button: Position an emergency stop button within arm's reach and verify it works.
  2. Clear Workspace: Remove all obstacles and personnel from the robot's workspace.
  3. Low-Speed Testing: Use low speed limits (10-20%) on first runs.
  4. Never Leave Unattended: An operator must always monitor the robot during operation.

Pre-Operation Checklist

  • Emergency stop button is within reach and functional
  • Workspace is clear of obstacles and personnel
  • Robot power cable and EtherCAT cable are properly connected
  • Low speed setting prepared for first run
warning

If you are unsure about safety, consult a certified robot integrator before proceeding.


Step 1: Create Project

Project Structure

quickstart_hardware/
├── CMakeLists.txt
├── package.xml
└── src/
└── main.cpp

Prepare Workspace

You need a colcon workspace to build user packages. A workspace follows the ROS2 standard structure where source packages are placed in the src/ directory and colcon build is run from the root.

# Create workspace (you can change the name freely)
mkdir -p ~/robot_ws/src
info

The PLEM library stack (plem_core, plem_robot, etc.) must already be installed on W-RC. To verify installation, refer to the Installation Guide.

Create Package

cd ~/robot_ws/src
ros2 pkg create quickstart_hardware --build-type ament_cmake --dependencies plem_robot plem_core
cd quickstart_hardware

CMakeLists.txt

File: CMakeLists.txt

cmake_minimum_required(VERSION 3.25)
project(quickstart_hardware)

# C++20 required
set(CMAKE_CXX_STANDARD 20)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

# Find dependencies
find_package(ament_cmake REQUIRED)
find_package(plem_core REQUIRED)
find_package(plem_robot REQUIRED)

# Executable
add_executable(quickstart_hardware_node src/main.cpp)

# Link libraries
target_link_libraries(quickstart_hardware_node
plem_core::plem_core
plem_robot::plem_robot
)

# Install
install(TARGETS quickstart_hardware_node
DESTINATION lib/${PROJECT_NAME}
)

ament_package()

package.xml

File: package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>quickstart_hardware</name>
<version>0.1.0</version>
<description>PLEM hardware quickstart example</description>
<maintainer email="you@example.com">Your Name</maintainer>
<license>Apache-2.0</license>

<buildtool_depend>ament_cmake</buildtool_depend>
<depend>plem_core</depend>
<depend>plem_robot</depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>

Step 2: Write Code

File: src/main.cpp

#include <chrono>
#include <csignal>
#include <iostream>
#include <memory>
#include <thread>

#include <plem/core/logging/logger.hpp>
#include <plem/robot/models/indy7.hpp>
#include <plem/robot/robot_builder.hpp>

// Global robot pointer for signal handler
std::unique_ptr<plem::robot::IRobot> g_robot;

void signal_handler(int signal) {
if (signal == SIGINT) {
plem::core::logger.info("Caught SIGINT, shutting down...");
if (g_robot) {
g_robot->stop();
}
}
}

int main(int argc, char* argv[]) {
using namespace plem;

// Parse command line arguments
std::string interface = "eth0"; // Default EtherCAT network interface for W-RC
if (argc > 1) {
interface = argv[1];
}

// Initialize logging
core::logger.set_level(spdlog::level::info);
core::logger.info("=== PLEM Hardware Quick Start ===");
core::logger.info("Using network interface: {}", interface);

// Build robot
core::logger.info("Creating robot...");
auto result = robot::Robot::builder<robot::models::Indy7>()
.with_network_interface(interface)
.build();

if (!result) {
core::logger.error("Failed to create robot: {}", result.message());
core::logger.error("WHAT: Robot initialization failed");
core::logger.error("WHY: {}", result.message());
core::logger.error("HOW: Check network interface name and EtherCAT cable");
return 1;
}
g_robot = std::move(result).value();
core::logger.info("Robot created successfully");

// Register signal handler
std::signal(SIGINT, signal_handler);

// Start RT thread
core::logger.info("Starting robot...");
auto rc = g_robot->start();
if (rc != 0) {
core::logger.error("Failed to start robot (code: {})", rc);
core::logger.error("WHAT: RT thread initialization failed");
core::logger.error("WHY: Possible permission issue or EtherCAT master busy");
core::logger.error("HOW: Check 'ethercat' group membership and /dev/EtherCAT0");
return 1;
}
core::logger.info("RT threads started");

// Wait for servo drives to initialize
core::logger.info("Waiting for servo drives to initialize (2 seconds)...");
std::this_thread::sleep_for(std::chrono::seconds(2));

// Brake release countdown
core::logger.warn("=== Releasing brakes in 3 seconds ===");
core::logger.warn("Press Ctrl+C now if workspace is not safe!");
std::this_thread::sleep_for(std::chrono::seconds(3));

// Release brakes
core::logger.info("Releasing brakes...");
g_robot->release_brake();

// Wait for servo activation to complete
std::this_thread::sleep_for(std::chrono::seconds(2));

core::logger.info("Brakes released. Robot is now active.");
core::logger.info("Robot mode: {}", static_cast<int>(g_robot->mode()));

// Wait for user
core::logger.info("Robot is running. Press Enter to shutdown...");
std::cin.get();

// Engage brakes before shutdown
core::logger.info("Engaging brakes...");
g_robot->engage_brake();
std::this_thread::sleep_for(std::chrono::seconds(1));

// Stop
core::logger.info("Stopping robot...");
g_robot->stop();
g_robot.reset();

core::logger.info("Shutdown complete");
return 0;
}

Code Explanation

Robot::builder Pattern

auto result = robot::Robot::builder<robot::models::Indy7>()
.with_network_interface(interface)
.build();
  • Robot::builder<T>(): Type-safe builder pattern
  • with_network_interface(): Set physical NIC name where EtherCAT is connected (e.g., "enp2s0")
  • with_robot_id(): Unique ID for IPC/ROS2 namespace isolation in multi-robot environments (optional)
  • build(): Returns Result<std::unique_ptr<IRobot>>
Important

network_interface is the physical network interface name where EtherCAT is connected. EtherCAT uses Layer 2 (raw Ethernet) communication, so no IP address configuration is needed.

Common interface names: eth0, enp2s0, eno1

Checking Network Interface

To find the interface where the EtherCAT cable is connected:

ip link show
# Example output:
# 2: eth0: <BROADCAST,MULTICAST,UP,LOWER_UP> mtu 1500 ...

Or check EtherCAT slaves:

sudo ethercat slaves
# Expected output:
# 0 0:0 PREOP + Neuromeka Indy7 Joint0
# 1 0:1 PREOP + Neuromeka Indy7 Joint1
# ...

Signal Handler

Handle SIGINT (Ctrl+C) to ensure graceful shutdown:

void signal_handler(int signal) {
if (signal == SIGINT) {
if (g_robot) {
g_robot->stop(); // Terminate RT thread
}
}
}

Brake Control

g_robot->release_brake();  // Activate servo (CiA 402 state transition)
g_robot->engage_brake(); // Deactivate servo

When releasing brakes, the CiA 402 state machine transitions in the following sequence:

SWITCH_ON_DISABLED → READY_TO_SWITCH_ON → SWITCHED_ON → OPERATION_ENABLED
Production Executable

This example is for minimal operation verification. For writing production executables with ROS2 topic monitoring (/rt_raw, /joint_states), refer to the Robot Integration Guide.


Step 3: Build and Run

Build

cd ~/robot_ws
colcon build --packages-select quickstart_hardware
source install/setup.bash

Run

# Pass interface name as argument
ros2 run quickstart_hardware quickstart_hardware_node eth0

Or use the default value ("eth0" in code):

ros2 run quickstart_hardware quickstart_hardware_node

Next Steps

Go to the Robot Integration Guide to learn how to integrate your robot with PLEM.

If you're using an already integrated robot:

Safety Reminder

Always test complex motions in simulation first, and start with low speeds on hardware.


Summary

What you learned:

  • Write a minimal hardware control program with PLEM
  • Create a robot instance using the Robot::builder pattern
  • Execute brake release sequence safely
  • Troubleshoot common EtherCAT issues

Key Takeaways:

  • network_interface is the physical NIC name where EtherCAT is connected (no IP configuration needed)
  • EtherCAT is Layer 2 communication (raw Ethernet)
  • Brake release goes through CiA 402 state transitions (takes 1-2 seconds)
  • Always engage brakes before shutdown