Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[19658] pkg/trunk (Page 42)

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Revision: 19658
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19658&view=rev
Author:   isucan
Date:     2009-07-25 03:04:37 +0000 (Sat, 25 Jul 2009)

Log Message:
-----------
print lag information

Modified Paths:
--------------
    pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/kinematic_state_constraint_evaluator.h
    pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
    pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
    pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp
    pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
    pkg/trunk/util/robot_self_filter/src/self_filter.cpp
    pkg/trunk/util/robot_self_filter/src/self_mask.cpp

Modified: pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
===================================================================

--- pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp	2009-07-25 02:10:38 UTC (rev 19657)
+++ pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp	2009-07-25 03:04:37 UTC (rev 19658)
@@ -69,7 +69,7 @@
 
 	// configure the self mask and the message notifier
 	std::vector<std::string> frames;
-	sf_.getLinkFrames(frames);
+	sf_.getLinkNames(frames);
 	if (std::find(frames.begin(), frames.end(), robotFrame_) == frames.end())
 	    frames.push_back(robotFrame_);
 	mnCloud_->setTargetFrame(frames);
@@ -251,6 +251,8 @@
     {
 	if (!mapProcessing_.try_lock())
 	    return;
+
+	ROS_DEBUG("Got pointcloud update that is %f seconds old", (ros::Time::now() - cloud->header.stamp).toSec());
 	
 	std::vector<int> mask;
 	robot_msgs::PointCloud out;
@@ -287,6 +289,8 @@
     
     void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
     {
+	ROS_DEBUG("Got pointcloud that is %f seconds old", (ros::Time::now() - cloud->header.stamp).toSec());
+	
 	mapProcessing_.lock();
 	
 	std::vector<int> mask;

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/kinematic_state_constraint_evaluator.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/kinematic_state_constraint_evaluator.h	2009-07-25 02:10:38 UTC (rev 19657)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/kinematic_state_constraint_evaluator.h	2009-07-25 03:04:37 UTC (rev 19658)
@@ -175,7 +175,7 @@
 	bool add(const planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::JointConstraint> &jc);
 
 	/** \brief Add a set of pose constraints */
-	bool add(const planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::PoseConstraint> &kc);
+	bool add(const planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::PoseConstraint> &pc);
 	
 	/** \brief Decide whether the set of constraints is satisfied  */
 	bool decide(const double *params, int groupID) const;
@@ -185,11 +185,25 @@
 
 	/** \brief Print the constraint data */
 	void print(std::ostream &out = std::cout) const;
+	
+	/** \brief Get the active joint constraints */
+	const std::vector<motion_planning_msgs::PoseConstraint>& getPoseConstraints(void) const
+	{
+	    return m_pc;
+	}
 
+	/** \brief Get the active pose constraints */
+	const std::vector<motion_planning_msgs::JointConstraint>& getJointConstraints(void) const
+	{
+	    return m_jc;
+	}
+	
     protected:
 	
-	std::vector<KinematicConstraintEvaluator*> m_kce;
-    
+	std::vector<KinematicConstraintEvaluator*>         m_kce;
+	std::vector<motion_planning_msgs::JointConstraint> m_jc;
+	std::vector<motion_planning_msgs::PoseConstraint>  m_pc;
+	
     };
 } // planning_environment
 

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-25 02:10:38 UTC (rev 19657)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp	2009-07-25 03:04:37 UTC (rev 19658)
@@ -142,7 +142,7 @@
     
     int n = collisionMap->get_boxes_size();
     
-    ROS_DEBUG("Received %d points (collision map)", n);
+    ROS_DEBUG("Received collision map with %d points that is %f seconds old", n, (ros::Time::now() - collisionMap->header.stamp).toSec());
     
     if (onBeforeMapUpdate_ != NULL)
 	onBeforeMapUpdate_(collisionMap, clear);

Modified: pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp	2009-07-25 02:10:38 UTC (rev 19657)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp	2009-07-25 03:04:37 UTC (rev 19658)
@@ -209,6 +209,8 @@
     
     void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
     {
+	ROS_DEBUG("Got pointcloud that is %f seconds old", (ros::Time::now() - cloud->header.stamp).toSec());
+	
 	std::vector<int> mask;
 	bool filter = false;
 	

Modified: pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp	2009-07-25 02:10:38 UTC (rev 19657)
+++ pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp	2009-07-25 03:04:37 UTC (rev 19658)
@@ -288,6 +288,8 @@
     for (unsigned int i = 0 ; i < m_kce.size() ; ++i)
 	delete m_kce[i];
     m_kce.clear();	
+    m_jc.clear();
+    m_pc.clear();
 }
 	
 bool planning_environment::KinematicConstraintEvaluatorSet::add(const planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::JointConstraint> &jc)
