TNeighborAgentMapProps.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "mrpt_graphslam_2d/TNeighborAgentMapProps.h"

#include <mrpt/version.h>
#if MRPT_VERSION>=0x199
using namespace mrpt::system;
#else
using namespace mrpt::utils;
#endif

using namespace mrpt_msgs;
using namespace mrpt::maps;
using namespace mrpt::graphslam;
using namespace ros;
using namespace nav_msgs;
using namespace std;


TNeighborAgentMapProps::TNeighborAgentMapProps(
                mrpt::utils::COutputLogger* logger_in,
                const mrpt_msgs::GraphSlamAgent& agent_in,
                ros::NodeHandle* nh_in):
        m_logger(logger_in),
        nh(nh_in),
        agent(agent_in),
        queue_size(1),
        has_init_class(false),
        has_setup_comm(false)
{
        ASSERT_(nh);
        ASSERT_(m_logger);
        m_logger->logFmt(LVL_WARN, "In TNeighborAgentMapProps constructor");

        this->map_topic = "/" + agent.topic_namespace.data + "/" +
                "feedback" + "/" + "gridmap";
        this->robot_trajectory_topic = "/" + agent.topic_namespace.data + "/" +
                "feedback" + "/" + "robot_trajectory";

        cout << "Map topic: " << this->map_topic << endl;
        cout << "Trajectory topic: " << this->robot_trajectory_topic << endl;

        has_init_class = true;
        readROSParameters();

}

void TNeighborAgentMapProps::readROSParameters() {
        //std::map<std::sttring, std::string> agent_pose;
        //bool res = nh->getParam("/" + agent.topic_namespace.data + "/" + "global_pos", agent_pose);

        //if (res) {
                //m_logger->logFmt(LVL_INFO, "%s", getMapAsString(agent_pose).c_str());

                //global_init_pos = pose_t(
                                //atof(pos_x),
                                //atof(pos_y),
                                //atof(rot_z));
                //m_logger->logFmt(LVL_INFO, "global_init_pos: %s\n", global_init_pos);
        //}
        //else {
                //cout << "parameter fetch was not successful" << endl;
        //}

}

void TNeighborAgentMapProps::setupComm() {
        ASSERT_(has_init_class);
        m_logger->logFmt(LVL_WARN, "In TNeighborAgentMapProps::setupComm");
        this->setupSubs();
        has_setup_comm = true;
}

void TNeighborAgentMapProps::setupSubs() {
        m_logger->logFmt(LVL_WARN, "In TNeighborAgentMapProps::setupSubs");
        this->map_sub  = nh->subscribe<nav_msgs::OccupancyGrid>(
                        this->map_topic,
                        this->queue_size,
                        &TNeighborAgentMapProps::updateGridMap, this);

        this->robot_trajectory_sub  = nh->subscribe<nav_msgs::Path>(
                        this->robot_trajectory_topic,
                        this->queue_size,
                        &TNeighborAgentMapProps::updateRobotTrajectory, this);
} // end of setupSubs

void TNeighborAgentMapProps::updateGridMap(
                const nav_msgs::OccupancyGrid::ConstPtr& nav_gridmap) {
        nav_map = nav_gridmap;
} // end of updateGridMap

void TNeighborAgentMapProps::updateRobotTrajectory(
                const nav_msgs::Path::ConstPtr& nav_robot_traj) {
        nav_robot_trajectory = nav_robot_traj;
}