planning_components_visualizer.cpp
Go to the documentation of this file.
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 <ORGANIZATION> 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 // Author: E. Gil Jones
00031 
00032 #include <termios.h>
00033 #include <signal.h>
00034 #include <cstdio>
00035 #include <cstdlib>
00036 #include <unistd.h>
00037 #include <math.h>
00038 
00039 #include <ros/ros.h>
00040 
00041 #include <planning_environment/models/collision_models.h>
00042 #include <arm_navigation_msgs/PlanningScene.h>
00043 #include <arm_navigation_msgs/SetPlanningSceneDiff.h>
00044 #include <planning_environment/models/model_utils.h>
00045 #include <rosgraph_msgs/Clock.h>
00046 #include <tf/transform_broadcaster.h>
00047 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00048 #include <kinematics_msgs/GetPositionIK.h>
00049 #include <actionlib/client/simple_action_client.h>
00050 #include <arm_navigation_msgs/GetMotionPlan.h>
00051 #include <arm_navigation_msgs/GetStateValidity.h>
00052 #include <trajectory_msgs/JointTrajectory.h>
00053 #include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
00054 #include <arm_navigation_msgs/convert_messages.h>
00055 #include <interactive_markers/interactive_marker_server.h>
00056 #include <interactive_markers/menu_handler.h>
00057 #include <sensor_msgs/JointState.h>
00058 
00059 using namespace std;
00060 using namespace arm_navigation_msgs;
00061 using namespace interactive_markers;
00062 using namespace visualization_msgs;
00063 using namespace planning_environment;
00064 using namespace planning_models;
00065 using namespace geometry_msgs;
00066 
00067 static const string VIS_TOPIC_NAME = "planning_components_visualizer";
00068 
00069 //in 100 hz ticks
00070 static const unsigned int CONTROL_SPEED = 10;
00071 
00072 static const ros::Duration PLANNING_DURATION = ros::Duration(5.0);
00073 
00074 static const double BASE_TRANS_SPEED = .3;
00075 static const double BASE_ROT_SPEED = .15;
00076 
00077 static const double HAND_TRANS_SPEED = .05;
00078 static const double HAND_ROT_SPEED = .15;
00079 
00080 static const string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
00081 static const string PLANNER_SERVICE_NAME = "/ompl_planning/plan_kinematic_path";
00082 static const string TRAJECTORY_FILTER_SERVICE_NAME = "/trajectory_filter_server/filter_trajectory_with_constraints";
00083 
00084 typedef map<MenuHandler::EntryHandle, string> MenuEntryMap;
00085 typedef map<string, MenuEntryMap> MenuMap;
00086 typedef map<string, MenuHandler> MenuHandlerMap;
00087 
00088 
00089 class PlanningComponentsVisualizer
00090 {
00091 
00092   public:
00093 
00095     enum IKControlType
00096     {
00097       StartPosition, EndPosition
00098     };
00099 
00101     enum InteractiveMarkerType
00102     {
00103       EndEffectorControl, JointControl, CollisionObject
00104     };
00105 
00107     struct SelectableMarker
00108     {
00110         InteractiveMarkerType type_;
00111 
00113         string name_;
00114 
00116         string controlName_;
00117 
00119         string controlDescription_;
00120     };
00121 
00122     struct StateTrajectoryDisplay
00123     {
00124 
00125         StateTrajectoryDisplay()
00126         {
00127           state_ = NULL;
00128           reset();
00129         }
00130 
00131         void reset()
00132         {
00133           if(state_ != NULL)
00134           {
00135             delete state_;
00136             state_ = NULL;
00137           }
00138           has_joint_trajectory_ = false;
00139           play_joint_trajectory_ = false;
00140           show_joint_trajectory_ = false;
00141           current_trajectory_point_ = 0;
00142           trajectory_bad_point_ = 0;
00143           trajectory_error_code_.val = 0;
00144         }
00145 
00146         KinematicState* state_;
00147         trajectory_msgs::JointTrajectory joint_trajectory_;unsigned int current_trajectory_point_;
00148         std_msgs::ColorRGBA color_;
00149         bool has_joint_trajectory_;
00150         bool play_joint_trajectory_;
00151         bool show_joint_trajectory_;
00152         ArmNavigationErrorCodes trajectory_error_code_;unsigned int trajectory_bad_point_;
00153     };
00154 
00155     struct GroupCollection
00156     {
00157         GroupCollection()
00158         {
00159           start_state_ = NULL;
00160           end_state_ = NULL;
00161           good_ik_solution_ = false;
00162 
00163           state_trajectory_display_map_["planner"].color_.a = .6;
00164           state_trajectory_display_map_["planner"].color_.r = 1.0;
00165           state_trajectory_display_map_["planner"].color_.g = 1.0;
00166           state_trajectory_display_map_["planner"].color_.b = 0.5;
00167 
00168           state_trajectory_display_map_["filter"].color_.a = .6;
00169           state_trajectory_display_map_["filter"].color_.r = 0.5;
00170           state_trajectory_display_map_["filter"].color_.g = 1.0;
00171           state_trajectory_display_map_["filter"].color_.b = 1.0;
00172         }
00173 
00174         ~GroupCollection()
00175         {
00176           reset();
00177         }
00178 
00179         void setState(IKControlType type, KinematicState* state)
00180         {
00181           switch (type)
00182           {
00183             case PlanningComponentsVisualizer::StartPosition:
00184               if(start_state_ != NULL)
00185               {
00186                 delete start_state_;
00187                 start_state_ = NULL;
00188               }
00189               start_state_ = state;
00190               break;
00191             case PlanningComponentsVisualizer::EndPosition:
00192               if(end_state_ != NULL)
00193               {
00194                 delete end_state_;
00195                 end_state_ = NULL;
00196               }
00197               end_state_ = state;
00198               break;
00199           }
00200         }
00201 
00202         KinematicState* getState(IKControlType type)
00203         {
00204           switch (type)
00205           {
00206             case PlanningComponentsVisualizer::StartPosition:
00207               return start_state_;
00208             case PlanningComponentsVisualizer::EndPosition:
00209               return end_state_;
00210           }
00211 
00212           return NULL;
00213         }
00214 
00215         void reset()
00216         {
00217           for(map<string, StateTrajectoryDisplay>::iterator it = state_trajectory_display_map_.begin(); it
00218               != state_trajectory_display_map_.end(); it++)
00219           {
00220             it->second.reset();
00221           }
00222 
00223           if(start_state_ != NULL)
00224           {
00225             delete start_state_;
00226             start_state_ = NULL;
00227           }
00228           if(end_state_ != NULL)
00229           {
00230             delete end_state_;
00231             end_state_ = NULL;
00232           }
00233 
00234         }
00235 
00236         string name_;
00237         string ik_link_name_;
00238         ros::ServiceClient coll_aware_ik_service_;
00239         ros::ServiceClient non_coll_aware_ik_service_;
00240         bool good_ik_solution_;
00241         KinematicState* start_state_;
00242         KinematicState* end_state_;
00243         map<string, StateTrajectoryDisplay> state_trajectory_display_map_;
00244         vector<string> joint_names_;
00245         tf::Transform last_good_state_;
00246     };
00247 
00248     PlanningComponentsVisualizer()
00249     {
00250       ik_control_type_ = EndPosition;
00251       num_collision_poles_ = 0;
00252       collision_aware_ = true;
00253       cm_ = new CollisionModels("robot_description");
00254       vis_marker_publisher_ = nh_.advertise<Marker> (VIS_TOPIC_NAME, 128);
00255       vis_marker_array_publisher_ = nh_.advertise<MarkerArray> (VIS_TOPIC_NAME + "_array", 128);
00256       joint_state_publisher_ = nh_.advertise<sensor_msgs::JointState> ("joint_states", 10);
00257       constrain_rp_ = false;
00258 
00259       process_function_ptr_ = boost::bind(&PlanningComponentsVisualizer::processInteractiveFeedback, this, _1);
00260 
00261       while(!ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME, ros::Duration(1.0)))
00262       {
00263         ROS_INFO_STREAM("Waiting for planning scene service " << SET_PLANNING_SCENE_DIFF_NAME);
00264       }
00265 
00266       set_planning_scene_diff_client_
00267           = nh_.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff> (SET_PLANNING_SCENE_DIFF_NAME);
00268 
00269       while(!ros::service::waitForService(PLANNER_SERVICE_NAME, ros::Duration(1.0)))
00270       {
00271         ROS_INFO_STREAM("Waiting for planner service " << PLANNER_SERVICE_NAME);
00272       }
00273 
00274       planner_service_client_ = nh_.serviceClient<GetMotionPlan> (PLANNER_SERVICE_NAME, true);
00275 
00276       while(!ros::service::waitForService(TRAJECTORY_FILTER_SERVICE_NAME, ros::Duration(1.0)))
00277       {
00278         ROS_INFO_STREAM("Waiting for trajectory filter service " << TRAJECTORY_FILTER_SERVICE_NAME);
00279       }
00280 
00281       trajectory_filter_service_client_
00282           = nh_.serviceClient<FilterJointTrajectoryWithConstraints> (TRAJECTORY_FILTER_SERVICE_NAME, true);
00283 
00284       const map<string, KinematicModel::GroupConfig>& group_config_map =
00285           cm_->getKinematicModel()->getJointModelGroupConfigMap();
00286 
00287       for(map<string, KinematicModel::GroupConfig>::const_iterator it = group_config_map.begin(); it
00288           != group_config_map.end(); it++)
00289       {
00290         if(!it->second.base_link_.empty())
00291         {
00292           group_map_[it->first].name_ = it->first;
00293           group_map_[it->first].ik_link_name_ = it->second.tip_link_;
00294           string ik_service_name = cm_->getKinematicModel()->getRobotName() + "_" + it->first + "_kinematics/";
00295           string coll_aware_name = ik_service_name + "get_constraint_aware_ik";
00296           string non_coll_aware_name = ik_service_name + "get_ik";
00297 
00298           while(!ros::service::waitForService(coll_aware_name, ros::Duration(1.0)))
00299           {
00300             ROS_INFO_STREAM("Waiting for service " << coll_aware_name);
00301           }
00302 
00303           while(!ros::service::waitForService(non_coll_aware_name, ros::Duration(1.0)))
00304           {
00305             ROS_INFO_STREAM("Waiting for service " << non_coll_aware_name);
00306           }
00307 
00308           group_map_[it->first].coll_aware_ik_service_ = nh_.serviceClient<
00309               kinematics_msgs::GetConstraintAwarePositionIK> (coll_aware_name, true);
00310 
00311           group_map_[it->first].non_coll_aware_ik_service_
00312               = nh_.serviceClient<kinematics_msgs::GetPositionIK> (non_coll_aware_name, true);
00313         }
00314       }
00315       robot_state_ = new KinematicState(cm_->getKinematicModel());
00316       robot_state_->setKinematicStateToDefault();
00317       sendPlanningScene();
00318 
00319       // Create a new interactive marker server.
00320       interactive_marker_server_.reset(new InteractiveMarkerServer("planning_visualizer_controls", "", false));
00321 
00322       // Allocate memory to each of the menu entry maps.
00323       menu_entry_maps_["End Effector"] = MenuEntryMap();
00324       menu_entry_maps_["End Effector Selection"] = MenuEntryMap();
00325       menu_entry_maps_["Top Level"] = MenuEntryMap();
00326       menu_entry_maps_["Collision Object"] = MenuEntryMap();
00327       menu_entry_maps_["Collision Object Selection"] = MenuEntryMap();
00328 
00329       // Allocate memory to the menu handlers
00330       menu_handler_map_["End Effector"];
00331       menu_handler_map_["End Effector Selection"];
00332       menu_handler_map_["Top Level"];
00333       menu_handler_map_["Collision Object"];
00334       menu_handler_map_["Collision Object Selection"];
00335 
00336       // Create end effector menus
00337       start_position_handle_ = registerMenuEntry(menu_handler_map_["End Effector"], menu_entry_maps_["End Effector"],
00338                                                  "Set Start Position");
00339       end_position_handle_ = registerMenuEntry(menu_handler_map_["End Effector"], menu_entry_maps_["End Effector"],
00340                                                "Set End Position");
00341       constrain_rp_handle_ = registerMenuEntry(menu_handler_map_["End Effector"], menu_entry_maps_["End Effector"],
00342                                                 "Constrain in Roll and Pitch");
00343                                                 
00344       menu_handler_map_["End Effector"].setCheckState(start_position_handle_, MenuHandler::UNCHECKED);
00345       menu_handler_map_["End Effector"].setCheckState(end_position_handle_, MenuHandler::CHECKED);
00346       menu_handler_map_["End Effector"].setCheckState(constrain_rp_handle_, MenuHandler::UNCHECKED);
00347 
00348       registerMenuEntry(menu_handler_map_["End Effector"], menu_entry_maps_["End Effector"], "Plan");
00349       registerMenuEntry(menu_handler_map_["End Effector"], menu_entry_maps_["End Effector"], "Filter Trajectory");
00350       registerMenuEntry(menu_handler_map_["End Effector"], menu_entry_maps_["End Effector"], "Randomly Perturb");
00351       registerMenuEntry(menu_handler_map_["End Effector"], menu_entry_maps_["End Effector"], "Go To Last Good State");
00352       registerMenuEntry(menu_handler_map_["End Effector"], menu_entry_maps_["End Effector"], "Deselect");
00353       registerMenuEntry(menu_handler_map_["End Effector Selection"], menu_entry_maps_["End Effector Selection"],
00354                         "Select");
00355 
00356 
00357       // Create collision object menus
00358       registerMenuEntry(menu_handler_map_["Collision Object Selection"],
00359                         menu_entry_maps_["Collision Object Selection"], "Select");
00360       registerMenuEntry(menu_handler_map_["Collision Object Selection"],
00361                         menu_entry_maps_["Collision Object Selection"], "Delete");
00362       registerMenuEntry(menu_handler_map_["Collision Object"], menu_entry_maps_["Collision Object"], "Deselect");
00363       registerMenuEntry(menu_handler_map_["Collision Object"], menu_entry_maps_["Collision Object"], "Delete");
00364 
00365       // Create top level menu.
00366       registerMenuEntry(menu_handler_map_["Top Level"], menu_entry_maps_["Top Level"], "Create Pole");
00367       ik_control_handle_ = registerMenuEntry(menu_handler_map_["Top Level"], menu_entry_maps_["Top Level"],
00368                                              "IK Control");
00369       joint_control_handle_ = registerMenuEntry(menu_handler_map_["Top Level"], menu_entry_maps_["Top Level"],
00370                                                 "Joint Control");
00371       collision_aware_handle_ = registerMenuEntry(menu_handler_map_["Top Level"], menu_entry_maps_["Top Level"],
00372                                                   "Collision Aware");
00373       menu_handler_map_["Top Level"].setCheckState(ik_control_handle_, MenuHandler::CHECKED);
00374       menu_handler_map_["Top Level"].setCheckState(joint_control_handle_, MenuHandler::UNCHECKED);
00375       menu_handler_map_["Top Level"].setCheckState(collision_aware_handle_, MenuHandler::CHECKED);
00376 
00377       is_ik_control_active_ = true;
00378       is_joint_control_active_ = false;
00379 
00380       MenuHandler::EntryHandle sub_menu_handle = menu_handler_map_["Top Level"].insert("Select Planning Chain");
00381 
00382       // "Select Planning Chain" sub menu.
00383       unsigned int cmd = 0;
00384       for(map<string, GroupCollection>::iterator it = group_map_.begin(); it != group_map_.end(); it++)
00385       {
00386         registerSubMenuEntry(menu_handler_map_["Top Level"], menu_entry_maps_["Top Level"], it->first, sub_menu_handle);
00387 
00388         // These positions will be reset by main()
00389         makeSelectableMarker(PlanningComponentsVisualizer::EndEffectorControl,
00390                              tf::Transform(tf::Quaternion(0.0f, 0.0f, 0.0f, 1.0f), tf::Vector3(0.0f, 0.0f, 0.0f)), it->first,
00391                              it->first, 0.5f);
00392         cmd++;
00393       }
00394       // Create menu marker.
00395       makeTopLevelMenu();
00396 
00397       interactive_marker_server_->applyChanges();
00398 
00399       ROS_INFO_STREAM("Initialized");
00400 
00401     }
00402 
00403     ~PlanningComponentsVisualizer()
00404     {
00405       deleteKinematicStates();
00406       delete robot_state_;
00407       delete cm_;
00408     }
00409 
00414     int nextCollisionPole()
00415     {
00416       return ++num_collision_poles_;
00417     }
00418 
00423     void removeCollisionPole(int num)
00424     {
00425       stringstream id;
00426       id << "pole_" << num;
00427       removeCollisionPoleByName(id.str());
00428     }
00429 
00434     void removeCollisionPoleByName(string id)
00435     {
00436       ROS_INFO("Removing collision pole %s", id.c_str());
00437       arm_navigation_msgs::CollisionObject& cylinder_object = collision_poles_[id];
00438       cylinder_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE;
00439     }
00440 
00446     void createCollisionPole(int num, Pose pose)
00447     {
00448       ROS_INFO("Creating collision pole %d", num);
00449 
00450       arm_navigation_msgs::CollisionObject cylinder_object;
00451       cylinder_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00452       cylinder_object.header.stamp = ros::Time::now();
00453       cylinder_object.header.frame_id = "/" + cm_->getWorldFrameId();
00454       arm_navigation_msgs::Shape object;
00455       object.type = arm_navigation_msgs::Shape::CYLINDER;
00456       object.dimensions.resize(2);
00457       object.dimensions[0] = .1;
00458       object.dimensions[1] = 2.0;
00459 
00460       cylinder_object.shapes.push_back(object);
00461       cylinder_object.poses.push_back(pose);
00462       stringstream id;
00463       id << "pole_" << num;
00464       cylinder_object.id = id.str();
00465       collision_poles_[id.str()] = cylinder_object;
00466 
00467       tf::Transform cur = toBulletTransform(pose);
00468       makePoleContextMenu(cur, id.str(), "", 2.0f);
00469     }
00470 
00471     void sendPlanningScene()
00472     {
00473       ROS_INFO("Sending Planning Scene....");
00474 
00475       lock_.lock();
00476       arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
00477       arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
00478 
00479       vector<string> removals;
00480       // Handle additions and removals of planning scene objects.
00481       for(map<string, arm_navigation_msgs::CollisionObject>::const_iterator it = collision_poles_.begin(); it
00482           != collision_poles_.end(); it++)
00483       {
00484         string name = it->first;
00485         arm_navigation_msgs::CollisionObject object = it->second;
00486 
00487         // Add or remove objects.
00488         if(object.operation.operation != arm_navigation_msgs::CollisionObjectOperation::REMOVE)
00489         {
00490           ROS_INFO("Adding Collision Pole %s", object.id.c_str());
00491           planning_scene_req.planning_scene_diff.collision_objects.push_back(object);
00492         }
00493         else
00494         {
00495           removals.push_back(it->first);
00496         }
00497       }
00498 
00499       // Delete collision poles from the map which were removed.
00500       for(size_t i = 0; i < removals.size(); i++)
00501       {
00502         collision_poles_.erase(removals[i]);
00503       }
00504 
00505       convertKinematicStateToRobotState(*robot_state_, ros::Time::now(), cm_->getWorldFrameId(),
00506                                         planning_scene_req.planning_scene_diff.robot_state);
00507 
00508 
00509       KinematicState* startState = NULL;
00510       KinematicState* endState = NULL;
00511       map<string, double> startStateValues;
00512       map<string, double> endStateValues;
00513 
00514       if(current_group_name_ != "")
00515       {
00516         startState = group_map_[current_group_name_].getState(StartPosition);
00517         endState = group_map_[current_group_name_].getState(EndPosition);
00518 
00519         if(startState != NULL)
00520         {
00521           startState->getKinematicStateValues(startStateValues);
00522         }
00523         if(endState != NULL)
00524         {
00525           endState->getKinematicStateValues(endStateValues);
00526         }
00527       }
00528 
00529       deleteKinematicStates();
00530 
00531 
00532       if(robot_state_ != NULL)
00533       {
00534         ROS_INFO("Reverting planning scene to default.");
00535         cm_->revertPlanningScene(robot_state_);
00536         robot_state_ = NULL;
00537       }
00538 
00539       if(!set_planning_scene_diff_client_.call(planning_scene_req, planning_scene_res))
00540       {
00541         ROS_WARN("Can't get planning scene");
00542         lock_.unlock();
00543         return;
00544       }
00545 
00546       robot_state_ = cm_->setPlanningScene(planning_scene_res.planning_scene);
00547 
00548       if(robot_state_ == NULL)
00549       {
00550         ROS_ERROR("Something wrong with planning scene");
00551         lock_.unlock();
00552         return;
00553       }
00554 
00555       if(current_group_name_ != "")
00556       {
00557         ROS_INFO("Resetting state...");
00558         group_map_[current_group_name_].setState(StartPosition, new KinematicState(robot_state_->getKinematicModel()));
00559         group_map_[current_group_name_].setState(EndPosition, new KinematicState(robot_state_->getKinematicModel()));
00560         startState = group_map_[current_group_name_].getState(StartPosition);
00561         endState = group_map_[current_group_name_].getState(EndPosition);
00562 
00563         if(startState != NULL)
00564         {          
00565           ROS_INFO("Resetting start state.");
00566           startState->setKinematicState(startStateValues);
00567         }
00568 
00569         if(endState != NULL)
00570         {
00571           ROS_INFO("Resetting end state.");
00572           endState->setKinematicState(endStateValues);
00573         }
00574       }
00575       lock_.unlock();
00576       ROS_INFO("Planning scene sent.");
00577     }
00578 
00579     void selectPlanningGroup(unsigned int entry)
00580     {
00581       ROS_INFO("Selecting planning group %u", entry);
00582       lock_.lock();
00583       vector<string> names;
00584       for(map<string, GroupCollection>::iterator it = group_map_.begin(); it != group_map_.end(); it++)
00585       {
00586         names.push_back(it->first);
00587       }
00588       string old_group_name = current_group_name_;
00589 
00590       current_group_name_ = names[entry];
00591 
00592       if(group_map_[current_group_name_].end_state_ != NULL)
00593       {
00594         ROS_WARN_STREAM("Selecting with non NULL");
00595         // group_map_[current_group_name_].setState(EndPosition,
00596         //                                          new KinematicState(*group_map_[current_group_name_].end_state_));
00597       }
00598       else
00599       {
00600         group_map_[current_group_name_].setState(EndPosition, new KinematicState(*robot_state_));
00601       }
00602 
00603       if(group_map_[current_group_name_].start_state_ != NULL)
00604       {
00605         ROS_WARN_STREAM("Selecting with non NULL");
00606         // group_map_[current_group_name_].setState(EndPosition,
00607         //                                          new KinematicState(*group_map_[current_group_name_].start_state_));
00608       }
00609       else
00610       {
00611         group_map_[current_group_name_].setState(StartPosition, new KinematicState(*robot_state_));
00612       }
00613 
00614       moveEndEffectorMarkers(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false);
00615 
00616       // If we previously selected a planning group, deselect its marker.
00617       if(old_group_name != "" && selectableMarkerExists(old_group_name + "_selectable"))
00618       {
00619         GroupCollection& gc = group_map_[old_group_name];
00620         tf::Transform cur = robot_state_->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform();
00621         deselectMarker(selectable_markers_[old_group_name + "_selectable"], cur);
00622 
00623         if(is_joint_control_active_)
00624         {
00625           deleteJointMarkers(group_map_[old_group_name]);
00626         }
00627       }
00628 
00629       // Select the new planning group's marker.
00630       if(is_ik_control_active_ && selectableMarkerExists(current_group_name_ + "_selectable"))
00631       {
00632         GroupCollection& gc = group_map_[current_group_name_];
00633         tf::Transform cur = gc.getState(ik_control_type_)->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform();
00634         selectMarker(selectable_markers_[current_group_name_ + "_selectable"], cur);
00635         createSelectableJointMarkers(gc);
00636       }
00637       else if(is_joint_control_active_)
00638       {
00639         GroupCollection& gc = group_map_[current_group_name_];
00640         createSelectableJointMarkers(gc);
00641       }
00642       interactive_marker_server_->erase(current_group_name_ + "_selectable");
00643       interactive_marker_server_->applyChanges();
00644       lock_.unlock();
00645       ROS_INFO("Planning group selected.");
00646     }
00647 
00648     bool isValidJointName(GroupCollection& gc, string name)
00649     {
00650       for(size_t i = 0; i < gc.joint_names_.size(); i++)
00651       {
00652         if(name == gc.joint_names_[i])
00653         {
00654           return true;
00655         }
00656       }
00657       return false;
00658     }
00659 
00660     void moveEndEffectorMarkers(double vx, double vy, double vz, double vr, double vp, double vw,
00661                                 bool coll_aware = true)
00662 
00663     {
00664       lock_.lock();
00665       GroupCollection& gc = group_map_[current_group_name_];
00666       tf::Transform cur = gc.getState(ik_control_type_)->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform();
00667       double mult = CONTROL_SPEED / 100.0;
00668 
00669       tf::Vector3& curOrigin = cur.getOrigin();
00670       tf::Vector3 newOrigin(curOrigin.x() + (vx * mult), curOrigin.y() + (vy * mult), curOrigin.z() + (vz * mult));
00671       cur.setOrigin(newOrigin);
00672 
00673       tfScalar roll, pitch, yaw;
00674 
00675       cur.getBasis().getRPY(roll, pitch, yaw);
00676       roll += vr * mult;
00677       pitch += vp * mult;
00678       yaw += vw * mult;
00679 
00680       if(roll > 2 * M_PI)
00681       {
00682         roll -= 2 * M_PI;
00683       }
00684       else if(roll < -2 * M_PI)
00685       {
00686         roll += 2 * M_PI;
00687       }
00688 
00689       if(pitch > 2 * M_PI)
00690       {
00691         pitch -= 2 * M_PI;
00692       }
00693       else if(pitch < -2 * M_PI)
00694       {
00695         pitch += 2 * M_PI;
00696       }
00697 
00698       cur.getBasis().setRPY(roll, pitch, yaw);
00699 
00700       setNewEndEffectorPosition(gc, cur, coll_aware);
00701 
00702       lock_.unlock();
00703     }
00704 
00705     void deleteJointMarkers(GroupCollection& gc)
00706     {
00707       for(size_t i = 0; i < gc.joint_names_.size(); i++)
00708       {
00709         interactive_marker_server_->erase(gc.joint_names_[i] + "_joint_control");
00710       }
00711     }
00712 
00717     void createSelectableJointMarkers(GroupCollection& gc)
00718     {
00719       if(!is_joint_control_active_)
00720       {
00721         return;
00722       }
00723 
00724       // For each joint model, find the location of its axis and make a control there.
00725       for(size_t i = 0; i < gc.joint_names_.size(); i++)
00726       {
00727         const string& jointName = gc.joint_names_[i];
00728         KinematicModel::JointModel* model =
00729             (KinematicModel::JointModel*)(gc.getState(ik_control_type_)->getKinematicModel()->getJointModel(jointName));
00730         KinematicModel::RevoluteJointModel* revoluteJoint = dynamic_cast<KinematicModel::RevoluteJointModel*> (model);
00731         KinematicModel::PrismaticJointModel* prismaticJoint =
00732             dynamic_cast<KinematicModel::PrismaticJointModel*> (model);
00733 
00734         joint_clicked_map_[jointName + "_joint_control"] = false;
00735 
00736         if(model->getParentLinkModel() != NULL)
00737         {
00738           string parentLinkName = model->getParentLinkModel()->getName();
00739           string childLinkName = model->getChildLinkModel()->getName();
00740           tf::Transform
00741               transform =
00742                   gc.getState(ik_control_type_)->getLinkState(parentLinkName)->getGlobalLinkTransform()
00743                       * (gc.getState(ik_control_type_)->getKinematicModel()->getLinkModel(childLinkName)->getJointOriginTransform()
00744                           * (gc.getState(ik_control_type_)->getJointState(jointName)->getVariableTransform()));
00745 
00746           joint_prev_transform_map_[jointName + "joint_control"] = transform;
00747 
00748           const shapes::Shape* linkShape = model->getChildLinkModel()->getLinkShape();
00749           const shapes::Mesh* meshShape = dynamic_cast<const shapes::Mesh*> (linkShape);
00750 
00751           double maxDimension = 0.0f;
00752           if(meshShape != NULL)
00753           {
00754             for(unsigned int i = 0; i < meshShape->vertexCount; i++)
00755             {
00756               double x = meshShape->vertices[3 * i];
00757               double y = meshShape->vertices[3 * i];
00758               double z = meshShape->vertices[3 * i];
00759 
00760               if(abs(maxDimension) < abs(sqrt(x*x+y*y+z*z)))
00761               {
00762                 maxDimension = abs(x);
00763               }
00764 
00765             }
00766 
00767             maxDimension *= 3.0;
00768 
00769             maxDimension = max(0.15, maxDimension);
00770             maxDimension = min(0.5, maxDimension);
00771           }
00772           else
00773           {
00774             maxDimension = 0.15;
00775           }
00776 
00777           if(revoluteJoint != NULL)
00778           {
00779             makeInteractive1DOFRotationMarker(transform,
00780                                               revoluteJoint->axis_,
00781                                               model->getName() + "_joint_control",
00782                                               "",
00783                                               (float)maxDimension,
00784                                               gc.getState(ik_control_type_)->getJointState(jointName)->getJointStateValues()[0]);
00785           }
00786           else if(prismaticJoint != NULL)
00787           {
00788             maxDimension *= 3.0;
00789             makeInteractive1DOFTranslationMarker(transform,
00790                                                  prismaticJoint->axis_,
00791                                                  model->getName() + "_joint_control",
00792                                                  "",
00793                                                  (float)maxDimension,
00794                                                  gc.getState(ik_control_type_)-> getJointState(jointName)->getJointStateValues()[0]);
00795           }
00796 
00797         }
00798       }
00799       interactive_marker_server_->applyChanges();
00800     }
00801 
00807   void setJointState(GroupCollection& gc, std::string& joint_name, tf::Transform value)
00808     {
00809 
00810 
00811       KinematicState* current_state = gc.getState(ik_control_type_);
00812       string parent_link =
00813           gc.getState(ik_control_type_)->getKinematicModel()->getJointModel(joint_name)->getParentLinkModel()->getName();
00814       string child_link =
00815           gc.getState(ik_control_type_)->getKinematicModel()->getJointModel(joint_name)->getChildLinkModel()->getName();
00816       KinematicState::JointState* joint_state = current_state->getJointState(joint_name);
00817       const KinematicModel::JointModel* joint_model = joint_state->getJointModel();
00818 
00819       const KinematicModel::RevoluteJointModel* rev_model = dynamic_cast<const KinematicModel::RevoluteJointModel*>(joint_model);
00820 
00821       bool is_rotational = (rev_model != NULL);
00822       bool is_continuous = is_rotational && rev_model->continuous_;
00823       bool is_prismatic = (dynamic_cast<const KinematicModel::PrismaticJointModel*>(joint_model) != NULL);
00824 
00825       KinematicState::LinkState* link_state = current_state->getLinkState(parent_link);
00826       tf::Transform transformed_value;
00827 
00828       if(is_prismatic)
00829       {
00830         value.setRotation(joint_state->getVariableTransform().getRotation());
00831         transformed_value = current_state->getLinkState(child_link)->getLinkModel()->getJointOriginTransform().inverse() * link_state->getGlobalLinkTransform().inverse() * value;
00832       }
00833       else if(is_rotational)
00834       {
00835         transformed_value = current_state->getLinkState(child_link)->getLinkModel()->getJointOriginTransform().inverse() * link_state->getGlobalLinkTransform().inverse() * value;
00836       }
00837       double old_actual_value = joint_state->getJointStateValues()[0];
00838       double old_value;
00839       if(prev_joint_control_value_map_.find(joint_name) == prev_joint_control_value_map_.end()) {
00840         old_value = old_actual_value;
00841       } else {
00842         old_value = prev_joint_control_value_map_[joint_name];
00843       }
00844 
00845       joint_state->setJointStateValues(transformed_value);
00846       double new_value = joint_state->getJointStateValues()[0];
00847       prev_joint_control_value_map_[joint_name] = new_value;
00848 
00849       double low_bound, high_bound;
00850       joint_state->getJointValueBounds(joint_state->getName(), low_bound, high_bound);
00851 
00852       double apply_diff;
00853 
00854       if(is_rotational) {
00855         //more neg
00856         if(old_value < -M_PI/2.0 && new_value > M_PI/2.0) {
00857           apply_diff = -((-M_PI-old_value)+(M_PI-new_value));
00858         } else if(old_value > M_PI/2.0 && new_value < -M_PI/2.0) {
00859           apply_diff = (M_PI-old_value)+(-M_PI-new_value);
00860         } else {
00861           apply_diff = new_value-old_value;
00862         }
00863         ROS_DEBUG_STREAM("Old value " << old_value << " new value " << new_value << " apply diff " << apply_diff);
00864         double apply_value;
00865         if(!is_continuous) {
00866           if(apply_diff > 0.0) {
00867             apply_value = fmin(old_actual_value+apply_diff, high_bound);
00868           } else {
00869             apply_value = fmax(old_actual_value+apply_diff, low_bound);
00870           }
00871         } else {
00872           apply_value = old_actual_value+apply_diff;
00873         }
00874         std::vector<double> val_vec(1, apply_value);
00875         joint_state->setJointStateValues(val_vec);
00876       } else {
00877         std::vector<double> val_vec;
00878         if(!current_state->isJointWithinBounds(joint_name))
00879         {
00880           if(new_value < low_bound) {
00881             val_vec.push_back(low_bound);
00882           } else if(new_value > high_bound) {
00883             val_vec.push_back(high_bound);
00884           } else {
00885             val_vec.push_back(old_value);
00886             ROS_INFO_STREAM("Weird bounds behavior " << new_value << " low " << low_bound << " high " << high_bound);
00887           }
00888           ROS_DEBUG_STREAM("New val " << new_value << " low bound " << low_bound << " high bound " << high_bound << " res " << val_vec[0]);
00889           joint_state->setJointStateValues(val_vec);
00890         }
00891       }
00892       map<string, double> state_map;
00893       current_state->getKinematicStateValues(state_map);
00894       current_state->setKinematicState(state_map);
00895       robot_state_->setKinematicState(state_map);
00896       // Send state to robot model.
00897       updateJointStates(gc);
00898 
00899       createSelectableJointMarkers(gc);
00900       
00901       if(is_ik_control_active_)
00902       {
00903         selectMarker(selectable_markers_[current_group_name_ + "_selectable"],
00904                      current_state->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform());
00905       }
00906     }
00907 
00908     void resetToLastGoodState(GroupCollection& gc)
00909     {
00910       setNewEndEffectorPosition(gc, gc.last_good_state_, collision_aware_);
00911       if(is_ik_control_active_)
00912       {
00913         selectMarker(selectable_markers_[current_group_name_ + "_selectable"],
00914                      gc.getState(ik_control_type_)->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform());
00915       }
00916     }
00917 
00918     void setNewEndEffectorPosition(GroupCollection& gc, tf::Transform& cur, bool coll_aware)
00919     {
00920       if(!gc.getState(ik_control_type_)->updateKinematicStateWithLinkAt(gc.ik_link_name_, cur))
00921       {
00922         ROS_INFO("Problem");
00923       }
00924 
00925       if(solveIKForEndEffectorPose(gc, coll_aware, constrain_rp_))
00926       {
00927         gc.good_ik_solution_ = true;
00928         gc.last_good_state_ = cur;
00929       }
00930       else
00931       {
00932         gc.good_ik_solution_ = false;
00933       }
00934     }
00935 
00936   void determinePitchRollConstraintsGivenState(const PlanningComponentsVisualizer::GroupCollection& gc,
00937                                                const planning_models::KinematicState& state,
00938                                                arm_navigation_msgs::OrientationConstraint& goal_constraint,
00939                                                arm_navigation_msgs::OrientationConstraint& path_constraint) const
00940   {
00941     tf::Transform cur = state.getLinkState(gc.ik_link_name_)->getGlobalLinkTransform();
00942     //tfScalar roll, pitch, yaw;
00943     //cur.getBasis().getRPY(roll,pitch,yaw);
00944     goal_constraint.header.frame_id = cm_->getWorldFrameId();
00945     goal_constraint.header.stamp = ros::Time::now();
00946     goal_constraint.link_name = gc.ik_link_name_;
00947     tf::quaternionTFToMsg(cur.getRotation(), goal_constraint.orientation);
00948     goal_constraint.absolute_roll_tolerance = 0.04;
00949     goal_constraint.absolute_pitch_tolerance = 0.04;
00950     goal_constraint.absolute_yaw_tolerance = M_PI;
00951     path_constraint.header.frame_id = cm_->getWorldFrameId();
00952     path_constraint.header.stamp = ros::Time::now();
00953     path_constraint.link_name = gc.ik_link_name_;
00954     tf::quaternionTFToMsg(cur.getRotation(), path_constraint.orientation);
00955     path_constraint.type = path_constraint.HEADER_FRAME;
00956     path_constraint.absolute_roll_tolerance = 0.1;
00957     path_constraint.absolute_pitch_tolerance = 0.1;
00958     path_constraint.absolute_yaw_tolerance = M_PI;
00959   }
00960 
00961 
00962   bool solveIKForEndEffectorPose(PlanningComponentsVisualizer::GroupCollection& gc, bool coll_aware = true,
00963                                  bool constrain_pitch_and_roll = false, double change_redundancy = 0.0)
00964     {
00965       kinematics_msgs::PositionIKRequest ik_request;
00966       ik_request.ik_link_name = gc.ik_link_name_;
00967       ik_request.pose_stamped.header.frame_id =  cm_->getWorldFrameId();
00968       ik_request.pose_stamped.header.stamp = ros::Time::now();
00969       tf::poseTFToMsg(gc.getState(ik_control_type_)->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform(),
00970                       ik_request.pose_stamped.pose);
00971       convertKinematicStateToRobotState(*gc.getState(ik_control_type_), ros::Time::now(), cm_->getWorldFrameId(),
00972                                         ik_request.robot_state);
00973       ik_request.ik_seed_state = ik_request.robot_state;
00974 
00975       // if(change_redundancy != 0.0) {
00976       //   for(unsigned int i = 0; i < ik_request.ik_seed_state.joint_state.name.size(); i++) {
00977       //     if(ik_request.ik_seed_state.joint_state.name[i] == redundancy_joint_) {
00978       //       ik_request.ik_seed_state.joint_state.position[i] += change_redundancy;
00979       //     }
00980       //   }
00981       // }
00982       map<string, double> joint_values;
00983       vector<string> joint_names;
00984 
00985       if(coll_aware)
00986       {
00987         kinematics_msgs::GetConstraintAwarePositionIK::Request ik_req;
00988         kinematics_msgs::GetConstraintAwarePositionIK::Response ik_res;
00989         if(constrain_pitch_and_roll) {
00990           IKControlType other_state;
00991           if(ik_control_type_ == EndPosition)
00992           {
00993             other_state = StartPosition;
00994           }
00995           else
00996           {
00997             other_state = EndPosition;
00998           }
00999           arm_navigation_msgs::Constraints goal_constraints;
01000           goal_constraints.orientation_constraints.resize(1);
01001           arm_navigation_msgs::Constraints path_constraints;
01002           path_constraints.orientation_constraints.resize(1);
01003           determinePitchRollConstraintsGivenState(gc,
01004                                                   *gc.getState(other_state),
01005                                                   goal_constraints.orientation_constraints[0],
01006                                                   path_constraints.orientation_constraints[0]);
01007           arm_navigation_msgs::ArmNavigationErrorCodes err;
01008           if(!cm_->isKinematicStateValid(*gc.getState(ik_control_type_),
01009                                          std::vector<std::string>(),
01010                                          err,
01011                                          goal_constraints,
01012                                          path_constraints)) {
01013             ROS_INFO_STREAM("Violates rp constraints");
01014             return false;
01015           }
01016           ik_req.constraints = goal_constraints;
01017         }
01018         ik_req.ik_request = ik_request;
01019         ik_req.timeout = ros::Duration(0.2);
01020         if(!gc.coll_aware_ik_service_.call(ik_req, ik_res))
01021         {
01022           ROS_INFO("Problem with ik service call");
01023           return false;
01024         }
01025         if(ik_res.error_code.val != ik_res.error_code.SUCCESS)
01026         {
01027           ROS_DEBUG_STREAM("Call yields bad error code " << ik_res.error_code.val);
01028           return false;
01029         }
01030         joint_names = ik_res.solution.joint_state.name;
01031         gc.joint_names_.clear();
01032         gc.joint_names_ = joint_names;
01033         for(unsigned int i = 0; i < ik_res.solution.joint_state.name.size(); i++)
01034         {
01035           joint_values[ik_res.solution.joint_state.name[i]] = ik_res.solution.joint_state.position[i];
01036         }
01037 
01038       }
01039       else
01040       {
01041         kinematics_msgs::GetPositionIK::Request ik_req;
01042         kinematics_msgs::GetPositionIK::Response ik_res;
01043         ik_req.ik_request = ik_request;
01044         ik_req.timeout = ros::Duration(0.2);
01045         if(!gc.non_coll_aware_ik_service_.call(ik_req, ik_res))
01046         {
01047           ROS_INFO("Problem with ik service call");
01048           return false;
01049         }
01050         if(ik_res.error_code.val != ik_res.error_code.SUCCESS)
01051         {
01052           ROS_DEBUG_STREAM("Call yields bad error code " << ik_res.error_code.val);
01053           return false;
01054         }
01055         for(unsigned int i = 0; i < ik_res.solution.joint_state.name.size(); i++)
01056         {
01057           joint_values[ik_res.solution.joint_state.name[i]] = ik_res.solution.joint_state.position[i];
01058         }
01059 
01060       }
01061 
01062       lock_.lock();
01063       gc.getState(ik_control_type_)->setKinematicState(joint_values);
01064       lock_.unlock();
01065 
01066       createSelectableJointMarkers(gc);
01067       if(coll_aware)
01068       {
01069         Constraints emp_con;
01070         ArmNavigationErrorCodes error_code;
01071 
01072         if(!cm_->isKinematicStateValid(*gc.getState(ik_control_type_), joint_names, error_code, emp_con, emp_con, true))
01073         {
01074           ROS_INFO_STREAM("Problem with response");
01075         }
01076       }
01077 
01078       updateJointStates(gc);
01079 
01080       return true;
01081     }
01082 
01083 
01088     void updateJointStates(GroupCollection& gc)
01089     {
01090       sensor_msgs::JointState msg;
01091       msg.header.frame_id =  cm_->getWorldFrameId();
01092       msg.header.stamp = ros::Time::now();
01093 
01094       vector<KinematicState::JointState*> jointStates = gc.getState(ik_control_type_)->getJointStateVector();
01095 
01096       map<string, double> stateMap;
01097       gc.getState(ik_control_type_)->getKinematicStateValues(stateMap);
01098       robot_state_->setKinematicState(stateMap);
01099 
01100       for(size_t i = 0; i < jointStates.size(); i++)
01101       {
01102         KinematicState::JointState* state = jointStates[i];
01103         msg.name.push_back(state->getName());
01104 
01105         // Assume that joints only have one value.
01106         if(state->getJointStateValues().size() > 0)
01107         {
01108           msg.position.push_back(state->getJointStateValues()[0]);
01109         }
01110         else
01111         {
01112           msg.position.push_back(0.0f);
01113         }
01114       }
01115       joint_state_lock_.lock();
01116       last_joint_state_msg_ = msg;
01117       joint_state_lock_.unlock();
01118 
01119     }
01120   void publishJointStates() {
01121     joint_state_lock_.lock();
01122     last_joint_state_msg_.header.frame_id =  cm_->getWorldFrameId();
01123     last_joint_state_msg_.header.stamp = ros::Time::now();
01124     joint_state_publisher_.publish(last_joint_state_msg_);
01125     joint_state_lock_.unlock();
01126 
01127     if(cm_->getWorldFrameId() != cm_->getRobotFrameId()) {
01128       TransformStamped trans;
01129       trans.header.frame_id = cm_->getWorldFrameId();
01130       trans.header.stamp = ros::Time::now();
01131       trans.child_frame_id = cm_->getRobotFrameId();
01132       trans.transform.rotation.w = 1.0;
01133       transform_broadcaster_.sendTransform(trans);
01134     }
01135   }
01136 
01137     bool planToEndEffectorState(PlanningComponentsVisualizer::GroupCollection& gc)
01138     {
01139       MotionPlanRequest motion_plan_request;
01140       motion_plan_request.group_name = gc.name_;
01141       motion_plan_request.num_planning_attempts = 1;
01142       motion_plan_request.allowed_planning_time = PLANNING_DURATION;
01143       if(!constrain_rp_) {
01144         const KinematicState::JointStateGroup* jsg = gc.getState(EndPosition)->getJointStateGroup(gc.name_);
01145         motion_plan_request.goal_constraints.joint_constraints.resize(jsg->getJointNames().size());
01146         vector<double> joint_values;
01147         jsg->getKinematicStateValues(joint_values);
01148         for(unsigned int i = 0; i < jsg->getJointNames().size(); i++)
01149         {
01150           motion_plan_request.goal_constraints.joint_constraints[i].joint_name = jsg->getJointNames()[i];
01151           motion_plan_request.goal_constraints.joint_constraints[i].position = joint_values[i];
01152           motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.01;
01153           motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.01;
01154         }
01155       } else {
01156         motion_plan_request.group_name += "_cartesian";
01157         motion_plan_request.goal_constraints.position_constraints.resize(1);
01158         motion_plan_request.goal_constraints.orientation_constraints.resize(1);    
01159         geometry_msgs::PoseStamped end_effector_wrist_pose;
01160         tf::poseTFToMsg(gc.getState(EndPosition)->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform(),
01161                         end_effector_wrist_pose.pose);
01162         end_effector_wrist_pose.header.frame_id = cm_->getWorldFrameId();
01163         arm_navigation_msgs::poseStampedToPositionOrientationConstraints(end_effector_wrist_pose,
01164                                                                           gc.ik_link_name_,
01165                                                                           motion_plan_request.goal_constraints.position_constraints[0],
01166                                                                           motion_plan_request.goal_constraints.orientation_constraints[0]);
01167         motion_plan_request.path_constraints.orientation_constraints.resize(1);
01168         determinePitchRollConstraintsGivenState(gc,
01169                                                 *gc.getState(StartPosition),
01170                                                 motion_plan_request.goal_constraints.orientation_constraints[0],
01171                                                 motion_plan_request.path_constraints.orientation_constraints[0]);
01172       }
01173       convertKinematicStateToRobotState(*gc.getState(StartPosition), ros::Time::now(), cm_->getWorldFrameId(),
01174                                         motion_plan_request.start_state);
01175       GetMotionPlan::Request plan_req;
01176       plan_req.motion_plan_request = motion_plan_request;
01177       GetMotionPlan::Response plan_res;
01178       if(!planner_service_client_.call(plan_req, plan_res))
01179       {
01180         ROS_INFO("Something wrong with planner client");
01181         return false;
01182       }
01183 
01184       if(gc.state_trajectory_display_map_.find("planner") != gc.state_trajectory_display_map_.end())
01185       {
01186         StateTrajectoryDisplay& disp = gc.state_trajectory_display_map_["planner"];
01187         if(plan_res.error_code.val != plan_res.error_code.SUCCESS)
01188         {
01189           disp.trajectory_error_code_ = plan_res.error_code;
01190           ROS_INFO_STREAM("Bad planning error code " << plan_res.error_code.val);
01191           gc.state_trajectory_display_map_["planner"].reset();
01192           return false;
01193         }
01194         last_motion_plan_request_ = motion_plan_request;
01195         playTrajectory(gc, "planner", plan_res.trajectory.joint_trajectory);
01196         return true;
01197       }
01198       else
01199       {
01200         return false;
01201       }
01202     }
01203 
01204     void randomlyPerturb(PlanningComponentsVisualizer::GroupCollection& gc)
01205     {
01206       tf::Transform currentPose = gc.getState(ik_control_type_)->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform();
01207 
01208       int maxTries = 10;
01209       int numTries = 0;
01210       bool found = false;
01211       double posVariance = 0.5;
01212       double angleVariance = 0.5;
01213 
01214       while(!found && numTries < maxTries)
01215       {
01216 
01217         double xVar = posVariance*((double)random()/(double)RAND_MAX) - posVariance/2.0;
01218         double yVar = posVariance*((double)random()/(double)RAND_MAX) - posVariance/2.0;
01219         double zVar = posVariance*((double)random()/(double)RAND_MAX) - posVariance/2.0;
01220 
01221         double xAngleVar = angleVariance*((double)random()/(double)RAND_MAX) - angleVariance/2.0;
01222         double yAngleVar = angleVariance*((double)random()/(double)RAND_MAX) - angleVariance/2.0;
01223         double zAngleVar = angleVariance*((double)random()/(double)RAND_MAX) - angleVariance/2.0;
01224 
01225         double x = currentPose.getOrigin().x() + xVar;
01226         double y = currentPose.getOrigin().y() + yVar;
01227         double z = currentPose.getOrigin().z() + zVar;
01228 
01229         double xA = currentPose.getRotation().x() + xAngleVar;
01230         double yA = currentPose.getRotation().y() + yAngleVar;
01231         double zA = currentPose.getRotation().z() + zAngleVar;
01232 
01233         tf::Vector3 newPos(x,y,z);
01234         tf::Quaternion newOrient(xA,yA,zA,1.0);
01235         tf::Transform newTrans(newOrient,newPos);
01236 
01237         setNewEndEffectorPosition(gc, newTrans, collision_aware_);
01238         if(gc.good_ik_solution_)
01239         {
01240           found = true;
01241           if(is_ik_control_active_)
01242           {
01243             selectMarker(selectable_markers_[current_group_name_ + "_selectable"],
01244                          gc.getState(ik_control_type_)->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform());
01245           }
01246         }
01247 
01248         numTries ++;
01249         posVariance *= 1.1;
01250       }
01251     }
01252 
01253     bool filterPlannerTrajectory(PlanningComponentsVisualizer::GroupCollection& gc)
01254     {
01255       FilterJointTrajectoryWithConstraints::Request filter_req;
01256       FilterJointTrajectoryWithConstraints::Response filter_res;
01257 
01258       convertKinematicStateToRobotState(*gc.getState(StartPosition), ros::Time::now(), cm_->getWorldFrameId(),
01259                                         filter_req.start_state);
01260       StateTrajectoryDisplay& planner_disp = gc.state_trajectory_display_map_["planner"];
01261       filter_req.trajectory = planner_disp.joint_trajectory_;
01262       filter_req.group_name = gc.name_;
01263 
01264       filter_req.goal_constraints = last_motion_plan_request_.goal_constraints;
01265       filter_req.path_constraints = last_motion_plan_request_.path_constraints;
01266       filter_req.allowed_time = ros::Duration(2.0);
01267 
01268       if(!trajectory_filter_service_client_.call(filter_req, filter_res))
01269       {
01270         ROS_INFO("Problem with trajectory filter");
01271         gc.state_trajectory_display_map_["filter"].reset();
01272         return false;
01273       }
01274       StateTrajectoryDisplay& filter_disp = gc.state_trajectory_display_map_["filter"];
01275       if(filter_res.error_code.val != filter_res.error_code.SUCCESS)
01276       {
01277         filter_disp.trajectory_error_code_ = filter_res.error_code;
01278         ROS_INFO_STREAM("Bad trajectory_filter error code " << filter_res.error_code.val);
01279         gc.state_trajectory_display_map_["filter"].reset();
01280         return false;
01281       }
01282       playTrajectory(gc, "filter", filter_res.trajectory);
01283       return true;
01284     }
01285 
01286     bool playTrajectory(PlanningComponentsVisualizer::GroupCollection& gc, const string& source_name,
01287                         const trajectory_msgs::JointTrajectory& traj)
01288     {
01289       lock_.lock();
01290       if(gc.state_trajectory_display_map_.find(source_name) == gc.state_trajectory_display_map_.end())
01291       {
01292         ROS_INFO_STREAM("No state display for group " << gc.name_ << " source name " << source_name);
01293         lock_.unlock();
01294         return false;
01295       }
01296       StateTrajectoryDisplay& disp = gc.state_trajectory_display_map_[source_name];
01297       disp.reset();
01298       disp.joint_trajectory_ = traj;
01299       disp.has_joint_trajectory_ = true;
01300       disp.show_joint_trajectory_ = true;
01301       disp.play_joint_trajectory_ = true;
01302       disp.state_ = new KinematicState(*robot_state_);
01303       vector<ArmNavigationErrorCodes> trajectory_error_codes;
01304 
01305       cm_->isJointTrajectoryValid(*disp.state_, disp.joint_trajectory_, last_motion_plan_request_.goal_constraints,
01306                                   last_motion_plan_request_.path_constraints, disp.trajectory_error_code_,
01307                                   trajectory_error_codes, false);
01308 
01309       if(disp.trajectory_error_code_.val != disp.trajectory_error_code_.SUCCESS)
01310       {
01311         disp.trajectory_bad_point_ = trajectory_error_codes.size() - 1;
01312       }
01313       else
01314       {
01315         disp.trajectory_bad_point_ = -1;
01316       }
01317 
01318       moveThroughTrajectory(gc, source_name, 0);
01319       lock_.unlock();
01320       return true;
01321     }
01322 
01323     void moveThroughTrajectory(PlanningComponentsVisualizer::GroupCollection& gc, const string& source_name, int step)
01324     {
01325       lock_.lock();
01326       StateTrajectoryDisplay& disp = gc.state_trajectory_display_map_[source_name];
01327       unsigned int tsize = disp.joint_trajectory_.points.size();
01328       if(tsize == 0 || disp.state_ == NULL)
01329       {
01330         lock_.unlock();
01331         return;
01332       }
01333       if((int)disp.current_trajectory_point_ + step < 0)
01334       {
01335         disp.current_trajectory_point_ = 0;
01336       }
01337       else
01338       {
01339         disp.current_trajectory_point_ = ((int)disp.current_trajectory_point_) + step;
01340       }
01341       if(disp.current_trajectory_point_ >= tsize - 1)
01342       {
01343         disp.current_trajectory_point_ = tsize - 1;
01344         disp.play_joint_trajectory_ = false;
01345         disp.show_joint_trajectory_ = false;
01346       }
01347       map<string, double> joint_values;
01348       for(unsigned int i = 0; i < disp.joint_trajectory_.joint_names.size(); i++)
01349       {
01350         joint_values[disp.joint_trajectory_.joint_names[i]]
01351             = disp.joint_trajectory_.points[disp.current_trajectory_point_].positions[i];
01352       }
01353       disp.state_->setKinematicState(joint_values);
01354       lock_.unlock();
01355     }
01356 
01365     MenuHandler::EntryHandle registerSubMenuEntry(MenuHandler& handler, MenuEntryMap& map, string name,
01366                                                   MenuHandler::EntryHandle subMenuHandle)
01367     {
01368       MenuHandler::EntryHandle toReturn = handler.insert(subMenuHandle, name, process_function_ptr_);
01369       map[toReturn] = name;
01370       return toReturn;
01371     }
01372 
01380     MenuHandler::EntryHandle registerMenuEntry(MenuHandler& handler, MenuEntryMap& map, string name)
01381     {
01382       MenuHandler::EntryHandle toReturn = handler.insert(name, process_function_ptr_);
01383       map[toReturn] = name;
01384       return toReturn;
01385     }
01386 
01387     void sendMarkers()
01388     {
01389       lock_.lock();
01390       MarkerArray arr;
01391 
01392       std_msgs::ColorRGBA stat_color_;
01393       stat_color_.a = 1.0;
01394       stat_color_.r = 0.1;
01395       stat_color_.g = 0.8;
01396       stat_color_.b = 0.3;
01397 
01398       std_msgs::ColorRGBA attached_color_;
01399       attached_color_.a = 1.0;
01400       attached_color_.r = 0.6;
01401       attached_color_.g = 0.4;
01402       attached_color_.b = 0.3;
01403 
01404       cm_->getAllCollisionSpaceObjectMarkers(*robot_state_, arr, "", stat_color_, attached_color_, ros::Duration(0.1));
01405 
01406 
01407 
01408       if(!current_group_name_.empty())
01409       {
01410         std_msgs::ColorRGBA group_color;
01411         group_color.a = 0.3;
01412         group_color.r = 0.5;
01413         group_color.g = 0.9;
01414         group_color.b = 0.5;
01415 
01416         std_msgs::ColorRGBA updated_color;
01417         updated_color.a = 0.3;
01418         updated_color.r = 1.0;
01419         updated_color.g = 0.5;
01420         updated_color.b = 1.0;
01421 
01422         std_msgs::ColorRGBA bad_color;
01423         bad_color.a = 0.6;
01424         bad_color.r = 1.0;
01425         bad_color.g = 0.0;
01426         bad_color.b = 0.0;
01427 
01428         GroupCollection& gc = group_map_[current_group_name_];
01429         const KinematicModel* kinematic_model = cm_->getKinematicModel();
01430 
01431         IKControlType otherState;
01432         if(ik_control_type_ == EndPosition)
01433         {
01434           otherState = StartPosition;
01435         }
01436         else
01437         {
01438           otherState = EndPosition;
01439         }
01440 
01441         if(is_ik_control_active_)
01442         {
01443           if(gc.getState(otherState) != NULL)
01444           {
01445             cm_->getGroupAndUpdatedJointMarkersGivenState(*gc.getState(otherState), arr, current_group_name_, group_color,
01446                                                         updated_color, ros::Duration(0.1));
01447           }
01448           else
01449           {
01450             ROS_ERROR("Other state invalid!");
01451           }
01452         }
01453 
01454         if(!gc.good_ik_solution_ && gc.getState(ik_control_type_) != NULL)
01455         {
01456           vector<string> lnames =
01457               kinematic_model->getChildLinkModelNames(kinematic_model->getLinkModel(gc.ik_link_name_));
01458 
01459           cm_->getRobotMarkersGivenState(*gc.getState(ik_control_type_), arr, bad_color,
01460                                          current_group_name_, ros::Duration(0.1), &lnames);
01461           cm_->getAttachedCollisionObjectMarkers(*gc.getState(ik_control_type_), arr, current_group_name_, bad_color,
01462                                                  ros::Duration(.2));
01463 
01464         }
01465         for(map<string, StateTrajectoryDisplay>::iterator it = gc.state_trajectory_display_map_.begin(); it
01466             != gc.state_trajectory_display_map_.end(); it++)
01467         {
01468 
01469           if(it->second.play_joint_trajectory_)
01470           {
01471             moveThroughTrajectory(gc, it->first, 5);
01472           }
01473 
01474           if(it->second.show_joint_trajectory_)
01475           {
01476             const vector<const KinematicModel::LinkModel*>& updated_links =
01477                 kinematic_model->getModelGroup(gc.name_)->getUpdatedLinkModels();
01478             vector<string> lnames;
01479             lnames.resize(updated_links.size());
01480             for(unsigned int i = 0; i < updated_links.size(); i++)
01481             {
01482               lnames[i] = updated_links[i]->getName();
01483             }
01484             cm_->getRobotMarkersGivenState(*(it->second.state_), arr, it->second.color_,
01485                                            it->first + "_trajectory", ros::Duration(0.1), &lnames);
01486 
01487             cm_->getAttachedCollisionObjectMarkers(*(it->second.state_), arr, it->first + "_trajectory",
01488                                                    it->second.color_, ros::Duration(0.1));
01489           }
01490         }
01491       }
01492       vis_marker_array_publisher_.publish(arr);
01493       lock_.unlock();
01494     }
01495 
01496     void sendTransforms()
01497     {
01498       lock_.lock();
01499       // TODO: Is it necessary to publish interactive markers each update?
01500       interactive_marker_server_->applyChanges();
01501       lock_.unlock();
01502 
01503       // TODO: This was replaced by another node, the robot state publisher.
01504       /*
01505        lock_.lock();
01506        ros::WallTime cur_time = ros::WallTime::now();
01507        rosgraph_msgs::Clock c;
01508        c.clock.nsec = cur_time.nsec;
01509        c.clock.sec = cur_time.sec;
01510        vector<TransformStamped> trans_vector;
01511        getAllKinematicStateStampedTransforms(*robot_state_, trans_vector, c.clock);
01512        //transform_broadcaster_.sendTransform(trans_vector);
01513        lock_.unlock();
01514        */
01515     }
01516 
01523     Marker makeMarkerBox(InteractiveMarker &msg, float alpha = 1.0f)
01524     {
01525       Marker marker;
01526       marker.type = Marker::CUBE;
01527       // Scale is arbitrarily 1/4 of the marker's scale.
01528       marker.scale.x = msg.scale * 0.25;
01529       marker.scale.y = msg.scale * 0.25;
01530       marker.scale.z = msg.scale * 0.25;
01531       marker.color.r = 1.0;
01532       marker.color.g = 1.0;
01533       marker.color.b = 1.0;
01534       marker.color.a = alpha;
01535 
01536       return marker;
01537     }
01538 
01545     Marker makeMarkerCylinder(InteractiveMarker &msg, float alpha = 1.0f)
01546     {
01547       Marker marker;
01548       marker.type = Marker::CYLINDER;
01549       // Scale is arbitrary
01550       marker.scale.x = msg.scale * 0.11;
01551       marker.scale.y = msg.scale * 0.11;
01552       marker.scale.z = msg.scale * 1.1;
01553       marker.color.r = 0.2;
01554       marker.color.g = 0.9;
01555       marker.color.b = 0.2;
01556       marker.color.a = alpha;
01557 
01558       return marker;
01559     }
01560 
01566     Marker makeMarkerSphere(InteractiveMarker &msg)
01567     {
01568       Marker marker;
01569 
01570       marker.type = Marker::SPHERE;
01571       // Scale is arbitrary.
01572       marker.scale.x = msg.scale * 0.75;
01573       marker.scale.y = msg.scale * 0.75;
01574       marker.scale.z = msg.scale * 0.75;
01575       marker.color.r = 0.8;
01576       marker.color.g = 0.8;
01577       marker.color.b = 1.0;
01578       marker.color.a = 0.1;
01579 
01580       return marker;
01581     }
01582 
01589     InteractiveMarkerControl& makeInteractiveBoxControl(InteractiveMarker &msg, float alpha = 1.0f)
01590     {
01591       InteractiveMarkerControl control;
01592       control.always_visible = true;
01593       control.markers.push_back(makeMarkerBox(msg, alpha));
01594       msg.controls.push_back(control);
01595       return msg.controls.back();
01596     }
01597 
01604     InteractiveMarkerControl& makeInteractiveCylinderControl(InteractiveMarker &msg, float alpha = 1.0f)
01605     {
01606       InteractiveMarkerControl control;
01607       control.always_visible = true;
01608       control.markers.push_back(makeMarkerCylinder(msg, alpha));
01609       msg.controls.push_back(control);
01610       return msg.controls.back();
01611     }
01612 
01616     void refreshEnvironment()
01617     {
01618       GroupCollection& gc = group_map_[current_group_name_];
01619       sendPlanningScene();
01620       moveEndEffectorMarkers(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false);
01621 
01622       tf::Transform cur = toBulletTransform(last_ee_poses_[current_group_name_]);
01623       setNewEndEffectorPosition(gc, cur, collision_aware_);
01624     }
01625 
01631     void processInteractiveFeedback(const InteractiveMarkerFeedbackConstPtr &feedback)
01632     {
01633       GroupCollection& gc = group_map_[current_group_name_];
01634       switch (feedback->event_type)
01635       {
01636         case InteractiveMarkerFeedback::BUTTON_CLICK:
01637           if(feedback->marker_name.rfind("_selectable") != string::npos)
01638           {
01639             if(feedback->marker_name == current_group_name_) {
01640               return;
01641             } 
01642             tf::Transform cur = toBulletTransform(feedback->pose);
01643             if(feedback->marker_name.rfind("pole_") != string::npos)
01644             {
01645               selectMarker(selectable_markers_[feedback->marker_name], cur);
01646             } else {
01647               deselectMarker(selectable_markers_[current_group_name_ + "_selectable"],
01648                              gc.getState(ik_control_type_)->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform());
01649               if(isGroupName(feedback->marker_name.substr(0, feedback->marker_name.rfind("_selectable"))))
01650               {
01651                 unsigned int cmd = 0;
01652                 for(map<string, GroupCollection>::iterator it = group_map_.begin(); it != group_map_.end(); it++)
01653                 {
01654                   
01655                   if(feedback->marker_name.substr(0, feedback->marker_name.rfind("_selectable")) == it->first)
01656                   {
01657                     deleteKinematicStates();
01658                     selectPlanningGroup(cmd);
01659                     break;
01660                   } else {
01661                     
01662                   }
01663                   cmd++;
01664                 }
01665               }
01666             }
01667           }
01668           break;
01669 
01670         case InteractiveMarkerFeedback::MENU_SELECT:
01671           MenuHandler::EntryHandle handle;
01672           if(feedback->marker_name.rfind("_selectable") != string::npos)
01673           {
01674             tf::Transform cur = toBulletTransform(feedback->pose);
01675             if(is_ik_control_active_
01676                 && isGroupName(feedback->marker_name.substr(0, feedback->marker_name.rfind("_selectable"))))
01677             {
01678               handle = feedback->menu_entry_id;
01679 
01680               unsigned int cmd = 0;
01681               for(map<string, GroupCollection>::iterator it = group_map_.begin(); it != group_map_.end(); it++)
01682               {
01683 
01684                 if(feedback->marker_name.substr(0, feedback->marker_name.rfind("_selectable")) == it->first)
01685                 {
01686                   deleteKinematicStates();
01687                   selectPlanningGroup(cmd);
01688                   break;
01689                 }
01690                 cmd++;
01691               }
01692 
01693             }
01694             else if(feedback->marker_name.rfind("pole_") != string::npos)
01695             {
01696               handle = feedback->menu_entry_id;
01697 
01698               if(menu_entry_maps_["Collision Object Selection"][handle] == "Select")
01699               {
01700                 selectMarker(selectable_markers_[feedback->marker_name], cur);
01701               }
01702               else if(menu_entry_maps_["Collision Object Selection"][handle] == "Delete")
01703               {
01704                 this->removeCollisionPoleByName(
01705                                                 feedback->marker_name.substr(0,
01706                                                                              feedback->marker_name.rfind("_selectable")));
01707                 interactive_marker_server_->erase(feedback->marker_name);
01708                 refreshEnvironment();
01709               }
01710             }
01711           }
01712           else if(feedback->marker_name == "top_level")
01713           {
01714             handle = feedback->menu_entry_id;
01715             if(handle == ik_control_handle_)
01716             {
01717               MenuHandler::CheckState currentState;
01718               menu_handler_map_["Top Level"].getCheckState(ik_control_handle_, currentState);
01719 
01720               if(currentState == MenuHandler::UNCHECKED)
01721               {
01722                 is_ik_control_active_ = true;
01723                 menu_handler_map_["Top Level"].setCheckState(ik_control_handle_, MenuHandler::CHECKED);
01724                 menu_handler_map_["Top Level"].reApply(*interactive_marker_server_);
01725                 selectMarker(selectable_markers_[current_group_name_ + "_selectable"],
01726                              gc.getState(ik_control_type_)->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform());
01727               }
01728               else
01729               {
01730                 is_ik_control_active_ = false;
01731                 menu_handler_map_["Top Level"].setCheckState(ik_control_handle_, MenuHandler::UNCHECKED);
01732                 menu_handler_map_["Top Level"].reApply(*interactive_marker_server_);
01733                 interactive_marker_server_->erase(current_group_name_);
01734                 interactive_marker_server_->applyChanges();
01735               }
01736             }
01737             else if(handle == joint_control_handle_)
01738             {
01739               MenuHandler::CheckState currentState;
01740               menu_handler_map_["Top Level"].getCheckState(joint_control_handle_, currentState);
01741               if(currentState == MenuHandler::UNCHECKED)
01742               {
01743                 is_joint_control_active_ = true;
01744                 menu_handler_map_["Top Level"].setCheckState(joint_control_handle_, MenuHandler::CHECKED);
01745                 createSelectableJointMarkers(gc);
01746                 menu_handler_map_["Top Level"].reApply(*interactive_marker_server_);
01747               }
01748               else
01749               {
01750                 is_joint_control_active_ = false;
01751                 menu_handler_map_["Top Level"].setCheckState(joint_control_handle_, MenuHandler::UNCHECKED);
01752                 deleteJointMarkers(gc);
01753                 menu_handler_map_["Top Level"].reApply(*interactive_marker_server_);
01754               }
01755             }
01756             else if(handle == collision_aware_handle_)
01757             {
01758               MenuHandler::CheckState currentState;
01759               menu_handler_map_["Top Level"].getCheckState(collision_aware_handle_, currentState);
01760               if(currentState == MenuHandler::UNCHECKED)
01761               {
01762                 collision_aware_ = true;
01763                 menu_handler_map_["Top Level"].setCheckState(collision_aware_handle_, MenuHandler::CHECKED);
01764                 createSelectableJointMarkers(gc);
01765                 menu_handler_map_["Top Level"].reApply(*interactive_marker_server_);
01766               }
01767               else if(currentState == MenuHandler::CHECKED)
01768               {
01769                 collision_aware_ = false;
01770                 menu_handler_map_["Top Level"].setCheckState(collision_aware_handle_, MenuHandler::UNCHECKED);
01771                 createSelectableJointMarkers(gc);
01772                 menu_handler_map_["Top Level"].reApply(*interactive_marker_server_);
01773               }
01774             }
01775             if(menu_entry_maps_["Top Level"][handle] == "Create Pole")
01776             {
01777               Pose polePose;
01778               polePose.position.x = 2.0f;
01779               polePose.position.z = 1.0f;
01780               polePose.position.y = 0.0f;
01781               polePose.orientation.x = 0.0f;
01782               polePose.orientation.y = 0.0f;
01783               polePose.orientation.z = 0.0f;
01784               polePose.orientation.w = 1.0f;
01785 
01786               createCollisionPole(nextCollisionPole(), polePose);
01787               refreshEnvironment();
01788             }
01789 
01790             unsigned int cmd = 0;
01791             for(map<string, GroupCollection>::iterator it = group_map_.begin(); it != group_map_.end(); it++)
01792             {
01793 
01794               if(menu_entry_maps_["Top Level"][handle] == it->first)
01795               {
01796                 selectPlanningGroup(cmd);
01797                 break;
01798               }
01799               cmd++;
01800             }
01801 
01802           }
01803           else if(is_ik_control_active_ && isGroupName(feedback->marker_name))
01804           {
01805             handle = feedback->menu_entry_id;
01806             MenuHandler::CheckState checkState;
01807 
01808             if(handle == start_position_handle_ || handle == end_position_handle_)
01809             {
01810               menu_handler_map_["End Effector"].getCheckState(handle, checkState);
01811 
01812               if(checkState != MenuHandler::CHECKED)
01813               {
01814                 menu_handler_map_["End Effector"].setCheckState(handle, MenuHandler::CHECKED);
01815                 menu_handler_map_["End Effector"].reApply(*interactive_marker_server_);
01816               }
01817 
01818               if(handle == start_position_handle_)
01819               {
01820                 menu_handler_map_["End Effector"].setCheckState(end_position_handle_, MenuHandler::UNCHECKED);
01821                 ik_control_type_ = StartPosition;
01822                 selectMarker(selectable_markers_[feedback->marker_name + "_selectable"],
01823                              gc.start_state_->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform());
01824                 createSelectableJointMarkers(gc);
01825                 menu_handler_map_["End Effector"].reApply(*interactive_marker_server_);
01826               }
01827               else if(handle == end_position_handle_)
01828               {
01829                 menu_handler_map_["End Effector"].setCheckState(start_position_handle_, MenuHandler::UNCHECKED);
01830                 ik_control_type_ = EndPosition;
01831                 selectMarker(selectable_markers_[feedback->marker_name + "_selectable"],
01832                              gc.end_state_->getLinkState(gc.ik_link_name_)->getGlobalLinkTransform());
01833                 createSelectableJointMarkers(gc);
01834                 menu_handler_map_["End Effector"].reApply(*interactive_marker_server_);
01835               }
01836 
01837               updateJointStates(gc);
01838 
01839             } 
01840             else if(handle == constrain_rp_handle_) {
01841               menu_handler_map_["End Effector"].getCheckState(handle, checkState);
01842               if(checkState == MenuHandler::UNCHECKED)
01843               {
01844                 menu_handler_map_["End Effector"].setCheckState(handle, MenuHandler::CHECKED);
01845                 constrain_rp_ = true;
01846               } else {
01847                 menu_handler_map_["End Effector"].setCheckState(handle, MenuHandler::UNCHECKED);
01848                 constrain_rp_ = false;
01849               }
01850               moveEndEffectorMarkers(0.0,0.0,0.0,0.0,0.0,0.0,true);
01851               menu_handler_map_["End Effector"].reApply(*interactive_marker_server_);
01852             }
01853             else if(menu_entry_maps_["End Effector"][handle] == "Plan")
01854             {
01855               planToEndEffectorState(gc);
01856             }
01857             else if(menu_entry_maps_["End Effector"][handle] == "Filter Trajectory")
01858             {
01859               filterPlannerTrajectory(gc);
01860             }
01861             else if(menu_entry_maps_["End Effector"][handle] == "Randomly Perturb")
01862             {
01863               randomlyPerturb(gc);
01864             }
01865             else if(menu_entry_maps_["End Effector"][handle] == "Go To Last Good State")
01866             {
01867               resetToLastGoodState(gc);
01868             }
01869             else if(menu_entry_maps_["End Effector"][handle] == "Deselect")
01870             {
01871               tf::Transform cur = toBulletTransform(feedback->pose);
01872               deselectMarker(selectable_markers_[feedback->marker_name + "_selectable"], cur);
01873             }
01874           }
01875           else if(feedback->marker_name.rfind("pole_") != string::npos)
01876           {
01877             handle = feedback->menu_entry_id;
01878 
01879             stringstream controlName;
01880             controlName << feedback->marker_name;
01881 
01882             if(menu_entry_maps_["Collision Object"][handle] == "Delete")
01883             {
01884               this->removeCollisionPoleByName(feedback->marker_name);
01885               interactive_marker_server_->erase(feedback->marker_name);
01886               refreshEnvironment();
01887             }
01888             else if(menu_entry_maps_["Collision Object"][handle] == "Deselect")
01889             {
01890               tf::Transform cur = toBulletTransform(feedback->pose);
01891               deselectMarker(selectable_markers_[feedback->marker_name + "_selectable"], cur);
01892             }
01893           }
01894 
01895           break;
01896 
01897         case InteractiveMarkerFeedback::MOUSE_UP:
01898           prev_joint_control_value_map_.clear();
01899           if(feedback->marker_name.rfind("pole_") != string::npos && feedback->marker_name.rfind("_selectable")
01900               == string::npos)
01901           {
01902             collision_poles_[feedback->marker_name].poses[0] = feedback->pose;
01903             refreshEnvironment();
01904           }
01905           else if(feedback->marker_name.rfind("_joint_control") != string::npos)
01906             {
01907               joint_clicked_map_[feedback->marker_name] = false;
01908             }
01909           break;
01910 
01911         case InteractiveMarkerFeedback::MOUSE_DOWN:
01912           if(feedback->marker_name.rfind("_joint_control") != string::npos)
01913           {
01914             if(!joint_clicked_map_[feedback->marker_name])
01915             {
01916               joint_clicked_map_[feedback->marker_name] = true;
01917               joint_prev_transform_map_[feedback->marker_name] = toBulletTransform(feedback->pose);
01918             }
01919           }
01920           break;
01921         case InteractiveMarkerFeedback::POSE_UPDATE:
01922           if(is_ik_control_active_ && isGroupName(feedback->marker_name))
01923           {
01924             tf::Transform cur = toBulletTransform(feedback->pose);
01925             setNewEndEffectorPosition(gc, cur, collision_aware_);
01926             last_ee_poses_[current_group_name_] = feedback->pose;
01927           }
01928           else if(is_joint_control_active_ && feedback->marker_name.rfind("_joint_control") != string::npos)
01929           {
01930             tf::Transform cur = toBulletTransform(feedback->pose);
01931             string jointName = feedback->marker_name.substr(0, feedback->marker_name.rfind("_joint_control"));
01932             setJointState(gc, jointName, cur);
01933           }
01934           break;
01935       }
01936       interactive_marker_server_->applyChanges();
01937     }
01938 
01944     bool isGroupName(const string& name)
01945     {
01946       return group_map_.find(name) != group_map_.end();
01947     }
01948 
01949     void makeTopLevelMenu()
01950     {
01951       InteractiveMarker int_marker;
01952       int_marker.pose.position.z = 2.25;
01953       int_marker.name = "top_level";
01954       int_marker.description = "Planning Visualizer";
01955       int_marker.header.frame_id = "/" + cm_->getWorldFrameId();
01956 
01957 
01958       InteractiveMarkerControl control;
01959       control.interaction_mode = InteractiveMarkerControl::MENU;
01960       control.always_visible = true;
01961 
01962       Marker labelMarker;
01963       labelMarker.type = Marker::TEXT_VIEW_FACING;
01964       labelMarker.text = "Command...";
01965       labelMarker.color.r = 1.0;
01966       labelMarker.color.g = 1.0;
01967       labelMarker.color.b = 1.0;
01968       labelMarker.color.a = 1.0;
01969       labelMarker.scale.x = 0.5;
01970       labelMarker.scale.y = 0.2;
01971       labelMarker.scale.z = 0.1;
01972       control.markers.push_back(labelMarker);
01973 
01974       int_marker.controls.push_back(control);
01975 
01976       interactive_marker_server_->insert(int_marker);
01977       interactive_marker_server_->setCallback(int_marker.name, process_function_ptr_);
01978       menu_handler_map_["Top Level"].apply(*interactive_marker_server_, int_marker.name);
01979     }
01980 
01984     void makePoleContextMenu(tf::Transform transform, string name, string description, float scale = 1.0f)
01985     {
01986 
01987       makeSelectableMarker(PlanningComponentsVisualizer::CollisionObject, transform, name, description, scale);
01988     }
01989 
01999     void makeSelectableMarker(InteractiveMarkerType type, tf::Transform transform, string name, string description,
02000                               float scale = 1.0f, bool publish = true)
02001     {
02002       SelectableMarker selectable_marker;
02003       selectable_marker.type_ = type;
02004       selectable_marker.name_ = name + "_selectable";
02005       selectable_marker.controlName_ = name;
02006       selectable_marker.controlDescription_ = description;
02007 
02008       InteractiveMarker marker;
02009       marker.header.frame_id = "/" + cm_->getWorldFrameId();
02010       ;
02011       marker.header.stamp = ros::Time::now();
02012       marker.pose.position.x = transform.getOrigin().x();
02013       marker.pose.position.y = transform.getOrigin().y();
02014       marker.pose.position.z = transform.getOrigin().z();
02015       marker.pose.orientation.w = transform.getRotation().w();
02016       marker.pose.orientation.x = transform.getRotation().x();
02017       marker.pose.orientation.y = transform.getRotation().y();
02018       marker.pose.orientation.z = transform.getRotation().z();
02019       marker.scale = scale;
02020       marker.name = name + "_selectable";
02021       marker.description = description;
02022       InteractiveMarkerControl control;
02023       control.interaction_mode = InteractiveMarkerControl::BUTTON;
02024       control.always_visible = true;
02025 
02026       switch (type)
02027       {
02028         case PlanningComponentsVisualizer::EndEffectorControl:
02029           control.markers.push_back(makeMarkerBox(marker, 0.5f));
02030           marker.controls.push_back(control);
02031           interactive_marker_server_->insert(marker);
02032           interactive_marker_server_->setCallback(marker.name, process_function_ptr_);
02033           menu_handler_map_["End Effector Selection"].apply(*interactive_marker_server_, marker.name);
02034           break;
02035         case PlanningComponentsVisualizer::CollisionObject:
02036           control.markers.push_back(makeMarkerCylinder(marker, 1.0f));
02037           marker.controls.push_back(control);
02038           interactive_marker_server_->insert(marker);
02039           interactive_marker_server_->setCallback(marker.name, process_function_ptr_);
02040           menu_handler_map_["Collision Object Selection"].apply(*interactive_marker_server_, marker.name);
02041           break;
02042         case PlanningComponentsVisualizer::JointControl:
02043           control.markers.push_back(makeMarkerBox(marker, 0.5f));
02044           marker.controls.push_back(control);
02045           interactive_marker_server_->insert(marker);
02046           interactive_marker_server_->setCallback(marker.name, process_function_ptr_);
02047           menu_handler_map_["Joint Selection"].apply(*interactive_marker_server_, marker.name);
02048           break;
02049       }
02050 
02051       selectable_markers_[marker.name] = selectable_marker;
02052 
02053       if(publish)
02054       {
02055         interactive_marker_server_->applyChanges();
02056       }
02057     }
02058 
02064     bool selectableMarkerExists(string name)
02065     {
02066       return selectable_markers_.find(name) != selectable_markers_.end();
02067     }
02068 
02074     void selectMarker(SelectableMarker& marker, tf::Transform transform)
02075     {
02076       InteractiveMarker dummy;
02077       if(interactive_marker_server_->get(marker.controlName_, dummy))
02078       {
02079         dummy.header.stamp = ros::Time::now();
02080         interactive_marker_server_->setPose(marker.controlName_, toGeometryPose(transform), dummy.header);
02081       }
02082       else
02083       {
02084         if(!interactive_marker_server_->erase(marker.name_))
02085         {
02086           return;
02087         }
02088 
02089         switch (marker.type_)
02090         {
02091           case PlanningComponentsVisualizer::EndEffectorControl:
02092             makeInteractive6DOFMarker(false, transform, marker.controlName_, marker.controlDescription_, 0.225f, false);
02093             break;
02094           case PlanningComponentsVisualizer::CollisionObject:
02095             makeInteractive6DOFMarker(false, transform, marker.controlName_, marker.controlDescription_, 2.0f, true);
02096             break;
02097           case PlanningComponentsVisualizer::JointControl:
02098             makeInteractive6DOFMarker(false, transform, marker.controlName_, marker.controlDescription_, 0.225f, false);
02099             break;
02100         }
02101       }
02102     }
02103 
02109     void deselectMarker(SelectableMarker& marker, tf::Transform transform)
02110     {
02111       if(!interactive_marker_server_->erase(marker.controlName_))
02112       {
02113         return;
02114       }
02115 
02116       float scale = 1.0f;
02117 
02118       switch (marker.type_)
02119       {
02120         case PlanningComponentsVisualizer::EndEffectorControl:
02121           scale = 0.5f;
02122           break;
02123         case PlanningComponentsVisualizer::CollisionObject:
02124           scale = 2.0f;
02125           break;
02126         case PlanningComponentsVisualizer::JointControl:
02127           scale = 0.225f;
02128           break;
02129       }
02130 
02131       makeSelectableMarker(marker.type_, transform, marker.controlName_, marker.controlDescription_, scale);
02132     }
02133 
02134     void makeInteractive1DOFTranslationMarker(tf::Transform transform, tf::Vector3 axis, string name, string description,
02135                                               float scale = 1.0f, float value = 0.0f)
02136     {
02137       InteractiveMarker marker;
02138       marker.header.frame_id = cm_->getWorldFrameId();
02139       marker.pose.position.x = transform.getOrigin().x();
02140       marker.pose.position.y = transform.getOrigin().y();
02141       marker.pose.position.z = transform.getOrigin().z();
02142       marker.pose.orientation.w = transform.getRotation().w();
02143       marker.pose.orientation.x = transform.getRotation().x();
02144       marker.pose.orientation.y = transform.getRotation().y();
02145       marker.pose.orientation.z = transform.getRotation().z();
02146       marker.scale = scale;
02147       marker.name = name;
02148       marker.description = description;
02149       InteractiveMarker dummy;
02150       InteractiveMarkerControl control;
02151       if(interactive_marker_server_->get(marker.name, dummy))
02152       {
02153         interactive_marker_server_->setPose(marker.name, marker.pose, marker.header);
02154       }
02155       else
02156       {
02157         control.orientation.x = axis.x();
02158         control.orientation.y = axis.z();
02159         control.orientation.z = axis.y();
02160         control.orientation.w = 1;
02161         control.independent_marker_orientation = false;
02162         control.always_visible = false;
02163         control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
02164         marker.controls.push_back(control);
02165         interactive_marker_server_->insert(marker);
02166         interactive_marker_server_->setCallback(marker.name, process_function_ptr_);
02167       }
02168 
02169     }
02170 
02171     void makeInteractive1DOFRotationMarker(tf::Transform transform, tf::Vector3 axis, string name, string description,
02172                                            float scale = 1.0f, float angle = 0.0f)
02173     {
02174       InteractiveMarker marker;
02175       marker.header.frame_id = cm_->getWorldFrameId();
02176       marker.pose.position.x = transform.getOrigin().x();
02177       marker.pose.position.y = transform.getOrigin().y();
02178       marker.pose.position.z = transform.getOrigin().z();
02179       marker.pose.orientation.w = transform.getRotation().w();
02180       marker.pose.orientation.x = transform.getRotation().x();
02181       marker.pose.orientation.y = transform.getRotation().y();
02182       marker.pose.orientation.z = transform.getRotation().z();
02183       marker.scale = scale;
02184       marker.name = name;
02185       marker.description = description;
02186 
02187       InteractiveMarker dummy;
02188       if(interactive_marker_server_->get(marker.name, dummy))
02189       {
02190         interactive_marker_server_->setPose(marker.name, marker.pose, marker.header);
02191       }
02192       else
02193       {
02194         InteractiveMarkerControl control;
02195         control.orientation.x = axis.x();
02196         control.orientation.y = axis.z();
02197         control.orientation.z = axis.y();
02198         control.orientation.w = 1;
02199         control.independent_marker_orientation = false;
02200         control.always_visible = false;
02201         control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
02202         marker.controls.push_back(control);
02203         interactive_marker_server_->insert(marker);
02204         interactive_marker_server_->setCallback(marker.name, process_function_ptr_);
02205       }
02206     }
02207 
02217     void makeInteractive6DOFMarker(bool fixed, tf::Transform transform, string name, string description,
02218                                    float scale = 1.0f, bool pole = false)
02219     {
02220       InteractiveMarker marker;
02221       marker.header.frame_id = "/" + cm_->getWorldFrameId();
02222       marker.pose.position.x = transform.getOrigin().x();
02223       marker.pose.position.y = transform.getOrigin().y();
02224       marker.pose.position.z = transform.getOrigin().z();
02225       marker.pose.orientation.w = transform.getRotation().w();
02226       marker.pose.orientation.x = transform.getRotation().x();
02227       marker.pose.orientation.y = transform.getRotation().y();
02228       marker.pose.orientation.z = transform.getRotation().z();
02229       marker.scale = scale;
02230       marker.name = name;
02231       marker.description = description;
02232 
02233       if(!pole)
02234       {
02235         makeInteractiveBoxControl(marker, 0.5f);
02236       }
02237       else
02238       {
02239         makeInteractiveCylinderControl(marker, 1.0f);
02240       }
02241 
02242       InteractiveMarkerControl control;
02243 
02244       if(fixed)
02245       {
02246         control.orientation_mode = InteractiveMarkerControl::FIXED;
02247       }
02248 
02249       control.orientation.w = 1;
02250       control.orientation.x = 1;
02251       control.orientation.y = 0;
02252       control.orientation.z = 0;
02253       control.always_visible = false;
02254       control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
02255       marker.controls.push_back(control);
02256       control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
02257       marker.controls.push_back(control);
02258 
02259       control.orientation.w = 1;
02260       control.orientation.x = 0;
02261       control.orientation.y = 1;
02262       control.orientation.z = 0;
02263       control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
02264       marker.controls.push_back(control);
02265       control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
02266       marker.controls.push_back(control);
02267 
02268       control.orientation.w = 1;
02269       control.orientation.x = 0;
02270       control.orientation.y = 0;
02271       control.orientation.z = 1;
02272       control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
02273       marker.controls.push_back(control);
02274       control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
02275       marker.controls.push_back(control);
02276 
02277       interactive_marker_server_->insert(marker);
02278 
02279       control.interaction_mode = InteractiveMarkerControl::MENU;
02280       //control.markers.push_back(makeMarkerSphere(marker));
02281       marker.controls.push_back(control);
02282 
02283       if(!pole)
02284       {
02285        menu_handler_map_["End Effector"].apply(*interactive_marker_server_, marker.name);
02286       }
02287       else
02288       {
02289        menu_handler_map_["Collision Object"].apply(*interactive_marker_server_, marker.name);
02290       }
02291 
02292       interactive_marker_server_->setCallback(marker.name, process_function_ptr_);
02293     }
02294 
02295     bool doesGroupHaveGoodIKSolution(const string& group) const
02296     {
02297       if(group_map_.find(group) == group_map_.end())
02298       {
02299         return false;
02300       }
02301       return group_map_.find(group)->second.good_ik_solution_;
02302     }
02303 
02304     bool doesGroupHaveGoodTrajectory(const string& group, const string& source) const
02305     {
02306       if(group_map_.find(group) == group_map_.end())
02307       {
02308         return false;
02309       }
02310       const GroupCollection& gc = group_map_.find(group)->second;
02311       if(gc.state_trajectory_display_map_.find(source) == gc.state_trajectory_display_map_.end())
02312       {
02313         return false;
02314       }
02315       return gc.state_trajectory_display_map_.find(source)->second.has_joint_trajectory_;
02316     }
02317 
02322     size_t getNumPlanningGroups()
02323     {
02324       return group_map_.size();
02325     }
02326 
02327     GroupCollection* getPlanningGroup(unsigned int i)
02328     {
02329       unsigned int cmd = 0;
02330       for(map<string, GroupCollection>::iterator it = group_map_.begin(); it != group_map_.end(); it++)
02331       {
02332         if(cmd == i)
02333         {
02334           return &(it->second);
02335         }
02336         cmd++;
02337       }
02338 
02339       return NULL;
02340     }
02341   protected:
02342 
02343     Pose toGeometryPose(tf::Transform transform)
02344     {
02345       Pose toReturn;
02346       toReturn.position.x = transform.getOrigin().x();
02347       toReturn.position.y = transform.getOrigin().y();
02348       toReturn.position.z = transform.getOrigin().z();
02349       toReturn.orientation.x = transform.getRotation().x();
02350       toReturn.orientation.y = transform.getRotation().y();
02351       toReturn.orientation.z = transform.getRotation().z();
02352       toReturn.orientation.w = transform.getRotation().w();
02353       return toReturn;
02354     }
02355 
02356     tf::Transform toBulletTransform(geometry_msgs::Pose pose)
02357     {
02358       tf::Quaternion quat = tf::Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
02359       tf::Vector3 vec = tf::Vector3(pose.position.x, pose.position.y, pose.position.z);
02360       return tf::Transform(quat, vec);
02361     }
02362 
02363     void deleteKinematicStates()
02364     {
02365       for(map<string, GroupCollection>::iterator it = group_map_.begin(); it != group_map_.end(); it++)
02366       {
02367         it->second.reset();
02368       }
02369     }
02370 
02371     IKControlType ik_control_type_;
02372 
02373 
02374   boost::recursive_mutex joint_state_lock_;
02375   sensor_msgs::JointState last_joint_state_msg_;
02376 
02377     boost::recursive_mutex lock_;
02378     boost::shared_ptr<InteractiveMarkerServer> interactive_marker_server_;
02379 
02380     CollisionModels* cm_;
02381 
02383     int num_collision_poles_;
02384 
02385     KinematicState* robot_state_;
02386 
02387     map<string, GroupCollection> group_map_;
02388 
02390     map<string, arm_navigation_msgs::CollisionObject> collision_poles_;
02391 
02393     map<string, Pose> last_ee_poses_;
02394 
02396     map<string, SelectableMarker> selectable_markers_;
02397 
02399     MenuHandler::FeedbackCallback process_function_ptr_;
02400 
02401     MenuHandler::EntryHandle start_position_handle_;
02402     MenuHandler::EntryHandle end_position_handle_;
02403     MenuHandler::EntryHandle ik_control_handle_;
02404     MenuHandler::EntryHandle joint_control_handle_;
02405     MenuHandler::EntryHandle collision_aware_handle_;
02406     MenuHandler::EntryHandle constrain_rp_handle_;
02407     bool constrain_rp_;
02408     bool collision_aware_;
02410     MenuHandlerMap menu_handler_map_;
02411 
02413     MenuMap menu_entry_maps_;
02414 
02415     MotionPlanRequest last_motion_plan_request_;
02416 
02417     ros::NodeHandle nh_;
02418     ros::Publisher joint_state_publisher_;
02419     ros::Publisher vis_marker_array_publisher_;
02420     ros::Publisher vis_marker_publisher_;
02421     ros::ServiceClient set_planning_scene_diff_client_;
02422     ros::ServiceClient planner_service_client_;
02423     ros::ServiceClient trajectory_filter_service_client_;
02424 
02425     string current_group_name_;
02426 
02427     tf::TransformBroadcaster transform_broadcaster_;
02428 
02429     bool is_ik_control_active_;
02430     bool is_joint_control_active_;
02431 
02432     map<string, bool> joint_clicked_map_;
02433     map<string, tf::Transform> joint_prev_transform_map_;
02434   map<string, double> prev_joint_control_value_map_;
02435 };
02436 
02437 PlanningComponentsVisualizer* pcv;
02438 
02439 bool inited = false;
02440 
02441 void spin_function()
02442 {
02443   ros::WallRate r(100.0);
02444   while(ros::ok())
02445   {
02446     r.sleep();
02447     ros::spinOnce();
02448   }
02449 }
02450 
02451 void update_function()
02452 {
02453   unsigned int counter = 0;
02454   while(ros::ok())
02455   {
02456     if(inited)
02457     {
02458       //pcv->sendTransforms();
02459       if(counter % CONTROL_SPEED == 0)
02460       {
02461         counter = 1;
02462         pcv->sendMarkers();
02463       }
02464       else
02465       {
02466         pcv->publishJointStates();
02467         counter++;
02468       }
02469     }
02470 
02471     usleep(5000);
02472   }
02473 }
02474 
02475 void quit(int sig)
02476 {
02477   if(pcv != NULL)
02478   {
02479     delete pcv;
02480   }
02481   exit(0);
02482 }
02483 
02484 int main(int argc, char** argv)
02485 {
02486   ros::init(argc, argv, "planning_components_visualizer", ros::init_options::NoSigintHandler);
02487 
02488   boost::thread spin_thread(boost::bind(&spin_function));
02489   boost::thread update_thread(boost::bind(&update_function));
02490   pcv = new PlanningComponentsVisualizer();
02491 
02492   inited = true;
02493 
02494   for(size_t i = 0; i < pcv->getNumPlanningGroups(); i++)
02495   {
02496     pcv->selectPlanningGroup(i);
02497     pcv->solveIKForEndEffectorPose((*pcv->getPlanningGroup(i)));
02498     pcv->updateJointStates((*pcv->getPlanningGroup(i)));
02499   }
02500 
02501   ros::waitForShutdown();
02502 
02503   return 0;
02504 }


move_arm
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Mon Dec 2 2013 12:35:17