collvoid_publishers.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2012, Daniel Claes, Maastricht University
 * 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 the Maastricht University 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.
 */



#include "collvoid_local_planner/collvoid_publishers.h"

namespace collvoid {
  
  void publishHoloSpeed(Vector2 pos, Vector2 vel, std::string base_frame, std::string name_space, ros::Publisher speed_pub) {
    visualization_msgs::Marker line_list;
    line_list.header.frame_id = base_frame;
    line_list.header.stamp = ros::Time::now();
    line_list.ns = name_space;
    line_list.action = visualization_msgs::Marker::ADD;
    line_list.pose.orientation.w = 1.0;
    line_list.type = visualization_msgs::Marker::LINE_LIST;
    line_list.scale.x = 0.02;
    line_list.color.r = 0.0;
    line_list.color.g = 1.0;
    line_list.color.a = 1.0;
    line_list.id = 0;
    geometry_msgs::Point p;
    p.x = pos.x();
    p.y = pos.y();
    p.z = 0.2;
    line_list.points.push_back(p);
    p.x = p.x + vel.x();
    p.y = p.y + vel.y();
    line_list.points.push_back(p);

    speed_pub.publish(line_list);
  }


  void publishVOs(Vector2& pos, const std::vector<VO>& truncated_vos, bool use_truncation, std::string base_frame, std::string name_space, ros::Publisher vo_pub){
    visualization_msgs::Marker line_list;
    line_list.header.frame_id = base_frame;
    line_list.header.stamp = ros::Time::now();
    line_list.ns = name_space;
    line_list.action = visualization_msgs::Marker::ADD;
    line_list.pose.orientation.w = 1.0;
    line_list.type = visualization_msgs::Marker::LINE_LIST;
    line_list.scale.x = 0.015;
    line_list.color.r = 1.0;
    line_list.color.a = 1.0;
    line_list.id = 0;
    if (!use_truncation){
      for (int i=0;i< (int)truncated_vos.size();i++) {
        geometry_msgs::Point p;

        //Apex
        p.x = pos.x() + truncated_vos[i].point.x();
        p.y = pos.y() + truncated_vos[i].point.y();

        line_list.points.push_back(p);
        p.x = p.x + 3 * truncated_vos[i].left_leg_dir.x();
        p.y = p.y + 3 * truncated_vos[i].left_leg_dir.y();
        line_list.points.push_back(p);

        //Apex
        p.x = pos.x() + truncated_vos[i].point.x();
        p.y = pos.y() + truncated_vos[i].point.y();

        line_list.points.push_back(p);
        p.x = p.x + 3 * truncated_vos[i].right_leg_dir.x();
        p.y = p.y + 3 * truncated_vos[i].right_leg_dir.y();
        line_list.points.push_back(p);

      }
    }
    else {
      for (int i=0;i< (int)truncated_vos.size();i++) {
        geometry_msgs::Point p;

        p.x = pos.x() + truncated_vos[i].trunc_left.x();
        p.y = pos.y() + truncated_vos[i].trunc_left.y();
         
        line_list.points.push_back(p);
        p.x = p.x + 3 * truncated_vos[i].left_leg_dir.x();
        p.y = p.y + 3 * truncated_vos[i].left_leg_dir.y();
        line_list.points.push_back(p);

        p.x = pos.x() + truncated_vos[i].trunc_right.x();
        p.y = pos.y() + truncated_vos[i].trunc_right.y();
         
        line_list.points.push_back(p);
        p.x = p.x + 3 * truncated_vos[i].right_leg_dir.x();
        p.y = p.y + 3 * truncated_vos[i].right_leg_dir.y();
        line_list.points.push_back(p);

         
        p.x = pos.x() + truncated_vos[i].trunc_left.x();
        p.y = pos.y() + truncated_vos[i].trunc_left.y();
        line_list.points.push_back(p);

        p.x = pos.x() + truncated_vos[i].trunc_right.x();
        p.y = pos.y() + truncated_vos[i].trunc_right.y();
        line_list.points.push_back(p);

        
         
      }
    }
    vo_pub.publish(line_list);
  }


