MultiRobotInfoVisual.h

Go to the documentation of this file.

#ifndef MULTI_ROBOT_INFO_VISUAL_HPP
#define MULTI_ROBOT_INFO_VISUAL_HPP
 
#include <rviz/tool.h>
#include <ros/ros.h>
#include <rviz/properties/vector_property.h>
#include <rviz/properties/int_property.h>
#include <rviz/properties/string_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/ogre_helpers/arrow.h>
#include <OgreManualObject.h>
#include <rviz/ogre_helpers/shape.h>
#include <map>
#include <memory>
#include <vector>
#include <boost/circular_buffer.hpp>
#include "TextVisual.h"
 
#include <tuw_multi_robot_msgs/RobotInfo.h>
#include <tuw_multi_robot_msgs/Route.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
 
namespace tuw_multi_robot_rviz
{
 
class RobotAttributes {
 
  public:
 
    using buf_type = boost::circular_buffer<geometry_msgs::PoseWithCovariance>;
    RobotAttributes(size_t id,
                    std::string &rname,
                    double rad,
                    buf_type &pose,
                    Ogre::ColourValue color,
                    Ogre::SceneManager *_scene_manager,
                    Ogre::SceneNode *_parent_node);
 
    ~RobotAttributes();
 
    std::string robot_name;
    double robot_radius;
    size_t robot_id;
    double path_length_;
    bool disabled;
    bool path_stale;
 
    std::vector<std::unique_ptr<TextVisual>> route_visuals;
 
    void updateColor(Ogre::ColourValue &c)
    {
      color = c;
    }
 
    void setDisabled()
    {
      disabled = true;
      delete circle;
      delete arrow;
      circle = NULL;
      arrow = NULL;
    }
 
    void updatePose(const geometry_msgs::PoseWithCovariance &pose)
    {
      ros_poses.push_front(pose);
      current_pos = Ogre::Vector3(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z);
      current_orient = Ogre::Quaternion(pose.pose.orientation.w, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z);
    }
 
    void updatePose(const buf_type &pose)
    {
      ros_poses = pose;
 
      if (pose.empty())
      {
        return;
      }
 
      const auto current_pose = pose.front();
      current_pos = Ogre::Vector3(current_pose.pose.position.x,
                                  current_pose.pose.position.y,
                                  current_pose.pose.position.z);
 
      current_orient = Ogre::Quaternion(current_pose.pose.orientation.w,
                                        current_pose.pose.orientation.x,
                                        current_pose.pose.orientation.y,
                                        current_pose.pose.orientation.z);
    }
 
    void setPoseBufferLength(size_t len)
    {
      ros_poses.resize(len);
    }
 
    double getPathLength()
    {
      if (path_stale)
      {
        updatePathLength();
      }
      return path_length_;
    }
 
    void render();
 
  private:
    buf_type ros_poses;
    std::shared_ptr<tuw_multi_robot_msgs::Route> route;
    ros::Subscriber sub_route;
    std::size_t seg_id_current;
 
    Ogre::ColourValue color;
    Ogre::Vector3 current_pos;
    Ogre::Quaternion current_orient;
    Ogre::SceneManager *scene_manager;
    Ogre::SceneNode *frame_node;
    Ogre::ManualObject* circle;
    Ogre::ManualObject* arrow;
    std::unique_ptr<TextVisual> text;
 
    void cbRoute(const ros::MessageEvent<const tuw_multi_robot_msgs::Route> &_event, int _topic);
 
    void updateCircle(bool first_time=false);
 
    void updateArrow(bool first_time=false);
 
    void updateText(bool first_time=false);
 
    void make_robot();
 
    void make_route();
 
    void updatePathLength()
    {
      if (route == nullptr)
        return;
 
      double min_dist = std::numeric_limits<double>::max();
 
      //Assumes sorted path!!
      for (size_t i=seg_id_current; i < route->segments.size(); ++i)
      {
        const auto s = route->segments[i];
        const auto position_s = s.start.position;
        const auto position_e = s.end.position;
        Eigen::Vector3f ps(position_s.x, position_s.y, position_s.z);
        Eigen::Vector3f dvec (ps - Eigen::Vector3f(current_pos.x, current_pos.y, current_pos.z));
        double dist = dvec.norm();
        if (dist < min_dist)
        {
          min_dist = dist;
          seg_id_current = i;
        }
      }
 
      double acc_len = 0;
      for (size_t i=seg_id_current; i < route->segments.size(); ++i)
      {
        const auto s = route->segments[i];
        auto position_s = s.start.position;
        if (i==seg_id_current)
        {
          position_s.x = current_pos.x;
          position_s.y = current_pos.y;
          position_s.z = current_pos.z;
        }
        const auto position_e = s.end.position;
        const double dist = pow(position_e.x - position_s.x, 2) + pow(position_e.y - position_s.y,2) + pow(position_e.z - position_s.z,2);
        acc_len += sqrt(dist);
      }
      path_length_ = acc_len;
    }
  };
 
class MultiRobotInfoVisual
{
 
public:
 
  MultiRobotInfoVisual(Ogre::SceneManager *_scene_manager, Ogre::SceneNode *_parent_node);
 
  virtual ~MultiRobotInfoVisual();
 
  void setMessage(const tuw_multi_robot_msgs::RobotInfoConstPtr _msg);
 
  void setFramePosition( const Ogre::Vector3& _position);
  void setFrameOrientation( const Ogre::Quaternion& orientation );
 
  // Set the scale of the visual, which is an user-editable
  // parameter and therefore don't come from the RobotGoalsArrayStamped message.
  void setScalePose( float scale );
 
  // Set the color of the visual's Pose, which is an user-editable
  // parameter and therefore don't come from the RobotGoalsArrayStamped message.
  void setColorPose( Ogre::ColourValue color );
 
  void disableRobot( const std::string &rName );
 
  void enableRobot( const std::string &rName );
 
  void resetDuration( const ros::Duration &ts);
 
  void resetKeepMeasurementsCount ( const unsigned int c );
 
  void doRender();
 
 
private:
  using internal_map_type = std::pair<std::string, std::shared_ptr<RobotAttributes>>;
  using map_type = std::map<std::string, std::shared_ptr<RobotAttributes>>;
  using map_iterator = std::map<std::string, std::shared_ptr<RobotAttributes>>::iterator;
  using recycle_map_type = std::map<std::string, ros::Time>;
 
  std::vector<std::string> recycle();
 
  // The object implementing the actual pose shape
  //std::map<std::string, std::vector<Ogre::ManualObject*>> robot_renderings_map_;
  //std::map<std::string, std::vector<std::unique_ptr<TextVisual>>> robot_route_renderings_map_;
  std::set<std::string> disabled_robots_;
 
  map_type robot2attribute_map_;
  recycle_map_type recycle_map_;
 
  int default_size_ = {5};
  int id_cnt = 0;
  ros::Duration recycle_thresh_ = ros::Duration(5,0);
  ros::Duration render_dur_thresh_ = ros::Duration(0.1);
  ros::Time last_render_time_;
  // A SceneNode whose pose is set to match the coordinate frame of
  // the Imu message header.
  Ogre::SceneNode* frame_node_;
 
  // The SceneManager, kept here only so the destructor can ask it to
  // destroy the ``frame_node_``.
  Ogre::SceneManager* scene_manager_;
 
  // The pose Shape object's scale
  float scale_pose_;
 
  // The pose Shape object's color
  Ogre::ColourValue color_pose_ = Ogre::ColourValue(1,0,0,1);
 
  // The variance Shape object's color
  Ogre::ColourValue color_variance_;
 
};
 
}
 
#endif