marker_visualization.cpp

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, 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: Sebastian Klose
 *********************************************************************/

#include <imu_filter/marker_visualization.h>

namespace imu_filter {

        MarkerVisualization::MarkerVisualization( size_t bufferSize, const std::string & imuFrameId ) : 
                maxMarkers_( bufferSize ),
                imuFrameId_( imuFrameId ),
                nextId_( 0 )
        {
                // create publisher for the markers
                ros::NodeHandle n;
                publisher_ = n.advertise<visualization_msgs::Marker>( "imutest_markers", 10 );

                gravityArrow_.header.frame_id = imuFrameId_;
                gravityArrow_.header.stamp = ros::Time::now();
                gravityArrow_.ns = "imu_arrows";
                gravityArrow_.action = visualization_msgs::Marker::ADD;
                gravityArrow_.type = visualization_msgs::Marker::ARROW;
                gravityArrow_.id = 0;
                gravityArrow_.color.a = 0.7;
                gravityArrow_.color.r = 0.0;
                gravityArrow_.color.g = 0.9;
                gravityArrow_.color.b = 0.1;
                gravityArrow_.points.resize( 2 );
                gravityArrow_.points[ 0 ].x = 0.0; gravityArrow_.points[ 0 ].y = 0.0; gravityArrow_.points[ 0 ].z = 0.0;
                gravityArrow_.points[ 1 ].x = 0.0; gravityArrow_.points[ 1 ].y = 0.0; gravityArrow_.points[ 1 ].z = 0.05*EARTH_GRAVITY;
                gravityArrow_.scale.x = 0.02;
                gravityArrow_.scale.y = 0.04;

                velocityArrow_.header.frame_id = imuFrameId_;
                velocityArrow_.header.stamp = ros::Time::now();
                velocityArrow_.ns = "imu_arrows";
                velocityArrow_.action = visualization_msgs::Marker::ADD;
                velocityArrow_.type = visualization_msgs::Marker::ARROW;
                velocityArrow_.id = 1;
                velocityArrow_.color.a = 0.7;
                velocityArrow_.color.r = 0.0;
                velocityArrow_.color.g = 0.1;
                velocityArrow_.color.b = 0.9;
                velocityArrow_.points.resize( 2 );
                velocityArrow_.points[ 0 ].x = 0.0; velocityArrow_.points[ 0 ].y = 0.0; velocityArrow_.points[ 0 ].z = 0.0;
                velocityArrow_.points[ 1 ].x = 0.0; velocityArrow_.points[ 1 ].y = 0.0; velocityArrow_.points[ 1 ].z = 0.0;
                velocityArrow_.scale.x = 0.02;
                velocityArrow_.scale.y = 0.04;

                accArrow_.header.frame_id = imuFrameId_;
                accArrow_.header.stamp = ros::Time::now();
                accArrow_.ns = "imu_arrows";
                accArrow_.action = visualization_msgs::Marker::ADD;
                accArrow_.type = visualization_msgs::Marker::ARROW;
                accArrow_.id = 2;
                accArrow_.color.a = 0.9;
                accArrow_.color.r = 0.9;
                accArrow_.color.g = 0.0;
                accArrow_.color.b = 0.0;
                accArrow_.points.resize( 2 );
                accArrow_.points[ 0 ].x = 0.0; velocityArrow_.points[ 0 ].y = 0.0; velocityArrow_.points[ 0 ].z = 0.0;
                accArrow_.points[ 1 ].x = 0.0; velocityArrow_.points[ 1 ].y = 0.0; velocityArrow_.points[ 1 ].z = 0.0;
                accArrow_.scale.x = 0.02;
                accArrow_.scale.y = 0.04;

                measAcc_.header.frame_id = imuFrameId_;
                measAcc_.header.stamp = ros::Time::now();
                measAcc_.ns = "imu_arrows";
                measAcc_.action = visualization_msgs::Marker::ADD;
                measAcc_.type = visualization_msgs::Marker::ARROW;
                measAcc_.id = 3;
                measAcc_.color.a = 0.9;
                measAcc_.color.r = 0.9;
                measAcc_.color.g = 0.0;
                measAcc_.color.b = 0.9;
                measAcc_.points.resize( 2 );
                measAcc_.points[ 0 ].x = 0.0; velocityArrow_.points[ 0 ].y = 0.0; velocityArrow_.points[ 0 ].z = 0.0;
                measAcc_.points[ 1 ].x = 0.0; velocityArrow_.points[ 1 ].y = 0.0; velocityArrow_.points[ 1 ].z = 0.0;
                measAcc_.scale.x = 0.02;
                measAcc_.scale.y = 0.04;
        }

