[Personalrobots-commit] SF.net SVN: personalrobots:[20965] pkg/trunk/motion_planning/ planning_environment

[<mr...@us...>]

Revision: 20965
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20965&view=rev
Author:   mrinal
Date:     2009-08-07 00:40:42 +0000 (Fri, 07 Aug 2009)

Log Message:
-----------
printing routines (ioan)

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
    pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp

Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h	2009-08-07 00:38:30 UTC (rev 20964)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h	2009-08-07 00:40:42 UTC (rev 20965)
@@ -41,6 +41,7 @@
 #include <motion_planning_msgs/KinematicJoint.h>
 #include <motion_planning_msgs/KinematicPath.h>
 #include <motion_planning_msgs/KinematicConstraints.h>
+#include <iostream>
 
 namespace planning_environment
 {
@@ -121,6 +122,9 @@
 	    return kcGoal_;
 	}
 
+	/** \brief Print active constraints */
+	void printConstraints(std::ostream &out = std::cout);
+
 	/** \brief Clear previously set constraints */
 	void clearConstraints(void);
 	
@@ -141,6 +145,9 @@
 	
 	/** \brief Get the set of contacts allowed when collision checking */
 	const std::vector<collision_space::EnvironmentModel::AllowedContact>& getAllowedContacts(void) const;
+
+	/** \brief Print allowed contacts */
+	void printAllowedContacts(std::ostream &out = std::cout);
 	
 	/** \brief Clear the set of allowed contacts */
 	void clearAllowedContacts(void);

Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp	2009-08-07 00:38:30 UTC (rev 20964)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp	2009-08-07 00:40:42 UTC (rev 20965)
@@ -575,6 +575,34 @@
     return allowedContacts_;
 }
 
+void planning_environment::PlanningMonitor::printAllowedContacts(std::ostream &out)
+{
+    out << allowedContacts_.size() << " allowed contacts" << std::endl;
+    for (unsigned int i = 0 ; i < allowedContacts_.size() ; ++i)
+    {
+        out << "  - allowing contacts up to depth " << allowedContacts_[i].depth << " between links: [";
+	for (unsigned int j = 0 ; j < allowedContacts_[i].links.size() ; ++j)
+	    out << allowedContacts_[i].links[j];
+	out << "] and bound " << allowedContacts_[i].bound.get() << std::endl;
+    }
+}
+
+void planning_environment::PlanningMonitor::printConstraints(std::ostream &out)
+{
+    out << "Path constraints:" << std::endl;
+
+    KinematicConstraintEvaluatorSet ks;
+    ks.add(getKinematicModel(), kcPath_.joint_constraint);
+    ks.add(getKinematicModel(), kcPath_.pose_constraint);
+    ks.print(out);
+
+    out << "Goal constraints:" << std::endl;
+    ks.clear();
+    ks.add(getKinematicModel(), kcGoal_.joint_constraint);
+    ks.add(getKinematicModel(), kcGoal_.pose_constraint);
+    ks.print(out);
+}
+
 void planning_environment::PlanningMonitor::clearAllowedContacts(void)
 {
     allowedContacts_.clear();


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