patches_tf.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, Bosch LLC


#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <vector>
#include <string>
#include <boost/lexical_cast.hpp>

using namespace std;

// Struct for keeping position vector and orientation quaternion internally
typedef struct
{
  tf::Vector3 position;
  tf::Quaternion orientation;
  string parent_frame;
} patch_position;

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, "patches_tf" );
  ros::NodeHandle nh;
  ros::NodeHandle nh_private("~");

  int patches = 0;
  vector<patch_position> patch_array;
  readParam( nh, "/prox_sensor_driver/sensor_count", patches );

  // Fill up data structure for each patch
  patch_position tmp;
  for( int current_patch = 0; current_patch < patches; current_patch++ )
  {
    // Read the parameters
    double x_pos, y_pos, z_pos;
    double x_ori, y_ori, z_ori;

    string patch_domain( "patch_" + boost::lexical_cast<string>( current_patch ) );

    readParam( nh_private, patch_domain + "/x_pos", x_pos );
    readParam( nh_private, patch_domain + "/y_pos", y_pos );
    readParam( nh_private, patch_domain + "/z_pos", z_pos );

    readParam( nh_private, patch_domain + "/x_ori", x_ori );
    readParam( nh_private, patch_domain + "/y_ori", y_ori );
    readParam( nh_private, patch_domain + "/z_ori", z_ori );

    readParam( nh_private, patch_domain + "/string_parent_frame", tmp.parent_frame );


    tf::Vector3 patch_center( x_pos, y_pos, z_pos);
    tf::Vector3 patch_normal( x_ori, y_ori, z_ori);

    if( patch_normal.isZero() )
    {
      ROS_ERROR("Normal of patch %d is zero vector!", current_patch);
      exit(1);
    }

    // Create the quaternion out of the normal
    //
    // For this we have to find out how to turn the x-axis on the normal of the patch
    // This is done by computing the cross-product of x-axis and normal of the patch
    // This vector serves as rotation axis, then we only have to turn the x-axis around this
    // vector by the amount of angle between normal of the patch and x-axis

    patch_normal.normalize();
    tf::Vector3 x_axis(1.0, 0.0, 0.0);
    tf::Vector3 rot_axis = x_axis.cross( patch_normal );
    double angle;

    if( rot_axis.isZero() ) // This happens when the normal of the patch is exactly the x-axis
    {
      rot_axis = x_axis;
      angle = 0.0;
    }
    else // Compute angle between normal and x-axis
    {
      angle = acos( patch_normal.dot(x_axis) );
    }

    tf::Quaternion patch_orientation( rot_axis, angle);
    patch_orientation.normalize(); // Normalize quaternion! Otherwise transform might not work correctly


    // Save data results for that we don't have to compute them before every transform
    tmp.position = patch_center;
    tmp.orientation = patch_orientation;
    patch_array.push_back( tmp );


    ROS_INFO("---------");
    ROS_INFO("Added patch %d", current_patch);
    ROS_INFO("Position: X:%lf Y:%lf Z:%lf", x_pos, y_pos, z_pos);
    ROS_INFO("Orientation: X:%lf Y:%lf Z:%lf", x_ori, y_ori, z_ori);
    ROS_INFO("Parent frame: %s", tmp.parent_frame.c_str());
  }
  // Now we have all the data we need stored in the vector patches

  tf::TransformBroadcaster br;
  tf::Transform transform;

  ros::Rate r( 20 );

  while( nh.ok() )
  {
    // Publish a transform for each patch
    for( int current_patch = 0; current_patch < patches; current_patch++ )
    {
      patch_position patch = patch_array.at( current_patch );

      transform.setOrigin( patch.position );
      transform.setRotation( patch.orientation );
      br.sendTransform( tf::StampedTransform( transform,
                                              ros::Time::now(),
                                              patch.parent_frame,
                                              "/patch_" + boost::lexical_cast<string>( current_patch )) );
    }
    r.sleep();
  }

  return(0);
}