Skip to content

GitLab

Commit ceb4e007 authored by Günter Niemeyer's avatar Günter Niemeyer
Browse files

Added the depthtolaserscan package 

This package includes the depthtolaserscan node, which converts the
RealSense depth image into a laser scan.  Note the python version runs
very slowly (0.3Hz!).  So I converted to CPP, which runs happily
@30Hz.  Simply catkin_make and use the non-python executable, i.e.
  rosrun depthtolaserscan depthtolaserscan

There are a few parameters, including the minimum height above the
floor (to avoid the ground being an obstacle), the maximum height (so
we can drive under tables), as well as a sample density.  The defaults
work well for me, but I am curious how sensitive they are to other
bots?
parent 0fef89f8
Hide whitespace changes
Inline Side-by-side
Showing
with 1501 additions and 0 deletions
+1501 -0
depthtolaserscan/CMakeLists.txt 0 → 100644
View file @ ceb4e007
cmake_minimum_required(VERSION 3.0.2)
project(depthtolaserscan)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
tf2_ros
sensor_msgs
geometry_msgs
)
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES depthtolaserscan
# CATKIN_DEPENDS roscpp rospy tf2_ros sensor_msgs geometry_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context.
## Make sure the target names don't collide across packages.
add_executable(depthtolaserscan src/depthtolaserscan.cpp)
## Add cmake target dependencies of the executable
## same as for the library above
#add_dependencies(depthtolaserscan ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a executable target against
target_link_libraries(depthtolaserscan ${catkin_LIBRARIES})
depthtolaserscan/launch/laserscan.launch 0 → 100644
View file @ ceb4e007
<launch>
<!-- Show the RealSense converted into a Laser Scan. -->
<!-- URDF parameter. -->
<arg name="urdf" default="bot.urdf"/>
<arg name="model" default="$(find bot_description)/urdf/$(arg urdf)"/>
<!-- Rviz configuration file parameter. -->
<arg name="cfg" default="$(find depthtolaserscan)/rviz/laserscan.rviz"/>
<!-- Load the URDF file into the robot_description parameter. -->
<param name="robot_description" textfile="$(arg model)"/>
<!-- Start the low-level wheelcontrol node -->
<node name="wheelcontrol"
pkg="example"
type="wheelcontrol.py"
output="screen"
required="true"/>
<!-- Start the odometry node -->
<node name="odometry"
pkg="example"
type="odometry.py"
output="screen"
required="true"/>
<!-- Start the teleop node SEPARATELY -->
<!--
<node name="teleop"
pkg="shared_demo"
type="teleop.py"
output="screen"/>
-->
<!-- Start the RealSense. I'm not enabling the pointcloud. -->
<!-- A resolution of (-1) implies using the default value. -->
<include file="$(find realsense2_camera)/launch/rs_camera.launch">
<arg name="depth_width" value="-1"/>
<arg name="depth_height" value="-1"/>
<arg name="depth_fps" value="30"/>
<arg name="color_width" value="-1"/>
<arg name="color_height" value="-1"/>
<arg name="color_fps" value="30"/>
<arg name="enable_pointcloud" value="false"/>
<arg name="allow_no_texture_points" value="true"/>
<!-- Replace RS2_STREAM_COLOR with RS_STREAM_ANY to -->
<!-- create the pointcloud without the RGB colors. -->
<arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
</include>
<!-- Start the depth image to laser scan conversion. -->
<!-- Note I'm *NOT* using python, it is a LOT slower. -->
<node name="depthtolaserscan"
pkg="depthtolaserscan"
type="depthtolaserscan"
output="screen"/>
<!-- Publish all the robot frames. -->
<node name="robot_state_publisher"
pkg="robot_state_publisher"
type="robot_state_publisher"/>
<!-- Run the rviz visualization, with the specified config file -->
<!-- Kill everything if this stops. -->
<node pkg="rviz"
type="rviz"
name="rviz"
args="-d $(arg cfg)"
output="screen"
required="true"/>
</launch>
depthtolaserscan/package.xml 0 → 100644
View file @ ceb4e007
<?xml version="1.0"?>
<package format="2">
<name>depthtolaserscan</name>
<version>0.0.0</version>
<description>
Convert a depth image to "fake" laser scan
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<maintainer email="jane.doe@example.com">Jane Doe</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/depthtolaserscan</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Use buildtool_depend for build tool packages: -->
<buildtool_depend>catkin</buildtool_depend>
<!-- Use depend for packages that are both build and exec dependencies -->
<depend>roscpp</depend>
<depend>rospy</depend>
<depend>tf2_ros</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
depthtolaserscan/rviz/laserscan.rviz 0 → 100644
View file @ ceb4e007
Panels:
- Class: rviz/Displays
Help Height: 112
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /RobotModel1/Links1
Splitter Ratio: 0.5
Tree Height: 332
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: LaserScan
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
axle:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base:
Alpha: 1
Show Axes: false
Show Trail: false
camera_link:
Alpha: 1
Show Axes: false
Show Trail: false
cameraplate:
Alpha: 1
Show Axes: false
Show Trail: false
laser:
Alpha: 1
Show Axes: false
Show Trail: false
leftwheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
rightwheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 100
Name: Odometry
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.029999999329447746
Head Radius: 0.019999999552965164
Shaft Length: 0.10000000149011612
Shaft Radius: 0.004999999888241291
Value: Arrow
Topic: /odom
Unreliable: false
Value: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
axle:
Value: true
base:
Value: true
camera_color_frame:
Value: true
camera_color_optical_frame:
Value: true
camera_depth_frame:
Value: true
camera_depth_optical_frame:
Value: true
camera_link:
Value: true
cameraplate:
Value: true
laser:
Value: true
leftwheel:
Value: true
odom:
Value: true
rightwheel:
Value: true
Marker Scale: 0.5
Name: TF
Show Arrows: false
Show Axes: true
Show Names: true
Tree:
odom:
base:
axle:
cameraplate:
camera_link:
camera_color_frame:
camera_color_optical_frame:
{}
camera_depth_frame:
camera_depth_optical_frame:
{}
laser:
{}
leftwheel:
{}
rightwheel:
{}
Update Interval: 0
Value: true
- Alpha: 1
Autocompute Intensity Bounds: false
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 255; 0; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: odom
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.1491851806640625
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.411437451839447
Y: 0.2044350802898407
Z: 0.6150152683258057
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.020398497581482
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 2.220398426055908
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 691
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000204fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006500fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004400000204000000de00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000223fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004400000223000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ee00000044fc0100000002fb0000000800540069006d00650100000000000003ee0000028000fffffffb0000000800540069006d006501000000000000045000000000000000000000027e0000020400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1006
X: -5
Y: 21
depthtolaserscan/scripts/depthtolaserscan.py 0 → 100755
View file @ ceb4e007
#!/usr/bin/env python
#
# depthtolaserscan.py
#
# Convert an depth image into a laser scan. This assumes the camera
# is mounted horizontally!
#
# Node: /depthtolaserscan
#
# Params: ~ground_frame_id Frame at ground level, with Z vertical up
# ~laser_frame_id Frame for laser scan
# (x forward, Z vertical up, Y left)
# ~min_height Min height for object to "be detected"
# ~max_height Max height for object to "be detected"
#
# Subscribe: /camera/depth/...
# .../camera_info sensor_msgs/CameraInfo
# .../image_rect_raw sensor_msgs/Image
#
# Publish: /scan sensor_msgs/LaserScan
#
# TF Required: ground_frame -> camera_frame
#
# TF Provided: camera_frame -> laser_frame (static)
#
import math
import numpy as np
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
from sensor_msgs.msg import CameraInfo
from sensor_msgs.msg import Image
from sensor_msgs.msg import LaserScan
#
# Constants
#
TIMEOUT = 5.0 # Timeout for the initialization
INFO_TOPIC = '/camera/depth/camera_info'
IMAGE_TOPIC = '/camera/depth/image_rect_raw'
SCAN_TOPIC = '/scan'
# Reporting a distance below the minimum implies the scanner was not
# able to get any information, equivalent to an error. Reporting a
# distance above the maximum implies there are no contacts (between
# the minimum to maximum distance), everything is clear.
RANGE_BAD = 0.000 # Range considered zero/unknown
RANGE_MINIMUM = 0.050 # Minimum valid contact distance
RANGE_MAXIMUM = 4.000 # Maximum valid contact distance
RANGE_NOCONTACT = 5.000 # Valid range is clear
RANGE_INFINITE = 6.000 # Never reported, used in sorting
######################################################################
# Mathematical/Algorithm Utilities
######################################################################
#
# Before we derive the algorithm, we define two coordinate frames in
# the same location, but with different axes. The Image frame is
# horizontal, but pitched up. The Laser frame is aligned with the
# ground (Z vertical).
#
# Image Frame: X image right horizontal, Y image down, Z into image
# Laser Frame: X forward horizontal, Y left horizontal, Z vertical up
#
# A point in space can be decribed in image frame (xi, yi, zi) or
# laser frame (xl, yl, zl) and converted with:
#
# xl = cos(pitch) zi + sin(pitch) yi = cp * zi + sp * yi
# yl = - xi = -xi
# zl = sin(pitch) zi - cos(pitch) yi = sp * zi - cp * yi
#
# xi = yl = yl
# yi = sin(pitch) xl - cos(pitch) zl = sp * xl - cp * zl
# zi = cos(pitch) xl + sin(pitch) zl = cp * xl + sp * zl
#
# The point (given in image frame) can also be mapped to the image
# coordinates (u,v) and depth (d):
#
# u = cu + fu * (xi/zi)
# v = cv + fv * (yi/zi)
# d = zi
#
# where cu/cv are the center of the image, and fu/fv are the focal
# lengths expressed in pixel size. This again can be inverted as:
#
# xi = d * (u-cu)/fu
# yi = d * (v-cv)/fv
# zi = d
#
# And then the image coordinates can be directly mapped to the
# laser frame (vertical up):
#
# xl = d * (cp + sp * vbar) = r * cos(alpha)
# yl = d * ( -ubar ) = r * sin(alpha)
# zl = d * (sp - cp * vbar) = r * tan(gamma)
#
# where r = range in horz plane (for laser scan)
# alpha = angle in horz plane (for laser scan)
# gamma = angle above horz plane (to be projected down)
#
# r = sqrt(xl^2 + yl^2) = d * sqrt(ubar^2 + (cp + sp*vbar)^2)
# tan(alpha) = yl/xl = -ubar/(cp+sp*vbar)
# tan(gamma) = zl/r = cos(alpha) * (sp - cp*vbar) / (cp + sp*vbar)
#
# or in reverse direction
#
# vbar = (calpha * sp - tgamma * cp) / (calpha * cp + tgamma * sp)
# vbar_horizon = sp/cp
# vbar_vanish = -cp/sp
#
# ubar = - tan(alpha) * (cp + sp*vbar)
# ubar_horizon = - tan(alpha) / cp
#
######################################################################
# General Setup Utilities
######################################################################
#
# Grab the Camera Info
#
def grabCameraInfo():
# Grab a single message of the camera_info topic.
infomsg = rospy.wait_for_message(INFO_TOPIC, CameraInfo, TIMEOUT)
# Note the image frame.
imageframe = infomsg.header.frame_id
# Note the width and height.
Nu = infomsg.width
Nv = infomsg.height
# Pull the image resolution and center numbers from the projection
# matrix. note this maps an (x,y,z) point to (u=column,v=row) as:
# u = cu + fu * (x/z)
# v = cv + fv * (y/z)
fu = infomsg.P[0]
cu = infomsg.P[2]
fv = infomsg.P[5]
cv = infomsg.P[6]
# Report.
rospy.loginfo("Projection: u = %7.3f + %7.3f * (x/z)" % (cu, fu))
rospy.loginfo(" v = %7.3f + %7.3f * (y/z)" % (cv, fv))
rospy.loginfo("Image %dx%d = %.0fx%.0f deg FOV"
% (Nu, Nv,
2 * math.atan(Nu/2/fu) * 180/math.pi,
2 * math.atan(Nv/2/fv) * 180/math.pi))
# Return the data.
return (imageframe, cu, fu, cv, fv, Nu, Nv)
#
# Grab the Camera Height (above ground) and Pitch angle (upward)
#
def grabCameraPose(groundframe, imageframe):
# Create a TF buffer and listener.
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
# Try to grab the ground to image transform. Use rospy.Time(0) to
# grab the latest transform, as opposed to the transform at a
# particular time. Use a timeout, in case no transforms appear.
try:
# Grab the transform as a TransformStamped message.
msg = tfBuffer.lookup_transform(groundframe, imageframe,
rospy.Time(0), rospy.Duration(5.0))
except Exception as ex:
rospy.logerr("Unable to get the '%s' to '%s' transform!"
% (groundframe, imageframe))
rospy.logerr("Have you started the robot_state_publisher?")
raise ex
# Remove the listener (don't need any more information).
listener.unregister()
# Extract the rotation and the vertical components of the X/Y/Z
# axes, knowing the ground frame's Z axis is vertical.
r = msg.transform.rotation
xvertical = 2*(r.x*r.z - r.w*r.y) # X = Image right
yvertical = 2*(r.y*r.z + r.w*r.x) # Y = Image down
zvertical = 1-2*(r.x**2 + r.y**2) # Z = Image forward
# Check the camera (Image-right) being horizontal.
if (abs(xvertical) > 1e-9):
errstr = "The depth camera is not mounted horizontally!"
rospy.logerr(errstr)
raise rospy.ROSException(errstr)
# Grab the pitch angle.
spitch = zvertical # Vertical component of Z axis
cpitch = -yvertical # Vertical component of -Y axis
pitch = math.atan2(spitch, cpitch)
# Also grab the height.
hcam = msg.transform.translation.z
# Report.
rospy.loginfo("The depth camera is located %.2fmm above ground"
% (hcam*1000))
rospy.loginfo("The depth camera is pitched up %6.3fdeg (%4.3frad)"
% (pitch * 180/math.pi, pitch))
# Return the data.
return (hcam, pitch, spitch, cpitch)
#
# Broadcast the Laser Frame, rotated down by Pitch angle
#
def broadcastLaserFrame(imageframe, laserframe, pitch):
# Create the broadcaster (for a static transform).
broadcaster = tf2_ros.StaticTransformBroadcaster()
# Precompute the sin/cos
s = math.sin(pitch/2)
c = math.cos(pitch/2)
# Prepare the transform message. Rotate such that the laser frame
# is X horizontal along image axis, Y left, Z up (vertical).
msg = TransformStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = imageframe
msg.child_frame_id = laserframe
msg.transform.translation.x = 0.0
msg.transform.translation.y = 0.0
msg.transform.translation.z = 0.0
msg.transform.rotation.x = 0.5 * (c - s)
msg.transform.rotation.y = 0.5 * (s - c)
msg.transform.rotation.z = 0.5 * (s + c)
msg.transform.rotation.w = 0.5 * (s + c)
# Broadcast.
broadcaster.sendTransform(msg)
######################################################################
# Prepare the Sampling
######################################################################
#
# Depth to Range Factor
#
# Multiply a depth (in image) to get matching range for laser scan.
#
def depthToRange(u,v):
return math.sqrt((cpitch + spitch * (v-cv)/fv)**2 + ((u-cu)/fu)**2)
#
# Max Depth for Contact
#
# Consider obstacles between hmin and hmax (relative to hcam). So
# once the depth exceeds the corresponding depth (in the given
# direction), we know there is no contact.
#
def maxDepthForContact(u,v):
# Depth corresponding to max contact range (in given direction).
dmax = RANGE_MAXIMUM / depthToRange(u,v)
# Also consider the depth limit due to height, looking up or down.
a = spitch - cpitch * (v-cv)/fv
if (a > 0):
return min(dmax, (hmax-hcam)/a)
elif (a < 0):
return min(dmax, (hmin-hcam)/a)
else:
return dmax
#
# Define the Sample List (including pre-computed information)
#
def setSampleList():
# Start with an empty list, add each angle.
samplelist = []
for i in range(angle_num):
angle = angle_min + i * angle_inc
ta = math.tan(angle)
# For each angle, collect a list of sample points.
samples = []
for v in range(0, Nv, dv):
vbar = (float(v) - cv) / fv
ubar = - ta * (cpitch + spitch * vbar)
u = int(round(cu + fu * ubar))
if (u < 0) or (u >= Nu):
break
# Precompute/save the important information.
samples.append((u, v,
1000*maxDepthForContact(u,v),
depthToRange(u,v)/1000))
# Push the list of sample points for this angle.
samplelist.append(samples)
# Return the complete sample list
return samplelist
######################################################################
# Callback - Sampling Algorithm
######################################################################
#
# Depth Image Callback
#
# Convert the depth image into a laser scan
#
def callback_image(imagemsg):
# Report (for debugging).
rospy.loginfo("Image #%4d at %10d secs"
% (imagemsg.header.seq, imagemsg.header.stamp.secs))
# Check the image width/height.
if (imagemsg.width != Nu) or (imagemsg.height != Nv):
rospy.logerr("Depth image has changed size (%dx%d) vs (%dx%d)!"
% (imagemsg.width, imagemsg.height, Nu, Nv))
return
# Check the data encoding.
encoding = '16UC1'
if (imagemsg.encoding != encoding):
rospy.logerr("Depth image type encoding '%s' different from '%s'!"
% (imagemsg.encoding, encoding))
return
# Extract the depth image information (distance in mm as uint16).
depth = np.frombuffer(imagemsg.data, np.uint16).reshape(Nv, Nu)
# # Testing...
# u = int(round(cu))
# v = int(round(cv))
# d = depth[v][u]
# rospy.loginfo("Distance at (row %d, col %d) = %dmm" % (v, u, d))
# Create the range list.
ranges = [RANGE_BAD] * angle_num
# Compute range for each angle, as minimum over all sample points.
for i in range(angle_num):
minrange = RANGE_INFINITE
for (u, v, maxdepth, depthtorange) in samplelist[i]:
if (depth[v][u] > maxdepth):
minrange = min(minrange, RANGE_NOCONTACT)
elif (depth[v][u] > 0.0):
minrange = min(minrange, depth[v][u] * depthtorange)
if (minrange == RANGE_INFINITE):
minrange = RANGE_BAD
ranges[i] = minrange
# Create scan msg.
scanmsg = LaserScan()
scanmsg.header.stamp = rospy.Time.now() # imagemsg.header.stamp
scanmsg.header.frame_id = laserframe
scanmsg.angle_min = angle_min
scanmsg.angle_max = angle_max
scanmsg.angle_increment = angle_inc
scanmsg.time_increment = 0.0
scanmsg.scan_time = 1/30.0
scanmsg.range_min = RANGE_MINIMUM # minimum range value [m]
scanmsg.range_max = RANGE_MAXIMUM # maximum range value [m]
scanmsg.ranges = ranges
# Publish.
pub.publish(scanmsg)
######################################################################
# Main Code
######################################################################
if __name__ == "__main__":
# Initialize the ROS node.
rospy.init_node('depthtolaserscan')
# Grab the Ros parameters.
groundframe = rospy.get_param('~ground_frame_id', 'base')
laserframe = rospy.get_param('~laser_frame_id', 'laser')
hmin = rospy.get_param('~min_height', 0.010)
hmax = rospy.get_param('~max_height', 0.250)
horzsamples = int(math.ceil(rospy.get_param('~horz_samples', -1)))
vertsamples = int(math.ceil(rospy.get_param('~vert_samples', -1)))
# Grab the camera image info.
rospy.loginfo("Getting depth camera info...")
(imageframe, cu, fu, cv, fv, Nu, Nv) = grabCameraInfo()
rospy.loginfo("Ground frame '%s'" % groundframe)
rospy.loginfo("Image frame '%s'" % imageframe)
rospy.loginfo("Laser frame '%s'" % laserframe)
# Grab the camera height and pitch angle.
rospy.loginfo("Getting depth camera pose...")
(hcam, pitch, spitch, cpitch) = grabCameraPose(groundframe, imageframe)
# Generate the laser frame (in which to publish the scan data).
broadcastLaserFrame(imageframe, laserframe, pitch)
# Determine the angles to sample, and the number of lines/angle.
angle_num = (1+Nu/2) if horzsamples <= 0 else horzsamples
angle_min = math.atan(-cpitch * (Nu-1-cu)/fu)
angle_max = math.atan(-cpitch * ( -cu)/fu)
angle_inc = (angle_max - angle_min) / (angle_num - 1)
dv = 2 if vertsamples <= 0 else (Nv/vertsamples)
line_num = Nv/dv
rospy.loginfo("Sampling %d angles, each on %d image lines"
% (angle_num, line_num))
# Possibly expand the min/max height to include the camera height.
hmin = min(hmin, hcam)
hmax = max(hmax, hcam)
rospy.loginfo("Scanning for objects %.2fmm to %.2fmm above ground"
% (hmin*1000, hmax*1000))
# For testing, compute the horizon line.
vhorizon = int(round(cv + fv * spitch/cpitch))
rospy.loginfo("The horizon is on row v = %d" % vhorizon)
# Create the list of angle samples, and points for each angle.
samplelist = setSampleList()
# Create a publisher for the laser scan.
pub = rospy.Publisher(SCAN_TOPIC, LaserScan, queue_size=10)
# Create a subscriber to listen to the depth images. Using a
# queue size of one means only the most recent message is stored
# for the next subscriber callback. Skip backed-up messages.
sub = rospy.Subscriber(IMAGE_TOPIC, Image, callback_image,
queue_size=1, buff_size=20*Nu*Nv*2)
# Proceed.
rospy.logwarn("THIS IS THE SLOW PYTHON VERSION!")
rospy.logwarn("caktin_make and then use the CPP version!")
rospy.spin()
rospy.loginfo("End convert depth image to laser scan.")
depthtolaserscan/src/depthtolaserscan.cpp 0 → 100644
View file @ ceb4e007
/*
** depthtolaserscan.cpp
**
** Convert an depth image into a laser scan. This assumes the camera
** is mounted horizontally!
**
** Node: /depthtolaserscan
**
** Params: ~ground_frame_id Frame at ground level, with Z vertical up
** ~laser_frame_id Frame for laser scan
** (x forward, Z vertical up, Y left)
** ~min_height Min height for object to "be detected"
** ~max_height Max height for object to "be detected"
**
** Subscribe: /camera/depth/...
** .../camera_info sensor_msgs/CameraInfo
** .../image_rect_raw sensor_msgs/Image
**
** Publish: /scan sensor_msgs/LaserScan
**
** TF Required: ground_frame -> camera_frame
**
** TF Provided: camera_frame -> laser_frame (static)
*/
#include <math.h>
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>
using namespace ros;
using namespace geometry_msgs;
using namespace sensor_msgs;
/*
** Constants
*/
#define TIMEOUT (5.0) // Timeout for the initialization
#define INFO_TOPIC "/camera/depth/camera_info"
#define IMAGE_TOPIC "/camera/depth/image_rect_raw"
#define SCAN_TOPIC "/scan"
// Reporting a distance below the minimum implies the scanner was not
// able to get any information, equivalent to an error. Reporting a
// distance above the maximum implies there are no contacts (between
// the minimum to maximum distance), everything is clear.
#define RANGE_BAD (0.000) // Range considered zero/unknown
#define RANGE_MINIMUM (0.050) // Minimum valid contact distance
#define RANGE_MAXIMUM (4.000) // Maximum valid contact distance
#define RANGE_NOCONTACT (5.000) // Valid range is clear
#define RANGE_INFINITE (6.000) // Never reported, used in sorting
/*
** Global Variables
*/
// Frames
std::string groundframe; // At ground level, Z vertical up
std::string imageframe; // At camera, X image right, Z image out
std::string laserframe; // At camera, X horiz forward, Z vert up
// Image parameters.
unsigned int Nu, Nv; // Width/Height (number of pixels)
double cu, cv; // Image Center (in pixels)
double fu, fv; // Focal length (in pixels)
// Pose parameters.
double pitch; // Camera pitched up angle
double spitch, cpitch;
double hcam; // Height of camera
double hmin; // Min height to consider contacts
double hmax; // Max height to consider contacts
// Sampling info.
int horzsamples; // Number of angles (-1 = default)
int vertsamples; // Number of lines (-1 = default)
unsigned int Nangles; // Number of angles to sample
unsigned int Nlines; // Number of image lines to sample per angle
int dlines; // Step size in lines between samples
double angle_min; // Scan minimum angle
double angle_max; // Scan maximum angle
double angle_inc; // Scan increment between sample angles
// Sampling specification data (pre-computed).
typedef struct _pointspec // Specifications of a point sample
{
int index; // Index to flattened depth data[row][col]
uint16_t maxdepth; // Max depth for contact in mm!
double depthtorange; // Depth to range factor mm to m!
} pointspec_t;
typedef struct _anglespec // Specifications of an angle sample
{
double angle; // Angle of this sample
int points; // Number of points (for this angle)
pointspec_t *pointspecs; // Points to sample
} anglespec_t;
anglespec_t *anglespecs; // Pre-computed sample info
// Scan message and publisher
LaserScan scanmsg;
Publisher scanpub;
/*********************************************************************
** Mathematical Definitions **
**********************************************************************
**
** Before we derive the algorithm, we define two coordinate frames
** in the same location, but with different axes. The Image frame
** is horizontal, but pitched up. The Laser frame is aligned with
** the ground (Z vertical).
**
** Image Frame: X image right horizontal, Y image down, Z into image
** Laser Frame: X forward horizontal, Y left horizontal, Z vertical up
**
** A point in space can be decribed in image frame (xi, yi, zi) or
** laser frame (xl, yl, zl) and converted with:
**
** xl = cos(pitch) zi + sin(pitch) yi = cp * zi + sp * yi
** yl = - xi = -xi
** zl = sin(pitch) zi - cos(pitch) yi = sp * zi - cp * yi
**
** xi = yl = yl
** yi = sin(pitch) xl - cos(pitch) zl = sp * xl - cp * zl
** zi = cos(pitch) xl + sin(pitch) zl = cp * xl + sp * zl
**
** The point (given in image frame) can also be mapped to the image
** coordinates (u,v) and depth (d):
**
** u = cu + fu * (xi/zi) ubar = xi/zi
** v = cv + fv * (yi/zi) vbar = yi/zi
** d = zi
**
** where cu/cv are the center of the image, and fu/fv are the focal
** lengths expressed in pixel size. This again can be inverted as:
**
** xi = d * (u-cu)/fu = d * ubar
** yi = d * (v-cv)/fv = d * vbar
** zi = d = d
**
** And then the image coordinates can be directly mapped to the
** laser frame (vertical up):
**
** xl = d * (cp + sp * vbar) = r * cos(alpha)
** yl = d * ( -ubar ) = r * sin(alpha)
** zl = d * (sp - cp * vbar) = r * tan(gamma)
**
** where r = range in horz plane (for laser scan)
** alpha = angle in horz plane (for laser scan)
** gamma = angle above horz plane (to be projected down)
**
** r = sqrt(xl^2 + yl^2) = d * sqrt(ubar^2 + (cp + sp*vbar)^2)
** tan(alpha) = yl/xl = -ubar/(cp+sp*vbar)
** tan(gamma) = zl/r = cos(alpha) * (sp - cp*vbar) / (cp + sp*vbar)
**
** or in reverse direction
**
** vbar = (calpha * sp - tgamma * cp) / (calpha * cp + tgamma * sp)
** vbar_horizon = sp/cp
** vbar_vanish = -cp/sp
**
** ubar = - tan(alpha) * (cp + sp*vbar)
** ubar_horizon = - tan(alpha) / cp
*/
/*********************************************************************
** General Setup Utilities **
*********************************************************************/
/*
** Grab the (private) ROS parameters
*/
void grabParameters()
{
// Create a handle to the private namespace.
ros::NodeHandle node_private("~");
// Pull out the private parameters.
node_private.param<std::string>("ground_frame_id", groundframe, "base");
node_private.param<std::string>("laser_frame_id", laserframe, "laser");
node_private.param("min_height", hmin, 0.030);
node_private.param("max_height", hmax, 0.250);
node_private.param("horz_samples", horzsamples, -1);
node_private.param("vert_samples", vertsamples, -1);
}
/*
** Grab the Camera Info
*/
int grabCameraInfo()
{
CameraInfo::ConstPtr msgp;
// Report.
ROS_INFO("Getting depth camera info...");
// Grab a single message of the camera_info topic.
msgp = topic::waitForMessage<CameraInfo>(INFO_TOPIC, Duration(TIMEOUT));
if (msgp == NULL)
{
ROS_ERROR("Unable to read the depth camera's info!");
return -1;
}
// Save the image frame ID.
imageframe = msgp->header.frame_id;
// Save the width/height.
Nu = msgp->width;
Nv = msgp->height;
// Save the image resolution and center numbers from the projection
// matrix. note this maps an (x,y,z) point to (u=column,v=row) as:
// u = cu + fu * (x/z)
// v = cv + fv * (y/z)
fu = msgp->P[0];
cu = msgp->P[2];
fv = msgp->P[5];
cv = msgp->P[6];
// Report.
ROS_INFO("Projection: u = %7.3f + %7.3f * (x/z)", cu, fu);
ROS_INFO(" v = %7.3f + %7.3f * (y/z)", cv, fv);
ROS_INFO("Image %dx%d = %.0fx%.0f deg FOV", Nu, Nv,
2.0 * atan((double)Nu/2.0/fu) * 180.0/M_PI,
2.0 * atan((double)Nv/2.0/fv) * 180.0/M_PI);
return 0;
}
/*
** Grab the Camera Pose (Optical/Image Frame)
**
** The comprises the Height (above ground) and Pitch angle (upward)
*/
int grabCameraPose()
{
TransformStamped msg;
// Report.
ROS_INFO("Getting depth camera pose...");
// Create a TF buffer and listener.
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
// Try to grab the ground to image transform. Use ros::Time(0) to
// grab the latest transform, as opposed to the transform at a
// particular time. Use a timeout, in case no transforms appear.
try
{
msg = buffer.lookupTransform(groundframe, imageframe,
Time(0), Duration(TIMEOUT));
}
catch (tf2::TransformException &e)
{
ROS_ERROR("Unable to get the '%s' to '%s' transform!",
groundframe.c_str(), imageframe.c_str());
ROS_ERROR("Have you started the robot_state_publisher?");
return -1;
}
// Extract the rotation and the vertical components of the X/Y/Z
// axes, knowing the ground frame's Z axis is vertical.
Quaternion r = msg.transform.rotation;
double xvertical = 2.0 * (r.x*r.z - r.w*r.y); // X = Image right
double yvertical = 2.0 * (r.y*r.z + r.w*r.x); // Y = Image down
double zvertical = 1.0 - 2.0 * (r.x*r.x + r.y*r.y); // Z = Image forward
// Check the camera (Image-right) being horizontal.
if (fabs(xvertical) > 1e-9)
{
ROS_ERROR("The depth camera is not mounted horizontally!");
return -1;
}
// Grab the pitch angle.
spitch = zvertical; // Vertical component of Z axis
cpitch = -yvertical; // Vertical component of -Y axis
pitch = atan2(spitch, cpitch);
// Also grab the height.
hcam = msg.transform.translation.z;
// Report.
ROS_INFO("The depth camera is located %.2fmm above ground",
hcam*1000.0);
ROS_INFO("The depth camera is pitched up %6.3fdeg (%4.3frad)",
pitch * 180.0/M_PI, pitch);
return 0;
}
/*
** Broadcast the Laser Frame, rotated down by Pitch angle
*/
void broadcastLaserFrame()
{
// Create the broadcaster (for a static transform).
tf2_ros::StaticTransformBroadcaster broadcaster;
// Precompute the sin/cos for the quaternion.
double s = sin(pitch/2.0);
double c = cos(pitch/2.0);
// Prepare the transform message. Rotate such that the laser frame
// is X horizontal along image axis, Y left, Z up (vertical).
TransformStamped msg;
msg.header.stamp = Time::now();
msg.header.frame_id = imageframe;
msg.child_frame_id = laserframe;
msg.transform.translation.x = 0.0;
msg.transform.translation.y = 0.0;
msg.transform.translation.z = 0.0;
msg.transform.rotation.x = 0.5 * (c - s);
msg.transform.rotation.y = 0.5 * (s - c);
msg.transform.rotation.z = 0.5 * (s + c);
msg.transform.rotation.w = 0.5 * (s + c);
// Broadcast.
broadcaster.sendTransform(msg);
}
/*
** Setup the parameters and frames.
*/
int setup()
{
// Grab the private ROS parameters.
grabParameters();
// Grab the camera image info.
if (grabCameraInfo())
return -1;
// Report the frames.
ROS_INFO("Ground frame '%s'", groundframe.c_str());
ROS_INFO("Image frame '%s'", imageframe.c_str());
ROS_INFO("Laser frame '%s'", laserframe.c_str());
// Grab the camera pose.
if (grabCameraPose())
return -1;
// Generate the laser frame (in which to publish the scan data).
broadcastLaserFrame();
return 0;
}
/*********************************************************************
** Prepare the Sampling **
*********************************************************************/
/*
** Depth to Range Factor
**
** Multiply a depth (in image) to get matching range for laser scan.
*/
double depthToRange(int u, int v)
{
// Precompute the normalized image coordinates.
double ubar = ((double) u - cu) / fu;
double vbar = ((double) v - cv) / fv;
return sqrt((cpitch + spitch*vbar)*(cpitch + spitch*vbar) + ubar*ubar);
}
/*
** Max Depth for Contact
**
** Consider only obstacles between hmin and hmax (relative to hcam).
** So once the depth exceeds the corresponding depth (in the given
** direction), we know there is no contact.
*/
double maxDepthForContact(int u, int v)
{
// Precompute the normalized image coordinate.
double vbar = ((double) v - cv) / fv;
// Depth corresponding to max contact range (in given direction).
double dmax = RANGE_MAXIMUM / depthToRange(u,v);
// Also consider the depth limit due to height, looking up or down.
double a = spitch - cpitch * vbar;
if (a > 0.0) return fmin(dmax, (hmax-hcam)/a); // Looking up
else if (a < 0.0) return fmin(dmax, (hmin-hcam)/a); // Looking down
else return dmax; // Looking flat
}
/*
** Set (pre-compute) the Sample Information
*/
int setSampleSpecs()
{
// Allocate space for the overall sampling specifications.
anglespecs = (anglespec_t *) malloc(Nangles * sizeof(anglespec_t));
if (anglespecs == NULL)
{
ROS_ERROR("Unable to malloc space for the angle sample specs!");
return -1;
}
// Iterate over all angles.
for (int iangle = 0 ; iangle < Nangles ; iangle++)
{
// Set the angle.
double angle = angle_min + (double) iangle * angle_inc;
double tangle = tan(angle);
anglespecs[iangle].angle = angle;
// Allocate space for the angle sampling specifications.
anglespecs[iangle].pointspecs =
(pointspec_t *) malloc(Nlines * sizeof(pointspec_t));
if (anglespecs[iangle].pointspecs == NULL)
{
ROS_ERROR("Unable to malloc space for the point sample specs!");
return -1;
}
// Interate over all lines.
for (int iline = 0 ; iline < Nlines ; iline++)
{
// Determine the pixels = image coordinates.
int v = iline * dlines;
double vbar = ((double) v - cv) / fv;
double ubar = - tangle * (cpitch + spitch * vbar);
int u = (int) round(cu + fu * ubar);
if ((u < 0) || (u >= Nu))
break;
// Set the specifications.
pointspec_t *ptspecp = &anglespecs[iangle].pointspecs[iline];
ptspecp->index = v*Nu + u;
ptspecp->maxdepth = (uint16_t)(1000.0 * maxDepthForContact(u,v));
ptspecp->depthtorange = depthToRange(u,v) / 1000.0;
anglespecs[iangle].points = iline+1;
}
}
return 0;
}
/*
** Prepare the Sample Information
*/
int prepareSampling()
{
// Determine the number of angles to sample. And the image line
// sampling step size.
if (horzsamples > 0) Nangles = horzsamples;
else Nangles = 1 + Nu/2;
if (vertsamples > 0) dlines = Nv / vertsamples;
else dlines = 2;
Nlines = Nv/dlines;
ROS_INFO("Sampling %d angles, each on %d image lines", Nangles, Nlines);
// Possibly expand the min/max height to include the camera height.
if (hmin > hcam) hmin = hcam;
if (hmax < hcam) hmax = hcam;
ROS_INFO("Scanning for objects %.2fmm to %.2fmm above ground",
hmin * 1000.0, hmax * 1000.0);
// For reporting, compute the horizon line.
// int vhorizon = (int) round(cv + fv * spitch/cpitch);
// ROS_INFO("The horizon is on row v = %d", vhorizon);
// Determine the angle samples.
angle_min = atan(-cpitch * ((double) (Nu-1) - cu)/fu);
angle_max = atan(-cpitch * ( - cu)/fu);
angle_inc = (angle_max - angle_min) / ((double) (Nangles - 1));
// Pre-compute the sample specs/info.
if (setSampleSpecs())
return -1;
// Pre-populate the scan message.
scanmsg.header.frame_id = laserframe;
scanmsg.angle_min = angle_min;
scanmsg.angle_max = angle_max;
scanmsg.angle_increment = angle_inc;
scanmsg.time_increment = 0.0;
scanmsg.scan_time = 1.0 / 30.0;
scanmsg.range_min = RANGE_MINIMUM; // Minimum range value [m]
scanmsg.range_max = RANGE_MAXIMUM; // Maximum range value [m]
scanmsg.ranges.clear();
for (int iangle = 0 ; iangle < Nangles ; iangle++)
scanmsg.ranges.push_back(RANGE_BAD);
return 0;
}
/*********************************************************************
** Callback - Sampling Algorithm **
*********************************************************************/
/*
** Depth Image Callback
*/
void callback_image(const Image::ConstPtr& imagemsgp)
{
// Report (for debugging).
// ROS_INFO("Image #%4d at %10d secs",
// imagemsgp->header.seq, imagemsgp->header.stamp.sec);
// Check the image width/height.
if ((imagemsgp->width != Nu) || (imagemsgp->height != Nv))
{
ROS_ERROR("Depth image has changed size (%dx%d) vs (%dx%d)!",
imagemsgp->width, imagemsgp->height, Nu, Nv);
return;
}
// Check the data encoding.
if (imagemsgp->encoding.compare("16UC1") != 0)
{
ROS_ERROR("Depth image type encoding '%s' different from '16UC1'!",
imagemsgp->encoding.c_str());
return;
}
// Grab a pointer to the depth data (appropriately cast).
const uint16_t *depthdata = (const uint16_t *) &imagemsgp->data[0];
// Testing...
// int u = Nu/2;
// int v = Nv/2;
// ROS_INFO("Distance at (row %d, col %d) = %dmm", v, u, dp[v*Nu+u]);
// Compute the ranges at each angle, as the minimum range of all
// sample points per angle.
for (int iangle = Nangles-1 ; iangle >= 0 ; iangle--)
{
// Initialize the minimum range bigger than anything to come.
double range = RANGE_INFINITE;
// Examine each sample point for this angle.
for (int ipoint = anglespecs[iangle].points-1 ; ipoint >= 0 ; ipoint--)
{
// Pointer to the sample point details.
pointspec_t *ptspecp = &anglespecs[iangle].pointspecs[ipoint];
// Grab the depth at the point of interest.
uint16_t depth = depthdata[ptspecp->index];
// Adjust the range as appropriate.
if (depth > ptspecp->maxdepth)
{
if (range > RANGE_NOCONTACT)
range = RANGE_NOCONTACT;
}
else if (depth > 0)
{
double samplerange = (double) depth * ptspecp->depthtorange;
if (range > samplerange)
range = samplerange;
}
}
// Check that we've found a valid range, and set.
if (range == RANGE_INFINITE) scanmsg.ranges[iangle] = RANGE_BAD;
else scanmsg.ranges[iangle] = range;
}
// Update the mssage and publish. Note the depth camera time is bad!
scanmsg.header.stamp = Time::now();
scanpub.publish(scanmsg);
}
/*
** Main Code
*/
int main(int argc, char **argv)
{
// Initialize ROS and grab regular/private node handles.
ros::init(argc, argv, "depthtolaserscan");
ros::NodeHandle node;
// Setup the parameters and frames.
if (setup())
return -1;
// Prepare the sampling specifications/data structures.
if (prepareSampling())
return -1;
// Create a publisher for the laser scan (queue size of 10).
scanpub = node.advertise<LaserScan>(SCAN_TOPIC, 10);
// Create a subscriber to listen to the depth images. Limiting the
// queue size would prevent too many messages from building up, if
// this subscriber can not keep up.
Subscriber imagesub = node.subscribe(IMAGE_TOPIC, 6, callback_image);
// Spin, only exit when killed.
spin();
ROS_INFO("End convert depth image to laser scan.");
return 0;
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment