eband_visualization.cpp

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Christian Connette
 *********************************************************************/


#include <eband_local_planner/eband_visualization.h>


namespace eband_local_planner{


  EBandVisualization::EBandVisualization() : initialized_(false) {}


  EBandVisualization::EBandVisualization(ros::NodeHandle& pn, costmap_2d::Costmap2DROS* costmap_ros)
  {
    initialize(pn, costmap_ros);
  }


  EBandVisualization::~EBandVisualization() {}


  void EBandVisualization::initialize(ros::NodeHandle& pn, costmap_2d::Costmap2DROS* costmap_ros)
  {
    // check if visualization already initialized
    if(!initialized_)
    {
      // read parameters from parameter server
      pn.param("marker_lifetime", marker_lifetime_, 0.5);

      // advertise topics
      one_bubble_pub_ = pn.advertise<visualization_msgs::Marker>("eband_visualization", 1);
      // although we want to publish MarkerArrays we have to advertise Marker topic first -> rviz searchs relative to this
      bubble_pub_ = pn.advertise<visualization_msgs::MarkerArray>("eband_visualization_array", 1);

      // copy adress of costmap and Transform Listener (handed over from move_base) -> to get robot shape
      costmap_ros_ = costmap_ros;

      initialized_ = true;
    }
    else
    {
      ROS_WARN("Trying to initialize already initialized visualization, doing nothing.");
    }
  }

  
  void EBandVisualization::reconfigure(
    eband_local_planner::EBandPlannerConfig& config)
  {
    marker_lifetime_ = config.marker_lifetime;
  }


  void EBandVisualization::publishBand(std::string marker_name_space, std::vector<Bubble> band)
  {
    // check if visualization was initialized
    if(!initialized_)
    {
      ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
      return;
    }

    visualization_msgs::MarkerArray eband_msg;
    eband_msg.markers.resize(band.size());

    visualization_msgs::MarkerArray eband_heading_msg;
    eband_heading_msg.markers.resize(band.size());
    std::string marker_heading_name_space = marker_name_space;
    marker_heading_name_space.append("_heading");

    // convert elastic band to msg
    for(int i = 0; i < ((int) band.size()); i++)
    {
      // convert bubbles in eband to marker msg
      bubbleToMarker(band[i], eband_msg.markers[i], marker_name_space, i, green);

      // convert bubbles in eband to marker msg
      // bubbleHeadingToMarker(band[i], eband_heading_msg.markers[i], marker_heading_name_space, i, green);
    }

    // publish
    bubble_pub_.publish(eband_msg);
    //bubble_pub_.publish(eband_heading_msg);
  }


  void EBandVisualization::publishBubble(std::string marker_name_space, int marker_id, Bubble bubble)
  {
    // check if visualization was initialized
    if(!initialized_)
    {
      ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
      return;
    }

    visualization_msgs::Marker bubble_msg;

    // convert bubble to marker msg
    bubbleToMarker(bubble, bubble_msg, marker_name_space, marker_id, green);

    // publish
    one_bubble_pub_.publish(bubble_msg);
  }


  void EBandVisualization::publishBubble(std::string marker_name_space, int marker_id, Color marker_color, Bubble bubble)
  {
    // check if visualization was initialized
    if(!initialized_)
    {
      ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
      return;
    }

    visualization_msgs::Marker bubble_msg;

    // convert bubble to marker msg
    bubbleToMarker(bubble, bubble_msg, marker_name_space, marker_id, marker_color);

    // publish
    one_bubble_pub_.publish(bubble_msg);
  }


  void EBandVisualization::publishForceList(std::string marker_name_space, std::vector<geometry_msgs::WrenchStamped> forces, std::vector<Bubble> band)
  {
    // check if visualization was initialized
    if(!initialized_)
    {
      ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
      return;
    }

    visualization_msgs::MarkerArray forces_msg;
    forces_msg.markers.resize(forces.size());

    //before converting to msg - check whether internal, external or resulting forces - switch color
    Color marker_color = green;
    if(marker_name_space.compare("internal_forces") == 0)
      marker_color = blue;

    if(marker_name_space.compare("external_forces") == 0)
      marker_color = red;

    if(marker_name_space.compare("resulting_forces") == 0)
      marker_color = green;

    for(int i = 0; i < ((int) forces.size()); i++)
    {
      // convert wrenches in force list into marker msg
      forceToMarker(forces[i], band[i].center.pose, forces_msg.markers[i], marker_name_space, i, marker_color);
    }

    // publish
    bubble_pub_.publish(forces_msg);
  }

