visualize_patches.cpp

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Robert Bosch LLC.
 *  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 Robert Bosch 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 Adrian Funk, Philip Roan, Bosch LLC

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <vector>
#include <math.h>
#include <proximity_sensor_driver/prox_sensor_measurement.h>

using namespace std;

// Global variables to be used in the callback
int sensor_count = 0;
vector<int> sensor_measurements;

// Reads the data of the proximity sensor and saves it in sensor_measurements
void proxSensorDataCallback( const boost::shared_ptr<proximity_sensor_driver::prox_sensor_measurement const> &msg )
{
  for( int int_current_patch = 0; int_current_patch < sensor_count; int_current_patch++ )
  {
    sensor_measurements[int_current_patch] = abs( msg->value[int_current_patch] - msg->offset[int_current_patch] );
  }
  return;
}

// Function for reading critical parameters
template <class T>
inline void readParam( const ros::NodeHandle &nh, const std::string &str_name, T &rhs )
{
  if( !nh.getParam(str_name, rhs) )
  {
    ROS_ERROR("Could not find parameter %s", str_name.c_str());
    exit(1);
  }
  return;
}


int main( int argc, char** argv )
{
  ros::init( argc, argv, "visualize_patches" );
  ros::NodeHandle nh;

  // Read parameters
  int int_noise;
  int int_min_intensity;
  readParam( nh, "/prox_sensor_driver/sensor_count", sensor_count );
  readParam( nh, "/prox_sensor_driver/noise_threshold", int_noise );
  readParam( nh, "/prox_sensor_driver/static_threshold", int_min_intensity );
  sensor_measurements.resize( sensor_count );

  // Subscribe and advertise
  ros::Subscriber prox_sensor_sub = nh.subscribe( "/prox_sensor_data", 100, proxSensorDataCallback );
  ros::Publisher marker_pub = nh.advertise<visualization_msgs::MarkerArray>( "proximity_sensor_marker_array", 20 );
  ros::Publisher fake_pub = nh.advertise<visualization_msgs::Marker>( "proximity_sensor_marker", 20 ); // This makes the marker array visible in rviz

  visualization_msgs::Marker patch;
  visualization_msgs::Marker arrow;

  // Set up the patch marker
  patch.ns = "patches";
  patch.type = visualization_msgs::Marker::CYLINDER;
  patch.action = visualization_msgs::Marker::ADD;
  patch.pose.position.x = 0;
  patch.pose.position.y = 0;
  patch.pose.position.z = 0;
  patch.pose.orientation.x = 0;
  patch.pose.orientation.y = sqrt(0.5);
  patch.pose.orientation.z = 0;
  patch.pose.orientation.w = sqrt(0.5);
  patch.scale.x = 0.06;
  patch.scale.y = 0.06;
  patch.scale.z = 0.005;
  patch.color.b = 0.0;
  patch.color.a = 0.6;
  patch.lifetime = ros::Duration();

  // Set up the arrow marker
  arrow.ns = "arrows";
  //arrow.type = visualization_msgs::Marker::ARROW;
  arrow.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
  arrow.action = visualization_msgs::Marker::ADD;
  arrow.pose.position.x = 0.01;
  arrow.pose.position.y = 0;
  arrow.pose.position.z = 0;
  arrow.pose.orientation.x = 0;
  arrow.pose.orientation.y = 0;
  arrow.pose.orientation.z = 0;
  arrow.pose.orientation.w = 1;
  arrow.scale.x = 0.075;
  arrow.scale.y = 0.075;
  arrow.scale.z = 0.025;
  arrow.color.g = 1.0;
  arrow.color.b = 1.0;
  arrow.color.a = 0.6;
  arrow.lifetime = ros::Duration();

  // Reserve space for an arrow and a patch marker for each patch
        

  ros::Rate r(5);

  while( ros::ok() )
  {
    visualization_msgs::MarkerArray marker_array;
    ros::spinOnce(); // Get current proximity sensor readings

    // Add patch and arrow marker for each sensor patch to a marker array
    for( int int_current_patch = 0; int_current_patch < sensor_count; int_current_patch++ )
    {
      // Patch
      patch.header.frame_id = "/patch_" + boost::lexical_cast<string>( int_current_patch );
      patch.header.stamp = ros::Time(0);
      patch.id = int_current_patch;

      // Determine whether the patch is "active" (red) or "inactive" (green)
      if( sensor_measurements[int_current_patch] >= int_min_intensity + int_noise )
      {
        patch.color.r = 1.0;
        patch.color.g = 0.0;
      }
      else
      {
        patch.color.r = 0.0;
        patch.color.g = 1.0;
      }

      marker_array.markers.push_back( patch );


      // Arrow
      arrow.header.frame_id = "/patch_" + boost::lexical_cast<string>( int_current_patch );
      arrow.header.stamp = ros::Time(0);
      arrow.id = int_current_patch;

      // When the patch is active the arrow is white
      if( sensor_measurements[int_current_patch] >= int_min_intensity + int_noise )
      {
        arrow.color.r = 1.0;
        arrow.text = boost::lexical_cast<string>( int_current_patch );
      }
      else // If it's not 100% active it becomes whiter and whiter, the stronger the reading gets
      {
        arrow.color.r = (double)(sensor_measurements[int_current_patch] - int_noise) / (double)int_min_intensity;
        arrow.text = boost::lexical_cast<string>( int_current_patch );
      }

      marker_array.markers.push_back( arrow );
    }
    marker_pub.publish( marker_array );
    r.sleep();
  }

  return 0;
}