GroundGrid.cpp
Go to the documentation of this file.
00001 /*+-------------------------------------------------------------------------+
00002   |                       MultiVehicle simulator (libmvsim)                 |
00003   |                                                                         |
00004   | Copyright (C) 2014  Jose Luis Blanco Claraco (University of Almeria)    |
00005   | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
00006   | Distributed under GNU General Public License version 3                  |
00007   |   See <http://www.gnu.org/licenses/>                                    |
00008   +-------------------------------------------------------------------------+ */
00009 
00010 #include <mvsim/WorldElements/GroundGrid.h>
00011 #include <mvsim/World.h>
00012 #include "xml_utils.h"
00013 
00014 #include <mrpt/opengl/COpenGLScene.h>
00015 #include <rapidxml.hpp>
00016 
00017 using namespace rapidxml;
00018 using namespace mvsim;
00019 using namespace std;
00020 
00021 GroundGrid::GroundGrid(World* parent, const rapidxml::xml_node<char>* root)
00022         : WorldElementBase(parent),
00023           m_is_floating(true),
00024           m_x_min(-25.0),
00025           m_x_max(25.0),
00026           m_y_min(-25.0),
00027           m_y_max(25.0),
00028           m_interval(5.0),
00029           m_color(0xe0, 0xe0, 0xe0, 0xff),
00030           m_line_width(1.0)
00031 {
00032         // Create opengl object: in this class, we'll store most state data directly
00033         // in the mrpt::opengl object.
00034         loadConfigFrom(root);
00035 }
00036 
00037 GroundGrid::~GroundGrid() {}
00038 void GroundGrid::loadConfigFrom(const rapidxml::xml_node<char>* root)
00039 {
00040         if (!root) return;  // Assume defaults
00041 
00042         std::map<std::string, TParamEntry> params;
00043         params["floating"] = TParamEntry("%bool", &m_is_floating);
00044         params["floating_focus"] =
00045                 TParamEntry("%s", &m_float_center_at_vehicle_name);
00046         params["color"] = TParamEntry("%color", &m_color);
00047         params["line_width"] = TParamEntry("%lf", &m_line_width);
00048 
00049         params["x_min"] = TParamEntry("%lf", &m_x_min);
00050         params["x_max"] = TParamEntry("%lf", &m_x_max);
00051         params["y_min"] = TParamEntry("%lf", &m_y_min);
00052         params["y_max"] = TParamEntry("%lf", &m_y_max);
00053         params["interval"] = TParamEntry("%lf", &m_interval);
00054 
00055         parse_xmlnode_children_as_param(*root, params);
00056 
00057         // If a vehicle name is given, setting "is_floating=true" by the user is
00058         // optional:
00059         if (!m_float_center_at_vehicle_name.empty()) m_is_floating = true;
00060 }
00061 
00062 void GroundGrid::gui_update(mrpt::opengl::COpenGLScene& scene)
00063 {
00064         using namespace mrpt::math;
00065 
00066         // 1st call OR gridmap changed?
00067         if (!m_gl_groundgrid)
00068         {
00069                 m_gl_groundgrid = mrpt::opengl::CGridPlaneXY::Create();
00070                 m_gl_groundgrid->setPlaneLimits(m_x_min, m_x_max, m_y_min, m_y_max);
00071                 m_gl_groundgrid->setGridFrequency(m_interval);
00072                 m_gl_groundgrid->setColor(TColorf(m_color));
00073                 m_gl_groundgrid->setLineWidth(m_line_width);
00074 
00075                 SCENE_INSERT_Z_ORDER(scene, 0, m_gl_groundgrid);
00076         }
00077 
00078         // Update:
00079         mrpt::math::TPoint3D center_offset(.0, .0, .0);
00080         if (m_is_floating)
00081         {
00082                 // Centered at a vehicle:
00083                 const World::TListVehicles& vehs = m_world->getListOfVehicles();
00084                 // Look for the vehicle by its name:
00085                 World::TListVehicles::const_iterator it_veh =
00086                         vehs.find(m_float_center_at_vehicle_name);
00087                 // not found -> error:
00088                 if (!m_float_center_at_vehicle_name.empty() && it_veh == vehs.end())
00089                         throw std::runtime_error(
00090                                 mrpt::format(
00091                                         "[GroundGrid] *ERROR* Cannot find vehicle named '%s' to "
00092                                         "focus on.",
00093                                         m_float_center_at_vehicle_name.c_str()));
00094 
00095                 // If the user didn't specify any name, assume it's the first vehicle,
00096                 // or ignore it if none exists:
00097                 if (it_veh == vehs.end()) it_veh = vehs.begin();
00098 
00099                 if (it_veh != vehs.end())
00100                 {
00101                         const mrpt::math::TPose3D& pose = it_veh->second->getPose();
00102                         center_offset.x = pose.x;
00103                         center_offset.y = pose.y;
00104                 }
00105         }
00106 
00107         // "Discretize" offset for a better visual impact:
00108         ASSERT_(m_interval > .0);
00109         center_offset.x = m_interval *
00110                                           ::floor(std::abs(center_offset.x) / m_interval) *
00111                                           (center_offset.x < 0 ? -1. : 1.);
00112         center_offset.y = m_interval *
00113                                           ::floor(std::abs(center_offset.y) / m_interval) *
00114                                           (center_offset.y < 0 ? -1. : 1.);
00115         m_gl_groundgrid->setLocation(center_offset);
00116 }


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:35