ROS2D显示激光雷达点
·
1.参考:How to view laser scan on navigation grid? · Issue #106 · RobotWebTools/ros2djs · GitHub
2.ros端需下载并启动 tf2_web_republisher
<distro>指ros发行版本
$ sudo apt-get install ros-<distro>-tf2-web-republisher
$ rosrun tf2_web_republisher tf2_web_republisher
3.运行代码
<!DOCTYPE html>
<html lang="en">
<head>
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>ros2d</title>
<script type="text/javascript" src="./js/ros/eventemitter2.min.js"></script>
<script type="text/javascript" src="./js/ros/roslib.js"></script>
<script type="text/javascript" src="./js/ros/easeljs.min.js"></script>
<script type="text/javascript" src="./js/ros/ros2d.min.js"></script>
<script type="text/javascript" src="./js/ros/three.min.js"></script>
<style>
.main {
display: flex;
}
#map {
width: 100%;
height: 100%;
}
</style>
</head>
<body>
<div class="main">
<button onclick="displayLaserScan()">显示雷达</button>
<div id="map"></div>
</div>
<script>
var ros = new ROSLIB.Ros({
url: 'ws://192.168.66.27:9090'
});
ros.on('connection', function () {
console.log('Connected to ROS');
});
ros.on('error', function (error) {
console.log('Error connecting to ROS:', error);
});
ros.on('close', function () {
console.log('Disconnected from ROS');
});
var viewer = new ROS2D.Viewer({
divID: 'map',
width: 1200,
height: 800
});
var gridClient = new ROS2D.OccupancyGridClient({
ros: ros,
rootObject: viewer.scene,
continuous: true
});
gridClient.on('change', function () {
viewer.scaleToDimensions(gridClient.currentGrid.width, gridClient.currentGrid.height);
viewer.shift(gridClient.currentGrid.pose.position.x, gridClient.currentGrid.pose.position.y);
});
// 雷达显示
function getYawFromQuat(q) {
var quat = new THREE.Quaternion(q.x, q.y, q.z, q.w);
var yaw = new THREE.Euler().setFromQuaternion(quat);
return yaw["_z"] * (180 / Math.PI);
}
var base_footprint_tf = null;
var tf_client = new ROSLIB.TFClient({
ros: ros,
fixedFrame: "/map",
angularThres: 0.01,
transThres: 0.01
});
tf_client.subscribe("/laser1", function (tf) { //Maybe /scan in your project
base_footprint_tf = tf;
});
function displayLaserScan() {
let marker_radius = 0.03;
let marker_fill_color = createjs.Graphics.getRGB(255, 0, 0, 1.0);
let laser_listener = new ROSLIB.Topic({
ros: ros,
name: '/scan',
messageType: 'sensor_msgs/LaserScan'
});
let prev_markers = null;
laser_listener.subscribe(function (msg) {
const num = msg.ranges.length
const angles = Array.from({ length: num }, (_, i) => msg.angle_min + (msg.angle_max - msg.angle_min) / num * i);
const poses_2d = angles.flatMap((angle, index) => {
const range = msg.ranges[index];
if (range > msg.range_min && range < msg.range_max) {
return [[Math.cos(angle) * range, Math.sin(angle) * range, -angle]]
}
return [] // Skip this point
});
if (base_footprint_tf === null) {
console.log('no tf');
return;
}
// TODO: We might be able to apply the tf transform to the container itself, and dont have to do it on each pose.
// Init the graphics component
const scan_markers = new createjs.Container();
const graphics = new createjs.Graphics().beginFill(marker_fill_color).drawCircle(0, 0, marker_radius).endFill();
// Transform each point and add it to the graphics
poses_2d.forEach(pt => {
// pt[2] += Math.PI / 2
const pose = new ROSLIB.Pose({
position: new ROSLIB.Vector3({
x: pt[0], y: pt[1], z: 0
}), orientation: new ROSLIB.Quaternion({
x: 0, y: 0, z: Math.cos(pt[2]), w: Math.sin(pt[2])
})
});
pose.applyTransform(base_footprint_tf)
const marker = new createjs.Shape(graphics)
marker.x = pose.position.x;
marker.y = -pose.position.y;
marker.rotation = - getYawFromQuat(pose.orientation).toFixed(2);
scan_markers.addChild(marker)
});
// TODO: Just update the old one, dont make new ones everytime
if (this.prev_markers !== null) {
viewer.scene.removeChild(prev_markers);
}
viewer.addObject(scan_markers);
prev_markers = scan_markers;
});
}
</script>
</body>
</html>
更多推荐
所有评论(0)