Initial commit
This commit is contained in:
@@ -0,0 +1,376 @@
|
||||
# XLOC Manual Control API Guide
|
||||
|
||||
Now you have **FULL MANUAL CONTROL** over XLOC SLAM operations! Control via:
|
||||
1. C# Service Methods
|
||||
2. REST API Endpoints
|
||||
3. SignalR Hub Methods
|
||||
|
||||
---
|
||||
|
||||
## 1. C# Service Methods (Direct)
|
||||
|
||||
Inject `XlocIntegrationService` into your service:
|
||||
|
||||
```csharp
|
||||
public class MyNavigationService
|
||||
{
|
||||
private readonly XlocIntegrationService _xloc;
|
||||
|
||||
public MyNavigationService(XlocIntegrationService xloc)
|
||||
{
|
||||
_xloc = xloc;
|
||||
}
|
||||
|
||||
public void StartLocalizationWithMap()
|
||||
{
|
||||
// Activate map
|
||||
if (_xloc.ActivateMap("/maps/factory_floor.pbstream"))
|
||||
{
|
||||
// Start localization
|
||||
_xloc.StartLocalization();
|
||||
}
|
||||
}
|
||||
|
||||
public void BeginMapping()
|
||||
{
|
||||
_xloc.StartMapping();
|
||||
}
|
||||
|
||||
public void SaveAndStopMapping()
|
||||
{
|
||||
_xloc.StopMapping("/maps/new_map.pbstream");
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 2. REST API Endpoints
|
||||
|
||||
### Activate Map
|
||||
```bash
|
||||
curl -X POST "https://localhost:7002/api/xloc/activate-map?mapPath=/maps/factory.pbstream"
|
||||
```
|
||||
|
||||
Response:
|
||||
```json
|
||||
{
|
||||
"success": true,
|
||||
"message": "Map activated"
|
||||
}
|
||||
```
|
||||
|
||||
### Start Mapping
|
||||
```bash
|
||||
dotnet build
|
||||
# Restart app
|
||||
pkill -f dotnet
|
||||
./run-quiet.sh
|
||||
# Test mapping
|
||||
curl -k -X POST https://127.0.0.1:7002/api/motion/ps5/enable
|
||||
curl -k -X POST https://localhost:7002/api/xloc/mapping/start
|
||||
# ... di chuyển robot ...
|
||||
|
||||
```
|
||||
|
||||
### Stop Mapping
|
||||
```bash
|
||||
curl -k -X POST https://localhost:7002/api/xloc/mapping/stop \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{"map_file_path": "test14"}'
|
||||
```
|
||||
### Active Map
|
||||
```bash
|
||||
curl -k -X POST https://localhost:7002/api/xloc/map/activate \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{"map_file_path": "test10"}'
|
||||
```
|
||||
### Start Localization
|
||||
```bash
|
||||
curl -k -X POST https://localhost:7002/api/xloc/localization/start
|
||||
```
|
||||
### Stop Localization
|
||||
```bash
|
||||
curl -k -X POST https://localhost:7002/api/xloc/localization/stop
|
||||
```
|
||||
|
||||
### Reset SLAM State
|
||||
```bash
|
||||
# Reset SLAM error state (automatically called before start mapping/localization)
|
||||
# Useful if you manually need to clear previous trajectory state
|
||||
curl -k -X POST https://localhost:7002/api/xloc/slam/reset
|
||||
```
|
||||
|
||||
**Note:** `StartMapping()` and `StartLocalization()` now automatically call reset before starting, so you typically don't need to call this manually.
|
||||
|
||||
### Stop Mapping & Save
|
||||
|
||||
**Option 1: Manual (will crash, but map is saved)**
|
||||
```bash
|
||||
# Save with timestamp
|
||||
MAP_NAME="map_$(date +%Y%m%d_%H%M%S).pbstream"
|
||||
curl -k -X POST "https://localhost:7002/api/xloc/stop-mapping?savePath=/home/robotics/sonvh/RobotNet10/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Xloc/map/$MAP_NAME"
|
||||
|
||||
# Or save with custom name (MUST include .pbstream extension!)
|
||||
curl -k -X POST "https://localhost:7002/api/xloc/stop-mapping?savePath=/home/robotics/sonvh/RobotNet10/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Xloc/map/my_map.pbstream"
|
||||
|
||||
cd Xloc
|
||||
./map-and-save.sh my_office_map.pbstream
|
||||
|
||||
|
||||
# Note: Application will crash due to XLOC Cairo bug, but map is saved successfully
|
||||
# Restart with: ./run-quiet.sh
|
||||
```
|
||||
|
||||
**Option 2: Automated script (recommended)**
|
||||
```bash
|
||||
# Use automated script that handles crash and restart
|
||||
cd Xloc
|
||||
./map-and-save.sh my_map.pbstream
|
||||
|
||||
# Script will:
|
||||
# 1. Wait for you to drive robot
|
||||
# 2. Save map on ENTER
|
||||
# 3. Handle crash gracefully
|
||||
# 4. Auto-restart application
|
||||
```
|
||||
|
||||
### Get Current Pose
|
||||
```bash
|
||||
curl "https://localhost:7002/api/xloc/pose"
|
||||
```
|
||||
|
||||
Response:
|
||||
```json
|
||||
{
|
||||
"x": 1.234,
|
||||
"y": 5.678,
|
||||
"yaw": 1.57,
|
||||
"yawDegrees": 90.0
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 3. SignalR Hub Methods
|
||||
|
||||
### TypeScript/JavaScript Client
|
||||
|
||||
```typescript
|
||||
import * as signalR from "@microsoft/signalr";
|
||||
|
||||
const connection = new signalR.HubConnectionBuilder()
|
||||
.withUrl("https://localhost:7002/hubs/xloc/pose")
|
||||
.build();
|
||||
|
||||
// Subscribe to pose updates (realtime streaming)
|
||||
connection.on("ReceivePose", (pose) => {
|
||||
console.log(`Position: (${pose.x}, ${pose.y}), Heading: ${pose.yawDegrees}°`);
|
||||
});
|
||||
|
||||
await connection.start();
|
||||
|
||||
// Control SLAM manually
|
||||
async function startLocalization() {
|
||||
const success = await connection.invoke("StartLocalization");
|
||||
console.log("Localization started:", success);
|
||||
}
|
||||
|
||||
async function activateMap(mapPath: string) {
|
||||
const success = await connection.invoke("ActivateMap", mapPath);
|
||||
console.log("Map activated:", success);
|
||||
}
|
||||
|
||||
async function startMapping() {
|
||||
const success = await connection.invoke("StartMapping");
|
||||
console.log("Mapping started:", success);
|
||||
}
|
||||
|
||||
async function stopMapping(savePath: string) {
|
||||
const success = await connection.invoke("StopMapping", savePath);
|
||||
console.log("Map saved:", success);
|
||||
}
|
||||
|
||||
async function getCurrentPose() {
|
||||
const pose = await connection.invoke("GetCurrentPose2D");
|
||||
console.log("Current pose:", pose);
|
||||
// { x: 1.234, y: 5.678, yaw: 1.57, yawDegrees: 90.0 }
|
||||
}
|
||||
```
|
||||
|
||||
### React Example
|
||||
|
||||
```tsx
|
||||
import { HubConnectionBuilder } from '@microsoft/signalr';
|
||||
import { useState, useEffect } from 'react';
|
||||
|
||||
export function XlocControl() {
|
||||
const [connection, setConnection] = useState(null);
|
||||
const [pose, setPose] = useState(null);
|
||||
|
||||
useEffect(() => {
|
||||
const conn = new HubConnectionBuilder()
|
||||
.withUrl("https://localhost:7002/hubs/xloc/pose")
|
||||
.build();
|
||||
|
||||
conn.on("ReceivePose", (data) => {
|
||||
setPose(data);
|
||||
});
|
||||
|
||||
conn.start();
|
||||
setConnection(conn);
|
||||
|
||||
return () => conn.stop();
|
||||
}, []);
|
||||
|
||||
const handleStartLocalization = async () => {
|
||||
const result = await connection.invoke("StartLocalization");
|
||||
console.log("Started:", result);
|
||||
};
|
||||
|
||||
const handleStartMapping = async () => {
|
||||
const result = await connection.invoke("StartMapping");
|
||||
console.log("Mapping started:", result);
|
||||
};
|
||||
|
||||
return (
|
||||
<div>
|
||||
<h2>XLOC Control Panel</h2>
|
||||
|
||||
{pose && (
|
||||
<div>
|
||||
<p>X: {pose.x.toFixed(2)}m</p>
|
||||
<p>Y: {pose.y.toFixed(2)}m</p>
|
||||
<p>Heading: {pose.yawDegrees.toFixed(1)}°</p>
|
||||
</div>
|
||||
)}
|
||||
|
||||
<button onClick={handleStartLocalization}>
|
||||
Start Localization
|
||||
</button>
|
||||
<button onClick={handleStartMapping}>
|
||||
Start Mapping
|
||||
</button>
|
||||
</div>
|
||||
);
|
||||
}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Typical Workflows
|
||||
|
||||
### Workflow 1: Localization (Using Existing Map)
|
||||
|
||||
```bash
|
||||
# 1. Activate map
|
||||
POST /api/xloc/activate-map?mapPath=/maps/factory.pbstream
|
||||
|
||||
# 2. Start localization
|
||||
POST /api/xloc/start-localization
|
||||
|
||||
# 3. Robot is now localizing!
|
||||
# Pose updates stream automatically via SignalR
|
||||
|
||||
# 4. When done
|
||||
POST /api/xloc/stop-localization
|
||||
```
|
||||
|
||||
### Workflow 2: Mapping (Create New Map)
|
||||
|
||||
```bash
|
||||
# 1. Start mapping
|
||||
POST /api/xloc/start-mapping
|
||||
|
||||
# 2. Drive robot around
|
||||
# Map is being created in realtime
|
||||
|
||||
# 3. Save and stop
|
||||
POST /api/xloc/stop-mapping?savePath=/maps/new_building.pbstream
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Available Methods in All Interfaces
|
||||
|
||||
| Method | XlocIntegrationService | REST API | SignalR Hub |
|
||||
|--------|----------------------|----------|-------------|
|
||||
| ActivateMap | ✅ `ActivateMap(mapPath)` | ✅ `POST /api/xloc/activate-map` | ✅ `connection.invoke("ActivateMap", mapPath)` |
|
||||
| StartLocalization | ✅ `StartLocalization()` | ✅ `POST /api/xloc/start-localization` | ✅ `connection.invoke("StartLocalization")` |
|
||||
| StopLocalization | ✅ `StopLocalization()` | ✅ `POST /api/xloc/stop-localization` | ✅ `connection.invoke("StopLocalization")` |
|
||||
| StartMapping | ✅ `StartMapping()` | ✅ `POST /api/xloc/start-mapping` | ✅ `connection.invoke("StartMapping")` |
|
||||
| StopMapping | ✅ `StopMapping(savePath)` | ✅ `POST /api/xloc/stop-mapping` | ✅ `connection.invoke("StopMapping", savePath)` |
|
||||
| GetCurrentPose2D | ✅ `GetCurrentPose2D()` | ✅ `GET /api/xloc/pose` | ✅ `connection.invoke("GetCurrentPose2D")` |
|
||||
|
||||
---
|
||||
|
||||
## Important Notes
|
||||
|
||||
✅ **No Auto-Start**: SLAM does NOT start automatically anymore!
|
||||
✅ **Manual Control Only**: You must explicitly call start methods
|
||||
✅ **Sensor Data Streaming**: Continues automatically at 20Hz (Odom + IMU)
|
||||
✅ **Pose Streaming**: Broadcasts via SignalR at 5Hz when SLAM is running
|
||||
|
||||
## Configuration
|
||||
|
||||
```json
|
||||
{
|
||||
"Xloc": {
|
||||
"Integration": {
|
||||
"Enabled": true,
|
||||
"Mode": "Mapping", // Ignored - now manual control
|
||||
"UpdateRateHz": 20,
|
||||
"MapFilePath": "", // Ignored - call ActivateMap() manually
|
||||
"SaveMapFilePath": "/tmp/xloc_map.pbstream"
|
||||
}
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
Note: `Mode` and `MapFilePath` in config are now ignored. Use manual control methods instead!
|
||||
|
||||
---
|
||||
|
||||
## Troubleshooting
|
||||
|
||||
### Cannot start mapping after stop & save
|
||||
|
||||
**Problem:** After stopping localization or mapping, `StartMapping()` fails.
|
||||
|
||||
**Root Cause:** XLOC library retains the finished trajectory state. Starting a new mapping/localization session requires clearing this state.
|
||||
|
||||
**Solution (Automatic):** `StartMapping()` and `StartLocalization()` now automatically call `ResetSlamError()` before starting, which clears the previous trajectory state.
|
||||
|
||||
**Manual Reset (if needed):**
|
||||
```bash
|
||||
# If automatic reset doesn't work, manually reset SLAM state
|
||||
curl -k -X POST https://localhost:7002/api/xloc/slam/reset
|
||||
|
||||
# Then try starting mapping again
|
||||
curl -k -X POST https://localhost:7002/api/xloc/mapping/start
|
||||
```
|
||||
|
||||
**What the fix does:**
|
||||
- Clears finished trajectory (trajectory ID from previous session)
|
||||
- Resets SLAM error state to Idle
|
||||
- Prepares XLOC library for new mapping/localization session
|
||||
|
||||
### General Workflow After Fix
|
||||
|
||||
```bash
|
||||
# 1. Stop previous session (if any)
|
||||
curl -k -X POST https://localhost:7002/api/xloc/localization/stop
|
||||
|
||||
# 2. Start mapping (automatic reset happens internally)
|
||||
curl -k -X POST https://localhost:7002/api/xloc/mapping/start
|
||||
|
||||
# 3. Drive robot around...
|
||||
|
||||
# 4. Stop and save
|
||||
curl -k -X POST https://localhost:7002/api/xloc/mapping/stop \
|
||||
-H "Content-Type: application/json" \
|
||||
-d '{"map_file_path": "my_new_map"}'
|
||||
|
||||
# 5. Start mapping again (works now!)
|
||||
curl -k -X POST https://localhost:7002/api/xloc/mapping/start
|
||||
```
|
||||
@@ -0,0 +1,739 @@
|
||||
using Microsoft.AspNetCore.Mvc;
|
||||
using System.Text.Json;
|
||||
using RobotNet10.RobotApp.Motion;
|
||||
using RobotNet10.RobotApp.Navigation;
|
||||
|
||||
namespace RobotNet10.RobotApp.Xloc;
|
||||
|
||||
public static class XlocApiEndpoints
|
||||
{
|
||||
// XLOC Control API
|
||||
public static IEndpointRouteBuilder MapXlocApiEndpoints(this IEndpointRouteBuilder app)
|
||||
{
|
||||
var xlocApi = app.MapGroup("/api/xloc").DisableAntiforgery();
|
||||
|
||||
// Mapping control
|
||||
xlocApi.MapPost("/mapping/start", ([FromServices] XlocIntegrationService service) => {
|
||||
return service.StartMapping()
|
||||
? Results.Ok(new { status = "success", message = "Mapping started" })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to start mapping" });
|
||||
});
|
||||
|
||||
xlocApi.MapPost("/mapping/stop", async (HttpRequest request, [FromServices] XlocIntegrationService service) => {
|
||||
var body = await request.ReadFromJsonAsync<Dictionary<string, string>>();
|
||||
var mapFile = body?["map_file_path"] ?? "/home/mic-733ao/.local/share/xloc/resources/maps";
|
||||
|
||||
return service.StopMapping(mapFile)
|
||||
? Results.Ok(new { status = "success", message = "Mapping stopped", map_file = mapFile })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to stop mapping" });
|
||||
});
|
||||
|
||||
// Localization control
|
||||
xlocApi.MapPost("/localization/start", ([FromServices] XlocIntegrationService service) => {
|
||||
return service.StartLocalization()
|
||||
? Results.Ok(new { status = "success", message = "Localization started" })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to start localization" });
|
||||
});
|
||||
|
||||
xlocApi.MapPost("/localization/stop", ([FromServices] XlocIntegrationService service) => {
|
||||
return service.StopLocalization()
|
||||
? Results.Ok(new { status = "success", message = "Localization stopped" })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to stop localization" });
|
||||
});
|
||||
|
||||
// Update map control (only allowed when in localization mode)
|
||||
xlocApi.MapPost("/update-map/start", ([FromServices] XlocIntegrationService service) => {
|
||||
return service.StartUpdateMap()
|
||||
? Results.Ok(new { status = "success", message = "Map update started" })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to start map update. Robot must be in localization mode." });
|
||||
});
|
||||
|
||||
xlocApi.MapPost("/update-map/stop", async (HttpRequest request, [FromServices] XlocIntegrationService service) => {
|
||||
var body = await request.ReadFromJsonAsync<Dictionary<string, object>>();
|
||||
bool saveUpdatedMap = true;
|
||||
if (body != null && body.TryGetValue("save_updated_map", out var saveVal))
|
||||
{
|
||||
if (saveVal is bool saveBool)
|
||||
saveUpdatedMap = saveBool;
|
||||
else if (saveVal is JsonElement jsonElem && jsonElem.ValueKind == JsonValueKind.True)
|
||||
saveUpdatedMap = true;
|
||||
else if (saveVal is JsonElement jsonElem2 && jsonElem2.ValueKind == JsonValueKind.False)
|
||||
saveUpdatedMap = false;
|
||||
}
|
||||
|
||||
return service.StopUpdateMap(saveUpdatedMap)
|
||||
? Results.Ok(new { status = "success", message = "Map update stopped", save_updated_map = saveUpdatedMap })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to stop map update" });
|
||||
});
|
||||
|
||||
// Reset SLAM error
|
||||
xlocApi.MapPost("/reset-error", ([FromServices] XlocIntegrationService service) => {
|
||||
return service.ResetSlamError()
|
||||
? Results.Ok(new { status = "success", message = "SLAM error reset successfully" })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to reset SLAM error" });
|
||||
});
|
||||
|
||||
// Initial pose control
|
||||
xlocApi.MapPost("/pose/initial", async (HttpRequest request, [FromServices] XlocIntegrationService service) => {
|
||||
var body = await request.ReadFromJsonAsync<Dictionary<string, double>>();
|
||||
if (body == null)
|
||||
return Results.BadRequest(new { status = "error", message = "Invalid request body" });
|
||||
|
||||
if (!body.TryGetValue("x", out var x) || !body.TryGetValue("y", out var y))
|
||||
return Results.BadRequest(new { status = "error", message = "x and y are required" });
|
||||
|
||||
var z = body.TryGetValue("z", out var zVal) ? zVal : 0.0;
|
||||
var roll = body.TryGetValue("roll", out var rollVal) ? rollVal : 0.0;
|
||||
var pitch = body.TryGetValue("pitch", out var pitchVal) ? pitchVal : 0.0;
|
||||
var yaw = body.TryGetValue("yaw", out var yawVal) ? yawVal : 0.0;
|
||||
|
||||
return service.SetInitialPose(x, y, z, roll, pitch, yaw)
|
||||
? Results.Ok(new { status = "success", message = "Initial pose set", position = new { x, y, z }, orientation = new { roll, pitch, yaw } })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to set initial pose" });
|
||||
});
|
||||
|
||||
// Map management
|
||||
// xlocApi.MapPost("/map/activate", async (HttpRequest request, [FromServices] XlocIntegrationService service) => {
|
||||
// var body = await request.ReadFromJsonAsync<Dictionary<string, string>>();
|
||||
// var mapFile = body?["map_file_path"];
|
||||
|
||||
// if (string.IsNullOrEmpty(mapFile))
|
||||
// return Results.BadRequest(new { status = "error", message = "map_file_path required" });
|
||||
|
||||
// // UI may send only the map folder name (e.g. "map_20260307_103646").
|
||||
// // Resolve it to an absolute pbstream path so native XLOC doesn't depend on HOME defaults (e.g. /root vs /home/...).
|
||||
// var resolved = XlocPaths.ResolvePbstreamPath(mapFile);
|
||||
// if (resolved == null)
|
||||
// return Results.BadRequest(new
|
||||
// {
|
||||
// status = "error",
|
||||
// message = "Failed to resolve pbstream path for map_file_path. " +
|
||||
// "Expected either an absolute .pbstream file, or a map folder name under: " +
|
||||
// XlocPaths.GetMapsDirectory()
|
||||
// });
|
||||
|
||||
// return service.ActivateMap(resolved)
|
||||
// ? Results.Ok(new { status = "success", message = "Map activated", map_file = resolved })
|
||||
// : Results.BadRequest(new { status = "error", message = $"Failed to activate map pbstream: {resolved}" });
|
||||
// });
|
||||
|
||||
xlocApi.MapPost("/map/activate", async (HttpRequest request, [FromServices] XlocIntegrationService service) => {
|
||||
var body = await request.ReadFromJsonAsync<Dictionary<string, string>>();
|
||||
var mapFile = body?["map_file_path"];
|
||||
|
||||
if (string.IsNullOrEmpty(mapFile))
|
||||
return Results.BadRequest(new { status = "error", message = "map_file_path required" });
|
||||
|
||||
return service.ActivateMap(mapFile)
|
||||
? Results.Ok(new { status = "success", message = "Map activated", map_file = mapFile })
|
||||
: Results.BadRequest(new { status = "error", message = "Failed to activate map" });
|
||||
});
|
||||
|
||||
// Grid map endpoints
|
||||
xlocApi.MapGet("/gridmap/static", ([FromServices] XlocIntegrationService service, [FromServices] ILogger<Program> logger, [FromQuery] bool reload = false) => {
|
||||
// Log when map is being requested (to track if it's called after initial pose)
|
||||
// logger.LogWarning("🔍 API /gridmap/static called (reload={Reload})", reload);
|
||||
|
||||
var grid = service.GetStaticGridMap(reload);
|
||||
if (grid == null)
|
||||
return Results.NotFound(new { status = "error", message = "No static grid map available" });
|
||||
|
||||
// Prefer origin from active map's YAML (Map frame in World frame); native often returns (0,0,0)
|
||||
double ox = grid.Origin.X, oy = grid.Origin.Y, oz = grid.Origin.Z;
|
||||
var diag = service.GetDiagnostics();
|
||||
if (!string.IsNullOrEmpty(diag?.CurrentActiveMap))
|
||||
{
|
||||
var (yx, yy, yz) = ReadOriginFromMapYaml(diag.CurrentActiveMap);
|
||||
ox = yx; oy = yy; oz = yz;
|
||||
}
|
||||
|
||||
//logger.LogWarning("🔍 API /gridmap/static returning origin: ({X}, {Y}, {Z})", ox, oy, oz);
|
||||
|
||||
return Results.Ok(new {
|
||||
status = "success",
|
||||
resolution = grid.Resolution,
|
||||
width = grid.Width,
|
||||
height = grid.Height,
|
||||
origin = new {
|
||||
x = ox,
|
||||
y = oy,
|
||||
z = oz,
|
||||
qx = grid.Origin.Qx,
|
||||
qy = grid.Origin.Qy,
|
||||
qz = grid.Origin.Qz,
|
||||
qw = grid.Origin.Qw
|
||||
},
|
||||
data = Convert.ToBase64String(grid.Data),
|
||||
frameId = grid.FrameId
|
||||
});
|
||||
});
|
||||
|
||||
xlocApi.MapGet("/gridmap/online", ([FromServices] XlocIntegrationService service) => {
|
||||
var grid = service.GetOnlineGridMap();
|
||||
if (grid == null)
|
||||
return Results.NotFound(new { status = "error", message = "No online grid map available" });
|
||||
|
||||
// CRITICAL: Use origin directly from online grid map to ensure alignment with LIDAR
|
||||
// Do NOT override with YAML origin during mapping, as online map origin may change
|
||||
// and must match the actual grid data for proper rendering
|
||||
double ox = grid.Origin.X, oy = grid.Origin.Y, oz = grid.Origin.Z;
|
||||
|
||||
// Only use YAML origin for static maps, not for online maps during mapping
|
||||
// Online map origin should come directly from xloc_get_online_grid_map()
|
||||
// var diag = service.GetDiagnostics();
|
||||
// if (!string.IsNullOrEmpty(diag?.CurrentActiveMap))
|
||||
// {
|
||||
// var (yx, yy, yz) = ReadOriginFromMapYaml(diag.CurrentActiveMap);
|
||||
// ox = yx; oy = yy; oz = yz;
|
||||
// }
|
||||
|
||||
return Results.Ok(new {
|
||||
status = "success",
|
||||
resolution = grid.Resolution,
|
||||
width = grid.Width,
|
||||
height = grid.Height,
|
||||
origin = new {
|
||||
x = ox,
|
||||
y = oy,
|
||||
z = oz,
|
||||
qx = grid.Origin.Qx,
|
||||
qy = grid.Origin.Qy,
|
||||
qz = grid.Origin.Qz,
|
||||
qw = grid.Origin.Qw
|
||||
},
|
||||
data = Convert.ToBase64String(grid.Data),
|
||||
frameId = grid.FrameId
|
||||
});
|
||||
});
|
||||
|
||||
// List available maps
|
||||
xlocApi.MapGet("/maps/list", () => {
|
||||
try
|
||||
{
|
||||
var mapsDir = XlocPaths.GetMapsDirectory();
|
||||
if (!Directory.Exists(mapsDir))
|
||||
return Results.Ok(new { status = "success", maps = Array.Empty<string>() });
|
||||
|
||||
var maps = Directory.GetDirectories(mapsDir)
|
||||
.Select(dir => Path.GetFileName(dir))
|
||||
.Where(name => !string.IsNullOrEmpty(name) && name != "tmp")
|
||||
.OrderBy(name => name)
|
||||
.ToArray();
|
||||
|
||||
return Results.Ok(new { status = "success", maps });
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = ex.Message });
|
||||
}
|
||||
});
|
||||
|
||||
// Load map from folder
|
||||
xlocApi.MapGet("/maps/load/{mapName}", (string mapName) => {
|
||||
try
|
||||
{
|
||||
var mapsDir = XlocPaths.GetMapsDirectory();
|
||||
var mapFolder = Path.Combine(mapsDir, mapName);
|
||||
|
||||
if (!Directory.Exists(mapFolder))
|
||||
return Results.NotFound(new { status = "error", message = $"Map '{mapName}' not found" });
|
||||
|
||||
// Find YAML file
|
||||
var yamlFiles = Directory.GetFiles(mapFolder, "*.yaml");
|
||||
if (yamlFiles.Length == 0)
|
||||
return Results.BadRequest(new { status = "error", message = "YAML file not found" });
|
||||
|
||||
var yamlFile = yamlFiles[0];
|
||||
var yamlContent = File.ReadAllText(yamlFile);
|
||||
|
||||
// Parse YAML (simple parsing)
|
||||
float resolution = 0.05f;
|
||||
double originX = 0.0, originY = 0.0, originZ = 0.0;
|
||||
string? imageFile = null;
|
||||
|
||||
foreach (var line in yamlContent.Split('\n'))
|
||||
{
|
||||
var trimmed = line.Trim();
|
||||
if (trimmed.StartsWith("resolution:"))
|
||||
{
|
||||
if (float.TryParse(trimmed.Substring("resolution:".Length).Trim(), out float res))
|
||||
resolution = res;
|
||||
}
|
||||
else if (trimmed.StartsWith("origin:"))
|
||||
{
|
||||
var originStr = trimmed.Substring("origin:".Length).Trim();
|
||||
if (originStr.StartsWith("[") && originStr.EndsWith("]"))
|
||||
{
|
||||
var coords = originStr.Substring(1, originStr.Length - 2).Split(',');
|
||||
if (coords.Length >= 1) double.TryParse(coords[0].Trim(), out originX);
|
||||
if (coords.Length >= 2) double.TryParse(coords[1].Trim(), out originY);
|
||||
if (coords.Length >= 3) double.TryParse(coords[2].Trim(), out originZ);
|
||||
}
|
||||
}
|
||||
else if (trimmed.StartsWith("image:"))
|
||||
{
|
||||
imageFile = trimmed.Substring("image:".Length).Trim();
|
||||
}
|
||||
}
|
||||
|
||||
// Find PGM file - always prefer .pgm over .png from YAML
|
||||
string? pgmFile = null;
|
||||
|
||||
// First try to find .pgm file directly
|
||||
var pgmFiles = Directory.GetFiles(mapFolder, "*.pgm");
|
||||
if (pgmFiles.Length > 0)
|
||||
{
|
||||
pgmFile = pgmFiles[0];
|
||||
}
|
||||
else if (imageFile != null)
|
||||
{
|
||||
// If YAML specifies .png, try replacing extension with .pgm
|
||||
var pgmFromYaml = Path.Combine(mapFolder, Path.ChangeExtension(imageFile, ".pgm"));
|
||||
if (File.Exists(pgmFromYaml))
|
||||
pgmFile = pgmFromYaml;
|
||||
else
|
||||
{
|
||||
// Last resort: use image file from YAML (might be .png)
|
||||
var imageFromYaml = Path.Combine(mapFolder, imageFile);
|
||||
if (File.Exists(imageFromYaml) && imageFile.EndsWith(".pgm", StringComparison.OrdinalIgnoreCase))
|
||||
pgmFile = imageFromYaml;
|
||||
}
|
||||
}
|
||||
|
||||
if (pgmFile == null)
|
||||
return Results.BadRequest(new { status = "error", message = "PGM file not found" });
|
||||
|
||||
// Read PGM file and convert to occupancy grid
|
||||
var (width, height, data) = ReadPgmFile(pgmFile);
|
||||
|
||||
// Convert to base64
|
||||
var dataBase64 = Convert.ToBase64String(data);
|
||||
|
||||
return Results.Ok(new {
|
||||
status = "success",
|
||||
mapName,
|
||||
width,
|
||||
height,
|
||||
resolution,
|
||||
origin = new {
|
||||
x = originX,
|
||||
y = originY,
|
||||
z = originZ,
|
||||
qx = 0.0,
|
||||
qy = 0.0,
|
||||
qz = 0.0,
|
||||
qw = 1.0
|
||||
},
|
||||
data = dataBase64
|
||||
});
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = ex.Message });
|
||||
}
|
||||
});
|
||||
|
||||
// Download map folder as .zip (single top-level folder matching map name inside the archive on re-import)
|
||||
xlocApi.MapGet("/maps/download/{mapName}", (string mapName) => XlocMapFileEndpoints.DownloadMapAsZip(mapName));
|
||||
|
||||
// Import map from .zip into maps directory
|
||||
xlocApi.MapPost("/maps/import", async (HttpRequest request) => await XlocMapFileEndpoints.ImportMapFromZipAsync(request));
|
||||
|
||||
// Delete map folder (explicit path avoids conflicting with /maps/list, /maps/load/...)
|
||||
xlocApi.MapDelete("/maps/delete/{mapName}", (string mapName) => XlocMapFileEndpoints.DeleteMapFolder(mapName));
|
||||
|
||||
// Map upload endpoint (kept for backward compatibility)
|
||||
xlocApi.MapPost("/map/upload", async (HttpRequest request, [FromServices] XlocIntegrationService service) => {
|
||||
try
|
||||
{
|
||||
if (!request.HasFormContentType)
|
||||
return Results.BadRequest(new { status = "error", message = "Request must be multipart/form-data" });
|
||||
|
||||
var form = await request.ReadFormAsync();
|
||||
var file = form.Files["mapFile"];
|
||||
if (file == null || file.Length == 0)
|
||||
return Results.BadRequest(new { status = "error", message = "No file uploaded" });
|
||||
|
||||
// Validate PNG
|
||||
if (!file.FileName.EndsWith(".png", StringComparison.OrdinalIgnoreCase))
|
||||
return Results.BadRequest(new { status = "error", message = "File must be a PNG image" });
|
||||
|
||||
// Parse origin and resolution from form
|
||||
if (!float.TryParse(form["resolution"], out float resolution))
|
||||
resolution = 0.05f; // default 5cm
|
||||
|
||||
if (!double.TryParse(form["originX"], out double originX))
|
||||
originX = 0.0;
|
||||
if (!double.TryParse(form["originY"], out double originY))
|
||||
originY = 0.0;
|
||||
if (!double.TryParse(form["originZ"], out double originZ))
|
||||
originZ = 0.0;
|
||||
if (!double.TryParse(form["originQx"], out double originQx))
|
||||
originQx = 0.0;
|
||||
if (!double.TryParse(form["originQy"], out double originQy))
|
||||
originQy = 0.0;
|
||||
if (!double.TryParse(form["originQz"], out double originQz))
|
||||
originQz = 0.0;
|
||||
if (!double.TryParse(form["originQw"], out double originQw))
|
||||
originQw = 1.0;
|
||||
|
||||
// Save file to temp directory
|
||||
var uploadsDir = Path.Combine(Path.GetTempPath(), "xloc_maps");
|
||||
Directory.CreateDirectory(uploadsDir);
|
||||
var filePath = Path.Combine(uploadsDir, file.FileName);
|
||||
|
||||
using (var stream = File.Create(filePath))
|
||||
{
|
||||
await file.CopyToAsync(stream);
|
||||
}
|
||||
|
||||
// Activate map with origin
|
||||
var originPose = new { x = originX, y = originY, z = originZ, qx = originQx, qy = originQy, qz = originQz, qw = originQw };
|
||||
|
||||
// Note: xloc may need the map in a specific format, so we save the PNG and metadata
|
||||
// The actual activation might need conversion to xloc's map format
|
||||
var metadata = new {
|
||||
filePath,
|
||||
resolution,
|
||||
origin = originPose
|
||||
};
|
||||
|
||||
return Results.Ok(new {
|
||||
status = "success",
|
||||
message = "Map uploaded successfully",
|
||||
filePath,
|
||||
resolution,
|
||||
origin = originPose
|
||||
});
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = ex.Message });
|
||||
}
|
||||
});
|
||||
|
||||
// Pose data
|
||||
xlocApi.MapGet("/pose/current", ([FromServices] XlocIntegrationService service) => {
|
||||
var pose = service.GetCurrentPose();
|
||||
return pose.HasValue
|
||||
? Results.Ok(new {
|
||||
status = "success",
|
||||
pose = new {
|
||||
position = new { x = pose.Value.x, y = pose.Value.y, z = pose.Value.z },
|
||||
orientation = new { x = pose.Value.qx, y = pose.Value.qy, z = pose.Value.qz, w = pose.Value.qw }
|
||||
}
|
||||
})
|
||||
: Results.NotFound(new { status = "error", message = "No pose available" });
|
||||
});
|
||||
|
||||
xlocApi.MapGet("/pose/current2d", ([FromServices] XlocIntegrationService service) => {
|
||||
var pose = service.GetCurrentPose2D();
|
||||
return pose.HasValue
|
||||
? Results.Ok(new {
|
||||
status = "success",
|
||||
pose = new { x = pose.Value.x, y = pose.Value.y, yaw = pose.Value.yaw }
|
||||
})
|
||||
: Results.NotFound(new { status = "error", message = "No pose available" });
|
||||
});
|
||||
|
||||
// Diagnostics data
|
||||
xlocApi.MapGet("/diagnostics", ([FromServices] XlocIntegrationService service) => {
|
||||
var diagnostics = service.GetDiagnostics();
|
||||
return diagnostics != null
|
||||
? Results.Ok(new {
|
||||
status = "success",
|
||||
diagnostics = new {
|
||||
header = new {
|
||||
seq = diagnostics.HeaderSeq,
|
||||
stamp = new {
|
||||
sec = diagnostics.HeaderStampSec,
|
||||
nsec = diagnostics.HeaderStampNsec
|
||||
},
|
||||
frameId = diagnostics.HeaderFrameId
|
||||
},
|
||||
xlocState = diagnostics.XlocState,
|
||||
stateString = diagnostics.StateString,
|
||||
currentActiveMap = diagnostics.CurrentActiveMap,
|
||||
reliability = diagnostics.Reliability,
|
||||
matchingScore = diagnostics.MatchingScore
|
||||
}
|
||||
})
|
||||
: Results.NotFound(new { status = "error", message = "No diagnostics available" });
|
||||
});
|
||||
|
||||
// Laser scan data (sampled at 1 degree intervals)
|
||||
xlocApi.MapGet("/laser/scan", ([FromServices] XlocIntegrationService service) => {
|
||||
var laserData = service.GetSampledLaserScan();
|
||||
if (laserData == null || laserData.Count == 0)
|
||||
return Results.NotFound(new { status = "error", message = "No laser scan available" });
|
||||
|
||||
var points = laserData.Select(p => new { angle = p.angle, range = p.range }).ToArray();
|
||||
|
||||
return Results.Ok(new {
|
||||
status = "success",
|
||||
pointCount = points.Length,
|
||||
points = points,
|
||||
timestamp = DateTime.UtcNow
|
||||
});
|
||||
});
|
||||
|
||||
// All 3 lidars combined with mode support (full = all points, minimal = 120 sampled points)
|
||||
xlocApi.MapGet("/laser/scan/all", (
|
||||
[FromServices] XlocIntegrationService service,
|
||||
[FromQuery] string? mode1 = "minimal",
|
||||
[FromQuery] string? mode2 = "minimal",
|
||||
[FromQuery] string? mode3 = "minimal") => {
|
||||
|
||||
// Get laser data based on mode (full = all points, minimal = ~120 sampled points)
|
||||
var laser1 = mode1 == "full" ? service.GetFullLaserScan() : service.GetSampledLaserScan();
|
||||
var laser2 = mode2 == "full" ? service.GetFullLaserScan2() : service.GetSampledLaserScan2();
|
||||
var laser3 = mode3 == "full" ? service.GetFullLaserScan3() : service.GetSampledLaserScan3();
|
||||
|
||||
return Results.Ok(new {
|
||||
status = "success",
|
||||
timestamp = DateTime.UtcNow,
|
||||
lidar1 = laser1 != null ? new {
|
||||
pointCount = laser1.Count,
|
||||
points = laser1.Select(p => new { angle = p.angle, range = p.range }).ToArray()
|
||||
} : null,
|
||||
lidar2 = laser2 != null ? new {
|
||||
pointCount = laser2.Count,
|
||||
points = laser2.Select(p => new { angle = p.angle, range = p.range }).ToArray()
|
||||
} : null,
|
||||
lidar3 = laser3 != null ? new {
|
||||
pointCount = laser3.Count,
|
||||
points = laser3.Select(p => new { angle = p.angle, range = p.range }).ToArray()
|
||||
} : null
|
||||
});
|
||||
});
|
||||
|
||||
// Velocity data (OdomVel and CmdVel)
|
||||
xlocApi.MapGet("/velocity", ([FromServices] OdometryService odometryService, [FromServices] ManualControlService manualControlService, [FromServices] PS5ControllerService ps5ControllerService, [FromServices] NavigationIntegrationService navigationService) => {
|
||||
try
|
||||
{
|
||||
// Get odometry velocity directly from OdometryService (raw encoder odometry, not EKF filtered)
|
||||
var odom = odometryService.CurrentOdometry;
|
||||
double odomLinearVel = odom.Twist.Twist.Linear.X;
|
||||
double odomAngularVel = odom.Twist.Twist.Angular.Z;
|
||||
|
||||
// Get command velocity from the active control service
|
||||
// Priority: PS5Controller > ManualControl > Navigation twist
|
||||
double cmdLinearVel = 0.0;
|
||||
double cmdAngularVel = 0.0;
|
||||
|
||||
// Check PS5Controller first (if active)
|
||||
if (ps5ControllerService.State == PS5ControllerState.Active)
|
||||
{
|
||||
var ps5Twist = ps5ControllerService.CurrentTwist;
|
||||
cmdLinearVel = ps5Twist.Linear.X;
|
||||
cmdAngularVel = ps5Twist.Angular.Z;
|
||||
}
|
||||
// Otherwise check ManualControl (if active)
|
||||
else if (manualControlService.State == ManualControlState.Active)
|
||||
{
|
||||
var manualTwist = manualControlService.CurrentTwist;
|
||||
cmdLinearVel = manualTwist.Linear.X;
|
||||
cmdAngularVel = manualTwist.Angular.Z;
|
||||
}
|
||||
// Otherwise try to get navigation twist (from GetTwist in navigation system)
|
||||
else
|
||||
{
|
||||
var navTwist = navigationService.GetTwist();
|
||||
if (navTwist.HasValue)
|
||||
{
|
||||
cmdLinearVel = navTwist.Value.x;
|
||||
cmdAngularVel = navTwist.Value.theta;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Fallback: last known value from ManualControl
|
||||
var manualTwist = manualControlService.CurrentTwist;
|
||||
cmdLinearVel = manualTwist.Linear.X;
|
||||
cmdAngularVel = manualTwist.Angular.Z;
|
||||
}
|
||||
}
|
||||
|
||||
return Results.Ok(new {
|
||||
status = "success",
|
||||
odomVel = new {
|
||||
linear = odomLinearVel,
|
||||
angular = odomAngularVel
|
||||
},
|
||||
cmdVel = new {
|
||||
linear = cmdLinearVel,
|
||||
angular = cmdAngularVel
|
||||
}
|
||||
});
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = ex.Message });
|
||||
}
|
||||
});
|
||||
|
||||
// Diagnostics endpoint
|
||||
// DISABLED: GetDiagnostics method removed from XlocIntegrationService
|
||||
/*
|
||||
app.MapGet("/api/xloc/diagnostics", ([FromServices] XlocIntegrationService xlocService) =>
|
||||
{
|
||||
var diagnostics = xlocService.GetDiagnostics();
|
||||
if (diagnostics.HasValue)
|
||||
{
|
||||
return Results.Ok(new { status = "success", diagnostics = new {
|
||||
state = diagnostics.Value.state,
|
||||
map_name = diagnostics.Value.mapName,
|
||||
reliability = diagnostics.Value.reliability,
|
||||
matching_score = diagnostics.Value.matchingScore
|
||||
}});
|
||||
}
|
||||
return Results.Ok(new { status = "error", message = "No diagnostics available" });
|
||||
});
|
||||
*/
|
||||
|
||||
return app;
|
||||
}
|
||||
|
||||
// Helper: read origin (Map frame in World frame) from active map's YAML
|
||||
private static (double x, double y, double z) ReadOriginFromMapYaml(string mapName)
|
||||
{
|
||||
try
|
||||
{
|
||||
var mapsDir = XlocPaths.GetMapsDirectory();
|
||||
var folderName = Path.GetFileName(mapName.TrimEnd(Path.DirectorySeparatorChar, '/'));
|
||||
if (string.IsNullOrEmpty(folderName)) return (0, 0, 0);
|
||||
var mapFolder = Path.Combine(mapsDir, folderName);
|
||||
if (!Directory.Exists(mapFolder)) return (0, 0, 0);
|
||||
var yamlFiles = Directory.GetFiles(mapFolder, "*.yaml");
|
||||
if (yamlFiles.Length == 0) return (0, 0, 0);
|
||||
var content = File.ReadAllText(yamlFiles[0]);
|
||||
double ox = 0, oy = 0, oz = 0;
|
||||
foreach (var line in content.Split('\n'))
|
||||
{
|
||||
var t = line.Trim();
|
||||
if (!t.StartsWith("origin:")) continue;
|
||||
var s = t.Substring("origin:".Length).Trim();
|
||||
if (s.StartsWith("[") && s.EndsWith("]"))
|
||||
{
|
||||
var parts = s.Substring(1, s.Length - 2).Split(',');
|
||||
if (parts.Length >= 1) double.TryParse(parts[0].Trim(), out ox);
|
||||
if (parts.Length >= 2) double.TryParse(parts[1].Trim(), out oy);
|
||||
if (parts.Length >= 3) double.TryParse(parts[2].Trim(), out oz);
|
||||
}
|
||||
break;
|
||||
}
|
||||
return (ox, oy, oz);
|
||||
}
|
||||
catch { return (0, 0, 0); }
|
||||
}
|
||||
|
||||
// Helper function to read PGM file
|
||||
private static (uint width, uint height, byte[] data) ReadPgmFile(string pgmPath)
|
||||
{
|
||||
// Read entire file as bytes first
|
||||
var allBytes = File.ReadAllBytes(pgmPath);
|
||||
|
||||
// Parse header manually to avoid StreamReader buffering issues
|
||||
int pos = 0;
|
||||
|
||||
// Read magic number (P5\n)
|
||||
string magic = "";
|
||||
while (pos < allBytes.Length && allBytes[pos] != '\n')
|
||||
{
|
||||
magic += (char)allBytes[pos];
|
||||
pos++;
|
||||
}
|
||||
pos++; // Skip newline
|
||||
|
||||
magic = magic.Trim();
|
||||
if (magic != "P5")
|
||||
throw new InvalidDataException($"Not a P5 PGM file, got: {magic}");
|
||||
|
||||
// Skip comments and read dimensions
|
||||
uint width = 0, height = 0;
|
||||
bool gotDimensions = false;
|
||||
|
||||
while (pos < allBytes.Length && !gotDimensions)
|
||||
{
|
||||
// Skip whitespace
|
||||
while (pos < allBytes.Length && (allBytes[pos] == ' ' || allBytes[pos] == '\t' || allBytes[pos] == '\r' || allBytes[pos] == '\n'))
|
||||
pos++;
|
||||
|
||||
// Check for comment
|
||||
if (pos < allBytes.Length && allBytes[pos] == '#')
|
||||
{
|
||||
// Skip comment line
|
||||
while (pos < allBytes.Length && allBytes[pos] != '\n')
|
||||
pos++;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Read dimensions line
|
||||
string line = "";
|
||||
while (pos < allBytes.Length && allBytes[pos] != '\n')
|
||||
{
|
||||
line += (char)allBytes[pos];
|
||||
pos++;
|
||||
}
|
||||
pos++; // Skip newline
|
||||
|
||||
var parts = line.Trim().Split(new[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
|
||||
if (parts.Length >= 2 && uint.TryParse(parts[0], out width) && uint.TryParse(parts[1], out height))
|
||||
{
|
||||
gotDimensions = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!gotDimensions || width == 0 || height == 0)
|
||||
throw new InvalidDataException($"Invalid PGM dimensions: {width}x{height}");
|
||||
|
||||
// Read max value
|
||||
while (pos < allBytes.Length && (allBytes[pos] == ' ' || allBytes[pos] == '\t' || allBytes[pos] == '\r' || allBytes[pos] == '\n'))
|
||||
pos++;
|
||||
|
||||
string maxValStr = "";
|
||||
while (pos < allBytes.Length && allBytes[pos] != '\n')
|
||||
{
|
||||
maxValStr += (char)allBytes[pos];
|
||||
pos++;
|
||||
}
|
||||
pos++; // Skip newline
|
||||
|
||||
if (!int.TryParse(maxValStr.Trim(), out int maxVal))
|
||||
throw new InvalidDataException($"Invalid max value in PGM: {maxValStr}");
|
||||
|
||||
// Read binary data
|
||||
int dataSize = (int)(width * height);
|
||||
if (pos + dataSize > allBytes.Length)
|
||||
throw new InvalidDataException($"Not enough data: expected {dataSize} bytes at position {pos}, file size {allBytes.Length}");
|
||||
|
||||
var rawData = new byte[dataSize];
|
||||
Array.Copy(allBytes, pos, rawData, 0, dataSize);
|
||||
|
||||
// Convert grayscale to occupancy grid format (int8_t):
|
||||
// - 0 (black/occupied) -> 100
|
||||
// - 255 (white/free) -> 0
|
||||
// - 205 (unknown) -> -1 (255 in unsigned byte)
|
||||
// IMPORTANT: Flip Y axis because PGM has row 0 at top, but ROS map has cell (0,0) at bottom-left
|
||||
var occupancyData = new byte[dataSize];
|
||||
for (int row = 0; row < height; row++)
|
||||
{
|
||||
for (int col = 0; col < width; col++)
|
||||
{
|
||||
// PGM index: row 0 = top
|
||||
int pgmIndex = row * (int)width + col;
|
||||
// ROS index: cell (x=col, y=0) at bottom → flip Y
|
||||
int rosIndex = ((int)height - 1 - row) * (int)width + col;
|
||||
|
||||
var pixel = rawData[pgmIndex];
|
||||
if (pixel >= 205) // Unknown (typically 205 in ROS maps)
|
||||
occupancyData[rosIndex] = 255; // Will be converted to -1 in JS
|
||||
else if (pixel < 128) // Dark = occupied
|
||||
occupancyData[rosIndex] = 100; // Occupied
|
||||
else // Light = free
|
||||
occupancyData[rosIndex] = 0; // Free
|
||||
}
|
||||
}
|
||||
|
||||
return (width, height, occupancyData);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,199 @@
|
||||
using System.Threading.Channels;
|
||||
|
||||
namespace RobotNet10.RobotApp.Xloc;
|
||||
|
||||
/// <summary>
|
||||
/// Single-thread async dispatcher for xloc sensor data — mirrors ROS ros::spin() model.
|
||||
/// All sensor types (IMU, Odom, Scan) are dispatched sequentially by ONE worker thread,
|
||||
/// eliminating lock contention on the native xloc library.
|
||||
///
|
||||
/// ROS model: ros::spin() processes all callbacks in a single thread, no mutex needed.
|
||||
/// This dispatcher replicates that pattern: one queue, one worker, zero lock contention.
|
||||
/// </summary>
|
||||
public class XlocAsyncDispatcher : IDisposable
|
||||
{
|
||||
private readonly XlocClient _xlocClient;
|
||||
private readonly ILogger? _logger;
|
||||
|
||||
// Single unified queue for ALL sensor types — like ROS callback queue
|
||||
private readonly Channel<SensorDispatchEvent> _dispatchQueue;
|
||||
private readonly Task _workerTask;
|
||||
private readonly CancellationTokenSource _cts;
|
||||
private bool _disposed = false;
|
||||
|
||||
private const int DefaultQueueCapacity = 300;
|
||||
|
||||
private readonly int _queueCapacity;
|
||||
private long _dropCount;
|
||||
|
||||
/// <summary>
|
||||
/// Sensor dispatch event wrapper
|
||||
/// </summary>
|
||||
private record SensorDispatchEvent(
|
||||
string SensorType,
|
||||
string SensorId,
|
||||
Action DispatchAction,
|
||||
long EnqueueTimestamp);
|
||||
|
||||
public XlocAsyncDispatcher(
|
||||
XlocClient xlocClient,
|
||||
int queueCapacity = DefaultQueueCapacity,
|
||||
ILogger? logger = null)
|
||||
{
|
||||
_xlocClient = xlocClient ?? throw new ArgumentNullException(nameof(xlocClient));
|
||||
_logger = logger;
|
||||
_queueCapacity = queueCapacity;
|
||||
|
||||
_dispatchQueue = Channel.CreateBounded<SensorDispatchEvent>(new BoundedChannelOptions(queueCapacity)
|
||||
{
|
||||
FullMode = BoundedChannelFullMode.DropOldest
|
||||
});
|
||||
|
||||
_cts = new CancellationTokenSource();
|
||||
_workerTask = RunWorker(_cts.Token);
|
||||
}
|
||||
|
||||
public ValueTask EnqueueOdometryAsync(Action dispatchAction, string sensorId = "odom")
|
||||
{
|
||||
ThrowIfDisposed();
|
||||
return EnqueueAsync("odometry", sensorId, dispatchAction);
|
||||
}
|
||||
|
||||
public ValueTask EnqueueImuAsync(Action dispatchAction, string sensorId = "imu")
|
||||
{
|
||||
ThrowIfDisposed();
|
||||
return EnqueueAsync("imu", sensorId, dispatchAction);
|
||||
}
|
||||
|
||||
public ValueTask EnqueueLaserScanAsync(Action dispatchAction, string sensorId)
|
||||
{
|
||||
ThrowIfDisposed();
|
||||
return EnqueueAsync("laserscan", sensorId, dispatchAction);
|
||||
}
|
||||
|
||||
private ValueTask EnqueueAsync(string sensorType, string sensorId, Action dispatchAction)
|
||||
{
|
||||
var enqueueTs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
|
||||
var @event = new SensorDispatchEvent(sensorType, sensorId, dispatchAction, enqueueTs);
|
||||
|
||||
if (_dispatchQueue.Reader.Count >= _queueCapacity)
|
||||
{
|
||||
Interlocked.Increment(ref _dropCount);
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
return _dispatchQueue.Writer.WriteAsync(@event, _cts.Token);
|
||||
}
|
||||
catch (ChannelClosedException)
|
||||
{
|
||||
throw new InvalidOperationException("XlocAsyncDispatcher has been disposed.");
|
||||
}
|
||||
}
|
||||
|
||||
private async Task RunWorker(CancellationToken ct)
|
||||
{
|
||||
_logger?.LogInformation("[XLOC-ASYNC] Single dispatch worker started (ROS spin model)");
|
||||
|
||||
try
|
||||
{
|
||||
await foreach (var @event in _dispatchQueue.Reader.ReadAllAsync(ct))
|
||||
{
|
||||
ExecuteDispatchEvent(@event);
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
_logger?.LogInformation("[XLOC-ASYNC] Dispatch worker cancelled");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger?.LogError(ex, "[XLOC-ASYNC-FATAL] Dispatch worker crashed: {Message}", ex.Message);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_logger?.LogInformation("[XLOC-ASYNC] Dispatch worker stopped");
|
||||
}
|
||||
}
|
||||
|
||||
private void ExecuteDispatchEvent(SensorDispatchEvent @event)
|
||||
{
|
||||
var queueWaitMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - @event.EnqueueTimestamp;
|
||||
|
||||
try
|
||||
{
|
||||
var dispatchStart = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
|
||||
|
||||
@event.DispatchAction();
|
||||
|
||||
var dispatchElapsedMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - dispatchStart;
|
||||
|
||||
_logger?.LogTrace(
|
||||
"[XLOC-ASYNC] {SensorType} sensor={SensorId} queue_wait={QueueWaitMs}ms dispatch={DispatchMs}ms",
|
||||
@event.SensorType,
|
||||
@event.SensorId,
|
||||
queueWaitMs,
|
||||
dispatchElapsedMs);
|
||||
|
||||
if (queueWaitMs >= 10 || dispatchElapsedMs >= 10)
|
||||
{
|
||||
// _logger?.LogWarning(
|
||||
// "[XLOC-DIAG] AsyncDispatcher {SensorType} sensor={SensorId} queue_wait={QueueWaitMs}ms dispatch={DispatchMs}ms",
|
||||
// @event.SensorType,
|
||||
// @event.SensorId,
|
||||
// queueWaitMs,
|
||||
// dispatchElapsedMs);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger?.LogError(ex,
|
||||
"[XLOC-ASYNC-ERROR] Failed to dispatch {SensorType} sensor={SensorId}: {Message}",
|
||||
@event.SensorType,
|
||||
@event.SensorId,
|
||||
ex.Message);
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
Dispose(true);
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
|
||||
protected virtual void Dispose(bool disposing)
|
||||
{
|
||||
if (_disposed)
|
||||
return;
|
||||
|
||||
_logger?.LogInformation("[XLOC-ASYNC] Shutting down dispatcher");
|
||||
|
||||
_dispatchQueue.Writer.TryComplete();
|
||||
_cts.Cancel();
|
||||
|
||||
try
|
||||
{
|
||||
_workerTask.Wait(TimeSpan.FromSeconds(5));
|
||||
}
|
||||
catch (AggregateException ex)
|
||||
{
|
||||
_logger?.LogWarning(ex, "[XLOC-ASYNC] Worker task did not complete gracefully");
|
||||
}
|
||||
|
||||
_cts.Dispose();
|
||||
|
||||
_disposed = true;
|
||||
_logger?.LogInformation("[XLOC-ASYNC] Dispatcher shut down");
|
||||
}
|
||||
|
||||
private void ThrowIfDisposed()
|
||||
{
|
||||
if (_disposed)
|
||||
throw new InvalidOperationException("XlocAsyncDispatcher has been disposed.");
|
||||
}
|
||||
|
||||
~XlocAsyncDispatcher()
|
||||
{
|
||||
Dispose(false);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,110 @@
|
||||
using Microsoft.Extensions.Configuration;
|
||||
using Microsoft.Extensions.Hosting;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace RobotNet10.RobotApp.Xloc;
|
||||
|
||||
/// <summary>
|
||||
/// Polls XLOC diagnostics and calls <see cref="XlocIntegrationService.StartLocalization"/> when
|
||||
/// state transitions to READY (3) with an active map. Does not depend on the web UI.
|
||||
/// </summary>
|
||||
public sealed class XlocAutoLocalizationHostedService : BackgroundService
|
||||
{
|
||||
private readonly XlocIntegrationService _xloc;
|
||||
private readonly ILogger<XlocAutoLocalizationHostedService> _logger;
|
||||
private readonly XlocIntegrationConfiguration _config = new();
|
||||
private byte? _previousXlocState;
|
||||
private bool _hasAutoStartedOnce;
|
||||
|
||||
public XlocAutoLocalizationHostedService(
|
||||
XlocIntegrationService xloc,
|
||||
IConfiguration configuration,
|
||||
ILogger<XlocAutoLocalizationHostedService> logger)
|
||||
{
|
||||
_xloc = xloc;
|
||||
_logger = logger;
|
||||
|
||||
var section = configuration.GetSection("Xloc:Integration");
|
||||
if (section.Exists())
|
||||
section.Bind(_config);
|
||||
}
|
||||
|
||||
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
|
||||
{
|
||||
if (!_config.Enabled)
|
||||
{
|
||||
_logger.LogInformation("XlocAutoLocalizationHostedService skipped: Xloc:Integration:Enabled is false");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_config.AutoStartLocalizationOnReady)
|
||||
{
|
||||
_logger.LogInformation("XlocAutoLocalizationHostedService skipped: AutoStartLocalizationOnReady is false");
|
||||
return;
|
||||
}
|
||||
|
||||
var pollMs = Math.Clamp(_config.AutoStartLocalizationPollIntervalMs, 100, 10_000);
|
||||
var cooldownMs = Math.Max(0, _config.AutoStartLocalizationCooldownAfterStopMs);
|
||||
|
||||
_logger.LogInformation(
|
||||
"XlocAutoLocalizationHostedService running (poll {PollMs}ms, stop cooldown {CooldownMs}ms)",
|
||||
pollMs,
|
||||
cooldownMs);
|
||||
|
||||
while (!stoppingToken.IsCancellationRequested)
|
||||
{
|
||||
try
|
||||
{
|
||||
await Task.Delay(pollMs, stoppingToken).ConfigureAwait(false);
|
||||
|
||||
var diag = _xloc.GetDiagnostics();
|
||||
if (diag == null)
|
||||
continue;
|
||||
|
||||
var s = diag.XlocState;
|
||||
var transitionedToReady = s == 3 && (!_previousXlocState.HasValue || _previousXlocState.Value != 3);
|
||||
_previousXlocState = s;
|
||||
|
||||
if (!transitionedToReady || _hasAutoStartedOnce)
|
||||
continue;
|
||||
|
||||
if (string.IsNullOrWhiteSpace(diag.CurrentActiveMap))
|
||||
{
|
||||
_logger.LogDebug("Auto-start localization skipped: no active map in diagnostics");
|
||||
continue;
|
||||
}
|
||||
|
||||
if (_xloc.LastLocalizationStopUtc.HasValue)
|
||||
{
|
||||
var elapsedMs = (DateTime.UtcNow - _xloc.LastLocalizationStopUtc.Value).TotalMilliseconds;
|
||||
if (elapsedMs < cooldownMs)
|
||||
{
|
||||
_logger.LogDebug(
|
||||
"Auto-start localization skipped: cooldown after stop ({ElapsedMs:F0}ms < {CooldownMs}ms)",
|
||||
elapsedMs,
|
||||
cooldownMs);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
_hasAutoStartedOnce = true;
|
||||
|
||||
_logger.LogInformation(
|
||||
"XLOC READY with active map '{Map}'. Auto-starting localization...",
|
||||
diag.CurrentActiveMap);
|
||||
|
||||
var ok = _xloc.StartLocalization();
|
||||
if (!ok)
|
||||
_logger.LogWarning("Auto-start localization failed (StartLocalization returned false)");
|
||||
}
|
||||
catch (OperationCanceledException) when (stoppingToken.IsCancellationRequested)
|
||||
{
|
||||
break;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error in XlocAutoLocalizationHostedService loop");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
1108
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Xloc/XlocClient.cs
Normal file
1108
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Xloc/XlocClient.cs
Normal file
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,357 @@
|
||||
using System.Runtime.InteropServices;
|
||||
using RobotNet10.Shared;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Sensor;
|
||||
// using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace RobotNet10.RobotApp.Xloc;
|
||||
|
||||
/// <summary>
|
||||
/// Extension methods to convert C# sensor structs to xloc C-compatible structs
|
||||
/// </summary>
|
||||
public static class XlocConversionExtensions
|
||||
{
|
||||
/// <summary>
|
||||
/// Convert C# Header to xloc_header_t
|
||||
/// NOTE: Caller must free FrameId using Marshal.FreeHGlobal
|
||||
/// </summary>
|
||||
public static xloc_header_t ToXlocHeader(this Header header)
|
||||
{
|
||||
var xlocHeader = new xloc_header_t
|
||||
{
|
||||
seq = header.Seq,
|
||||
stamp = header.Stamp.ToXlocUnixTime(),
|
||||
frame_id = IntPtr.Zero
|
||||
};
|
||||
|
||||
// Marshal frame_id string to unmanaged memory
|
||||
if (!string.IsNullOrEmpty(header.FrameId))
|
||||
{
|
||||
xlocHeader.frame_id = Marshal.StringToHGlobalAnsi(header.FrameId);
|
||||
}
|
||||
|
||||
return xlocHeader;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert DateTime to xloc_unix_time_t (Unix epoch time)
|
||||
/// </summary>
|
||||
// public static xloc_unix_time_t ToXlocUnixTime(this DateTime timestamp)
|
||||
// {
|
||||
// // Convert to Unix time (seconds and nanoseconds since 1970-01-01)
|
||||
// var epoch = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);
|
||||
// var duration = timestamp.ToUniversalTime() - epoch;
|
||||
|
||||
// var totalSeconds = (long)duration.TotalSeconds;
|
||||
// var nanoseconds = (long)((duration.TotalSeconds - totalSeconds) * 1_000_000_000);
|
||||
|
||||
// var xlocTime = new xloc_unix_time_t
|
||||
// {
|
||||
// sec = (uint)totalSeconds,
|
||||
// nsec = (uint)nanoseconds
|
||||
// };
|
||||
// return xlocTime;
|
||||
// }
|
||||
public static xloc_unix_time_t ToXlocUnixTime(this DateTime timestamp)
|
||||
{
|
||||
var utc = timestamp.Kind == DateTimeKind.Utc
|
||||
? timestamp
|
||||
: timestamp.ToUniversalTime();
|
||||
|
||||
long ticksSinceEpoch = utc.Ticks - DateTime.UnixEpoch.Ticks;
|
||||
long totalNanoseconds = ticksSinceEpoch * 100; // 1 tick = 100 ns
|
||||
|
||||
uint sec = (uint)(totalNanoseconds / 1_000_000_000);
|
||||
uint nsec = (uint)(totalNanoseconds % 1_000_000_000);
|
||||
|
||||
return new xloc_unix_time_t
|
||||
{
|
||||
sec = sec,
|
||||
nsec = nsec
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// Convert C# Odometry to xloc_odometry_t
|
||||
/// NOTE: Caller must free frame_id and child_frame_id using FreeXlocOdometry
|
||||
/// </summary>
|
||||
public static xloc_odometry_t ToXlocOdometry(this Odometry odom)
|
||||
{
|
||||
var xlocOdom = new xloc_odometry_t
|
||||
{
|
||||
header = odom.Header.ToXlocHeader(),
|
||||
child_frame_id = Marshal.StringToHGlobalAnsi(odom.ChildFrameId ?? string.Empty),
|
||||
|
||||
// Pose - flattened arrays (matching C API)
|
||||
pose_position = new double[]
|
||||
{
|
||||
odom.Pose.Pose.Position.X,
|
||||
odom.Pose.Pose.Position.Y,
|
||||
odom.Pose.Pose.Position.Z
|
||||
},
|
||||
pose_orientation = new double[]
|
||||
{
|
||||
odom.Pose.Pose.Orientation.X,
|
||||
odom.Pose.Pose.Orientation.Y,
|
||||
odom.Pose.Pose.Orientation.Z,
|
||||
odom.Pose.Pose.Orientation.W
|
||||
},
|
||||
pose_covariance = odom.Pose.Covariance ?? new double[36],
|
||||
|
||||
// Twist - flattened arrays (matching C API)
|
||||
twist_linear = new double[]
|
||||
{
|
||||
odom.Twist.Twist.Linear.X,
|
||||
odom.Twist.Twist.Linear.Y,
|
||||
odom.Twist.Twist.Linear.Z
|
||||
},
|
||||
twist_angular = new double[]
|
||||
{
|
||||
odom.Twist.Twist.Angular.X,
|
||||
odom.Twist.Twist.Angular.Y,
|
||||
odom.Twist.Twist.Angular.Z
|
||||
},
|
||||
twist_covariance = odom.Twist.Covariance ?? new double[36]
|
||||
};
|
||||
|
||||
return xlocOdom;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert C# Imu to xloc_imu_t
|
||||
/// NOTE: Caller must free FrameId using FreeXlocImu
|
||||
/// </summary>
|
||||
public static xloc_imu_t ToXlocImu(this Imu imu)
|
||||
{
|
||||
// Validate and normalize quaternion
|
||||
var quat = imu.Orientation;
|
||||
double quatLength = Math.Sqrt(quat.X * quat.X + quat.Y * quat.Y + quat.Z * quat.Z + quat.W * quat.W);
|
||||
|
||||
if (quatLength < 0.001 || double.IsNaN(quatLength) || double.IsInfinity(quatLength))
|
||||
{
|
||||
// Invalid quaternion, use identity
|
||||
Console.WriteLine("[XLOC] Invalid IMU quaternion detected, using identity");
|
||||
quat = new RobotNet10.Shared.Geometry.Quaternion { X = 0, Y = 0, Z = 0, W = 1 };
|
||||
quatLength = 1.0;
|
||||
}
|
||||
|
||||
// Normalize quaternion
|
||||
quat = new RobotNet10.Shared.Geometry.Quaternion
|
||||
{
|
||||
X = quat.X / quatLength,
|
||||
Y = quat.Y / quatLength,
|
||||
Z = quat.Z / quatLength,
|
||||
W = quat.W / quatLength
|
||||
};
|
||||
|
||||
// Validate angular velocity (clamp extremely large values)
|
||||
const double MAX_ANGULAR_VEL = 10.0; // rad/s
|
||||
var angVel = imu.AngularVelocity;
|
||||
if (Math.Abs(angVel.X) > MAX_ANGULAR_VEL || Math.Abs(angVel.Y) > MAX_ANGULAR_VEL ||
|
||||
Math.Abs(angVel.Z) > MAX_ANGULAR_VEL ||
|
||||
double.IsNaN(angVel.X) || double.IsNaN(angVel.Y) || double.IsNaN(angVel.Z))
|
||||
{
|
||||
Console.WriteLine($"[XLOC] Invalid angular velocity detected: ({angVel.X}, {angVel.Y}, {angVel.Z}), clamping");
|
||||
angVel = new Vector3
|
||||
{
|
||||
X = Math.Clamp(double.IsNaN(angVel.X) ? 0 : angVel.X, -MAX_ANGULAR_VEL, MAX_ANGULAR_VEL),
|
||||
Y = Math.Clamp(double.IsNaN(angVel.Y) ? 0 : angVel.Y, -MAX_ANGULAR_VEL, MAX_ANGULAR_VEL),
|
||||
Z = Math.Clamp(double.IsNaN(angVel.Z) ? 0 : angVel.Z, -MAX_ANGULAR_VEL, MAX_ANGULAR_VEL)
|
||||
};
|
||||
}
|
||||
|
||||
var xlocImu = new xloc_imu_t
|
||||
{
|
||||
header = imu.Header.ToXlocHeader(),
|
||||
|
||||
orientation = new double[4]
|
||||
{
|
||||
quat.X,
|
||||
quat.Y,
|
||||
quat.Z,
|
||||
quat.W
|
||||
},
|
||||
orientation_covariance = imu.OrientationCovariance ?? new double[9],
|
||||
|
||||
angular_velocity = new double[3]
|
||||
{
|
||||
angVel.X,
|
||||
angVel.Y,
|
||||
angVel.Z
|
||||
},
|
||||
angular_velocity_covariance = imu.AngularVelocityCovariance ?? new double[9],
|
||||
|
||||
linear_acceleration = new double[3]
|
||||
{
|
||||
imu.LinearAcceleration.X,
|
||||
imu.LinearAcceleration.Y,
|
||||
imu.LinearAcceleration.Z
|
||||
},
|
||||
linear_acceleration_covariance = imu.LinearAccelerationCovariance ?? new double[9]
|
||||
};
|
||||
|
||||
return xlocImu;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert C# LaserScan to xloc_laserscan_t
|
||||
/// NOTE: Caller must free all allocated memory using FreeXlocLaserScan
|
||||
/// </summary>
|
||||
public static xloc_laserscan_t ToXlocLaserScan(this LaserScan scan)
|
||||
{
|
||||
var xlocScan = new xloc_laserscan_t
|
||||
{
|
||||
header = scan.Header.ToXlocHeader(),
|
||||
angle_min = (float)scan.AngleMin,
|
||||
angle_max = (float)scan.AngleMax,
|
||||
angle_increment = (float)scan.AngleIncrement,
|
||||
time_increment = (float)scan.TimeIncrement,
|
||||
scan_time = (float)scan.ScanTime,
|
||||
range_min = (float)scan.RangeMin,
|
||||
range_max = (float)scan.RangeMax,
|
||||
ranges_length = (nuint)(scan.Ranges?.Length ?? 0),
|
||||
intensities_length = 0
|
||||
};
|
||||
|
||||
// Allocate and copy ranges array with validation
|
||||
if (scan.Ranges != null && scan.Ranges.Length > 0)
|
||||
{
|
||||
// Sanitize ranges: replace NaN/Infinity/negative with max range
|
||||
var sanitizedRanges = new float[scan.Ranges.Length];
|
||||
int invalidCount = 0;
|
||||
|
||||
for (int i = 0; i < scan.Ranges.Length; i++)
|
||||
{
|
||||
float range = (float)scan.Ranges[i];
|
||||
if (float.IsNaN(range) || float.IsInfinity(range) || range < 0)
|
||||
{
|
||||
sanitizedRanges[i] = (float)scan.RangeMax;
|
||||
invalidCount++;
|
||||
}
|
||||
else
|
||||
{
|
||||
sanitizedRanges[i] = range;
|
||||
}
|
||||
}
|
||||
|
||||
if (invalidCount > 0)
|
||||
{
|
||||
// Console.WriteLine($"[XLOC] Sanitized {invalidCount}/{scan.Ranges.Length} invalid laser scan ranges");
|
||||
}
|
||||
|
||||
int rangesSize = sanitizedRanges.Length * sizeof(float);
|
||||
xlocScan.ranges = Marshal.AllocHGlobal(rangesSize);
|
||||
Marshal.Copy(sanitizedRanges, 0, xlocScan.ranges, sanitizedRanges.Length);
|
||||
}
|
||||
else
|
||||
{
|
||||
xlocScan.ranges = IntPtr.Zero;
|
||||
}
|
||||
|
||||
// Intensities are optional; when provided they should match ranges count.
|
||||
if (scan.Intensities != null && scan.Intensities.Length > 0)
|
||||
{
|
||||
int expectedLength = scan.Ranges?.Length ?? 0;
|
||||
if (scan.Intensities.Length == expectedLength)
|
||||
{
|
||||
var sanitizedIntensities = new float[scan.Intensities.Length];
|
||||
for (int i = 0; i < scan.Intensities.Length; i++)
|
||||
{
|
||||
float intensity = (float)scan.Intensities[i];
|
||||
sanitizedIntensities[i] = (float.IsNaN(intensity) || float.IsInfinity(intensity)) ? 0f : intensity;
|
||||
}
|
||||
|
||||
int intensitiesSize = sanitizedIntensities.Length * sizeof(float);
|
||||
xlocScan.intensities = Marshal.AllocHGlobal(intensitiesSize);
|
||||
Marshal.Copy(sanitizedIntensities, 0, xlocScan.intensities, sanitizedIntensities.Length);
|
||||
xlocScan.intensities_length = (nuint)sanitizedIntensities.Length;
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine($"[XLOC] Ignoring intensities due to length mismatch: ranges={expectedLength}, intensities={scan.Intensities.Length}");
|
||||
xlocScan.intensities = IntPtr.Zero;
|
||||
xlocScan.intensities_length = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
xlocScan.intensities = IntPtr.Zero;
|
||||
xlocScan.intensities_length = 0;
|
||||
}
|
||||
|
||||
return xlocScan;
|
||||
}
|
||||
|
||||
#region Memory Management
|
||||
|
||||
/// <summary>
|
||||
/// Free memory allocated for xloc_odometry_t
|
||||
/// </summary>
|
||||
public static void FreeXlocOdometry(ref xloc_odometry_t odom)
|
||||
{
|
||||
if (odom.header.frame_id != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(odom.header.frame_id);
|
||||
odom.header.frame_id = IntPtr.Zero;
|
||||
}
|
||||
|
||||
if (odom.child_frame_id != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(odom.child_frame_id);
|
||||
odom.child_frame_id = IntPtr.Zero;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Free memory allocated for xloc_imu_t
|
||||
/// </summary>
|
||||
public static void FreeXlocImu(ref xloc_imu_t imu)
|
||||
{
|
||||
if (imu.header.frame_id != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(imu.header.frame_id);
|
||||
imu.header.frame_id = IntPtr.Zero;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Free memory allocated for xloc_laserscan_t
|
||||
/// </summary>
|
||||
public static void FreeXlocLaserScan(ref xloc_laserscan_t scan)
|
||||
{
|
||||
if (scan.header.frame_id != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(scan.header.frame_id);
|
||||
scan.header.frame_id = IntPtr.Zero;
|
||||
}
|
||||
|
||||
if (scan.ranges != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(scan.ranges);
|
||||
scan.ranges = IntPtr.Zero;
|
||||
}
|
||||
|
||||
if (scan.intensities != IntPtr.Zero)
|
||||
{
|
||||
Marshal.FreeHGlobal(scan.intensities);
|
||||
scan.intensities = IntPtr.Zero;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get managed string from xloc status response and free the C string
|
||||
/// </summary>
|
||||
public static string GetMessageAndFree(ref xloc_status_response_t response)
|
||||
{
|
||||
if (response.message == IntPtr.Zero)
|
||||
return string.Empty;
|
||||
|
||||
string message = Marshal.PtrToStringUTF8(response.message) ?? string.Empty;
|
||||
XlocNativeInterface.xloc_free_cstring(response.message);
|
||||
response.message = IntPtr.Zero;
|
||||
return message;
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
162
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Xloc/XlocInterop.cs
Normal file
162
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Xloc/XlocInterop.cs
Normal file
@@ -0,0 +1,162 @@
|
||||
using System.Runtime.InteropServices;
|
||||
using RobotNet10.RobotApp.Navigation;
|
||||
|
||||
namespace RobotNet10.RobotApp.Xloc;
|
||||
|
||||
/// <summary>
|
||||
/// C-compatible structs for xloc C API interop
|
||||
/// Based on /home/robotics/sonvh/xloc_Linux_0.1.0/_CPack_Packages/Linux/DEB/xloc-0.1.0-Linux/usr/include/xloc/xloc_c_api.h
|
||||
/// </summary>
|
||||
|
||||
/// <summary>
|
||||
/// Unix timestamp with seconds and nanoseconds (xloc_unix_time_t)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct xloc_unix_time_t
|
||||
{
|
||||
public uint sec;
|
||||
public uint nsec;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Message header (xloc_header_t)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct xloc_header_t
|
||||
{
|
||||
public uint seq;
|
||||
public xloc_unix_time_t stamp;
|
||||
public IntPtr frame_id; // char* - must be allocated and freed
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Odometry message (xloc_odometry_t)
|
||||
/// CRITICAL: Must match exact C API definition in xloc_c_api.h (lines 138-149)
|
||||
/// Uses flattened arrays, NOT nested xloc_pose_t or xloc_twist_t structs
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct xloc_odometry_t
|
||||
{
|
||||
public xloc_header_t header;
|
||||
public IntPtr child_frame_id; // char* - must be allocated and freed
|
||||
|
||||
// Pose with covariance (flattened, not using xloc_pose_t)
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
|
||||
public double[] pose_position; // [3] - x, y, z
|
||||
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
|
||||
public double[] pose_orientation; // [4] - x, y, z, w (quaternion)
|
||||
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 36)]
|
||||
public double[] pose_covariance; // [36] - 6x6 matrix
|
||||
|
||||
// Twist with covariance (flattened, not using xloc_twist_t)
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
|
||||
public double[] twist_linear; // [3] - x, y, z
|
||||
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
|
||||
public double[] twist_angular; // [3] - x, y, z
|
||||
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 36)]
|
||||
public double[] twist_covariance; // [36] - 6x6 matrix
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// IMU message (xloc_imu_t)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct xloc_imu_t
|
||||
{
|
||||
public xloc_header_t header;
|
||||
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
|
||||
public double[] orientation; // [4] - x, y, z, w (quaternion)
|
||||
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)]
|
||||
public double[] orientation_covariance; // [9] - 3x3 matrix
|
||||
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
|
||||
public double[] angular_velocity; // [3] - x, y, z
|
||||
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)]
|
||||
public double[] angular_velocity_covariance; // [9] - 3x3 matrix
|
||||
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
|
||||
public double[] linear_acceleration; // [3] - x, y, z
|
||||
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)]
|
||||
public double[] linear_acceleration_covariance; // [9] - 3x3 matrix
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// LaserScan message (xloc_laserscan_t)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct xloc_laserscan_t
|
||||
{
|
||||
public xloc_header_t header;
|
||||
public float angle_min;
|
||||
public float angle_max;
|
||||
public float angle_increment;
|
||||
public float time_increment;
|
||||
public float scan_time;
|
||||
public float range_min;
|
||||
public float range_max;
|
||||
|
||||
public IntPtr ranges; // float* - pointer to array
|
||||
public nuint ranges_length; // size_t - number of elements
|
||||
|
||||
public IntPtr intensities; // float* - pointer to array
|
||||
public nuint intensities_length; // size_t - number of elements
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Status response from xloc (xloc_status_response_t)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct xloc_status_response_t
|
||||
{
|
||||
public byte code;
|
||||
public IntPtr message; // char* - must be freed with xloc_free_cstring
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Pose (xloc_pose_t)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct xloc_pose_t
|
||||
{
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
|
||||
public double[] position; // [3] - x, y, z
|
||||
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
|
||||
public double[] orientation; // [4] - x, y, z, w (quaternion)
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Diagnostics information from xloc (xloc_diagnostics_t)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct xloc_diagnostics_t
|
||||
{
|
||||
public xloc_header_t header;
|
||||
public byte xloc_state; // 0=MAPPING, 1=LOCALIZATION, 2=PROCESSING, 3=READY, 4=ERROR
|
||||
public IntPtr current_active_map; // char* - must be freed
|
||||
public double reliability; // 0.0 to 1.0
|
||||
public double matching_score; // SLAM matching quality
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Occupancy grid map (xloc_occupancy_grid_t)
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
public struct xloc_occupancy_grid_t
|
||||
{
|
||||
public xloc_header_t header;
|
||||
public float resolution; // meters per cell
|
||||
public uint width; // cells
|
||||
public uint height; // cells
|
||||
public xloc_pose_t origin; // pose of cell (0,0) in map frame
|
||||
public IntPtr data; // int8_t* - pointer to occupancy data (width*height bytes)
|
||||
public nuint data_length; // size_t - number of bytes
|
||||
}
|
||||
@@ -0,0 +1,225 @@
|
||||
using System.IO.Compression;
|
||||
using Microsoft.AspNetCore.Http;
|
||||
|
||||
namespace RobotNet10.RobotApp.Xloc;
|
||||
|
||||
public static class XlocMapFileEndpoints
|
||||
{
|
||||
public static bool TryValidateMapFolderName(string mapName, out string safeName)
|
||||
{
|
||||
safeName = "";
|
||||
if (string.IsNullOrWhiteSpace(mapName))
|
||||
return false;
|
||||
|
||||
var t = mapName.Trim();
|
||||
if (t.Length > 200)
|
||||
return false;
|
||||
if (string.Equals(t, "tmp", StringComparison.OrdinalIgnoreCase))
|
||||
return false;
|
||||
if (t.IndexOfAny(Path.GetInvalidFileNameChars()) >= 0)
|
||||
return false;
|
||||
if (t.Contains('/', StringComparison.Ordinal) || t.Contains('\\', StringComparison.Ordinal))
|
||||
return false;
|
||||
|
||||
safeName = t;
|
||||
return true;
|
||||
}
|
||||
|
||||
public static IResult DeleteMapFolder(string mapName)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (!TryValidateMapFolderName(mapName, out var safeName))
|
||||
return Results.BadRequest(new { status = "error", message = "Invalid map name" });
|
||||
|
||||
var mapsDir = XlocPaths.GetMapsDirectory();
|
||||
if (!Directory.Exists(mapsDir))
|
||||
return Results.NotFound(new { status = "error", message = "Maps directory not found" });
|
||||
|
||||
var mapFolder = Path.Combine(mapsDir, safeName);
|
||||
if (!Directory.Exists(mapFolder))
|
||||
return Results.NotFound(new { status = "error", message = $"Map '{safeName}' not found" });
|
||||
|
||||
Directory.Delete(mapFolder, recursive: true);
|
||||
return Results.Ok(new { status = "success", message = "Map deleted", map_name = safeName });
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = ex.Message });
|
||||
}
|
||||
}
|
||||
|
||||
public static IResult DownloadMapAsZip(string mapName)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (!TryValidateMapFolderName(mapName, out var safeName))
|
||||
return Results.BadRequest(new { status = "error", message = "Invalid map name" });
|
||||
|
||||
var mapsDir = XlocPaths.GetMapsDirectory();
|
||||
var mapFolder = Path.Combine(mapsDir, safeName);
|
||||
if (!Directory.Exists(mapFolder))
|
||||
return Results.NotFound(new { status = "error", message = $"Map '{safeName}' not found" });
|
||||
|
||||
using var ms = new MemoryStream();
|
||||
using (var archive = new ZipArchive(ms, ZipArchiveMode.Create, leaveOpen: true))
|
||||
{
|
||||
foreach (var filePath in Directory.EnumerateFiles(mapFolder, "*", SearchOption.AllDirectories))
|
||||
{
|
||||
var rel = Path.GetRelativePath(mapFolder, filePath);
|
||||
var entryName = $"{safeName}/{rel.Replace('\\', '/')}";
|
||||
var entry = archive.CreateEntry(entryName, CompressionLevel.Fastest);
|
||||
using var entryStream = entry.Open();
|
||||
using var fileStream = File.OpenRead(filePath);
|
||||
fileStream.CopyTo(entryStream);
|
||||
}
|
||||
}
|
||||
|
||||
return Results.File(ms.ToArray(), "application/zip", $"{safeName}.zip");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = ex.Message });
|
||||
}
|
||||
}
|
||||
|
||||
public static async Task<IResult> ImportMapFromZipAsync(HttpRequest request)
|
||||
{
|
||||
if (!request.HasFormContentType)
|
||||
return Results.BadRequest(new { status = "error", message = "Multipart form required" });
|
||||
|
||||
var form = await request.ReadFormAsync();
|
||||
var file = form.Files.GetFile("file");
|
||||
if (file == null || file.Length == 0)
|
||||
return Results.BadRequest(new { status = "error", message = "Zip file (field: file) required" });
|
||||
|
||||
if (!file.FileName.EndsWith(".zip", StringComparison.OrdinalIgnoreCase))
|
||||
return Results.BadRequest(new { status = "error", message = "File must be a .zip archive" });
|
||||
|
||||
var zipStem = Path.GetFileNameWithoutExtension(file.FileName);
|
||||
if (string.IsNullOrWhiteSpace(zipStem) || zipStem is "." or "..")
|
||||
return Results.BadRequest(new { status = "error", message = "Invalid zip file name" });
|
||||
|
||||
if (!TryValidateMapFolderName(zipStem, out var finalTargetName))
|
||||
return Results.BadRequest(new
|
||||
{
|
||||
status = "error",
|
||||
message = "Map name is taken from the zip file name. Rename the file (letters, numbers, dash, underscore; no path characters)."
|
||||
});
|
||||
|
||||
var tempZip = Path.Combine(Path.GetTempPath(), $"xloc_import_{Guid.NewGuid():N}.zip");
|
||||
var tempExtract = Path.Combine(Path.GetTempPath(), $"xloc_extract_{Guid.NewGuid():N}");
|
||||
try
|
||||
{
|
||||
await using (var fs = File.Create(tempZip))
|
||||
await file.CopyToAsync(fs);
|
||||
|
||||
Directory.CreateDirectory(tempExtract);
|
||||
ZipFile.ExtractToDirectory(tempZip, tempExtract);
|
||||
|
||||
if (!TryResolveImportedMap(tempExtract, out var sourceDir))
|
||||
return Results.BadRequest(new
|
||||
{
|
||||
status = "error",
|
||||
message = "Could not find a valid map. Use a zip with one top-level folder containing a .yaml file, or with .yaml files at the archive root."
|
||||
});
|
||||
|
||||
var mapsDir = XlocPaths.GetMapsDirectory();
|
||||
Directory.CreateDirectory(mapsDir);
|
||||
var destPath = Path.Combine(mapsDir, finalTargetName);
|
||||
if (Directory.Exists(destPath))
|
||||
return Results.Conflict(new { status = "error", message = $"A map named '{finalTargetName}' already exists. Rename the zip file or remove the existing map folder." });
|
||||
|
||||
if (string.Equals(sourceDir, tempExtract, StringComparison.Ordinal))
|
||||
{
|
||||
Directory.CreateDirectory(destPath);
|
||||
CopyDirectoryRecursive(tempExtract, destPath);
|
||||
}
|
||||
else
|
||||
{
|
||||
Directory.Move(sourceDir, destPath);
|
||||
}
|
||||
|
||||
return Results.Ok(new { status = "success", map_name = finalTargetName });
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return Results.BadRequest(new { status = "error", message = ex.Message });
|
||||
}
|
||||
finally
|
||||
{
|
||||
TryDeleteFile(tempZip);
|
||||
TryDeleteDirectory(tempExtract);
|
||||
}
|
||||
}
|
||||
|
||||
private static bool TryResolveImportedMap(string extractRoot, out string sourceDir)
|
||||
{
|
||||
sourceDir = "";
|
||||
|
||||
var rootFiles = Directory.GetFiles(extractRoot);
|
||||
var rootDirs = Directory.GetDirectories(extractRoot)
|
||||
.Where(d => !string.Equals(Path.GetFileName(d), "__MACOSX", StringComparison.OrdinalIgnoreCase))
|
||||
.ToArray();
|
||||
|
||||
if (Directory.GetFiles(extractRoot, "*.yaml", SearchOption.TopDirectoryOnly).Length > 0)
|
||||
{
|
||||
sourceDir = extractRoot;
|
||||
return true;
|
||||
}
|
||||
|
||||
var dirsWithYaml = rootDirs
|
||||
.Where(d => Directory.GetFiles(d, "*.yaml", SearchOption.AllDirectories).Length > 0)
|
||||
.ToList();
|
||||
|
||||
if (dirsWithYaml.Count == 1 && rootFiles.Length == 0)
|
||||
{
|
||||
sourceDir = dirsWithYaml[0];
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private static void CopyDirectoryRecursive(string sourceDir, string destDir)
|
||||
{
|
||||
foreach (var file in Directory.GetFiles(sourceDir))
|
||||
{
|
||||
var destFile = Path.Combine(destDir, Path.GetFileName(file));
|
||||
File.Copy(file, destFile, overwrite: false);
|
||||
}
|
||||
|
||||
foreach (var dir in Directory.GetDirectories(sourceDir))
|
||||
{
|
||||
var destSub = Path.Combine(destDir, Path.GetFileName(dir));
|
||||
Directory.CreateDirectory(destSub);
|
||||
CopyDirectoryRecursive(dir, destSub);
|
||||
}
|
||||
}
|
||||
|
||||
private static void TryDeleteFile(string path)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (File.Exists(path))
|
||||
File.Delete(path);
|
||||
}
|
||||
catch
|
||||
{
|
||||
// ignore
|
||||
}
|
||||
}
|
||||
|
||||
private static void TryDeleteDirectory(string path)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (Directory.Exists(path))
|
||||
Directory.Delete(path, recursive: true);
|
||||
}
|
||||
catch
|
||||
{
|
||||
// ignore
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,217 @@
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
namespace RobotNet10.RobotApp.Xloc;
|
||||
|
||||
/// <summary>
|
||||
/// P/Invoke declarations for xloc C API
|
||||
/// </summary>
|
||||
public static class XlocNativeInterface
|
||||
{
|
||||
// Library path - adjust/ if needed
|
||||
private const string LibraryPath = "/usr/lib/libxloc.so";
|
||||
|
||||
#region Creation / Destruction
|
||||
|
||||
/// <summary>
|
||||
/// Create an xloc instance
|
||||
/// </summary>
|
||||
/// <param name="tfBuffer">TF3 buffer core (pass IntPtr.Zero if not using TF)</param>
|
||||
/// <returns>Handle to xloc instance</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern IntPtr xloc_create(IntPtr tfBuffer);
|
||||
|
||||
/// <summary>
|
||||
/// Destroy an xloc instance
|
||||
/// </summary>
|
||||
/// <param name="handle">Handle to xloc instance</param>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void xloc_destroy(IntPtr handle);
|
||||
|
||||
#endregion
|
||||
|
||||
#region Sensor Data Dispatch
|
||||
|
||||
/// <summary>
|
||||
/// Dispatch laser scan data to xloc
|
||||
/// </summary>
|
||||
/// <param name="handle">Handle to xloc instance</param>
|
||||
/// <param name="sensorId">Sensor ID (null-terminated string)</param>
|
||||
/// <param name="scan">Pointer to laser scan struct</param>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void xloc_dispatch_laserscan(
|
||||
IntPtr handle,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string sensorId,
|
||||
ref xloc_laserscan_t scan);
|
||||
|
||||
/// <summary>
|
||||
/// Dispatch IMU data to xloc
|
||||
/// </summary>
|
||||
/// <param name="handle">Handle to xloc instance</param>
|
||||
/// <param name="sensorId">Sensor ID (null-terminated string)</param>
|
||||
/// <param name="imu">Pointer to IMU struct</param>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void xloc_dispatch_imu(
|
||||
IntPtr handle,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string sensorId,
|
||||
ref xloc_imu_t imu);
|
||||
|
||||
/// <summary>
|
||||
/// Dispatch odometry data to xloc
|
||||
/// </summary>
|
||||
/// <param name="handle">Handle to xloc instance</param>
|
||||
/// <param name="sensorId">Sensor ID (null-terminated string)</param>
|
||||
/// <param name="odom">Pointer to odometry struct</param>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void xloc_dispatch_odometry(
|
||||
IntPtr handle,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string sensorId,
|
||||
ref xloc_odometry_t odom);
|
||||
|
||||
#endregion
|
||||
|
||||
#region Localization Control
|
||||
|
||||
/// <summary>
|
||||
/// Activate a map
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern xloc_status_response_t xloc_activate_map(
|
||||
IntPtr handle,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string mapFileName);
|
||||
|
||||
/// <summary>
|
||||
/// Switch to a different map
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern xloc_status_response_t xloc_switch_map(
|
||||
IntPtr handle,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string mapFileName,
|
||||
int useInitialPose,
|
||||
ref xloc_pose_t initialPose);
|
||||
|
||||
/// <summary>
|
||||
/// Start localization
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern xloc_status_response_t xloc_start_localization(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Stop localization
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern xloc_status_response_t xloc_stop_localization(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Start mapping
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern xloc_status_response_t xloc_start_mapping(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Stop mapping and optionally save the map
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern xloc_status_response_t xloc_stop_mapping(
|
||||
IntPtr handle,
|
||||
[MarshalAs(UnmanagedType.LPUTF8Str)] string mapFileName);
|
||||
|
||||
/// <summary>
|
||||
/// Set initial pose
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern xloc_status_response_t xloc_set_initial_pose(
|
||||
IntPtr handle,
|
||||
ref xloc_pose_t initialPose);
|
||||
|
||||
/// <summary>
|
||||
/// Change map origin
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern xloc_status_response_t xloc_change_map_origin(
|
||||
IntPtr handle,
|
||||
ref xloc_pose_t newMapOrigin);
|
||||
|
||||
/// <summary>
|
||||
/// Start updating existing map
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern xloc_status_response_t xloc_start_update_map(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Stop updating map and optionally save
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern xloc_status_response_t xloc_stop_update_map(
|
||||
IntPtr handle,
|
||||
int saveUpdatedMap);
|
||||
|
||||
/// <summary>
|
||||
/// Reset SLAM error state
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern xloc_status_response_t xloc_reset_slam_error(IntPtr handle);
|
||||
|
||||
#endregion
|
||||
|
||||
#region Getters
|
||||
|
||||
/// <summary>
|
||||
/// Get current pose estimate
|
||||
/// </summary>
|
||||
/// <returns>Pointer to pose (must be freed with xloc_free_pose)</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern IntPtr xloc_get_current_pose(IntPtr handle);
|
||||
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern IntPtr xloc_get_diagnostics(IntPtr handle);
|
||||
|
||||
/// <summary>
|
||||
/// Get static grid map (from loaded map file)
|
||||
/// </summary>
|
||||
/// <param name="handle">Handle to xloc instance</param>
|
||||
/// <param name="reloadFromFile">If non-zero, reload map from file</param>
|
||||
/// <returns>Pointer to occupancy grid (must be freed with xloc_free_occupancy_grid)</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern IntPtr xloc_get_static_grid_map(IntPtr handle, int reloadFromFile);
|
||||
|
||||
/// <summary>
|
||||
/// Get online grid map (from SLAM)
|
||||
/// </summary>
|
||||
/// <param name="handle">Handle to xloc instance</param>
|
||||
/// <returns>Pointer to occupancy grid (must be freed with xloc_free_occupancy_grid)</returns>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern IntPtr xloc_get_online_grid_map(IntPtr handle);
|
||||
|
||||
#endregion
|
||||
|
||||
#region Memory Management
|
||||
|
||||
/// <summary>
|
||||
/// Free status response
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void xloc_free_status_response(ref xloc_status_response_t response);
|
||||
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void xloc_free_diagnostics(IntPtr diagnostics);
|
||||
|
||||
/// <summary>
|
||||
/// Free C string allocated by xloc
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void xloc_free_cstring(IntPtr str);
|
||||
|
||||
/// <summary>
|
||||
/// Free pose allocated by xloc
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void xloc_free_pose(IntPtr pose);
|
||||
|
||||
/// <summary>
|
||||
/// Free occupancy grid allocated by xloc
|
||||
/// </summary>
|
||||
[DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
|
||||
public static extern void xloc_free_occupancy_grid(IntPtr grid);
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,65 @@
|
||||
using System;
|
||||
using System.IO;
|
||||
|
||||
namespace RobotNet10.RobotApp.Xloc;
|
||||
|
||||
public static class XlocPaths
|
||||
{
|
||||
private static string GetHomeDir()
|
||||
{
|
||||
// systemd/Docker services thường chạy với HOME=/root hoặc thiếu HOME
|
||||
var home = Environment.GetEnvironmentVariable("HOME");
|
||||
if (!string.IsNullOrWhiteSpace(home))
|
||||
return home;
|
||||
|
||||
return Environment.GetFolderPath(Environment.SpecialFolder.UserProfile);
|
||||
}
|
||||
|
||||
public static string GetXdgDataHome()
|
||||
{
|
||||
// XDG_DATA_HOME mặc định là $HOME/.local/share
|
||||
var xdg = Environment.GetEnvironmentVariable("XDG_DATA_HOME");
|
||||
if (!string.IsNullOrWhiteSpace(xdg))
|
||||
return xdg;
|
||||
|
||||
return Path.Combine(GetHomeDir(), ".local", "share");
|
||||
}
|
||||
|
||||
public static string GetMapsDirectory()
|
||||
{
|
||||
return Path.Combine(GetXdgDataHome(), "xloc", "resources", "maps");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Resolve a map folder name (e.g. "map_20260307_103646") into a pbstream file path.
|
||||
/// If <paramref name="mapFilePathOrName"/> is already an absolute existing file, returns it as-is.
|
||||
/// </summary>
|
||||
public static string? ResolvePbstreamPath(string mapFilePathOrName)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(mapFilePathOrName))
|
||||
return null;
|
||||
|
||||
// If caller already provided an absolute pbstream path, just validate it.
|
||||
if (Path.IsPathRooted(mapFilePathOrName) && File.Exists(mapFilePathOrName))
|
||||
return mapFilePathOrName;
|
||||
|
||||
// Otherwise treat it as a map folder name under the configured maps directory.
|
||||
var mapsDir = GetMapsDirectory();
|
||||
var mapName = Path.GetFileName(mapFilePathOrName.TrimEnd(Path.DirectorySeparatorChar, '/'));
|
||||
if (string.IsNullOrWhiteSpace(mapName))
|
||||
return null;
|
||||
|
||||
var mapFolder = Path.Combine(mapsDir, mapName);
|
||||
if (!Directory.Exists(mapFolder))
|
||||
return null;
|
||||
|
||||
// Prefer canonical name: map.pbstream, then fall back to any *.pbstream.
|
||||
var canonical = Path.Combine(mapFolder, "map.pbstream");
|
||||
if (File.Exists(canonical))
|
||||
return canonical;
|
||||
|
||||
var pbstreams = Directory.GetFiles(mapFolder, "*.pbstream", SearchOption.TopDirectoryOnly);
|
||||
return pbstreams.Length > 0 ? pbstreams[0] : null;
|
||||
}
|
||||
}
|
||||
|
||||
Binary file not shown.
Reference in New Issue
Block a user