[Personalrobots-commit] SF.net SVN: personalrobots:[19679] pkg/trunk

[<is...@us...>]

Revision: 19679
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19679&view=rev
Author:   isucan
Date:     2009-07-26 02:48:48 +0000 (Sun, 26 Jul 2009)

Log Message:
-----------
looping solution paths in rviz

Modified Paths:
--------------
    pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
    pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
    pkg/trunk/stacks/visualization/rviz/src/rviz/displays/planning_display.cpp
    pkg/trunk/stacks/visualization/rviz/src/rviz/displays/planning_display.h

Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp	2009-07-26 02:34:20 UTC (rev 19678)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp	2009-07-26 02:48:48 UTC (rev 19679)
@@ -415,7 +415,7 @@
 			psetup->smoother->smoothMax(path);
 			double tsmooth = (ros::WallTime::now() - startTime).toSec();
 			ROS_DEBUG("          Smoother spent %g seconds (%g seconds in total)", tsmooth, tsmooth + tsolve);
-			dynamic_cast<SpaceInformationKinematicModel*>(psetup->si)->interpolatePath(path, 3.0);
+			dynamic_cast<SpaceInformationKinematicModel*>(psetup->si)->interpolatePath(path, 0.1);
 		    }
 		}		
 		

Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp	2009-07-26 02:34:20 UTC (rev 19678)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp	2009-07-26 02:48:48 UTC (rev 19679)
@@ -254,6 +254,7 @@
 		btTransform pose;
 		tf::poseMsgToTF(pso.pose, pose);
 		collisionSpace_->lock();
+		collisionSpace_->clearObstacles(objectInMap->id);
 		collisionSpace_->addObject(objectInMap->id, shape, pose);
 		collisionSpace_->unlock();
 		ROS_INFO("Added object '%s' to collision space", objectInMap->id.c_str());

Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/planning_display.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/planning_display.cpp	2009-07-26 02:34:20 UTC (rev 19678)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/planning_display.cpp	2009-07-26 02:48:48 UTC (rev 19679)
@@ -54,6 +54,7 @@
   setCollisionVisible(true);
   robot_->setUserData(Ogre::Any((void*) this));
 
+  setLoopDisplay(false);
   setAlpha(0.6f);
 }
 
@@ -83,6 +84,12 @@
   }
 }
 
+void  PlanningDisplay::setLoopDisplay(bool loop_display)
+{
+    loop_display_ = loop_display;
+    propertyChanged(loop_display_property_);
+}
+
 void PlanningDisplay::setAlpha(float alpha)
 {
   alpha_ = alpha;
@@ -201,6 +208,12 @@
   if (!kinematic_model_)
     return;
 
+  if (!animating_path_ && !new_kinematic_path_ && loop_display_ && displaying_kinematic_path_message_)
+  {
+      new_kinematic_path_ = true;
+      incoming_kinematic_path_message_ = displaying_kinematic_path_message_;
+  }
+  
   if (!animating_path_ && new_kinematic_path_)
   {
     displaying_kinematic_path_message_ = incoming_kinematic_path_message_;
@@ -315,6 +328,9 @@
   FloatPropertyPtr float_prop = state_display_time_property_.lock();
   float_prop->setMin(0.0001);
 
+  loop_display_property_ = property_manager_->createProperty<BoolProperty>("Loop Display", property_prefix_, boost::bind(&PlanningDisplay::getLoopDisplay, this),
+									   boost::bind(&PlanningDisplay::setLoopDisplay, this, _1), category_, this);
+  
   alpha_property_ = property_manager_->createProperty<FloatProperty> ("Alpha", property_prefix_, boost::bind(&PlanningDisplay::getAlpha, this),
                                                                       boost::bind(&PlanningDisplay::setAlpha, this, _1), category_, this);
   robot_description_property_ = property_manager_->createProperty<StringProperty> ("Robot Description", property_prefix_,

Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/planning_display.h
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/planning_display.h	2009-07-26 02:34:20 UTC (rev 19678)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/planning_display.h	2009-07-26 02:48:48 UTC (rev 19679)
@@ -114,6 +114,9 @@
   float getAlpha() { return alpha_; }
   void setAlpha( float alpha );
 
+  bool getLoopDisplay() { return loop_display_; }
+  void setLoopDisplay(bool loop_display);
+        
   bool isVisualVisible();
   bool isCollisionVisible();
 
@@ -171,6 +174,7 @@
   bool new_kinematic_path_;
   bool animating_path_;
   int current_state_;
+  bool loop_display_;
   float state_display_time_;
   float current_state_time_;
   float alpha_;
@@ -178,6 +182,7 @@
   BoolPropertyWPtr visual_enabled_property_;
   BoolPropertyWPtr collision_enabled_property_;
   FloatPropertyWPtr state_display_time_property_;
+  BoolPropertyWPtr loop_display_property_;
   StringPropertyWPtr robot_description_property_;
   ROSTopicStringPropertyWPtr topic_property_;
   FloatPropertyWPtr alpha_property_;


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.