diff -r ebdb3ee14a58 move_arm/src/planning_components_visualizer.cpp
--- a/move_arm/src/planning_components_visualizer.cpp	Wed Jun 13 10:25:26 2012 -0700
+++ b/move_arm/src/planning_components_visualizer.cpp	Sun Jan 20 21:30:41 2013 +0900
@@ -56,6 +56,9 @@
 #include <interactive_markers/menu_handler.h>
 #include <sensor_msgs/JointState.h>
 
+#include <control_msgs/FollowJointTrajectoryAction.h>
+typedef actionlib::ActionClient<control_msgs::FollowJointTrajectoryAction> JointExecutorActionClient;
+
 using namespace std;
 using namespace arm_navigation_msgs;
 using namespace interactive_markers;
@@ -245,7 +248,7 @@
         tf::Transform last_good_state_;
     };
 
-    PlanningComponentsVisualizer()
+    PlanningComponentsVisualizer() : private_handle_("~")
     {
       ik_control_type_ = EndPosition;
       num_collision_poles_ = 0;
@@ -256,6 +259,9 @@
       joint_state_publisher_ = nh_.advertise<sensor_msgs::JointState> ("joint_states", 10);
       constrain_rp_ = false;
 
+      joint_state_subscriber_ = nh_.subscribe<sensor_msgs::JointState> ("/joint_states", 1,
+                                                                        &PlanningComponentsVisualizer::joint_states_callback, this);
+
       process_function_ptr_ = boost::bind(&PlanningComponentsVisualizer::processInteractiveFeedback, this, _1);
 
       while(!ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME, ros::Duration(1.0)))
@@ -347,6 +353,7 @@
 
       registerMenuEntry(menu_handler_map_["End Effector"], menu_entry_maps_["End Effector"], "Plan");
       registerMenuEntry(menu_handler_map_["End Effector"], menu_entry_maps_["End Effector"], "Filter Trajectory");
+      registerMenuEntry(menu_handler_map_["End Effector"], menu_entry_maps_["End Effector"], "Exec Trajectory");
       registerMenuEntry(menu_handler_map_["End Effector"], menu_entry_maps_["End Effector"], "Randomly Perturb");
       registerMenuEntry(menu_handler_map_["End Effector"], menu_entry_maps_["End Effector"], "Go To Last Good State");
       registerMenuEntry(menu_handler_map_["End Effector"], menu_entry_maps_["End Effector"], "Deselect");
@@ -396,8 +403,8 @@
 
       interactive_marker_server_->applyChanges();
 
+      initializeControllerInterface();
       ROS_INFO_STREAM("Initialized");
-
     }
 
     ~PlanningComponentsVisualizer()
@@ -1118,6 +1125,8 @@
 
     }
   void publishJointStates() {
+    return; // do nothing
+
     joint_state_lock_.lock();
     last_joint_state_msg_.header.frame_id =  cm_->getWorldFrameId();
     last_joint_state_msg_.header.stamp = ros::Time::now();
@@ -1133,8 +1142,21 @@
       transform_broadcaster_.sendTransform(trans);
     }
   }
+  void updateToRealJointStates(GroupCollection& gc) {
+    map<string, double> joint_values;
+    for (unsigned int i = 0; i < current_real_joint_states_.name.size(); i++) {
+      joint_values[current_real_joint_states_.name[i]] = current_real_joint_states_.position[i];
+    }
+    lock_.lock();
+    gc.getState(ik_control_type_)->setKinematicState(joint_values);
 
-    bool planToEndEffectorState(PlanningComponentsVisualizer::GroupCollection& gc)
+    map<string, double> stateMap;
+    gc.getState(ik_control_type_)->getKinematicStateValues(stateMap);
+    robot_state_->setKinematicState(stateMap);
+    lock_.unlock();
+  }
+
+    bool planToEndEffectorState(PlanningComponentsVisualizer::GroupCollection& gc, bool is_display = true)
     {
       MotionPlanRequest motion_plan_request;
       motion_plan_request.group_name = gc.name_;
@@ -1192,7 +1214,12 @@
           return false;
         }
         last_motion_plan_request_ = motion_plan_request;
-        playTrajectory(gc, "planner", plan_res.trajectory.joint_trajectory);
+        if (is_display) {
+          playTrajectory(gc, "planner", plan_res.trajectory.joint_trajectory);
+        } else {
+          disp.reset();
+          disp.joint_trajectory_ = plan_res.trajectory.joint_trajectory;
+        }
         return true;
       }
       else
@@ -1250,7 +1277,7 @@
       }
     }
 