  void EBandVisualization::publishForce(std::string marker_name_space, int id, Color marker_color, geometry_msgs::WrenchStamped force, Bubble bubble)
  {
    // check if visualization was initialized
    if(!initialized_)
    {
      ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
      return;
    }

    visualization_msgs::Marker force_msg;

    // convert wrenches in force list into marker msg
    forceToMarker(force, bubble.center.pose, force_msg, marker_name_space, id, marker_color);

    // publish
    one_bubble_pub_.publish(force_msg);
  }


  void EBandVisualization::bubbleToMarker(Bubble bubble, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color)
  {
    geometry_msgs::Pose2D tmp_pose2d;

    // header
    marker.header.stamp = ros::Time::now();
    marker.header.frame_id = bubble.center.header.frame_id;

    // identifier and cmds
    marker.ns = marker_name_space;
    marker.id = marker_id;
    marker.type = visualization_msgs::Marker::SPHERE;
    marker.action = visualization_msgs::Marker::ADD;

    // body
    marker.pose = bubble.center.pose;
    // get theta-angle to display as elevation
    PoseToPose2D(bubble.center.pose, tmp_pose2d);
    marker.pose.position.z = 0;//tmp_pose2d.theta * getCircumscribedRadius(*costmap_ros_);
    // scale ~ diameter --> is 2x expansion ~ radius
    marker.scale.x = 2.0*bubble.expansion;
    marker.scale.y = 2.0*bubble.expansion;
    marker.scale.z = 2.0*bubble.expansion;

    // color (rgb)
    marker.color.r = 0.0f;
    marker.color.g = 0.0f;
    marker.color.b = 0.0f;
    switch(marker_color)
    {
      case red: { marker.color.r = 1.0f; break; }
      case green:       { marker.color.g = 1.0f; break; }
      case blue:        { marker.color.b = 1.0f; break; }
    }
    // transparency (alpha value < 1 : displays marker transparent)
    marker.color.a = 0.75;

    // lifetime of this marker
    marker.lifetime = ros::Duration(marker_lifetime_);
  }


  void EBandVisualization::bubbleHeadingToMarker(Bubble bubble, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color)
  {
    geometry_msgs::Pose2D tmp_pose2d;

    // header
    marker.header.stamp = ros::Time::now();
    marker.header.frame_id = bubble.center.header.frame_id;

    // identifier and cmds
    marker.ns = marker_name_space;
    marker.id = marker_id;
    marker.type = visualization_msgs::Marker::ARROW;
    marker.action = visualization_msgs::Marker::ADD;

    // body
    marker.pose = bubble.center.pose;
    // get theta-angle to display as elevation
    PoseToPose2D(bubble.center.pose, tmp_pose2d);
    marker.pose.position.z = tmp_pose2d.theta * getCircumscribedRadius(*costmap_ros_);
    // scale ~ diameter --> is 2x expansion ~ radius
    marker.scale.x = 0.9;
    marker.scale.y = 0.45;
    marker.scale.z = 0.45;

    // color (rgb)
    marker.color.r = 0.0f;
    marker.color.g = 0.0f;
    marker.color.b = 0.0f;
    switch(marker_color)
    {
      case red: { marker.color.r = 1.0f; break; }
      case green:       { marker.color.g = 1.0f; break; }
      case blue:        { marker.color.b = 1.0f; break; }
    }
    // transparency (alpha value < 1 : displays marker transparent)
    marker.color.a = 1.0;

    // lifetime of this marker
    marker.lifetime = ros::Duration(marker_lifetime_);
  }


