$search
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/visualization_manager.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::VisualizationManager* manager, 00046 const std::string& name) : 00047 rviz::ViewController( manager, name, manager->getSceneManager()->getRootSceneNode() ), 00048 texture_(texture), 00049 point_head_action_client_(0) 00050 { 00051 ros::NodeHandle n; 00052 #ifdef DEBUG_DISPLAY 00053 marker_pub_ = n.advertise<visualization_msgs::Marker>("point_head_view_controller/pointing_direction", 1); 00054 #endif 00055 global_orientation_ = Ogre::Quaternion::IDENTITY; 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.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.event.GetX() / (float)width, (float)event.event.GetY() / (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 00097 void PointHeadViewController::lookAt( const Ogre::Vector3& look_at_point ) 00098 { 00099 if ( !point_head_action_client_ ) 00100 { 00101 return; 00102 } 00103 00104 if ( !texture_.getImage().get() ) 00105 { 00106 ROS_ERROR( "No image received. Cannot compute look-at point." ); 00107 return; 00108 } 00109 00110 point_head_action_client_->waitForServer( ros::Duration( 0.1 ) ); 00111 00112 if ( !point_head_action_client_->isServerConnected() ) 00113 { 00114 ROS_ERROR( "Point head action client is not connected!" ); 00115 return; 00116 } 00117 00118 Ogre::Vector3 look_at_point_robot = look_at_point; 00119 00120 // send point head goal 00121 00122 pr2_controllers_msgs::PointHeadGoal point_head_goal; 00123 00124 point_head_goal.pointing_axis.x = 0; 00125 point_head_goal.pointing_axis.y = 0; 00126 point_head_goal.pointing_axis.z = 1; 00127 00128 point_head_goal.pointing_frame = texture_.getImage()->header.frame_id; 00129 00130 point_head_goal.target.header.frame_id = manager_->getFixedFrame(); 00131 point_head_goal.target.header.stamp = ros::Time::now(); 00132 00133 point_head_goal.target.point.x = look_at_point_robot.x; 00134 point_head_goal.target.point.y = look_at_point_robot.y; 00135 point_head_goal.target.point.z = look_at_point_robot.z; 00136 00137 point_head_goal.max_velocity = 1.0; 00138 00139 point_head_action_client_->sendGoal( point_head_goal ); 00140 00141 #ifdef DEBUG_DISPLAY 00142 00143 ROS_INFO( "Looking at position %f %f %f in pointing frame %s (fixed frame is %s)", point_head_goal.target.point.x, 00144 point_head_goal.target.point.y, point_head_goal.target.point.z, 00145 point_head_goal.pointing_frame.c_str(), 00146 point_head_goal.target.header.frame_id.c_str() ); 00147 00148 // publish debug marker 00149 00150 visualization_msgs::Marker marker; 00151 // Set the frame ID and timestamp. See the TF tutorials for information on these. 00152 marker.header.frame_id = manager_->getFixedFrame(); 00153 marker.header.stamp = ros::Time::now(); 00154 00155 // Set the namespace and id for this marker. This serves to create a unique ID 00156 // Any marker sent with the same namespace and id will overwrite the old one 00157 marker.ns = "PointHeadViewController"; 00158 marker.id = 0; 00159 00160 // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER 00161 marker.type = visualization_msgs::Marker::SPHERE; 00162 00163 // Set the marker action. Options are ADD and DELETE 00164 marker.action = visualization_msgs::Marker::ADD; 00165 00166 // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header 00167 marker.pose.position.x = look_at_point_robot.x; 00168 marker.pose.position.y = look_at_point_robot.y; 00169 marker.pose.position.z = look_at_point_robot.z; 00170 marker.pose.orientation.x = 0.0; 00171 marker.pose.orientation.y = 0.0; 00172 marker.pose.orientation.z = 0.0; 00173 marker.pose.orientation.w = 1.0; 00174 00175 // Set the scale of the marker -- 1x1x1 here means 1m on a side 00176 marker.scale.x = 0.05; 00177 marker.scale.y = 0.05; 00178 marker.scale.z = 0.05; 00179 00180 // Set the color -- be sure to set alpha to something non-zero! 00181 marker.color.r = 0.0f; 00182 marker.color.g = 1.0f; 00183 marker.color.b = 0.0f; 00184 marker.color.a = 1.0; 00185 00186 marker.lifetime = ros::Duration(); 00187 00188 // Publish the marker 00189 marker_pub_.publish(marker); 00190 00191 #endif 00192 } 00193 00194 }