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


move_arm_warehouse
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Thu Dec 12 2013 11:09:11