  void EBandVisualization::forceToMarker(geometry_msgs::WrenchStamped wrench, geometry_msgs::Pose wrench_origin, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color)
  {
    geometry_msgs::Pose2D tmp_pose2d;

    // header
    marker.header.stamp = ros::Time::now();
    marker.header.frame_id = wrench.header.frame_id;

    // identifier and cmds
    marker.ns = marker_name_space;
    marker.id = marker_id;
    marker.type = visualization_msgs::Marker::ARROW;
    marker.action = visualization_msgs::Marker::ADD;

    // body - origin
    marker.pose.position = wrench_origin.position;
    // get theta-angle to display as elevation
    PoseToPose2D(wrench_origin, tmp_pose2d);
    marker.pose.position.z = tmp_pose2d.theta * getCircumscribedRadius(*costmap_ros_);

    // body - orientation of vector (calc. quaternion that transform xAxis into force-vector)
    // check if force different from zero
    if( (wrench.wrench.force.x != 0) || (wrench.wrench.force.y != 0) || (wrench.wrench.torque.z != 0) )
    {
      // find AxisAngle Representation then convert to Quaternion
      // (Axis = normalize(vec1) cross normalize(vec2) // cos(Angle) = normalize(vec1) dot normalize(vec2))
      Eigen::Vector3d x_axis(1.0, 0.0, 0.0);
      Eigen::Vector3d target_vec( wrench.wrench.force.x, wrench.wrench.force.y, wrench.wrench.torque.z / getCircumscribedRadius(*costmap_ros_) );
      Eigen::Vector3d rotation_axis(1.0, 0.0, 0.0);
      double rotation_angle = 0.0;
      // get Axis orthogonal to both vectors ()
      x_axis.normalize(); // unneccessary but just in case
      target_vec.normalize();
      if(!(x_axis == target_vec))
      {
        // vector not identical - cross-product defined
        rotation_axis = x_axis.cross(target_vec);
        rotation_angle = x_axis.dot(target_vec);
        rotation_angle = acos(rotation_angle);
      }
      // create AngleAxis representation
      rotation_axis.normalize(); // normalize vector -> otherwise AngleAxis will be invalid!
      const double s = sin(rotation_angle/2);
      const double c = cos(rotation_angle/2);
      Eigen::Quaterniond rotate_quat(c, s*rotation_axis.x(), s*rotation_axis.y(), s*rotation_axis.z());

      // transform quaternion back from Eigen to ROS
      tf::Quaternion orientation_tf;
      geometry_msgs::Quaternion orientation_msg;
      tf::quaternionEigenToTF(rotate_quat, orientation_tf);
      tf::quaternionTFToMsg(orientation_tf, orientation_msg);

      // finally set orientation of marker
      marker.pose.orientation = orientation_msg;

      // scale ~ diameter --> is 2x expansion ~ radius
      double scale = sqrt( (wrench.wrench.force.x * wrench.wrench.force.x) + (wrench.wrench.force.y * wrench.wrench.force.y) +
          ( (wrench.wrench.torque.z/getCircumscribedRadius(*costmap_ros_))*(wrench.wrench.torque.z/getCircumscribedRadius(*costmap_ros_)) ) );
      marker.scale.x = scale; //1.0;
      marker.scale.y = scale; //1.0;
      marker.scale.z = scale; //1.0;

      // color (rgb)
      marker.color.r = 0.0f;
      marker.color.g = 0.0f;
      marker.color.b = 0.0f;
      switch(marker_color)
      {
        case red:       { marker.color.r = 1.0f; break; }
        case green:     { marker.color.g = 1.0f; break; }
        case blue:      { marker.color.b = 1.0f; break; }
      }
      // transparency (alpha value < 1 : displays marker transparent)
      marker.color.a = 1.25;
    }
    else
    {
      // force on this bubble is zero -> make marker invisible
      marker.pose.orientation = wrench_origin.orientation; // just take orientation of bubble as dummy-value

      // scale
      marker.scale.x = 0.0;
      marker.scale.y = 0.0;
      marker.scale.z = 0.0;

      // color (rgb)
      marker.color.r = 0.0f;
      marker.color.g = 0.0f;
      marker.color.b = 0.0f;

      // transparency (alpha value < 1 : displays marker transparent)
      marker.color.a = 0.0;
    }

    // lifetime of this marker
    marker.lifetime = ros::Duration(marker_lifetime_);
  }

};