Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[19658] pkg/trunk
|
From: <is...@us...> - 2009-07-25 03:04:46
|
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.
|
