Index: src/rviz/tools/pose_tool.cpp
===================================================================
--- src/rviz/tools/pose_tool.cpp	(リビジョン 38308)
+++ src/rviz/tools/pose_tool.cpp	(作業コピー)
@@ -29,9 +29,13 @@
 
 #include "pose_tool.h"
 #include "visualization_manager.h"
+#include "frame_manager.h"
+#include "selection/forwards.h"
+#include "selection/selection_manager.h"
 #include "viewport_mouse_event.h"
 #include "properties/property.h"
 #include "properties/property_manager.h"
+#include "default_plugin/map_display.h"
 
 #include "ogre_tools/camera_base.h"
 #include "ogre_tools/arrow.h"
@@ -94,12 +98,33 @@
 
 int PoseTool::processMouseEvent( ViewportMouseEvent& event )
 {
+  static bool need_overwrite = false;
   int flags = 0;
 
   if ( event.event.LeftDown() )
   {
     ROS_ASSERT( state_ == Position );
 
+    SelectionManager* sel_manager = manager_->getSelectionManager();
+    M_Picked selection;
+    sel_manager->pick(event.viewport, event.event.GetX(), event.event.GetY(), event.event.GetX(), event.event.GetY(), selection);
+
+    M_Picked::const_iterator it = selection.begin();
+    for (; it != selection.end(); ++it)
+    {
+      SelectionHandlerPtr pt = sel_manager->getHandler(it->first);
+      MapDisplaySelectionHandlerPtr map_handler;
+      if(map_handler = boost::dynamic_pointer_cast<MapDisplaySelectionHandler>(pt)) {
+	pick_frame_ = map_handler->getFrame();
+        ROS_INFO("picked frame = %s", pick_frame_.c_str());
+	old_frame_ = manager_->getFixedFrame();
+	manager_->setFixedFrame(pick_frame_);
+
+	need_overwrite = true;
+	break;
+      }
+    }
+
     pos_ = getPositionFromMouseXY( event.viewport, event.event.GetX(), event.event.GetY() );
     arrow_->setPosition( pos_ );
 
@@ -112,6 +137,11 @@
     {
       //compute angle in x-y plane
       Ogre::Vector3 cur_pos = getPositionFromMouseXY( event.viewport, event.event.GetX(), event.event.GetY() );
+      if(need_overwrite){
+	pos_ = cur_pos;
+	arrow_->setPosition( pos_ );
+	need_overwrite = false;
+      }
       double angle = atan2( cur_pos.y - pos_.y, cur_pos.x - pos_.x );
 
       arrow_->getSceneNode()->setVisible( true );
@@ -136,6 +166,10 @@
 
       flags |= (Finished|Render);
     }
+    if(old_frame_ != std::string("")) {
+      manager_->setFixedFrame(old_frame_);
+      old_frame_ = std::string("");
+    }
   }
 
   return flags;
Index: src/rviz/tools/pose_tool.h
===================================================================
--- src/rviz/tools/pose_tool.h	(リビジョン 38308)
+++ src/rviz/tools/pose_tool.h	(作業コピー)
@@ -72,6 +72,7 @@
   State state_;
 
   Ogre::Vector3 pos_;
+  std::string old_frame_, pick_frame_;
 };
 
 }
Index: src/rviz/selection/selection_manager.cpp
===================================================================
--- src/rviz/selection/selection_manager.cpp	(リビジョン 38308)
+++ src/rviz/selection/selection_manager.cpp	(作業コピー)
@@ -1128,6 +1128,31 @@
   return coll;
 }
 
