point_head_view_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "pr2_interactive_manipulation/point_head_view_controller.h"
00031 
00032 #include <rviz/display_context.h>
00033 #include <OGRE/OgreSceneManager.h>
00034 
00035 #include <OGRE/OgreViewport.h>
00036 #include <OGRE/OgreRay.h>
00037 #include <OGRE/OgreVector3.h>
00038 
00039 #include <visualization_msgs/Marker.h>
00040 
00041 namespace pr2_interactive_manipulation
00042 {
00043 
00044 PointHeadViewController::PointHeadViewController( rviz::ROSImageTexture &texture, 
00045                                                   rviz::DisplayContext* context ) :
00046     rviz::ViewController( ),
00047     texture_(texture),
00048     point_head_action_client_(0)
00049 {
00050   ros::NodeHandle n;
00051 #ifdef DEBUG_DISPLAY
00052   marker_pub_ = n.advertise<visualization_msgs::Marker>("point_head_view_controller/pointing_direction", 1);
00053 #endif
00054  // global_orientation_ = Ogre::Quaternion::IDENTITY;
00055   initialize( context );
00056 }
00057 
00058 PointHeadViewController::~PointHeadViewController()
00059 {
00060   delete point_head_action_client_;
00061 }
00062 
00063 void PointHeadViewController::handleMouseEvent( rviz::ViewportMouseEvent& event)
00064 {
00065   if ( event.leftDown() )
00066   {
00067     int width = event.viewport->getActualWidth();
00068     int height = event.viewport->getActualHeight();
00069 
00070     Ogre::Ray mouse_ray = event.viewport->getCamera()->getCameraToViewportRay(
00071         (float)event.x / (float)width, (float)event.y / (float)height );
00072 
00073     lookAt( mouse_ray.getPoint(1.0) );
00074   }
00075 
00076 }
00077 
00078 void PointHeadViewController::setTopic(const std::string& topic)
00079 {
00080   delete point_head_action_client_;
00081   topic_ = topic;
00082 
00083   //remove the trailing '/goal' from the topic
00084 
00085   std::string action_topic = topic_;
00086 
00087   size_t goal_str_pos = topic_.rfind("/goal");
00088   if (goal_str_pos != std::string::npos)
00089   {
00090     action_topic.erase(goal_str_pos);
00091   }
00092 
00093   point_head_action_client_ = new PointHeadActionClient(action_topic, true);
00094 }
00095 
00096 void PointHeadViewController::reset()
00097 {
00098   if ( !point_head_action_client_ )
00099   {
00100     return;
00101   }
00102 
00103   if ( !texture_.getImage().get() )
00104   {
00105     ROS_ERROR( "No image received. Cannot compute look-at point." );
00106     return;
00107   }
00108 
00109   point_head_action_client_->waitForServer( ros::Duration( 0.1 ) );
00110 
00111   if ( !point_head_action_client_->isServerConnected() )
00112   {
00113     ROS_ERROR( "Point head action client is not connected!" );
00114     return;
00115   }
00116 
00117   // send a point head goal which is 5 meters in front of the robot, on the ground.
00118   pr2_controllers_msgs::PointHeadGoal point_head_goal;
00119 
00120   point_head_goal.pointing_axis.x = 0;
00121   point_head_goal.pointing_axis.y = 0;
00122   point_head_goal.pointing_axis.z = 1;
00123 
00124   point_head_goal.pointing_frame = texture_.getImage()->header.frame_id;
00125 
00126   point_head_goal.target.header.frame_id = "base_footprint";
00127   point_head_goal.target.header.stamp = ros::Time::now();
00128 
00129   point_head_goal.target.point.x = 5;
00130   point_head_goal.target.point.y = 0;
00131   point_head_goal.target.point.z = 0;
00132 
00133   point_head_goal.max_velocity = 1.0;
00134 
00135   point_head_action_client_->sendGoal( point_head_goal );
00136 }
00137 
00138 void PointHeadViewController::lookAt( const Ogre::Vector3& look_at_point )
00139 {
00140   if ( !point_head_action_client_ )
00141   {
00142     return;
00143   }
00144 
00145   if ( !texture_.getImage().get() )
00146   {
00147     ROS_ERROR( "No image received. Cannot compute look-at point." );
00148     return;
00149   }
00150 
00151   point_head_action_client_->waitForServer( ros::Duration( 0.1 ) );
00152 
00153   if ( !point_head_action_client_->isServerConnected() )
00154   {
00155     ROS_ERROR( "Point head action client is not connected!" );
00156     return;
00157   }
00158 
00159   Ogre::Vector3 look_at_point_robot = look_at_point;
00160 
00161   // send point head goal
00162 
00163   pr2_controllers_msgs::PointHeadGoal point_head_goal;
00164 
00165   point_head_goal.pointing_axis.x = 0;
00166   point_head_goal.pointing_axis.y = 0;
00167   point_head_goal.pointing_axis.z = 1;
00168 
00169   point_head_goal.pointing_frame = texture_.getImage()->header.frame_id;
00170 
00171   std::string fixed_frame = context_->getFixedFrame().toStdString();
00172   point_head_goal.target.header.frame_id = fixed_frame;
00173   point_head_goal.target.header.stamp = ros::Time::now();
00174 
00175   point_head_goal.target.point.x = look_at_point_robot.x;
00176   point_head_goal.target.point.y = look_at_point_robot.y;
00177   point_head_goal.target.point.z = look_at_point_robot.z;
00178 
00179   point_head_goal.max_velocity = 1.0;
00180 
00181   point_head_action_client_->sendGoal( point_head_goal );
00182 
00183 #ifdef DEBUG_DISPLAY
00184 
00185   ROS_INFO( "Looking at position %f %f %f in pointing frame %s (fixed frame is %s)", point_head_goal.target.point.x,
00186       point_head_goal.target.point.y, point_head_goal.target.point.z,
00187       point_head_goal.pointing_frame.c_str(),
00188       point_head_goal.target.header.frame_id.c_str() );
00189 
00190   // publish debug marker
00191 
00192   visualization_msgs::Marker marker;
00193   // Set the frame ID and timestamp.  See the TF tutorials for information on these.
00194   marker.header.frame_id = context_->getFixedFrame().toStdString();
00195   marker.header.stamp = ros::Time::now();
00196 
00197   // Set the namespace and id for this marker.  This serves to create a unique ID
00198   // Any marker sent with the same namespace and id will overwrite the old one
00199   marker.ns = "PointHeadViewController";
00200   marker.id = 0;
00201 
00202   // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
00203   marker.type = visualization_msgs::Marker::SPHERE;
00204 
00205   // Set the marker action.  Options are ADD and DELETE
00206   marker.action = visualization_msgs::Marker::ADD;
00207 
00208   // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
00209   marker.pose.position.x = look_at_point_robot.x;
00210   marker.pose.position.y = look_at_point_robot.y;
00211   marker.pose.position.z = look_at_point_robot.z;
00212   marker.pose.orientation.x = 0.0;
00213   marker.pose.orientation.y = 0.0;
00214   marker.pose.orientation.z = 0.0;
00215   marker.pose.orientation.w = 1.0;
00216 
00217   // Set the scale of the marker -- 1x1x1 here means 1m on a side
00218   marker.scale.x = 0.05;
00219   marker.scale.y = 0.05;
00220   marker.scale.z = 0.05;
00221 
00222   // Set the color -- be sure to set alpha to something non-zero!
00223   marker.color.r = 0.0f;
00224   marker.color.g = 1.0f;
00225   marker.color.b = 0.0f;
00226   marker.color.a = 1.0;
00227 
00228   marker.lifetime = ros::Duration();
00229 
00230   // Publish the marker
00231   marker_pub_.publish(marker);
00232 
00233 #endif
00234 }
00235 
00236 }


pr2_interactive_manipulation_frontend
Author(s): Jonathan Binney
autogenerated on Mon Oct 6 2014 12:06:28