        MarkerVisualization::~MarkerVisualization()
        {
        }

        void MarkerVisualization::addImuMarker( const ImuState & state, const Eigen::Vector3d & acceleration )
        {
                marker_.push_back( visualization_msgs::Marker() );

                visualization_msgs::Marker & m = marker_.back();

                m.header.frame_id = imuFrameId_;
                m.header.stamp = ros::Time::now();

                m.ns = "imu_markers";
                m.action = visualization_msgs::Marker::ADD;
                m.type = visualization_msgs::Marker::SPHERE;
                m.id = nextId();
                m.color.a = 0.5;
                m.color.r = 0.7;
                m.color.g = 0.3;
                m.color.b = 0.1;

                this->updateMarkerPose( m, state );

                //      dump( m );

                if( maxMarkers_ && marker_.size() > maxMarkers_ ){
                        marker_.erase( marker_.begin() );
                }
        }

        void MarkerVisualization::publish() const
        {
                Container::const_iterator it = marker_.begin(); 
                Container::const_iterator end = marker_.end();

                while( it != end ){
                        publisher_.publish( *it );
                        it++;
                }

                publisher_.publish( gravityArrow_ );
                publisher_.publish( velocityArrow_ );
                publisher_.publish( accArrow_ );
                publisher_.publish( measAcc_ );
        }


        void MarkerVisualization::updateMarkerPose( visualization_msgs::Marker & m,
                                                                                           const ImuState & state )

        {
                Eigen::Quaterniond q = state.orientation();
                m.pose.orientation.w = q.w();
                m.pose.orientation.x = q.x();
                m.pose.orientation.y = q.y();
                m.pose.orientation.z = q.z();

                m.pose.position.x = state.position().x();
                m.pose.position.y = state.position().y();
                m.pose.position.z = state.position().z();

                m.scale.x = sqrt(state.covariance()( 4, 4 ));
                m.scale.y = sqrt(state.covariance()( 5, 5 ));
                m.scale.z = sqrt(state.covariance()( 6, 6 ));
                

                // update the velocity vector
                Eigen::Vector3d velocity = state.velocity();
                velocity += state.position();
                velocityArrow_.points[ 0 ].x = state.position().x();
                velocityArrow_.points[ 0 ].y = state.position().y();
                velocityArrow_.points[ 0 ].z = state.position().z();
                velocityArrow_.points[ 1 ].x = velocity.x();
                velocityArrow_.points[ 1 ].y = velocity.y();
                velocityArrow_.points[ 1 ].z = velocity.z();

                gravityArrow_.points[ 0 ].x = state.position().x();
                gravityArrow_.points[ 0 ].y = state.position().y();
                gravityArrow_.points[ 0 ].z = state.position().z();
                gravityArrow_.points[ 1 ].x = state.position().x() + 0.1 * state.gravity().x();
                gravityArrow_.points[ 1 ].y = state.position().y() + 0.1 * state.gravity().y();
                gravityArrow_.points[ 1 ].z = state.position().z() + 0.1 * state.gravity().z();
        }

        void MarkerVisualization::dump( visualization_msgs::Marker & m )
        {
                std::cout << m << std::endl;
        }

        void MarkerVisualization::setAcceleration( const Eigen::Vector3d & acc )
        {
                accArrow_.points[ 1 ].x = 0.1 * acc.x();
                accArrow_.points[ 1 ].y = 0.1 * acc.y();
                accArrow_.points[ 1 ].z = 0.1 * acc.z();
        }

        void MarkerVisualization::setMeasuredAcc( const Eigen::Vector3d & acc )
        {
                measAcc_.points[ 1 ].x =0.1 * acc.x();
                measAcc_.points[ 1 ].y =0.1 * acc.y();
                measAcc_.points[ 1 ].z =0.1 * acc.z();
        }

        int MarkerVisualization::nextId()
        {
                nextId_++;
                if( maxMarkers_ ){
                        nextId_ %= maxMarkers_;
                        nextId_++;
                }
                return nextId_;
        }

}