-    bool filterPlannerTrajectory(PlanningComponentsVisualizer::GroupCollection& gc)
+    bool filterPlannerTrajectory(PlanningComponentsVisualizer::GroupCollection& gc, bool exec = false)
     {
       FilterJointTrajectoryWithConstraints::Request filter_req;
       FilterJointTrajectoryWithConstraints::Response filter_res;
@@ -1279,7 +1306,29 @@
         gc.state_trajectory_display_map_["filter"].reset();
         return false;
       }
-      playTrajectory(gc, "filter", filter_res.trajectory);
+      if (exec) {
+        execTrajectory(gc, "filter", filter_res.trajectory);
+      } else {
+        playTrajectory(gc, "filter", filter_res.trajectory);
+      }
+      return true;
+    }
+
+    bool execTrajectory(PlanningComponentsVisualizer::GroupCollection& gc, const string& source_name,
+                        const trajectory_msgs::JointTrajectory& traj) {
+      ROS_INFO("exec Trajectory");
+      lock_.lock();
+      if(gc.state_trajectory_display_map_.find(source_name) == gc.state_trajectory_display_map_.end())
+      {
+        ROS_INFO_STREAM("No state display for group " << gc.name_ << " source name " << source_name);
+        lock_.unlock();
+        return false;
+      }
+      // followjointtrajectory
+      trajectory_msgs::JointTrajectory tmp_traj(traj);
+      sendTrajectory (tmp_traj);
+
+      lock_.unlock();
       return true;
     }
 
@@ -1425,6 +1474,18 @@
         bad_color.g = 0.0;
         bad_color.b = 0.0;
 
+        std_msgs::ColorRGBA start_color;
+        start_color.a = 0.4;
+        start_color.r = 0.4;
+        start_color.g = 0.8;
+        start_color.b = 0.8;
+
+        std_msgs::ColorRGBA end_color;
+        end_color.a = 0.4;
+        end_color.r = 0.8;
+        end_color.g = 0.8;
+        end_color.b = 0.4;
+
         GroupCollection& gc = group_map_[current_group_name_];
         const KinematicModel* kinematic_model = cm_->getKinematicModel();
 
@@ -1442,25 +1503,40 @@
         {
           if(gc.getState(otherState) != NULL)
           {
-            cm_->getGroupAndUpdatedJointMarkersGivenState(*gc.getState(otherState), arr, current_group_name_, group_color,
-                                                        updated_color, ros::Duration(0.1));
+            if (otherState == StartPosition) {
+              cm_->getGroupAndUpdatedJointMarkersGivenState(*gc.getState(otherState), arr, current_group_name_, start_color,
+                                                            updated_color, ros::Duration(0.1));
+            } else {
+              cm_->getGroupAndUpdatedJointMarkersGivenState(*gc.getState(otherState), arr, current_group_name_, end_color,
+                                                            updated_color, ros::Duration(0.1));
+            }
           }
           else
           {
             ROS_ERROR("Other state invalid!");
           }
         }
-
         if(!gc.good_ik_solution_ && gc.getState(ik_control_type_) != NULL)
         {
           vector<string> lnames =
               kinematic_model->getChildLinkModelNames(kinematic_model->getLinkModel(gc.ik_link_name_));
 
+          cm_->getGroupAndUpdatedJointMarkersGivenState(*gc.getState(ik_control_type_), arr, current_group_name_, bad_color,
+                                                        updated_color, ros::Duration(0.1));
+
           cm_->getRobotMarkersGivenState(*gc.getState(ik_control_type_), arr, bad_color,
                                          current_group_name_, ros::Duration(0.1), &lnames);
           cm_->getAttachedCollisionObjectMarkers(*gc.getState(ik_control_type_), arr, current_group_name_, bad_color,
                                                  ros::Duration(.2));
 
+        } else if (gc.getState(ik_control_type_) != NULL) {
+          if (ik_control_type_ == StartPosition) {
+            cm_->getGroupAndUpdatedJointMarkersGivenState(*gc.getState(ik_control_type_), arr, current_group_name_, start_color,
+                                                          updated_color, ros::Duration(0.1));
+          } else {
+            cm_->getGroupAndUpdatedJointMarkersGivenState(*gc.getState(ik_control_type_), arr, current_group_name_, end_color,
+                                                          updated_color, ros::Duration(0.1));
+          }
         }
         for(map<string, StateTrajectoryDisplay>::iterator it = gc.state_trajectory_display_map_.begin(); it
             != gc.state_trajectory_display_map_.end(); it++)
@@ -1489,6 +1565,10 @@
           }
         }
       }
