Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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.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
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
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
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 point_head_goal.target.header.frame_id = manager_->getFixedFrame();
00172 point_head_goal.target.header.stamp = ros::Time::now();
00173
00174 point_head_goal.target.point.x = look_at_point_robot.x;
00175 point_head_goal.target.point.y = look_at_point_robot.y;
00176 point_head_goal.target.point.z = look_at_point_robot.z;
00177
00178 point_head_goal.max_velocity = 1.0;
00179
00180 point_head_action_client_->sendGoal( point_head_goal );
00181
00182 #ifdef DEBUG_DISPLAY
00183
00184 ROS_INFO( "Looking at position %f %f %f in pointing frame %s (fixed frame is %s)", point_head_goal.target.point.x,
00185 point_head_goal.target.point.y, point_head_goal.target.point.z,
00186 point_head_goal.pointing_frame.c_str(),
00187 point_head_goal.target.header.frame_id.c_str() );
00188
00189
00190
00191 visualization_msgs::Marker marker;
00192
00193 marker.header.frame_id = manager_->getFixedFrame();
00194 marker.header.stamp = ros::Time::now();
00195
00196
00197
00198 marker.ns = "PointHeadViewController";
00199 marker.id = 0;
00200
00201
00202 marker.type = visualization_msgs::Marker::SPHERE;
00203
00204
00205 marker.action = visualization_msgs::Marker::ADD;
00206
00207
00208 marker.pose.position.x = look_at_point_robot.x;
00209 marker.pose.position.y = look_at_point_robot.y;
00210 marker.pose.position.z = look_at_point_robot.z;
00211 marker.pose.orientation.x = 0.0;
00212 marker.pose.orientation.y = 0.0;
00213 marker.pose.orientation.z = 0.0;
00214 marker.pose.orientation.w = 1.0;
00215
00216
00217 marker.scale.x = 0.05;
00218 marker.scale.y = 0.05;
00219 marker.scale.z = 0.05;
00220
00221
00222 marker.color.r = 0.0f;
00223 marker.color.g = 1.0f;
00224 marker.color.b = 0.0f;
00225 marker.color.a = 1.0;
00226
00227 marker.lifetime = ros::Duration();
00228
00229
00230 marker_pub_.publish(marker);
00231
00232 #endif
00233 }
00234
00235 }