dummy_slam_broadcaster.cpp
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 }


dummy_slam_broadcaster
Author(s):
autogenerated on Sat Sep 17 2016 04:01:06