Guest User

Untitled

a guest
Mar 1st, 2015
279
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <ros/ros.h>
  2. #include <iostream>
  3. //#include <move_base_msgs/MoveBaseAction.h>
  4. //#include <actionlib/client/simple_action_client.h>
  5.  
  6. #include <catkin_costmap_test/t.h>
  7. //#include <costmap_2d/costmap_2d.h>
  8. //#include <costmap_2d/costmap_2d_ros.h>
  9.  
  10. //#include <nav_msgs/GridCells.h>
  11. //#include <opencv2/core/core.hpp>
  12. //#include <opencv2/highgui/highgui.hpp>
  13. //#include <opencv2/imgproc/imgproc.hpp>
  14.  
  15. //
  16. #define REFRESHING_FREQ (0.5f) // [s]
  17. //
  18.  
  19. class LocalGoalPlanner
  20. {
  21. public:
  22.     LocalGoalPlanner()
  23.         :m_Handle("LocalGoalPlanner"),
  24.         m_tf(),
  25.         m_CostmapRos("local_costmap", m_tf)
  26.     {
  27.         m_CostmapRos.stop();
  28.         m_CostmapRos.getCostmapCopy(m_Costmap);
  29.         //m_subObstacles=m_Handle.subscribe("/move_base_node/global_costmap/obstacles", 100, &LocalGoalPlanner::CostmapCallback, this);
  30.         cv::namedWindow( "Costmap", cv::WINDOW_AUTOSIZE );
  31.  
  32.         //do odświeżania mapy
  33.         m_Timer= m_Handle.createTimer(ros::Duration(REFRESHING_FREQ), boost::bind(&LocalGoalPlanner::OnTimeout ,this));
  34.  
  35.         m_CostmapRos.stop();
  36.         m_CostmapRos.updateMap();
  37.         m_CostmapRos.start();
  38.     }
  39.     void ReadManualy(){
  40.         m_CostmapRos.updateMap();
  41.         m_CostmapRos.getCostmapCopy(m_Costmap);
  42.         int max_x=m_Costmap.getSizeInCellsX();
  43.         int max_y=m_Costmap.getSizeInCellsY();
  44.         for(int x=0; x<max_x; x++){
  45.             for(int y=0; y<max_y; y++){
  46.                 std::cout<<(int)m_Costmap.getCost(x,y)<<" ";
  47.             }
  48.             std::cout<<std::endl;
  49.  
  50.             std::cout<<std::endl;
  51.         }
  52.         std::cout<<std::endl;
  53.        
  54.         std::cout<<std::endl;
  55.     }
  56.     void PrintCostmap(){
  57.         m_CostmapRos.updateMap();
  58.         m_CostmapRos.getCostmapCopy(m_Costmap);
  59.         m_MatCostmap = cv::Mat(m_Costmap.getSizeInCellsX(), m_Costmap.getSizeInCellsY(),CV_8UC1, (unsigned char*)m_Costmap.getCharMap() );
  60.         cv::imshow("Costmap", m_MatCostmap);
  61.         cv::waitKey(5);
  62.     }
  63.  
  64. private:
  65.     tf::TransformListener           m_tf;
  66.     costmap_2d::Costmap2D           m_Costmap;
  67.     costmap_2d::Costmap2DROS        m_CostmapRos;
  68.     ros::NodeHandle                 m_Handle;
  69.     ros::Subscriber                 m_subObstacles;
  70.     nav_msgs::GridCellsConstPtr     m_CurrentObstacles;
  71.     cv::Mat                         m_MatCostmap;
  72.     ros::Timer                      m_Timer;
  73.  
  74.     void OnTimeout(){
  75.         PrintCostmap();
  76.         //ReadManualy();
  77.     }
  78. };
  79.  
  80. int main(int argc, char** argv){
  81.     ros::init(argc, argv, "my_costmap");
  82.     LocalGoalPlanner x;
  83.  
  84.  
  85.  
  86.     ros::spin();
  87.     return 0;
  88. }
RAW Paste Data