+      // reindex id (dirty hack)
+      for (unsigned int i = 0; i < arr.markers.size(); i++) {
+        arr.markers[i].id = i;
+      }
       vis_marker_array_publisher_.publish(arr);
       lock_.unlock();
     }
@@ -1819,8 +1899,14 @@
               {
                 menu_handler_map_["End Effector"].setCheckState(end_position_handle_, MenuHandler::UNCHECKED);
                 ik_control_type_ = StartPosition;
+                // update for real robot
+                updateToRealJointStates (gc);
+                tf::Transform cur = robot_state_->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform();
+                setNewEndEffectorPosition(gc, cur, collision_aware_);
+
                 selectMarker(selectable_markers_[feedback->marker_name + "_selectable"],
                              gc.start_state_->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform());
+
                 createSelectableJointMarkers(gc);
                 menu_handler_map_["End Effector"].reApply(*interactive_marker_server_);
               }
@@ -1856,8 +1942,14 @@
             }
             else if(menu_entry_maps_["End Effector"][handle] == "Filter Trajectory")
             {
+              planToEndEffectorState(gc,false);
               filterPlannerTrajectory(gc);
             }
+            else if(menu_entry_maps_["End Effector"][handle] == "Exec Trajectory")
+            {
+              planToEndEffectorState(gc,false);
+              filterPlannerTrajectory(gc, true);
+            }
             else if(menu_entry_maps_["End Effector"][handle] == "Randomly Perturb")
             {
               randomlyPerturb(gc);
@@ -1924,6 +2016,42 @@
             tf::Transform cur = toBulletTransform(feedback->pose);
             setNewEndEffectorPosition(gc, cur, collision_aware_);
             last_ee_poses_[current_group_name_] = feedback->pose;
+            if (ik_control_type_ == StartPosition) {
+            // create trajectory
+              map<string, double> joint_values;
+              for (unsigned int i = 0; i < current_real_joint_states_.name.size(); i++) {
+                joint_values[current_real_joint_states_.name[i]] = current_real_joint_states_.position[i];
+              }
+              map<string, double> stateMap;
+              lock_.lock();
+              gc.getState(ik_control_type_)->getKinematicStateValues(stateMap);
+              lock_.unlock();
+
+              trajectory_msgs::JointTrajectory tmp_traj;
+              for (unsigned int i = 0; i < current_real_joint_states_.name.size(); i++) {
+                tmp_traj.joint_names.push_back (current_real_joint_states_.name[i]);
+              }
+#if 0
+              { // set real position
+                trajectory_msgs::JointTrajectoryPoint p;
+                p.time_from_start = ros::Duration (0.05);
+                for (unsigned int i = 0; i < current_real_joint_states_.name.size(); i++) {
+                  p.positions.push_back (current_real_joint_states_.position[i]);
+                }
+                tmp_traj.points.push_back (p);
+              }
+#endif
+              { // set desire position
+                trajectory_msgs::JointTrajectoryPoint p;
+                p.time_from_start = ros::Duration (0.2); //
+                for (unsigned int i = 0; i < tmp_traj.joint_names.size(); i++) {
+                  p.positions.push_back (stateMap[tmp_traj.joint_names[i]]);
+                }
+                tmp_traj.points.push_back (p);
+              }
+              //
+              sendTrajectory (tmp_traj);
+            }
           }
           else if(is_joint_control_active_ && feedback->marker_name.rfind("_joint_control") != string::npos)
           {
@@ -2368,11 +2496,133 @@
       }
     }
 
