34 #ifndef MRPT_LOCALIZATION_NODE_H 35 #define MRPT_LOCALIZATION_NODE_H 42 #include <std_msgs/Header.h> 43 #include <nav_msgs/Odometry.h> 44 #include <nav_msgs/GetMap.h> 45 #include <sensor_msgs/LaserScan.h> 46 #include <geometry_msgs/PoseWithCovarianceStamped.h> 47 #include <nav_msgs/OccupancyGrid.h> 48 #include <nav_msgs/MapMetaData.h> 49 #include <dynamic_reconfigure/server.h> 51 #include <mrpt/version.h> 52 #include <mrpt/obs/CObservationOdometry.h> 55 #include "mrpt_localization/MotionConfig.h" 57 #include "mrpt_msgs/ObservationRangeBeacon.h" 73 dynamic_reconfigure::Server<mrpt_localization::MotionConfig>
75 dynamic_reconfigure::Server<
77 void update(
const unsigned long& loop_count);
147 const std::string& source_frame,
const ros::Time& time,
151 nav_msgs::GetMap::Request& req, nav_msgs::GetMap::Response&
res);
156 #endif // MRPT_LOCALIZATION_NODE_H
void callbackRobotPose(const geometry_msgs::PoseWithCovarianceStamped &)
std::string base_frame_id
Parameters(PFLocalizationNode *p)
ros::ServiceClient client_map_
void publishTF()
Publish map -> odom tf; as the filter provides map -> base, we multiply it by base -> odom...
dynamic_reconfigure::Server< mrpt_localization::MotionConfig > reconfigure_server_
std::string global_frame_id
ros::Subscriber sub_init_pose_
virtual bool waitForMap()
ros::Time time_last_input_
static const int MOTION_MODEL_THRUN
int parameter_update_skip
we wait before start complaining
void publishPose()
Publish the current pose of the robot.
ros::Publisher pub_particles_
ros::Publisher pub_metadata_
void updateSensorPose(std::string frame_id)
dynamic_reconfigure::Server< mrpt_localization::MotionConfig >::CallbackType reconfigure_cb_
std::map< std::string, mrpt::poses::CPose3D > beacon_poses_
void callbackParameters(mrpt_localization::MotionConfig &config, uint32_t level)
void odometryForCallback(CObservationOdometry::Ptr &, const std_msgs::Header &)
void callbackOdometry(const nav_msgs::Odometry &)
tf::TransformListener tf_listener_
static const int MOTION_MODEL_GAUSSIAN
bool waitForTransform(mrpt::poses::CPose3D &des, const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01))
std::vector< ros::Subscriber > sub_sensors_
ros::Subscriber sub_odometry_
PFLocalizationNode(ros::NodeHandle &n)
void callbackLaser(const sensor_msgs::LaserScan &)
std::string odom_frame_id
void callbackBeacon(const mrpt_msgs::ObservationRangeBeacon &)
bool mapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
tf::TransformBroadcaster tf_broadcaster_
double transform_tolerance
unsigned long long loop_count_
bool update_while_stopped
nav_msgs::GetMap::Response resp_
void update(const unsigned long &loop_count)
double no_inputs_tolerance
using filter time instead of Time::now
int particlecloud_update_skip
ros::ServiceServer service_map_
virtual ~PFLocalizationNode()
std::map< std::string, mrpt::poses::CPose3D > laser_poses_
void updateMap(const nav_msgs::OccupancyGrid &)
void callbackInitialpose(const geometry_msgs::PoseWithCovarianceStamped &)
void callbackMap(const nav_msgs::OccupancyGrid &)
double no_update_tolerance
the published tf to extend its validity