eband_visualization.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2010, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Christian Connette
00036 *********************************************************************/
00037 
00038 
00039 #include <eband_local_planner/eband_visualization.h>
00040 
00041 
00042 namespace eband_local_planner{
00043 
00044 
00045 EBandVisualization::EBandVisualization() : initialized_(false) {}
00046 
00047 
00048 EBandVisualization::EBandVisualization(ros::NodeHandle& pn, costmap_2d::Costmap2DROS* costmap_ros)
00049 {
00050         initialize(pn, costmap_ros);
00051 }
00052 
00053 
00054 EBandVisualization::~EBandVisualization() {}
00055 
00056 
00057 void EBandVisualization::initialize(ros::NodeHandle& pn, costmap_2d::Costmap2DROS* costmap_ros)
00058 {
00059         // check if visualization already initialized
00060         if(!initialized_)
00061         {
00062                 // read parameters from parameter server
00063                 pn.param("marker_lifetime", marker_lifetime_, 0.5);
00064 
00065                 // advertise topics
00066                 one_bubble_pub_ = pn.advertise<visualization_msgs::Marker>("eband_visualization", 1);
00067                 // although we want to publish MarkerArrays we have to advertise Marker topic first -> rviz searchs relative to this
00068                 bubble_pub_ = pn.advertise<visualization_msgs::MarkerArray>("eband_visualization_array", 1);
00069 
00070                 // copy adress of costmap and Transform Listener (handed over from move_base) -> to get robot shape
00071                 costmap_ros_ = costmap_ros;
00072 
00073                 initialized_ = true;
00074         }
00075         else
00076         {
00077                 ROS_WARN("Trying to initialize already initialized visualization, doing nothing.");
00078         }
00079 }
00080 
00081 
00082 void EBandVisualization::publishBand(std::string marker_name_space, std::vector<Bubble> band)
00083 {
00084         // check if visualization was initialized
00085         if(!initialized_)
00086         {
00087                 ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
00088                 return;
00089         }
00090 
00091         visualization_msgs::MarkerArray eband_msg;
00092         eband_msg.markers.resize(band.size());
00093 
00094         visualization_msgs::MarkerArray eband_heading_msg;
00095         eband_heading_msg.markers.resize(band.size());
00096         std::string marker_heading_name_space = marker_name_space;
00097         marker_heading_name_space.append("_heading");
00098 
00099         // convert elastic band to msg
00100         for(int i = 0; i < ((int) band.size()); i++)
00101         {
00102                 // convert bubbles in eband to marker msg
00103                 bubbleToMarker(band[i], eband_msg.markers[i], marker_name_space, i, green);
00104 
00105                 // convert bubbles in eband to marker msg
00106                 bubbleHeadingToMarker(band[i], eband_heading_msg.markers[i], marker_heading_name_space, i, green);
00107         }
00108 
00109         // publish
00110         bubble_pub_.publish(eband_msg);
00111         bubble_pub_.publish(eband_heading_msg);
00112 }
00113 
00114 
00115 void EBandVisualization::publishBubble(std::string marker_name_space, int marker_id, Bubble bubble)
00116 {
00117         // check if visualization was initialized
00118         if(!initialized_)
00119         {
00120                 ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
00121                 return;
00122         }
00123 
00124         visualization_msgs::Marker bubble_msg;
00125 
00126         // convert bubble to marker msg
00127         bubbleToMarker(bubble, bubble_msg, marker_name_space, marker_id, green);
00128 
00129         // publish
00130         one_bubble_pub_.publish(bubble_msg);
00131 }
00132 
00133 
00134 void EBandVisualization::publishBubble(std::string marker_name_space, int marker_id, Color marker_color, Bubble bubble)
00135 {
00136         // check if visualization was initialized
00137         if(!initialized_)
00138         {
00139                 ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
00140                 return;
00141         }
00142 
00143         visualization_msgs::Marker bubble_msg;
00144 
00145         // convert bubble to marker msg
00146         bubbleToMarker(bubble, bubble_msg, marker_name_space, marker_id, marker_color);
00147 
00148         // publish
00149         one_bubble_pub_.publish(bubble_msg);
00150 }
00151 
00152 
00153 void EBandVisualization::publishForceList(std::string marker_name_space, std::vector<geometry_msgs::WrenchStamped> forces, std::vector<Bubble> band)
00154 {
00155         // check if visualization was initialized
00156         if(!initialized_)
00157         {
00158                 ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
00159                 return;
00160         }
00161 
00162         visualization_msgs::MarkerArray forces_msg;
00163         forces_msg.markers.resize(forces.size());
00164 
00165         //before converting to msg - check whether internal, external or resulting forces - switch color
00166         Color marker_color = green;
00167         if(marker_name_space.compare("internal_forces") == 0)
00168                 marker_color = blue;
00169 
00170         if(marker_name_space.compare("external_forces") == 0)
00171                 marker_color = red;
00172 
00173         if(marker_name_space.compare("resulting_forces") == 0)
00174                 marker_color = green;
00175 
00176         for(int i = 0; i < ((int) forces.size()); i++)
00177         {
00178                 // convert wrenches in force list into marker msg
00179                 forceToMarker(forces[i], band[i].center.pose, forces_msg.markers[i], marker_name_space, i, marker_color);
00180         }
00181 
00182         // publish
00183         bubble_pub_.publish(forces_msg);
00184 }
00185 
00186 void EBandVisualization::publishForce(std::string marker_name_space, int id, Color marker_color, geometry_msgs::WrenchStamped force, Bubble bubble)
00187 {
00188         // check if visualization was initialized
00189         if(!initialized_)
00190         {
00191                 ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
00192                 return;
00193         }
00194 
00195         visualization_msgs::Marker force_msg;
00196 
00197         // convert wrenches in force list into marker msg
00198         forceToMarker(force, bubble.center.pose, force_msg, marker_name_space, id, marker_color);
00199 
00200         // publish
00201         one_bubble_pub_.publish(force_msg);
00202 }
00203 
00204 
00205 void EBandVisualization::bubbleToMarker(Bubble bubble, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color)
00206 {
00207         geometry_msgs::Pose2D tmp_pose2d;
00208 
00209         // header
00210         marker.header.stamp = ros::Time::now();
00211         marker.header.frame_id = bubble.center.header.frame_id;
00212 
00213         // identifier and cmds
00214         marker.ns = marker_name_space;
00215         marker.id = marker_id;
00216         marker.type = visualization_msgs::Marker::SPHERE;
00217         marker.action = visualization_msgs::Marker::ADD;
00218 
00219         // body
00220         marker.pose = bubble.center.pose;
00221         // get theta-angle to display as elevation
00222         PoseToPose2D(bubble.center.pose, tmp_pose2d);
00223         marker.pose.position.z = tmp_pose2d.theta * costmap_ros_->getCircumscribedRadius();
00224         // scale ~ diameter --> is 2x expansion ~ radius
00225         marker.scale.x = 2.0*bubble.expansion;
00226         marker.scale.y = 2.0*bubble.expansion;
00227         marker.scale.z = 2.0*bubble.expansion;
00228 
00229         // color (rgb)
00230         marker.color.r = 0.0f;
00231         marker.color.g = 0.0f;
00232         marker.color.b = 0.0f;
00233         switch(marker_color)
00234         {
00235                 case red:       { marker.color.r = 1.0f; break; }
00236                 case green:     { marker.color.g = 1.0f; break; }
00237                 case blue:      { marker.color.b = 1.0f; break; }
00238         }
00239         // transparency (alpha value < 1 : displays marker transparent)
00240         marker.color.a = 0.25;
00241 
00242         // lifetime of this marker
00243         marker.lifetime = ros::Duration(marker_lifetime_);
00244 }
00245 
00246 
00247 void EBandVisualization::bubbleHeadingToMarker(Bubble bubble, visualization_msgs::Marker& marker, std::string marker_name_space, int marker_id, Color marker_color)
00248 {
00249         geometry_msgs::Pose2D tmp_pose2d;
00250 
00251         // header
00252         marker.header.stamp = ros::Time::now();
00253         marker.header.frame_id = bubble.center.header.frame_id;
00254 
00255         // identifier and cmds
00256         marker.ns = marker_name_space;
00257         marker.id = marker_id;
00258         marker.type = visualization_msgs::Marker::ARROW;
00259         marker.action = visualization_msgs::Marker::ADD;
00260 
00261         // body
00262         marker.pose = bubble.center.pose;
00263         // get theta-angle to display as elevation
00264         PoseToPose2D(bubble.center.pose, tmp_pose2d);
00265         marker.pose.position.z = tmp_pose2d.theta * costmap_ros_->getCircumscribedRadius();
00266         // scale ~ diameter --> is 2x expansion ~ radius
00267         marker.scale.x = 0.9;
00268         marker.scale.y = 0.45;
00269         marker.scale.z = 0.45;
00270 
00271         // color (rgb)
00272         marker.color.r = 0.0f;
00273         marker.color.g = 0.0f;
00274         marker.color.b = 0.0f;
00275         switch(marker_color)
00276         {
00277                 case red:       { marker.color.r = 1.0f; break; }
00278                 case green:     { marker.color.g = 1.0f; break; }
00279                 case blue:      { marker.color.b = 1.0f; break; }
00280         }
00281         // transparency (alpha value < 1 : displays marker transparent)
00282         marker.color.a = 1.0;
00283 
00284         // lifetime of this marker
00285         marker.lifetime = ros::Duration(marker_lifetime_);
00286 }
00287 
00288 
00289 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)
00290 {
00291         geometry_msgs::Pose2D tmp_pose2d;
00292 
00293         // header
00294         marker.header.stamp = ros::Time::now();
00295         marker.header.frame_id = wrench.header.frame_id;
00296 
00297         // identifier and cmds
00298         marker.ns = marker_name_space;
00299         marker.id = marker_id;
00300         marker.type = visualization_msgs::Marker::ARROW;
00301         marker.action = visualization_msgs::Marker::ADD;
00302 
00303         // body - origin
00304         marker.pose.position = wrench_origin.position;
00305         // get theta-angle to display as elevation
00306         PoseToPose2D(wrench_origin, tmp_pose2d);
00307         marker.pose.position.z = tmp_pose2d.theta * costmap_ros_->getCircumscribedRadius();
00308 
00309         // body - orientation of vector (calc. quaternion that transform xAxis into force-vector)
00310         // check if force different from zero
00311         if( (wrench.wrench.force.x != 0) || (wrench.wrench.force.y != 0) || (wrench.wrench.torque.z != 0) )
00312         {
00313                 // find AxisAngle Representation then convert to Quaternion
00314                 // (Axis = normalize(vec1) cross normalize(vec2) // cos(Angle) = normalize(vec1) dot normalize(vec2))
00315                 Eigen::Vector3d x_axis(1.0, 0.0, 0.0);
00316                 Eigen::Vector3d target_vec( wrench.wrench.force.x, wrench.wrench.force.y, wrench.wrench.torque.z / costmap_ros_->getCircumscribedRadius() );
00317                 Eigen::Vector3d rotation_axis(1.0, 0.0, 0.0);
00318                 double rotation_angle = 0.0;
00319                 // get Axis orthogonal to both vectors ()
00320                 x_axis.normalize(); // unneccessary but just in case
00321                 target_vec.normalize();
00322                 if(!(x_axis == target_vec))
00323                 {
00324                         // vector not identical - cross-product defined
00325                         rotation_axis = x_axis.cross(target_vec);
00326                         rotation_angle = x_axis.dot(target_vec);
00327                         rotation_angle = acos(rotation_angle);
00328                 }
00329                 // create AngleAxis representation
00330                 rotation_axis.normalize(); // normalize vector -> otherwise AngleAxis will be invalid!
00331                 const double s = sin(rotation_angle/2);
00332                 const double c = cos(rotation_angle/2);
00333                 Eigen::Quaterniond rotate_quat(c, s*rotation_axis.x(), s*rotation_axis.y(), s*rotation_axis.z());
00334         
00335                 // transform quaternion back from Eigen to ROS
00336                 tf::Quaternion orientation_tf;
00337                 geometry_msgs::Quaternion orientation_msg;
00338                 tf::RotationEigenToTF(rotate_quat, orientation_tf);
00339                 tf::quaternionTFToMsg(orientation_tf, orientation_msg);
00340         
00341                 // finally set orientation of marker
00342                 marker.pose.orientation = orientation_msg;
00343 
00344                 // scale ~ diameter --> is 2x expansion ~ radius
00345                 double scale = sqrt( (wrench.wrench.force.x * wrench.wrench.force.x) + (wrench.wrench.force.y * wrench.wrench.force.y) +
00346                                                 ( (wrench.wrench.torque.z/costmap_ros_->getCircumscribedRadius())*(wrench.wrench.torque.z/costmap_ros_->getCircumscribedRadius()) ) );
00347                 marker.scale.x = scale; //1.0;
00348                 marker.scale.y = scale; //1.0;
00349                 marker.scale.z = scale; //1.0;
00350         
00351                 // color (rgb)
00352                 marker.color.r = 0.0f;
00353                 marker.color.g = 0.0f;
00354                 marker.color.b = 0.0f;
00355                 switch(marker_color)
00356                 {
00357                         case red:       { marker.color.r = 1.0f; break; }
00358                         case green:     { marker.color.g = 1.0f; break; }
00359                         case blue:      { marker.color.b = 1.0f; break; }
00360                 }
00361                 // transparency (alpha value < 1 : displays marker transparent)
00362                 marker.color.a = 1.25;
00363         }
00364         else
00365         {
00366                 // force on this bubble is zero -> make marker invisible
00367                 marker.pose.orientation = wrench_origin.orientation; // just take orientation of bubble as dummy-value
00368 
00369                 // scale
00370                 marker.scale.x = 0.0;
00371                 marker.scale.y = 0.0;
00372                 marker.scale.z = 0.0;
00373 
00374                 // color (rgb)
00375                 marker.color.r = 0.0f;
00376                 marker.color.g = 0.0f;
00377                 marker.color.b = 0.0f;
00378 
00379                 // transparency (alpha value < 1 : displays marker transparent)
00380                 marker.color.a = 0.0;
00381         }
00382 
00383         // lifetime of this marker
00384         marker.lifetime = ros::Duration(marker_lifetime_);
00385 }
00386 
00387 };
00388 


eband_local_planner
Author(s): Christian Connette, Bhaskara Marthi
autogenerated on Fri Jan 3 2014 11:34:16