  void publishPoints(Vector2& pos, const std::vector<VelocitySample>& points, std::string base_frame, std::string name_space, ros::Publisher samples_pub){
    visualization_msgs::MarkerArray point_array;
    // point_array.markers.resize(MAX_POINTS);
    // point_array.markers.clear();

    for (int i=0;i< (int)points.size();i++) {
  
      visualization_msgs::Marker sphere;

      sphere.header.frame_id = base_frame;
      sphere.header.stamp = ros::Time::now();
      sphere.ns = name_space;
      sphere.action = visualization_msgs::Marker::ADD;
      sphere.pose.orientation.w = 1.0;
      sphere.type = visualization_msgs::Marker::SPHERE;
      sphere.scale.x = 0.1;
      sphere.scale.y = 0.1;
      sphere.scale.z = 0.1;
      sphere.color.r = 0.5;
      sphere.color.a = 1.0;
      sphere.id = i;
      geometry_msgs::Point p;
      p.x = pos.x() + points[i].velocity.x();
      p.y = pos.y() + points[i].velocity.y();
      p.z = 0.1;
      sphere.points.push_back(p);
      sphere.pose.position.x = p.x;
      sphere.pose.position.y = p.y;
      sphere.pose.position.z = p.z;
      point_array.markers.push_back(sphere);
    }
    
    for (int i=point_array.markers.size();i<MAX_POINTS_;i++) {
      visualization_msgs::Marker sphere_list;
      sphere_list.header.frame_id = base_frame;
      sphere_list.header.stamp = ros::Time::now();
      sphere_list.ns = name_space;
      sphere_list.action = visualization_msgs::Marker::DELETE;
      sphere_list.id = i;
      geometry_msgs::Point p;
      sphere_list.points.push_back(p);
      sphere_list.pose.position.x = p.x;
      sphere_list.pose.position.y = p.y;
      sphere_list.pose.position.z = p.z;
      point_array.markers.push_back(sphere_list);
    
    }
    samples_pub.publish(point_array);
  }



  void publishOrcaLines(const std::vector<Line>& orca_lines, Vector2& position, std::string base_frame, std::string name_space, ros::Publisher line_pub){
    visualization_msgs::Marker line_list;
    line_list.header.frame_id = base_frame;
    line_list.header.stamp = ros::Time::now();
    line_list.ns = name_space;
    line_list.action = visualization_msgs::Marker::ADD;
    line_list.pose.orientation.w = 1.0;
    line_list.type = visualization_msgs::Marker::LINE_LIST;
    line_list.scale.x = 0.015;
    line_list.color.r = 1.0;
    line_list.color.a = 1.0;
    line_list.id = 1;
    geometry_msgs::Point p;
    for (int i=0;i<(int) orca_lines.size();i++) {
      p.x = position.x() + orca_lines[i].point.x() - orca_lines[i].dir.x();
      p.y = position.y() + orca_lines[i].point.y() - orca_lines[i].dir.y();
    
      line_list.points.push_back(p);
      p.x = p.x + 3 * orca_lines[i].dir.x();
      p.y = p.y + 3 * orca_lines[i].dir.y();
      line_list.points.push_back(p);
     
    }
    line_pub.publish(line_list);
    
  }

  void publishObstacleLines(const std::vector<Obstacle>& obstacles_lines, std::string base_frame, std::string name_space, ros::Publisher line_pub){
    visualization_msgs::Marker line_list;
    line_list.header.frame_id = base_frame;
    line_list.header.stamp = ros::Time::now();
    line_list.ns = name_space;
    line_list.action = visualization_msgs::Marker::ADD;
    line_list.pose.orientation.w = 1.0;
    line_list.type = visualization_msgs::Marker::LINE_LIST;
    line_list.scale.x = 0.015;
    line_list.color.r = 1.0;
    line_list.color.a = 1.0;
    line_list.id = 1;
    geometry_msgs::Point p;
    for (int i=0;i<(int) obstacles_lines.size();i++) {
      if (obstacles_lines[i].point1 == obstacles_lines[i].point2) {
        continue;
      }
      p.x = obstacles_lines[i].point1.x();
      p.y = obstacles_lines[i].point1.y();
      line_list.points.push_back(p);

      p.x = obstacles_lines[i].point2.x();
      p.y = obstacles_lines[i].point2.y();
      line_list.points.push_back(p);
     
    }
    line_pub.publish(line_list);
    
  }


