robotics
Version:
Robotics.dev P2P ROS2 robot controller CLI with ROS telemetry and video streaming
361 lines (301 loc) • 12.3 kB
JavaScript
import {createRequire } from "module";
const require = createRequire(import.meta.url);
import Configstore from 'configstore';
const config = new Configstore('robotics');
var robotId = config.get('ROBOT_ID');
var apiToken = config.get('API_TOKEN');
const rclnodejs = require('rclnodejs');
const gpio = require('@iiot2k/gpiox');
const math = require('mathjs');
// Default robot configuration
let TICKS_PER_REVOLUTION = 1992 * 2;
let DISTANCE_BETWEEN_WHEELS_M = 0.26;
let WHEEL_CIRCUMFERENCE_MM = 217;
const MAX_SPEED = 0.3;
const MAX_PWM = 100;
// Fetch robot details
async function fetchRobotDetails() {
try {
const response = await fetch(`https://robotics.dev/robot/${robotId}`, {
headers: {
"Content-Type": "application/json",
"api_token": apiToken
}
});
const data = await response.json();
console.log('Robot Details:', data);
if (data.wheelBetween) {
DISTANCE_BETWEEN_WHEELS_M = data.wheelBetween;
} else {
console.error('Warning: wheelBetween not found in robot details, using default value');
}
if (data.wheelDiameter) {
WHEEL_CIRCUMFERENCE_MM = data.wheelDiameter * Math.PI;
} else {
console.error('Warning: wheelDiameter not found in robot details, using default value');
}
if (data.ticksPerRevolution) {
TICKS_PER_REVOLUTION = data.ticksPerRevolution;
} else {
console.error('Warning: ticksPerRevolution not found in robot details, using default value');
}
} catch (error) {
console.error('Error fetching robot details:', error);
}
}
// Checks for --pins and if it has a value
const pinsIndex = process.argv.indexOf('--pins');
let pinsValue;
let pins;
if (pinsIndex > -1) {
pinsValue = process.argv[pinsIndex + 1];
pins = (pinsValue.split(","))
} else {
pins = "27,22,17,18".split(",");
}
console.log('Pins:', `${pins}`);
// Motor pin configuration
const LEFT_MOTOR_PIN1 = pins[0];
const LEFT_MOTOR_PIN2 = pins[1];
const RIGHT_MOTOR_PIN1 = pins[2];
const RIGHT_MOTOR_PIN2 = pins[3];
// Encoder pin configuration
const LEFT_ENCODER_GPIO = 37;
const RIGHT_ENCODER_GPIO = 35;
class RobotController {
constructor(node) {
this.node = node;
this.setupGPIO();
this.setupROS2();
this.initializeState();
}
setupGPIO() {
// Initialize GPIO pins for encoders as inputs with pull-up
gpio.init_gpio(LEFT_ENCODER_GPIO, gpio.GPIO_MODE_INPUT_PULLUP, 0);
gpio.init_gpio(RIGHT_ENCODER_GPIO, gpio.GPIO_MODE_INPUT_PULLUP, 0);
// Initialize GPIO pins for motors as outputs
gpio.init_gpio(LEFT_MOTOR_PIN1, gpio.GPIO_MODE_OUTPUT, 0);
gpio.init_gpio(LEFT_MOTOR_PIN2, gpio.GPIO_MODE_OUTPUT, 0);
gpio.init_gpio(RIGHT_MOTOR_PIN1, gpio.GPIO_MODE_OUTPUT, 0);
gpio.init_gpio(RIGHT_MOTOR_PIN2, gpio.GPIO_MODE_OUTPUT, 0);
// Initialize PWM for motors (if needed, otherwise handled by setMotorSpeed)
// If you need to start PWM with 0 duty cycle, you can uncomment below:
// gpio.pwm_gpio(LEFT_MOTOR_PIN1, 200, 0);
// gpio.pwm_gpio(LEFT_MOTOR_PIN2, 200, 0);
// gpio.pwm_gpio(RIGHT_MOTOR_PIN1, 200, 0);
// gpio.pwm_gpio(RIGHT_MOTOR_PIN2, 200, 0);
}
setupROS2() {
// Create publishers
this.odometryPublisher = this.node.createPublisher('nav_msgs/msg/Odometry', `robot${robotId.replace(/-/g, "")}/odom`);
this.wheelTickPublisher = this.node.createPublisher('std_msgs/msg/String', '/wheel_ticks');
this.mySpeedPublisher = this.node.createPublisher('geometry_msgs/msg/Vector3', '/my_speed');
this.mySetSpeedPublisher = this.node.createPublisher('geometry_msgs/msg/Vector3', '/my_set_speed');
// Create subscriber for velocity commands
this.node.createSubscription('geometry_msgs/msg/Twist', `robot${robotId.replace(/-/g, "")}/cmd_vel`, (msg) => this.handleVelocityCommand(msg));
// Create TF broadcaster
this.tfBroadcaster = this.node.createPublisher('tf2_msgs/msg/TFMessage', '/tf');
}
initializeState() {
this.leftEncoderCount = 0;
this.rightEncoderCount = 0;
this.leftWheelForward = true;
this.rightWheelForward = true;
this.lastEncoderLeft = 0;
this.lastEncoderRight = 0;
this.positionX = 0;
this.positionY = 0;
this.orientation = 0;
this.lastTime = this.node.now();
this.lastLeftEncoderValue = false;
this.lastRightEncoderValue = false;
}
handleVelocityCommand(msg) {
const forwardSpeed = -msg.linear.x; // Inverted for consistency with motors-rpi5.js
const angularSpeed = msg.angular.z;
// Calculate motor speeds
let leftMotorSpeed = forwardSpeed + angularSpeed * DISTANCE_BETWEEN_WHEELS_M / 2;
let rightMotorSpeed = forwardSpeed - angularSpeed * DISTANCE_BETWEEN_WHEELS_M / 2;
// Clip speeds
leftMotorSpeed = Math.max(-MAX_SPEED, Math.min(leftMotorSpeed, MAX_SPEED));
rightMotorSpeed = Math.max(-MAX_SPEED, Math.min(rightMotorSpeed, MAX_SPEED));
// Convert speeds to PWM values
const leftPWM = this.speedToPWM(leftMotorSpeed);
const rightPWM = this.speedToPWM(rightMotorSpeed);
// Set motor directions and speeds
this.setMotorSpeed(LEFT_MOTOR_PIN1, LEFT_MOTOR_PIN2, leftPWM);
this.setMotorSpeed(RIGHT_MOTOR_PIN1, RIGHT_MOTOR_PIN2, rightPWM);
// Publish set speeds for debugging
const setSpeedMsg = rclnodejs.createMessage('geometry_msgs/msg/Vector3');
setSpeedMsg.x = leftMotorSpeed;
setSpeedMsg.y = rightMotorSpeed;
setSpeedMsg.z = 0.0;
this.mySetSpeedPublisher.publish(setSpeedMsg);
}
speedToPWM(speed) {
const speedToPower = [
[0.0, 0.0],
[0.001, 0.0],
[0.01, 15.0],
[0.04, 18.0],
[0.07, 20.0],
[0.15, 30.0],
[0.20, 40.0],
[0.22, 50.0],
[0.25, 60.0],
[0.26, 70.0],
[0.27, 85.0],
[0.3, 100.0]
];
const absSpeed = Math.abs(speed);
for (const [s, p] of speedToPower) {
if (s > absSpeed) {
return p;
}
}
return 100.0;
}
setMotorSpeed(pin1, pin2, speed) {
if (speed > 0) {
gpio.pwm_gpio(pin1, 200, speed);
gpio.pwm_gpio(pin2, 200, 0);
} else {
gpio.pwm_gpio(pin1, 200, 0);
gpio.pwm_gpio(pin2, 200, Math.abs(speed));
}
}
updateOdometry() {
const now = this.node.now();
const deltaTime = Number(now.nanoseconds - this.lastTime.nanoseconds) / 1e9;
if (deltaTime === 0) return;
// Poll encoder pins and detect edges
const leftEncoderValue = gpio.get_gpio(LEFT_ENCODER_GPIO);
const rightEncoderValue = gpio.get_gpio(RIGHT_ENCODER_GPIO);
if (leftEncoderValue && !this.lastLeftEncoderValue) {
this.leftEncoderCount += this.leftWheelForward ? 1 : -1;
}
if (rightEncoderValue && !this.lastRightEncoderValue) {
this.rightEncoderCount += this.rightWheelForward ? 1 : -1;
}
this.lastLeftEncoderValue = leftEncoderValue;
this.lastRightEncoderValue = rightEncoderValue;
const leftTicks = this.leftEncoderCount - this.lastEncoderLeft;
const rightTicks = this.rightEncoderCount - this.lastEncoderRight;
// Publish wheel ticks for debugging
const tickMsg = rclnodejs.createMessage('std_msgs/msg/String');
tickMsg.data = `Left: ${leftTicks}, Right: ${rightTicks}`;
this.wheelTickPublisher.publish(tickMsg);
const leftDistance = leftTicks / TICKS_PER_REVOLUTION;
const rightDistance = rightTicks / TICKS_PER_REVOLUTION;
const leftSpeed = leftDistance / deltaTime;
const rightSpeed = rightDistance / deltaTime;
// Publish speeds for debugging
const speedMsg = rclnodejs.createMessage('geometry_msgs/msg/Vector3');
speedMsg.x = leftSpeed;
speedMsg.y = rightSpeed;
speedMsg.z = 0.0;
this.mySpeedPublisher.publish(speedMsg);
const deltaXY = (leftDistance + rightDistance) / 2;
const deltaTheta = (leftDistance - rightDistance) / DISTANCE_BETWEEN_WHEELS_M;
const velocityXY = deltaXY / deltaTime;
const velocityTheta = deltaTheta / deltaTime;
this.orientation += deltaTheta;
this.positionX += deltaXY * Math.cos(this.orientation);
this.positionY += deltaXY * Math.sin(this.orientation);
// Publish odometry
this.publishOdometry(now, velocityXY, velocityTheta);
this.publishTransform(now);
this.lastEncoderLeft = this.leftEncoderCount;
this.lastEncoderRight = this.rightEncoderCount;
this.lastTime = now;
}
publishOdometry(timestamp, velocityXY, velocityTheta) {
const odomMsg = rclnodejs.createMessage('nav_msgs/msg/Odometry');
odomMsg.header.stamp = timestamp;
odomMsg.header.frame_id = 'odom';
odomMsg.child_frame_id = 'base_footprint';
odomMsg.pose.pose.position.x = this.positionX;
odomMsg.pose.pose.position.y = this.positionY;
odomMsg.pose.pose.position.z = 0.0;
const q = this.quaternionFromEuler(0, 0, this.orientation);
odomMsg.pose.pose.orientation.x = q[0];
odomMsg.pose.pose.orientation.y = q[1];
odomMsg.pose.pose.orientation.z = q[2];
odomMsg.pose.pose.orientation.w = q[3];
odomMsg.twist.twist.linear.x = velocityXY;
odomMsg.twist.twist.linear.y = 0.0;
odomMsg.twist.twist.linear.z = 0.0;
odomMsg.twist.twist.angular.x = 0.0;
odomMsg.twist.twist.angular.y = 0.0;
odomMsg.twist.twist.angular.z = velocityTheta;
this.odometryPublisher.publish(odomMsg);
}
publishTransform(timestamp) {
const tfMsg = rclnodejs.createMessage('tf2_msgs/msg/TFMessage');
const transform = rclnodejs.createMessage('geometry_msgs/msg/TransformStamped');
transform.header.stamp = timestamp;
transform.header.frame_id = 'odom';
transform.child_frame_id = 'base_footprint';
transform.transform.translation.x = this.positionX;
transform.transform.translation.y = this.positionY;
transform.transform.translation.z = 0.0;
const q = this.quaternionFromEuler(0, 0, this.orientation);
transform.transform.rotation.x = q[0];
transform.transform.rotation.y = q[1];
transform.transform.rotation.z = q[2];
transform.transform.rotation.w = q[3];
tfMsg.transforms = [transform];
this.tfBroadcaster.publish(tfMsg);
}
quaternionFromEuler(ai, aj, ak) {
ai /= 2.0;
aj /= 2.0;
ak /= 2.0;
const ci = Math.cos(ai);
const si = Math.sin(ai);
const cj = Math.cos(aj);
const sj = Math.sin(aj);
const ck = Math.cos(ak);
const sk = Math.sin(ak);
const cc = ci * ck;
const cs = ci * sk;
const sc = si * ck;
const ss = si * sk;
return [
cj * sc - sj * cs,
cj * ss + sj * cc,
cj * cs - sj * sc,
cj * cc + sj * ss
];
}
start() {
// Comment out the timer to disable odometry due to Raspberry Pi 5 RAM limitations
// Start odometry update loop
// this.odometryTimer = this.node.createTimer(
// 100n, // 100ms as BigInt
// () => this.updateOdometry()
// );
this.node.getLogger().info('Robot Controller started');
}
cleanup() {
// Destroy ROS2 node
this.node.destroy();
}
}
// Initialize robot details before starting the controller
async function main() {
// Fetch robot details first
await fetchRobotDetails();
await rclnodejs.init();
const node = rclnodejs.createNode('robot_controller');
const controller = new RobotController(node);
controller.start();
rclnodejs.spin(node);
// Handle cleanup on exit
process.on('SIGINT', () => {
controller.cleanup();
rclnodejs.shutdown();
process.exit();
});
}
main();