Skip to content

GitLab

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

Added the fake localization skeleton 

This provides a framework for the localization node, setting up the
necessary subscribers and transforms.  The actual alignment code is
*not* provided.
parent c41b96b4
Hide whitespace changes
Inline Side-by-side
Showing
with 714 additions and 0 deletions
+714 -0
shared_demo/launch/fakelocalize.launch 0 → 100644
View file @ e7b3f0ce
<launch>
<!-- Show the fake localization (including the depthtolaser). -->
<!-- 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 shared_demo)/rviz/localize.rviz"/>
<!-- Map Yaml File. -->
<arg name="map" default="$(find shared_demo)/maps/gazebohouse.yaml"/>
<!-- 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"/>
<!-- Start the map server -->
<node name="map_server"
pkg ="map_server"
type="map_server"
args="$(arg map)"/>
<!-- Finally start the fake localization. -->
<node name="localizatoin"
pkg="shared_demo"
type="fakelocalize.py"
output="screen">
<param name="update_fraction" value="0.3"/>
</node>
<!-- 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>
shared_demo/rviz/localize.rviz 0 → 100644
View file @ e7b3f0ce
Panels:
- Class: rviz/Displays
Help Height: 91
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
Splitter Ratio: 0.5
Tree Height: 353
- 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: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /map
Unreliable: false
Use Timestamp: false
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
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
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
map:
Value: true
odom:
Value: true
rightwheel:
Value: true
Marker Scale: 0.5
Name: TF
Show Arrows: false
Show Axes: true
Show Names: true
Tree:
map:
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
- 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: true
Value: true
- Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Class: rviz/Pose
Color: 255; 255; 0
Enabled: true
Head Length: 0.10000000149011612
Head Radius: 0.10000000149011612
Name: Pose
Shaft Length: 0.25
Shaft Radius: 0.019999999552965164
Shape: Arrow
Topic: /pose
Unreliable: false
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: map
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.773106098175049
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.08908334374427795
Y: 0.21809834241867065
Z: 0.4886871874332428
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: 9
Y: 36
shared_demo/scripts/fakelocalize.py 0 → 100755
View file @ e7b3f0ce
#!/usr/bin/env python
#
# fakelocalize.py
#
# This is simply a node with structure similar to the localization.
# I.e. it reads the messages and transforms, as I expect the
# localization might needs. And it publishes the map->odom
# transform.
#
# BUT it really needs to be replaced/edited with an actual
# localization algorithm!
#
# Node: /localization
#
# Params: ~update_fraction Fraction of udpate to apply
#
# Subscribe: /map nav_msgs/OccupancyGrid
# /scan sensor_msgs/LaserScan
# /odom nav_msgs/Odometry
# /initialpose geometry_msgs/PoseWithCovarianceStamped
#
# Publish: /pose geometry_msgs/PoseStamped
#
# TF Required: odom -> laser
#
# TF Provided: map -> odom
#
import math
import numpy as np
import rospy
import tf2_ros
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import TransformStamped
from nav_msgs.msg import OccupancyGrid
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
#
# Global Variables
#
# Location of the odom frame (Odometry starting pose) w.r.t. map
odom_x = 0.0
odom_y = 0.0
odom_theta = 0.0
# The map (2D numpy array).
map = None
######################################################################
# Odometry to Pose Message
######################################################################
#
# Odometry Callback
#
# The odometry message provides the robot's base w.r.t. the odom
# frame. Here we compute the robot w.r.t. the map frame.
# I.e. simply concatenate the map->odom and odom->base poses.
#
# We could skip this entirely. Just to view the pose with respect
# to the map in RVIZ.
#
def callback_odom(odommsg):
# Make sure the odometry is published w.r.t. the odom frame.
if not (odommsg.header.frame_id == 'odom'):
rospy.logerr("Odometry is not in 'odom' frame!")
return
# Extract the pose w.r.t. the odom frame, i.e. the odom->base info.
x = odommsg.pose.pose.position.x
y = odommsg.pose.pose.position.y
theta = 2*math.atan2(odommsg.pose.pose.orientation.z,
odommsg.pose.pose.orientation.w)
# Add to the odom pose, i.e. make absolute w.r.t. the map.
abs_x = odom_x + math.cos(odom_theta) * x - math.sin(odom_theta) * y
abs_y = odom_y + math.sin(odom_theta) * x + math.cos(odom_theta) * y
abs_theta = odom_theta + theta
# Create and publish the pose message. Note the original Software
# Architecture doc called for PoseWithCovarianceStamped. But RVIZ
# display wants a plain PoseStamped. And we don't have the
# covariance anyway.
posemsg = PoseStamped()
posemsg.header.stamp = odommsg.header.stamp
posemsg.header.frame_id = 'map'
posemsg.pose.position.x = abs_x
posemsg.pose.position.y = abs_y
posemsg.pose.position.z = 0.0
posemsg.pose.orientation.x = 0.0
posemsg.pose.orientation.y = 0.0
posemsg.pose.orientation.z = math.sin(abs_theta/2)
posemsg.pose.orientation.w = math.cos(abs_theta/2)
posepub.publish(posemsg)
######################################################################
# Initial Pose Message
######################################################################
#
# Initial Pose Callback
#
# This comes from the RVIZ "2D Pose Estimate" button, telling us
# where we want the robot to be.
#
def callback_init(posemsg):
# Use the odom global variables
global odom_x, odom_y, odom_theta
# Make sure the pose is published w.r.t. the map frame.
if not (posemsg.header.frame_id == 'map'):
rospy.logerr("Initial Pose is not in 'map' frame!")
return
# Extract the bot's (base) pose w.r.t. the map frame.
bot_x = posemsg.pose.pose.position.x
bot_y = posemsg.pose.pose.position.y
bot_theta = 2*math.atan2(posemsg.pose.pose.orientation.z,
posemsg.pose.pose.orientation.w)
# Also grab the base w.r.t. the odom frame, at the same time.
tfmsg = tfBuffer.lookup_transform('base', 'odom',
posemsg.header.stamp,
rospy.Duration(1.0))
x = tfmsg.transform.translation.x
y = tfmsg.transform.translation.y
theta = 2*math.atan2(tfmsg.transform.rotation.z,
tfmsg.transform.rotation.w)
# Compute the matching odom frame, effectively reseting the
# localization.
odom_theta = bot_theta - theta
odom_x = bot_x - math.cos(odom_theta) * x + math.sin(odom_theta) * y
odom_y = bot_y - math.sin(odom_theta) * x - math.cos(odom_theta) * y
######################################################################
# Setup and Utility Routines
######################################################################
#
# Broadcast the Map->Odom Transform
#
# This provides the latest localization, implicitly adjusting the
# bot's pose and aligning future laser scans to the map.
#
def broadcastOdom(timestamp):
# Prepare the transform message.
tfmsg = TransformStamped()
tfmsg.header.stamp = timestamp
tfmsg.header.frame_id = 'map'
tfmsg.child_frame_id = 'odom'
tfmsg.transform.translation.x = odom_x
tfmsg.transform.translation.y = odom_y
tfmsg.transform.translation.z = 0.0
tfmsg.transform.rotation.x = 0.0
tfmsg.transform.rotation.y = 0.0
tfmsg.transform.rotation.z = math.sin(odom_theta/2)
tfmsg.transform.rotation.w = math.cos(odom_theta/2)
# Broadcast.
tfbroadcast.sendTransform(tfmsg)
# Report.
rospy.loginfo("Broadcast map->odom: x %6.3f m, y %6.3f m, theta %6.2f deg"
% (odom_x, odom_y, odom_theta * 180.0/math.pi))
#
# Grab the Map
#
# We could do this repeatedly, i.e. set up a subscriber. But I'm
# assuming the map will remain constant?
#
def grabMap():
# Report
rospy.loginfo("Waiting for a map...")
# Grab a single message of the map topic. Give it 5 seconds.
mapmsg = rospy.wait_for_message('/map', OccupancyGrid, 5.0)
# Note the map info.
w = mapmsg.info.width
h = mapmsg.info.height
res = mapmsg.info.resolution
x = mapmsg.info.origin.position.x
y = mapmsg.info.origin.position.y
theta = math.atan2(mapmsg.info.origin.orientation.z,
mapmsg.info.origin.orientation.w)
# Save the map. From the map message documentation:
# The map data, in row-major order, starting with (0,0).
# Occupancy probabilities are in the range [0,100]. Unknown is -1.
global map
map = np.array(mapmsg.data, dtype=np.int8).reshape(h, w)
# Report.
rospy.loginfo("Have the map: %dx%d = %5.3fm x %5.3fm (%4.3fm resolution)"
% (w, h, w*res, h*res, res))
rospy.loginfo("Note map origin x=%4.3fm y=%4.3fm theta=%5.2fdeg"
% (x, y, theta * 180.0/math.pi))
######################################################################
# Main Callback
######################################################################
#
# Laser Scan Callback
#
# Do the fun stuff!
#
def callback_scan(scanmsg):
# Use the odom global variables
global odom_x, odom_y, odom_theta
# Extract the valid ranges from the scan information.
ranges = np.array(scanmsg.ranges)
alphas = (scanmsg.angle_min +
scanmsg.angle_increment * np.arange(len(ranges)))
valid = (ranges >= scanmsg.range_min) & (ranges <= scanmsg.range_max)
ranges = ranges[valid]
alphas = alphas[valid]
# For debugging report the message.
rospy.loginfo("Scan #%4d at %10d secs with %d valid ranges"
% (scanmsg.header.seq, scanmsg.header.stamp.secs,
len(ranges)))
# Also grab the transform from odom to the laser scan data's frame
# (effectively the odometry). In particular, ask for the
# transform at the time the scan data was captured - this makes
# things as self-consistent as possible. Give it up to 100ms, in
# case Linux has (temporarily) swapped out the odometry node.
tfmsg = tfBuffer.lookup_transform('odom', scanmsg.header.frame_id,
scanmsg.header.stamp,
rospy.Duration(0.100))
x = tfmsg.transform.translation.x
y = tfmsg.transform.translation.y
theta = 2*math.atan2(tfmsg.transform.rotation.z,
tfmsg.transform.rotation.w)
# Use the stored map->odom transform to find the laser in the map
# frame.
laser_x = odom_x + math.cos(odom_theta) * x - math.sin(odom_theta) * y
laser_y = odom_y + math.sin(odom_theta) * x + math.cos(odom_theta) * y
laser_theta = odom_theta + theta
# Compute the array of contact points (x,y) in map frame.
ptx = laser_x + ranges * np.cos(alphas + laser_theta)
pty = laser_y + ranges * np.sin(alphas + laser_theta)
# DO LOTS OF THINGS
# =================
# For fun: shift the odometry frame (translation only) so that the
# center of the laser scan moves to the origin.
odom_x -= updatefrac * np.mean(ptx)
odom_y -= updatefrac * np.mean(pty)
# Finally broadcast the latest odom frame estimate. Might as well
# use the timestamp of the scan data that was used to align.
broadcastOdom(scanmsg.header.stamp)
######################################################################
# Main code
######################################################################
if __name__ == "__main__":
# Initialize the ROS node.
rospy.init_node('localization')
# Grab the ROS parameters.
updatefrac = rospy.get_param('~update_fraction', 0.2)
# Grab a single copy the map (rather than subscribing).
grabMap()
# First create a TF2 listener. This implicily fills a local
# buffer, so we can always retrive the transforms we want.
tfBuffer = tf2_ros.Buffer()
tflisten = tf2_ros.TransformListener(tfBuffer)
# And create a TF2 transform broadcaster. Give it a little time
# to connect, then broadcast the initial odom pose.
tfbroadcast = tf2_ros.TransformBroadcaster()
rospy.sleep(0.25)
broadcastOdom(rospy.Time.now())
# Create a publisher to send the pose messages.
posepub = rospy.Publisher('/pose', PoseStamped, queue_size=10)
# Finally create subscribers to listen to odometry, scan, initial
# pose messages (to last to reset the localization).
rospy.Subscriber('/odom', Odometry, callback_odom)
rospy.Subscriber('/scan', LaserScan, callback_scan)
rospy.Subscriber('/initialpose', PoseWithCovarianceStamped, callback_init)
# Report and spin (waiting while callbacks do their work).
rospy.loginfo("Localization spinning... (update fraction %f)"
% updatefrac)
rospy.spin()
rospy.loginfo("Localization stopped.")
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