Go to the documentation of this file.00001 #include "dummy_slam_broadcaster.h"
00002
00003
00004 DummySLAMBroadcaster::DummySLAMBroadcaster():node_handle_("~")
00005 {
00006 map_publisher_ = node_handle_.advertise<nav_msgs::OccupancyGrid>("/map",1);
00007 pub_map_timer_ = node_handle_.createTimer(ros::Duration(1.0), &DummySLAMBroadcaster::broadcastMap, this);
00008
00009 node_handle_.param("broadcast_zero_pose_enabled",broadcast_zero_pose_enabled_,1);
00010 if(broadcast_zero_pose_enabled_)
00011 {
00012 pose_publisher_ = node_handle_.advertise<geometry_msgs::PoseStamped>("/slam_out_pose",10);
00013 pub_pose_timer_ = node_handle_.createTimer(ros::Duration(0.05), &DummySLAMBroadcaster::broadcastPose, this);
00014 }
00015 }
00016
00017
00018 void DummySLAMBroadcaster::broadcastMap(const ros::TimerEvent &e)
00019 {
00020 empty_map_.header.stamp = ros::Time::now();
00021 map_publisher_.publish( empty_map_ );
00022 }
00023
00024
00025 void DummySLAMBroadcaster::broadcastPose(const ros::TimerEvent &e)
00026 {
00027 tf::Transform t;
00028 t.setOrigin( tf::Vector3(0,0,0) );
00029 t.setRotation(tf::Quaternion( 0,0,0,1 ));
00030 tf_pose_broadcaster_.sendTransform(tf::StampedTransform(t,ros::Time::now(),"/map","/scan"));
00031
00032 geometry_msgs::PoseStamped pose;
00033 pose.pose.position.x = 0;
00034 pose.pose.position.y = 0;
00035 pose.pose.position.z = 0;
00036
00037 pose.pose.orientation.w = 0;
00038 pose.pose.orientation.x = 0;
00039 pose.pose.orientation.y = 0;
00040 pose.pose.orientation.z = 0;
00041
00042 pose.header.frame_id = "/map";
00043 pose.header.stamp = ros::Time::now();
00044
00045 pose_publisher_.publish(pose);
00046 }
00047
00048
00049 void DummySLAMBroadcaster::initMap()
00050 {
00051 const std::size_t mapSize = 2500;
00052 empty_map_.header.frame_id = "/map";
00053 empty_map_.info.resolution = 0.04;
00054 empty_map_.info.width = mapSize;
00055 empty_map_.info.height = mapSize;
00056 empty_map_.info.origin.position.x = -50;
00057 empty_map_.info.origin.position.y = -50;
00058 empty_map_.info.origin.position.z = 0;
00059
00060 empty_map_.info.origin.orientation.x = 0;
00061 empty_map_.info.origin.orientation.y = 0;
00062 empty_map_.info.origin.orientation.z = 0;
00063 empty_map_.info.origin.orientation.w = 1;
00064
00065 empty_map_.data.resize(mapSize*mapSize,-1);
00066 }
00067
00068
00069 int main(int argc, char **argv)
00070 {
00071 ros::init(argc, argv, "dummy_slam_broadcaster");
00072 new DummySLAMBroadcaster();
00073 ros::spin();
00074 return 0;
00075 }