+  void joint_states_callback (const sensor_msgs::JointStateConstPtr &msg) {
+    current_real_joint_states_ = *msg;
+  }
+  ///
+  /// Control (copy from move_arm_simple_action.cpp)
+  ///
+  enum ControllerStatus {
+    QUEUED,
+    ACTIVE,
+    SUCCESS,
+    FAILED
+  };
+  bool initializeControllerInterface()
+  {
+    std::string controller_action_name;
+    private_handle_.param<std::string>("controller_action_name", controller_action_name, "action");
+
+    ROS_INFO("Connecting to controller using action: %s",controller_action_name.c_str());
+    controller_action_client_ = new JointExecutorActionClient(controller_action_name);
+    if(!controller_action_client_) {
+      ROS_ERROR("Controller action client hasn't been initialized yet");
+      return false;
+    }
+    while(!controller_action_client_->waitForActionServerToStart(ros::Duration(1.0))){
+      ROS_INFO("Waiting for the joint_trajectory_action server to come up.");
+      if(!nh_.ok()) {
+        return false;
+      }
+    }
+    ROS_INFO("Connected to the controller");
+    return true;
+  }
+  bool stopTrajectory()
+  {
+    if (controller_goal_handle_.isExpired())
+      ROS_ERROR("Expired goal handle.  controller_status = %d", controller_status_);
+    else
+      controller_goal_handle_.cancel();
+    return true;
+  }
+  bool sendTrajectory(trajectory_msgs::JointTrajectory &current_trajectory)
+  {
+    current_trajectory.header.stamp = ros::Time::now()+ros::Duration(0.2);
+
+    control_msgs::FollowJointTrajectoryGoal goal;
+    goal.trajectory = current_trajectory;
+
+    ROS_DEBUG("Sending trajectory with %d points and timestamp: %f",(int)goal.trajectory.points.size(),goal.trajectory.header.stamp.toSec());
+    for(unsigned int i=0; i < goal.trajectory.joint_names.size(); i++)
+      ROS_DEBUG("Joint: %d name: %s",i,goal.trajectory.joint_names[i].c_str());
+
+    controller_goal_handle_ = controller_action_client_->sendGoal(goal,boost::bind(&PlanningComponentsVisualizer::controllerTransitionCallback, this, _1));
+
+    controller_status_ = QUEUED;
+    //    printTrajectory(goal.trajectory);
+    return true;
+  }
+  void controllerTransitionCallback(JointExecutorActionClient::GoalHandle gh)
+  {
+    if(gh != controller_goal_handle_)
+      return;
+    actionlib::CommState comm_state = gh.getCommState();
+    switch( comm_state.state_)
+    {
+    case actionlib::CommState::WAITING_FOR_GOAL_ACK:
+    case actionlib::CommState::PENDING:
+    case actionlib::CommState::RECALLING:
+      controller_status_ = QUEUED;
+      return;
+    case actionlib:: CommState::ACTIVE:
+    case actionlib::CommState::PREEMPTING:
+      controller_status_ = ACTIVE;
+      return;
+    case actionlib::CommState::DONE:
+      {
+        switch(gh.getTerminalState().state_)
+        {
+        case actionlib::TerminalState::RECALLED:
+        case actionlib::TerminalState::REJECTED:
+        case actionlib::TerminalState::PREEMPTED:
+        case actionlib::TerminalState::ABORTED:
+        case actionlib::TerminalState::LOST:
+          {
+            ROS_INFO("Trajectory controller status came back as failed");
+            controller_status_ = FAILED;
+            controller_goal_handle_.reset();
+            return;
+          }
+        case actionlib::TerminalState::SUCCEEDED:
+          {
+            controller_goal_handle_.reset();
+            controller_status_ = SUCCESS;
+            return;
+          }
+        default:
+          ROS_ERROR("Unknown terminal state [%u]. This is a bug in ActionClient", gh.getTerminalState().state_);
+        }
+      }
+    default:
+      break;
+    }
+  }
+  bool isControllerDone(arm_navigation_msgs::ArmNavigationErrorCodes& error_code)
+  {
+    if (controller_status_ == SUCCESS)
+    {
+      error_code.val = error_code.SUCCESS;
+      return true;
+    } else if(controller_status_ == FAILED)
+    {
+      error_code.val = error_code.TRAJECTORY_CONTROLLER_FAILED;
+      return true;
+    } else {
+      return false;
+    }
+  }
+  ControllerStatus controller_status_;
+  JointExecutorActionClient* controller_action_client_;
+  JointExecutorActionClient::GoalHandle controller_goal_handle_;
+  // end (copy from move_arm_simple_action.cpp)
+
     IKControlType ik_control_type_;
 
 
   boost::recursive_mutex joint_state_lock_;
   sensor_msgs::JointState last_joint_state_msg_;
+  sensor_msgs::JointState current_real_joint_states_;
 
     boost::recursive_mutex lock_;
     boost::shared_ptr<InteractiveMarkerServer> interactive_marker_server_;
@@ -2415,9 +2665,11 @@
     MotionPlanRequest last_motion_plan_request_;
 
     ros::NodeHandle nh_;
+    ros::NodeHandle private_handle_;
     ros::Publisher joint_state_publisher_;
     ros::Publisher vis_marker_array_publisher_;
     ros::Publisher vis_marker_publisher_;
+    ros::Subscriber joint_state_subscriber_;
     ros::ServiceClient set_planning_scene_diff_client_;
     ros::ServiceClient planner_service_client_;
     ros::ServiceClient trajectory_filter_service_client_;
@@ -2432,6 +2684,8 @@
     map<string, bool> joint_clicked_map_;
     map<string, tf::Transform> joint_prev_transform_map_;
   map<string, double> prev_joint_control_value_map_;
+
+
 };
 
 PlanningComponentsVisualizer* pcv;