@@ -298,18 +300,20 @@
 	JointConstraintEvaluator *ev = new JointConstraintEvaluator();
 	result = result && ev->use(kmodel, jc[i]);
 	m_kce.push_back(ev);
+	m_jc.push_back(jc[i]);
     }
     return result;
 }
 
-bool planning_environment::KinematicConstraintEvaluatorSet::add(const planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::PoseConstraint> &kc)
+bool planning_environment::KinematicConstraintEvaluatorSet::add(const planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::PoseConstraint> &pc)
 {
     bool result = true;
-    for (unsigned int i = 0 ; i < kc.size() ; ++i)
+    for (unsigned int i = 0 ; i < pc.size() ; ++i)
     {
 	PoseConstraintEvaluator *ev = new PoseConstraintEvaluator();
-	result = result && ev->use(kmodel, kc[i]);
+	result = result && ev->use(kmodel, pc[i]);
 	m_kce.push_back(ev);
+	m_pc.push_back(pc[i]);
     }
     return result;
 }

Modified: pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
===================================================================
--- pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h	2009-07-25 02:10:38 UTC (rev 19657)
+++ pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h	2009-07-25 03:04:37 UTC (rev 19658)
@@ -72,6 +72,12 @@
 	}
 
 	/** \brief Construct the filter */
+	SelfMask(tf::TransformListener &tf, double scale, double padd) : rm_("robot_description"), tf_(tf)
+	{
+	    configure(scale, padd);
+	}
+	
+	/** \brief Construct the filter */
 	SelfMask(tf::TransformListener &tf, const std::vector<std::string> &links, double scale, double padd) : rm_("robot_description"), tf_(tf)
 	{
 	    configure(links, scale, padd);
@@ -97,7 +103,7 @@
 	int  getMask(double x, double y, double z) const;
 	
 	/** \brief Get the set of frames that correspond to the links */
-	void getLinkFrames(std::vector<std::string> &frames) const;
+	void getLinkNames(std::vector<std::string> &frames) const;
 	
     private:
 
@@ -108,6 +114,9 @@
 	bool configure(void);
 
 	/** \brief Configure the filter. */
+	bool configure(double scale, double padd);
+
+	/** \brief Configure the filter. */
 	bool configure(const std::vector<std::string> &links, double scale, double padd);
 	
 	/** \brief Compute bounding spheres for the checked robot links. */

Modified: pkg/trunk/util/robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_filter.cpp	2009-07-25 02:10:38 UTC (rev 19657)
+++ pkg/trunk/util/robot_self_filter/src/self_filter.cpp	2009-07-25 03:04:37 UTC (rev 19658)
@@ -45,7 +45,7 @@
     SelfFilter(void) : sf_(tf_), mn_(tf_, boost::bind(&SelfFilter::cloudCallback, this, _1), "cloud_in", "", 1)
     {
 	std::vector<std::string> frames;
-	sf_.getLinkFrames(frames);
+	sf_.getLinkNames(frames);
 	mn_.setTargetFrame(frames);
 	nh_.param<std::string>("~annotate", annotate_, std::string());
 	pointCloudPublisher_ = nh_.advertise<robot_msgs::PointCloud>("cloud_out", 1);
@@ -57,6 +57,8 @@
     
     void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
     {
+	ROS_DEBUG("Got pointcloud that is %f seconds old", (ros::Time::now() - cloud->header.stamp).toSec());
+	
 	robot_msgs::PointCloud out;
 	std::vector<int> mask;
 	ros::WallTime tm = ros::WallTime::now();

Modified: pkg/trunk/util/robot_self_filter/src/self_mask.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_mask.cpp	2009-07-25 02:10:38 UTC (rev 19657)
+++ pkg/trunk/util/robot_self_filter/src/self_mask.cpp	2009-07-25 03:04:37 UTC (rev 19658)
@@ -85,12 +85,17 @@
     return true; 
 }
 
+bool robot_self_filter::SelfMask::configure(double scale, double padd)
+{
+    return configure(rm_.getSelfSeeLinks(), scale, padd);
+}
+
 bool robot_self_filter::SelfMask::configure(void)
 {
     return configure(rm_.getSelfSeeLinks(), rm_.getSelfSeeScale(), rm_.getSelfSeePadding());
 }
 
-void robot_self_filter::SelfMask::getLinkFrames(std::vector<std::string> &frames) const
+void robot_self_filter::SelfMask::getLinkNames(std::vector<std::string> &frames) const
 {
     for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
 	frames.push_back(bodies_[i].name);


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




Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[19658] pkg/trunk (Page 42)

personalrobots-commit