pr2_interactive_manipulation: point_head_view

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/common.h>
00033 #include <rviz/visualization_manager.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, rviz::VisualizationManager* manager, const std::string& name) :
00045     rviz::ViewController( manager, name ),
00046     texture_(texture),
00047     point_head_action_client_(0)
00048 {
00049   ros::NodeHandle n;
00050 #ifdef DEBUG_DISPLAY
00051   marker_pub_ = n.advertise<visualization_msgs::Marker>("point_head_view_controller/pointing_direction", 1);
00052 #endif
00053 }
00054 
00055 PointHeadViewController::~PointHeadViewController()
00056 {
00057   delete point_head_action_client_;
00058 }
00059 
00060 void PointHeadViewController::handleMouseEvent( rviz::ViewportMouseEvent& event)
00061 {
00062   if ( event.event.LeftDown() )
00063   {
00064     int width = event.viewport->getActualWidth();
00065     int height = event.viewport->getActualHeight();
00066 
00067     Ogre::Ray mouse_ray = event.viewport->getCamera()->getCameraToViewportRay(
00068         (float)event.event.GetX() / (float)width, (float)event.event.GetY() / (float)height );
00069 
00070     lookAt( mouse_ray.getPoint(1.0) );
00071   }
00072 
00073 }
00074 
00075 void PointHeadViewController::setTopic(const std::string& topic)
00076 {
00077   delete point_head_action_client_;
00078   topic_ = topic;
00079 
00080   //remove the trailing '/goal' from the topic
00081 
00082   std::string action_topic = topic_;
00083 
00084   size_t goal_str_pos = topic_.rfind("/goal");
00085   if (goal_str_pos != std::string::npos)
00086   {
00087     action_topic.erase(goal_str_pos);
00088   }
00089 
00090   point_head_action_client_ = new PointHeadActionClient(action_topic, true);
00091 }
00092 
00093 
00094 void PointHeadViewController::lookAt( const Ogre::Vector3& look_at_point )
00095 {
00096   if ( !point_head_action_client_ )
00097   {
00098     return;
00099   }
00100 
00101   if ( !texture_.getImage().get() )
00102   {
00103     ROS_ERROR( "No image received. Cannot compute look-at point." );
00104     return;
00105   }
00106 
00107   point_head_action_client_->waitForServer( ros::Duration( 0.1 ) );
00108 
00109   if ( !point_head_action_client_->isServerConnected() )
00110   {
00111     ROS_ERROR( "Point head action client is not connected!" );
00112     return;
00113   }
00114 
00115   Ogre::Vector3 look_at_point_robot = look_at_point;
00116   rviz::ogreToRobot( look_at_point_robot );
00117 
00118   // send point head goal
00119 
00120   pr2_controllers_msgs::PointHeadGoal point_head_goal;
00121 
00122   point_head_goal.pointing_axis.x = 0;
00123   point_head_goal.pointing_axis.y = 0;
00124   point_head_goal.pointing_axis.z = 1;
00125 
00126   point_head_goal.pointing_frame = texture_.getImage()->header.frame_id;
00127 
00128   point_head_goal.target.header.frame_id = manager_->getFixedFrame();
00129   point_head_goal.target.header.stamp = ros::Time::now();
00130 
00131   point_head_goal.target.point.x = look_at_point_robot.x;
00132   point_head_goal.target.point.y = look_at_point_robot.y;
00133   point_head_goal.target.point.z = look_at_point_robot.z;
00134 
00135   point_head_goal.max_velocity = 1.0;
00136 
00137   point_head_action_client_->sendGoal( point_head_goal );
00138 
00139 #ifdef DEBUG_DISPLAY
00140 
00141   ROS_INFO( "Looking at position %f %f %f in pointing frame %s (fixed frame is %s)", point_head_goal.target.point.x,
00142       point_head_goal.target.point.y, point_head_goal.target.point.z,
00143       point_head_goal.pointing_frame.c_str(),
00144       point_head_goal.target.header.frame_id.c_str() );
00145 
00146   // publish debug marker
00147 
00148   visualization_msgs::Marker marker;
00149   // Set the frame ID and timestamp.  See the TF tutorials for information on these.
00150   marker.header.frame_id = manager_->getFixedFrame();
00151   marker.header.stamp = ros::Time::now();
00152 
00153   // Set the namespace and id for this marker.  This serves to create a unique ID
00154   // Any marker sent with the same namespace and id will overwrite the old one
00155   marker.ns = "PointHeadViewController";
00156   marker.id = 0;
00157 
00158   // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
00159   marker.type = visualization_msgs::Marker::SPHERE;
00160 
00161   // Set the marker action.  Options are ADD and DELETE
00162   marker.action = visualization_msgs::Marker::ADD;
00163 
00164   // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
00165   marker.pose.position.x = look_at_point_robot.x;
00166   marker.pose.position.y = look_at_point_robot.y;
00167   marker.pose.position.z = look_at_point_robot.z;
00168   marker.pose.orientation.x = 0.0;
00169   marker.pose.orientation.y = 0.0;
00170   marker.pose.orientation.z = 0.0;
00171   marker.pose.orientation.w = 1.0;
00172 
00173   // Set the scale of the marker -- 1x1x1 here means 1m on a side
00174   marker.scale.x = 0.05;
00175   marker.scale.y = 0.05;
00176   marker.scale.z = 0.05;
00177 
00178   // Set the color -- be sure to set alpha to something non-zero!
00179   marker.color.r = 0.0f;
00180   marker.color.g = 1.0f;
00181   marker.color.b = 0.0f;
00182   marker.color.a = 1.0;
00183 
00184   marker.lifetime = ros::Duration();
00185 
00186   // Publish the marker
00187   marker_pub_.publish(marker);
00188 
00189 #endif
00190 }
00191 
00192 }
point_head_view_controller.cpp