  void publishMePosition(ROSAgent* me, std::string base_frame, std::string name_space, ros::Publisher me_pub){
    visualization_msgs::MarkerArray sphere_list;
    sphere_list.markers.clear();
    fillMarkerWithROSAgent(sphere_list, me, base_frame, name_space);
    me_pub.publish(sphere_list);
  }


  void publishNeighborPositions(std::vector<AgentPtr>& neighbors, std::string base_frame, std::string name_space, ros::Publisher neighbors_pub){
    visualization_msgs::MarkerArray sphere_list;
    sphere_list.markers.clear();

    for (int i = 0; i<(int)neighbors.size(); i++){
      ROSAgentPtr agent = boost::dynamic_pointer_cast<ROSAgent>(neighbors[i]);

      fillMarkerWithROSAgent(sphere_list, agent.get(), base_frame, name_space);
    }
    neighbors_pub.publish(sphere_list);
  }

  void fillMarkerWithROSAgent(visualization_msgs::MarkerArray& marker, ROSAgent* agent, std::string base_frame, std::string name_space) {
    int id = (int)marker.markers.size();
    marker.markers.resize(marker.markers.size()+2);
    ros::Time timestamp = ros::Time::now();

    marker.markers[id].header.frame_id = base_frame;
    marker.markers[id].header.stamp = ros::Time::now();
    marker.markers[id].ns = name_space;
    marker.markers[id].action = visualization_msgs::Marker::ADD;
    marker.markers[id].pose.orientation.w = 1.0;
    marker.markers[id].type = visualization_msgs::Marker::SPHERE;
    marker.markers[id].scale.x = 2.0*agent->getRadius();
    marker.markers[id].scale.y = 2.0*agent->getRadius();
    marker.markers[id].scale.z = 0.1;
    marker.markers[id].color.r = 1.0;
    marker.markers[id].color.a = 1.0;
    marker.markers[id].id = id; 

    double yaw, x_dif, y_dif, th_dif, time_dif;
    time_dif = (ros::Time::now() - agent->lastSeen()).toSec();
    yaw = tf::getYaw(agent->base_odom_.pose.pose.orientation);
    //ROS_ERROR("time_dif %f", time_dif);
    //time_dif = 0.0;
    //forward projection?
    th_dif =  time_dif * agent->base_odom_.twist.twist.angular.z;
    if (agent->isHoloRobot()) {
      x_dif = time_dif * agent->base_odom_.twist.twist.linear.x;
      y_dif = time_dif * agent->base_odom_.twist.twist.linear.y;
    }
    else {
      x_dif = time_dif * agent->base_odom_.twist.twist.linear.x * cos(yaw + th_dif/2.0);
      y_dif = time_dif * agent->base_odom_.twist.twist.linear.x * sin(yaw + th_dif/2.0);
    }
    marker.markers[id].pose.position.x = agent->base_odom_.pose.pose.position.x + x_dif;
    marker.markers[id].pose.position.y = agent->base_odom_.pose.pose.position.y + y_dif;
    marker.markers[id].pose.position.z = 0.2;

    id = id +1;
    marker.markers[id].header.frame_id = base_frame;
    marker.markers[id].header.stamp = ros::Time::now();
    marker.markers[id].ns = name_space;
    marker.markers[id].action = visualization_msgs::Marker::ADD;
    marker.markers[id].pose.orientation.w = 1.0;//tf::createQuaternionMsgFromYaw(yaw+th_dif);
    marker.markers[id].type = visualization_msgs::Marker::ARROW;
    marker.markers[id].scale.x = 0.1;
    marker.markers[id].scale.y = 0.2;
    marker.markers[id].scale.z = 0.1;
    marker.markers[id].color.r = 1.0;
    marker.markers[id].color.a = 1.0;
    marker.markers[id].id = id; 
   
    geometry_msgs::Point p;
    p.x = agent->base_odom_.pose.pose.position.x + x_dif;
    p.y = agent->base_odom_.pose.pose.position.y + y_dif;
    p.z = 0.1;
    marker.markers[id].points.push_back(p);
   
    p.x += agent->getRadius() * 2.0 * cos(yaw + th_dif); 
    p.y += agent->getRadius() * 2.0 * sin(yaw + th_dif);
    marker.markers[id].points.push_back(p);
  }
}