Adding absolute path to saveImageCB

CMakeLists.txt

@@ -62,6 +62,7 @@ find_package(graspit)
    SearchContact.msg
    Energy.msg
    Contact.msg
+   SimAnnParams.msg
  )
 
 ## Generate services in the 'srv' folder

action/PlanGrasps.action

@@ -5,6 +5,7 @@ Planner planner
 string search_energy
 SearchSpace search_space
 SearchContact search_contact
+SimAnnParams sim_ann_params
 
 int32 max_steps
 int32 feedback_num_steps

msg/SimAnnParams.msg

@@ -0,0 +1,22 @@
+
+# flag to switch to custom params defined in this message. If not set, GraspIt's default settings are kept
+bool set_custom_params
+
+
+# //Annealing parameters
+# //! Annealing constant for neighbor generation schedule
+float64 YC	 	#	GraspIt! default: 7.0
+# //! Annealing constant for error acceptance schedule
+float64 HC	 	#	GraspIt! default: 7.0
+# //! Number of dimensions for neighbor generation schedule
+float64 YDIMS	#	GraspIt! default: 8.0
+# //! Number of dimensions for error acceptance schedule
+float64 HDIMS	#	GraspIt! default: 8.0
+# //! Adjust factor for neighbor generation schedule
+float64 NBR_ADJ	#	GraspIt! default: 1.0
+# //! Adjust raw errors reported by states to be in the relevant range of the annealing schedule
+float64 ERR_ADJ	#	GraspIt! default: 1.0e-6
+# //! Starting temperatue
+float64 DEF_T0	#	GraspIt! default: 1e6
+# //! Starting step
+float64 DEF_K0	#	GraspIt! default: 30000

src/graspit_interface.cpp

@@ -13,6 +13,7 @@
 #include <graspit/EGPlanner/searchState.h>
 #include <graspit/EGPlanner/egPlanner.h>
 #include <graspit/EGPlanner/simAnnPlanner.h>
+#include <graspit/EGPlanner/simAnnParams.h>
 #include <graspit/EGPlanner/guidedPlanner.h>
 #include <graspit/EGPlanner/energy/searchEnergyFactory.h>
 #include <graspit/EGPlanner/energy/searchEnergy.h>
@@ -613,10 +614,16 @@ bool GraspitInterface::clearWorldCB(graspit_interface::ClearWorld::Request &requ
 bool GraspitInterface::saveImageCB(graspit_interface::SaveImage::Request &request,
                    graspit_interface::SaveImage::Response &response)
 {
-    QString filename = QString(getenv("GRASPIT"))+
-            QString("/images/") +
-            QString(request.filename.data()) +
-            QString(".jpg");
+    QString filename;
+    std::string filenamePath(request.filename.data());
+
+    if(filenamePath[0] == '/')
+        filename = QString(request.filename.data());
+    else
+        filename = QString(getenv("GRASPIT"))+
+               QString("/images/") +
+               QString(request.filename.data()) +
+               QString(".jpg");
 
     ROS_INFO("Saving Image: %s",filename.toStdString().c_str());
     graspitCore->getIVmgr()->saveImage(filename);
@@ -887,6 +894,20 @@ void GraspitInterface::runPlannerInMainThread()
 
     mPlanner->setEnergyType(goal.search_energy);
 
+    if (goal.sim_ann_params.set_custom_params)
+    {
+        ROS_INFO("Switching SimAnn Annealing parameters to your custom defined values!!! ");
+        SimAnnParams simAnnParams;
+        simAnnParams.YC = goal.sim_ann_params.YC;
+        simAnnParams.HC = goal.sim_ann_params.HC;
+        simAnnParams.YDIMS = goal.sim_ann_params.YDIMS;
+        simAnnParams.NBR_ADJ = goal.sim_ann_params.NBR_ADJ;
+        simAnnParams.ERR_ADJ = goal.sim_ann_params.ERR_ADJ;
+        simAnnParams.DEF_T0 = goal.sim_ann_params.DEF_T0;
+        simAnnParams.DEF_K0 = goal.sim_ann_params.DEF_K0;
+        mPlanner->setAnnealingParameters(simAnnParams);
+    }
+
 
     switch(goal.search_contact.type) {
         case graspit_interface::SearchContact::CONTACT_PRESET :