handles localization for the robot base More...
#include <localizer.h>
Public Member Functions | |
void | baseOdometryCallback (const nav_msgs::Odometry &odometry) |
float | convertAngle (float theta) |
localizer () | |
void | positionFromCameraCallback (const youbot_overhead_vision::PositionFromCamera &newPos) |
Public Attributes | |
ros::Publisher | baseLocalizationPublisher |
ros::Subscriber | baseOdometrySubscriber |
ros::ServiceClient | coordConversionClient |
nav_msgs::Odometry | lastReceivedOdometry |
ros::NodeHandle | n |
bool | odometryInitialized |
float | P [3] |
youbot_overhead_localization::Pose2d | pose |
ros::Subscriber | positionFromCameraSubscriber |
float | Q [3] |
float | R [3] |
handles localization for the robot base
This class keeps track of the localization of the robot base, represented by a 2d pose consisting of x, y, and heading. This is updated periodically with the base localization, and with the position determined by the overhead cameras when that information is available.
Definition at line 36 of file localizer.h.
Constructor to initialize localization
This file contains the definition of the localizer class, which localizes the robot base using a Kalman filter with odometry and the overhead camera information as inputs.
Definition at line 11 of file localizer.cpp.
void localizer::baseOdometryCallback | ( | const nav_msgs::Odometry & | odometry | ) |
Update localization based on base odometry
odometry | updated odometry information |
Definition at line 42 of file localizer.cpp.
float localizer::convertAngle | ( | float | theta | ) |
Converts an angle (in radians) so that it is within the range of -pi to pi
theta | the angle to be converted |
Definition at line 126 of file localizer.cpp.
void localizer::positionFromCameraCallback | ( | const youbot_overhead_vision::PositionFromCamera & | newPos | ) |
Update robot pose based on the position determined by the overhead cameras
newPos | the new position calculated from the camera image |
Definition at line 89 of file localizer.cpp.
Definition at line 41 of file localizer.h.
Definition at line 43 of file localizer.h.
Definition at line 42 of file localizer.h.
nav_msgs::Odometry localizer::lastReceivedOdometry |
Definition at line 56 of file localizer.h.
Definition at line 40 of file localizer.h.
Definition at line 55 of file localizer.h.
float localizer::P[3] |
Definition at line 50 of file localizer.h.
Definition at line 47 of file localizer.h.
Definition at line 44 of file localizer.h.
float localizer::Q[3] |
Definition at line 51 of file localizer.h.
float localizer::R[3] |
Definition at line 52 of file localizer.h.