TNeighborAgentMapProps.cpp
Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |                     Mobile Robot Programming Toolkit (MRPT)               |
00003    |                          http://www.mrpt.org/                             |
00004    |                                                                           |
00005    | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
00006    | See: http://www.mrpt.org/Authors - All rights reserved.                   |
00007    | Released under BSD License. See details in http://www.mrpt.org/License    |
00008    +---------------------------------------------------------------------------+ */
00009 
00010 #include "mrpt_graphslam_2d/TNeighborAgentMapProps.h"
00011 
00012 #include <mrpt/version.h>
00013 #if MRPT_VERSION>=0x199
00014 using namespace mrpt::system;
00015 #else
00016 using namespace mrpt::utils;
00017 #endif
00018 
00019 using namespace mrpt_msgs;
00020 using namespace mrpt::maps;
00021 using namespace mrpt::graphslam;
00022 using namespace ros;
00023 using namespace nav_msgs;
00024 using namespace std;
00025 
00026 
00027 TNeighborAgentMapProps::TNeighborAgentMapProps(
00028                 mrpt::utils::COutputLogger* logger_in,
00029                 const mrpt_msgs::GraphSlamAgent& agent_in,
00030                 ros::NodeHandle* nh_in):
00031         m_logger(logger_in),
00032         nh(nh_in),
00033         agent(agent_in),
00034         queue_size(1),
00035         has_init_class(false),
00036         has_setup_comm(false)
00037 {
00038         ASSERT_(nh);
00039         ASSERT_(m_logger);
00040         m_logger->logFmt(LVL_WARN, "In TNeighborAgentMapProps constructor");
00041 
00042         this->map_topic = "/" + agent.topic_namespace.data + "/" +
00043                 "feedback" + "/" + "gridmap";
00044         this->robot_trajectory_topic = "/" + agent.topic_namespace.data + "/" +
00045                 "feedback" + "/" + "robot_trajectory";
00046 
00047         cout << "Map topic: " << this->map_topic << endl;
00048         cout << "Trajectory topic: " << this->robot_trajectory_topic << endl;
00049 
00050         has_init_class = true;
00051         readROSParameters();
00052 
00053 }
00054 
00055 void TNeighborAgentMapProps::readROSParameters() {
00056         //std::map<std::sttring, std::string> agent_pose;
00057         //bool res = nh->getParam("/" + agent.topic_namespace.data + "/" + "global_pos", agent_pose);
00058 
00059         //if (res) {
00060                 //m_logger->logFmt(LVL_INFO, "%s", getMapAsString(agent_pose).c_str());
00061 
00062                 //global_init_pos = pose_t(
00063                                 //atof(pos_x),
00064                                 //atof(pos_y),
00065                                 //atof(rot_z));
00066                 //m_logger->logFmt(LVL_INFO, "global_init_pos: %s\n", global_init_pos);
00067         //}
00068         //else {
00069                 //cout << "parameter fetch was not successful" << endl;
00070         //}
00071 
00072 }
00073 
00074 void TNeighborAgentMapProps::setupComm() {
00075         ASSERT_(has_init_class);
00076         m_logger->logFmt(LVL_WARN, "In TNeighborAgentMapProps::setupComm");
00077         this->setupSubs();
00078         has_setup_comm = true;
00079 }
00080 
00081 void TNeighborAgentMapProps::setupSubs() {
00082         m_logger->logFmt(LVL_WARN, "In TNeighborAgentMapProps::setupSubs");
00083         this->map_sub  = nh->subscribe<nav_msgs::OccupancyGrid>(
00084                         this->map_topic,
00085                         this->queue_size,
00086                         &TNeighborAgentMapProps::updateGridMap, this);
00087 
00088         this->robot_trajectory_sub  = nh->subscribe<nav_msgs::Path>(
00089                         this->robot_trajectory_topic,
00090                         this->queue_size,
00091                         &TNeighborAgentMapProps::updateRobotTrajectory, this);
00092 } // end of setupSubs
00093 
00094 void TNeighborAgentMapProps::updateGridMap(
00095                 const nav_msgs::OccupancyGrid::ConstPtr& nav_gridmap) {
00096         nav_map = nav_gridmap;
00097 } // end of updateGridMap
00098 
00099 void TNeighborAgentMapProps::updateRobotTrajectory(
00100                 const nav_msgs::Path::ConstPtr& nav_robot_traj) {
00101         nav_robot_trajectory = nav_robot_traj;
00102 }
00103 


mrpt_graphslam_2d
Author(s): Nikos Koukis
autogenerated on Thu Jun 6 2019 21:40:26