+CollObjectHandle SelectionManager::createCollisionForMaterial(Ogre::MaterialPtr& material, const SelectionHandlerPtr& handler, CollObjectHandle coll)
+{
+  boost::recursive_mutex::scoped_lock lock(global_mutex_);
+
+  bool use_original = false;
+
+  if (coll)
+  {
+    use_original = true;
+  }
+  else
+  {
+    coll = createHandle();
+  }
+
+  addPickTechnique(coll, material);
+
+  if (!use_original)
+  {
+    addObject(coll, handler);
+  }
+
+  return coll;
+}
+
 SelectionHandlerPtr SelectionManager::getHandler(CollObjectHandle obj)
 {
   boost::recursive_mutex::scoped_lock lock(global_mutex_);
Index: src/rviz/selection/selection_manager.h
===================================================================
--- src/rviz/selection/selection_manager.h	(リビジョン 38308)
+++ src/rviz/selection/selection_manager.h	(作業コピー)
@@ -105,6 +105,7 @@
   // create handle, add or modify the picking scheme of the object's material accordingly
   CollObjectHandle createCollisionForObject(ogre_tools::Object* obj, const SelectionHandlerPtr& handler, CollObjectHandle coll = 0);
   CollObjectHandle createCollisionForEntity(Ogre::Entity* entity, const SelectionHandlerPtr& handler, CollObjectHandle coll = 0);
+  CollObjectHandle createCollisionForMaterial(Ogre::MaterialPtr& material, const SelectionHandlerPtr& handler, CollObjectHandle coll = 0);
 
   void update();
 
Index: src/rviz/default_plugin/map_display.cpp
===================================================================
--- src/rviz/default_plugin/map_display.cpp	(リビジョン 38308)
+++ src/rviz/default_plugin/map_display.cpp	(作業コピー)
@@ -74,6 +74,10 @@
   material_->setCullingMode( Ogre::CULL_NONE );
   material_->setDepthWriteEnabled(false);
 
+  SelectionManager* sel_manager = manager->getSelectionManager();
+  coll_handler_.reset(new MapDisplaySelectionHandler(name));
+  coll_ = sel_manager->createCollisionForMaterial(material_, coll_handler_);
+
   setAlpha( 0.7f );
 }
 
@@ -82,6 +86,9 @@
   unsubscribe();
 
   clear();
+
+  SelectionManager* sel_manager = vis_manager_->getSelectionManager();
+  sel_manager->removeObject(coll_);
 }
 
 void MapDisplay::onEnable()
@@ -264,6 +271,8 @@
     frame_ = "/map";
   }
 
+  coll_handler_->setFrame(frame_);
+
   // Expand it to be RGB data
   unsigned int pixels_size = width_ * height_;
   unsigned char* pixels = new unsigned char[pixels_size];
Index: src/rviz/default_plugin/map_display.h
===================================================================
--- src/rviz/default_plugin/map_display.h	(リビジョン 38308)
+++ src/rviz/default_plugin/map_display.h	(作業コピー)
@@ -31,6 +31,8 @@
 #define RVIZ_MAP_DISPLAY_H
 
 #include "rviz/display.h"
+#include "rviz/selection/forwards.h"
+#include "rviz/selection/selection_manager.h"
 #include "rviz/properties/forwards.h"
 
 #include <OGRE/OgreTexture.h>
@@ -53,7 +55,29 @@
 
 namespace rviz
 {
+  class MapDisplaySelectionHandler : public SelectionHandler
+  {
+  public:
+    MapDisplaySelectionHandler(const std::string& name)
+      : name_(name)
+    {}
 
+    void setFrame(const std::string& frame_id)
+    {
+      frame_id_ = frame_id;
+    }
+
+    std::string getFrame()
+      {
+	return frame_id_;
+      }
+
+  private:
+    std::string name_;
+    std::string frame_id_;
+  };
+  typedef boost::shared_ptr<MapDisplaySelectionHandler> MapDisplaySelectionHandlerPtr;
+
 /**
  * \class MapDisplay
  * \brief Displays a map along the XZ plane (XY in robot space)
@@ -103,6 +127,9 @@
 
   void requestThreadFunc();
 
+  CollObjectHandle coll_;
+  MapDisplaySelectionHandlerPtr coll_handler_;
+
   Ogre::SceneNode* scene_node_;
   Ogre::ManualObject* manual_object_;
   Ogre::TexturePtr texture_;
