|
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.
|
|
From: <is...@us...> - 2009-07-25 21:49:09
|
Revision: 19671
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19671&view=rev
Author: isucan
Date: 2009-07-25 21:49:00 +0000 (Sat, 25 Jul 2009)
Log Message:
-----------
forgot to add these to previous commits
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/CMakeLists.txt
pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/kinematic_state_constraint_evaluator.h
pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
Modified: pkg/trunk/mapping/collision_map/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-07-25 21:44:47 UTC (rev 19670)
+++ pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-07-25 21:49:00 UTC (rev 19671)
@@ -2,7 +2,7 @@
set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-set(ROS_BUILD_TYPE RelWithDebInfo)
+set(ROS_BUILD_TYPE Release)
rospack(collision_map)
include(CheckIncludeFile)
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 21:44:47 UTC (rev 19670)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/kinematic_state_constraint_evaluator.h 2009-07-25 21:49:00 UTC (rev 19671)
@@ -62,6 +62,9 @@
/** \brief This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
virtual bool use(const planning_models::KinematicModel *kmodel, const ros::Message *kc) = 0;
+
+ /** \brief This function updates the kinematic model used by the constraint */
+ virtual void use(const planning_models::KinematicModel *kmodel) = 0;
/** \brief Decide whether the constraint is satisfied. The kinematic model is assumed to be at the state we want to decide. */
virtual bool decide(const double *params, const int groupID) const = 0;
@@ -84,13 +87,16 @@
m_joint = NULL;
m_kmodel = NULL;
}
-
+
/** \brief This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
virtual bool use(const planning_models::KinematicModel *kmodel, const ros::Message *kc);
/** \brief This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
bool use(const planning_models::KinematicModel *kmodel, const motion_planning_msgs::JointConstraint &jc);
+ /** \brief This function updates the kinematic model used by the constraint */
+ virtual void use(const planning_models::KinematicModel *kmodel);
+
/** \brief Decide whether the constraint is satisfied. The kinematic model is assumed to be at the state we want to decide. */
virtual bool decide(const double *params, const int groupID) const;
@@ -127,6 +133,9 @@
/** \brief This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
bool use(const planning_models::KinematicModel *kmodel, const motion_planning_msgs::PoseConstraint &pc);
+ /** \brief This function updates the kinematic model used by the constraint */
+ virtual void use(const planning_models::KinematicModel *kmodel);
+
/** \brief Clear the stored constraint */
virtual void clear(void);
@@ -177,6 +186,9 @@
/** \brief Add a set of pose constraints */
bool add(const planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::PoseConstraint> &pc);
+ /** \brief Update the kinematic model to be used for the constraints */
+ void use(const planning_models::KinematicModel *kmodel);
+
/** \brief Decide whether the set of constraints is satisfied */
bool decide(const double *params, int groupID) const;
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 21:44:47 UTC (rev 19670)
+++ pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp 2009-07-25 21:49:00 UTC (rev 19671)
@@ -61,6 +61,15 @@
return true;
}
+void planning_environment::JointConstraintEvaluator::use(const planning_models::KinematicModel *kmodel)
+{
+ if (m_joint)
+ {
+ m_kmodel = kmodel;
+ m_joint = kmodel->getJoint(m_jc.joint_name);
+ }
+}
+
bool planning_environment::JointConstraintEvaluator::decide(const double *params, const int groupID) const
{
const double *val = params + (groupID >= 0 ? m_kmodel->getJointIndexInGroup(m_joint->name, groupID) : m_kmodel->getJointIndex(m_joint->name));
@@ -132,6 +141,12 @@
return true;
}
+void planning_environment::PoseConstraintEvaluator::use(const planning_models::KinematicModel *kmodel)
+{
+ if (m_link)
+ m_link = kmodel->getLink(m_pc.link_name);
+}
+
void planning_environment::PoseConstraintEvaluator::clear(void)
{
m_link = NULL;
@@ -331,6 +346,12 @@
return decide(params, -1);
}
+void planning_environment::KinematicConstraintEvaluatorSet::use(const planning_models::KinematicModel *kmodel)
+{
+ for (unsigned int i = 0 ; i < m_kce.size() ; ++i)
+ m_kce[i]->use(kmodel);
+}
+
void planning_environment::KinematicConstraintEvaluatorSet::print(std::ostream &out) const
{
out << m_kce.size() << " kinematic constraints" << std::endl;
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-25 21:44:47 UTC (rev 19670)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-25 21:49:00 UTC (rev 19671)
@@ -53,7 +53,7 @@
ODEInitCountLock.lock();
if (ODEInitCount == 0)
{
- m_msg.inform("Initializing ODE");
+ m_msg.message("Initializing ODE");
dInitODE2(0);
}
ODEInitCount++;
@@ -69,7 +69,7 @@
ODEInitCount--;
if (ODEInitCount == 0)
{
- m_msg.inform("Closing ODE");
+ m_msg.message("Closing ODE");
dCloseODE();
}
ODEInitCountLock.unlock();
@@ -82,7 +82,7 @@
if (ODEThreadMap.find(id) == ODEThreadMap.end())
{
ODEThreadMap[id] = 1;
- m_msg.inform("Initializing new thread (%d total)", (int)ODEThreadMap.size());
+ m_msg.message("Initializing new thread (%d total)", (int)ODEThreadMap.size());
dAllocateODEDataForThread(dAllocateMaskAll);
}
ODEThreadMapLock.unlock();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-26 02:48:57
|
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.
|
|
From: <ge...@us...> - 2009-07-26 17:31:41
|
Revision: 19685
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19685&view=rev
Author: gerkey
Date: 2009-07-26 17:31:33 +0000 (Sun, 26 Jul 2009)
Log Message:
-----------
Blacklisting actionlib and packages that depend on it, for failure to build
under gcc 4.3.
Added Paths:
-----------
pkg/trunk/demos/2dnav_erratic/ROS_BUILD_BLACKLIST
pkg/trunk/demos/2dnav_gazebo/ROS_BUILD_BLACKLIST
pkg/trunk/demos/2dnav_pr2/ROS_BUILD_BLACKLIST
pkg/trunk/demos/door_demos_gazebo/ROS_BUILD_BLACKLIST
pkg/trunk/demos/erratic_gazebo/ROS_BUILD_BLACKLIST
pkg/trunk/demos/milestone2/milestone2/ROS_BUILD_BLACKLIST
pkg/trunk/demos/milestone2/milestone2_actions/ROS_BUILD_BLACKLIST
pkg/trunk/demos/plug_in_gazebo/ROS_BUILD_BLACKLIST
pkg/trunk/demos/test_2dnav_gazebo/ROS_BUILD_BLACKLIST
pkg/trunk/highlevel/move_base_stage/ROS_BUILD_BLACKLIST
pkg/trunk/sandbox/actionlib/ROS_BUILD_BLACKLIST
pkg/trunk/sandbox/annotated_map_builder/ROS_BUILD_BLACKLIST
pkg/trunk/sandbox/object_modeler/ROS_BUILD_BLACKLIST
pkg/trunk/stacks/nav/move_base/ROS_BUILD_BLACKLIST
pkg/trunk/stacks/trex/trex_pr2_writing_test/ROS_BUILD_BLACKLIST
Added: pkg/trunk/demos/2dnav_erratic/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/2dnav_erratic/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/demos/2dnav_erratic/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1 @@
+Depends on actionlib, which is blacklisted
Added: pkg/trunk/demos/2dnav_gazebo/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/demos/2dnav_gazebo/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1 @@
+Depends on actionlib, which is blacklisted
Added: pkg/trunk/demos/2dnav_pr2/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/2dnav_pr2/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/demos/2dnav_pr2/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1 @@
+Depends on actionlib, which is blacklisted
Added: pkg/trunk/demos/door_demos_gazebo/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/door_demos_gazebo/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/demos/door_demos_gazebo/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1 @@
+Depends on actionlib, which is blacklisted
Added: pkg/trunk/demos/erratic_gazebo/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/erratic_gazebo/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/demos/erratic_gazebo/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1 @@
+Depends on actionlib, which is blacklisted
Added: pkg/trunk/demos/milestone2/milestone2/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/milestone2/milestone2/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/demos/milestone2/milestone2/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1 @@
+Depends on actionlib, which is blacklisted
Added: pkg/trunk/demos/milestone2/milestone2_actions/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/milestone2/milestone2_actions/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/demos/milestone2/milestone2_actions/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1 @@
+Depends on actionlib, which is blacklisted
Added: pkg/trunk/demos/plug_in_gazebo/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/plug_in_gazebo/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/demos/plug_in_gazebo/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1 @@
+Depends on actionlib, which is blacklisted
Added: pkg/trunk/demos/test_2dnav_gazebo/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/demos/test_2dnav_gazebo/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1 @@
+Depends on actionlib, which is blacklisted
Added: pkg/trunk/highlevel/move_base_stage/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/highlevel/move_base_stage/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/highlevel/move_base_stage/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1 @@
+Depends on actionlib, which is blacklisted
Added: pkg/trunk/sandbox/actionlib/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/sandbox/actionlib/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/sandbox/actionlib/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1,13 @@
+When building move_base with gcc 4.3.2 (and presumably greater versions):
+
+[ 97%] Building CXX object CMakeFiles/bin/move_base_new.dir/src/move_base.o
+In file included from /home/rosbuild/hudson/workspace/personalrobots-update-64/personalrobots/stacks/nav/move_base/include/move_base/move_base.h:41,
+ from /home/rosbuild/hudson/workspace/personalrobots-update-64/personalrobots/stacks/nav/move_base/src/move_base.cpp:37:
+/home/rosbuild/hudson/workspace/personalrobots-update-64/ros/../personalrobots/sandbox/actionlib/include/actionlib/single_goal_action_server.h:57: error: declaration of 'typedef class actionlib::ActionServer<ActionGoal, Goal, ActionResult, Result, ActionFeedback, Feedback> actionlib::SingleGoalActionServer<ActionGoal, Goal, ActionResult, Result, ActionFeedback, Feedback>::ActionServer'
+/home/rosbuild/hudson/workspace/personalrobots-update-64/ros/../personalrobots/sandbox/actionlib/include/actionlib/action_server.h:63: error: changes meaning of 'ActionServer' from 'class actionlib::ActionServer<ActionGoal, Goal, ActionResult, Result, ActionFeedback, Feedback>'
+/home/rosbuild/hudson/workspace/personalrobots-update-64/personalrobots/stacks/nav/move_base/src/move_base.cpp: In function 'int main(int, char**)':
+/home/rosbuild/hudson/workspace/personalrobots-update-64/personalrobots/stacks/nav/move_base/src/move_base.cpp:631: warning: 'const std::string& ros::NodeHandle::getName() const' is deprecated (declared at /home/rosbuild/hudson/workspace/personalrobots-update-64/ros/core/roscpp/include/ros/node_handle.h:1041)
+make[3]: *** [CMakeFiles/bin/move_base_new.dir/src/move_base.o] Error 1
+
+
+
Added: pkg/trunk/sandbox/annotated_map_builder/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/sandbox/annotated_map_builder/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/sandbox/annotated_map_builder/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1 @@
+Depends on actionlib, which is blacklisted
Added: pkg/trunk/sandbox/object_modeler/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/sandbox/object_modeler/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/sandbox/object_modeler/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1 @@
+Depends on actionlib, which is blacklisted
Added: pkg/trunk/stacks/nav/move_base/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/stacks/nav/move_base/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/stacks/nav/move_base/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1 @@
+Depends on actionlib, which is blacklisted
Added: pkg/trunk/stacks/trex/trex_pr2_writing_test/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_test/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_test/ROS_BUILD_BLACKLIST 2009-07-26 17:31:33 UTC (rev 19685)
@@ -0,0 +1 @@
+Depends on actionlib, which is blacklisted
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-07-27 18:19:02
|
Revision: 19723
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19723&view=rev
Author: eitanme
Date: 2009-07-27 18:18:52 +0000 (Mon, 27 Jul 2009)
Log Message:
-----------
Removing packages from blacklist now that actions compiler error for gcc 4.3.2 is fixed
Removed Paths:
-------------
pkg/trunk/demos/2dnav_erratic/ROS_BUILD_BLACKLIST
pkg/trunk/demos/2dnav_gazebo/ROS_BUILD_BLACKLIST
pkg/trunk/demos/2dnav_pr2/ROS_BUILD_BLACKLIST
pkg/trunk/demos/door_demos_gazebo/ROS_BUILD_BLACKLIST
pkg/trunk/demos/erratic_gazebo/ROS_BUILD_BLACKLIST
pkg/trunk/demos/milestone2/milestone2/ROS_BUILD_BLACKLIST
pkg/trunk/demos/milestone2/milestone2_actions/ROS_BUILD_BLACKLIST
pkg/trunk/demos/plug_in_gazebo/ROS_BUILD_BLACKLIST
pkg/trunk/demos/test_2dnav_gazebo/ROS_BUILD_BLACKLIST
pkg/trunk/highlevel/move_base_stage/ROS_BUILD_BLACKLIST
pkg/trunk/sandbox/annotated_map_builder/ROS_BUILD_BLACKLIST
pkg/trunk/sandbox/object_modeler/ROS_BUILD_BLACKLIST
pkg/trunk/stacks/nav/move_base/ROS_BUILD_BLACKLIST
pkg/trunk/stacks/trex/trex_pr2_writing_test/ROS_BUILD_BLACKLIST
Deleted: pkg/trunk/demos/2dnav_erratic/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/2dnav_erratic/ROS_BUILD_BLACKLIST 2009-07-27 18:15:14 UTC (rev 19722)
+++ pkg/trunk/demos/2dnav_erratic/ROS_BUILD_BLACKLIST 2009-07-27 18:18:52 UTC (rev 19723)
@@ -1 +0,0 @@
-Depends on actionlib, which is blacklisted
Deleted: pkg/trunk/demos/2dnav_gazebo/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/ROS_BUILD_BLACKLIST 2009-07-27 18:15:14 UTC (rev 19722)
+++ pkg/trunk/demos/2dnav_gazebo/ROS_BUILD_BLACKLIST 2009-07-27 18:18:52 UTC (rev 19723)
@@ -1 +0,0 @@
-Depends on actionlib, which is blacklisted
Deleted: pkg/trunk/demos/2dnav_pr2/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/2dnav_pr2/ROS_BUILD_BLACKLIST 2009-07-27 18:15:14 UTC (rev 19722)
+++ pkg/trunk/demos/2dnav_pr2/ROS_BUILD_BLACKLIST 2009-07-27 18:18:52 UTC (rev 19723)
@@ -1 +0,0 @@
-Depends on actionlib, which is blacklisted
Deleted: pkg/trunk/demos/door_demos_gazebo/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/door_demos_gazebo/ROS_BUILD_BLACKLIST 2009-07-27 18:15:14 UTC (rev 19722)
+++ pkg/trunk/demos/door_demos_gazebo/ROS_BUILD_BLACKLIST 2009-07-27 18:18:52 UTC (rev 19723)
@@ -1 +0,0 @@
-Depends on actionlib, which is blacklisted
Deleted: pkg/trunk/demos/erratic_gazebo/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/erratic_gazebo/ROS_BUILD_BLACKLIST 2009-07-27 18:15:14 UTC (rev 19722)
+++ pkg/trunk/demos/erratic_gazebo/ROS_BUILD_BLACKLIST 2009-07-27 18:18:52 UTC (rev 19723)
@@ -1 +0,0 @@
-Depends on actionlib, which is blacklisted
Deleted: pkg/trunk/demos/milestone2/milestone2/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/milestone2/milestone2/ROS_BUILD_BLACKLIST 2009-07-27 18:15:14 UTC (rev 19722)
+++ pkg/trunk/demos/milestone2/milestone2/ROS_BUILD_BLACKLIST 2009-07-27 18:18:52 UTC (rev 19723)
@@ -1 +0,0 @@
-Depends on actionlib, which is blacklisted
Deleted: pkg/trunk/demos/milestone2/milestone2_actions/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/milestone2/milestone2_actions/ROS_BUILD_BLACKLIST 2009-07-27 18:15:14 UTC (rev 19722)
+++ pkg/trunk/demos/milestone2/milestone2_actions/ROS_BUILD_BLACKLIST 2009-07-27 18:18:52 UTC (rev 19723)
@@ -1 +0,0 @@
-Depends on actionlib, which is blacklisted
Deleted: pkg/trunk/demos/plug_in_gazebo/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/plug_in_gazebo/ROS_BUILD_BLACKLIST 2009-07-27 18:15:14 UTC (rev 19722)
+++ pkg/trunk/demos/plug_in_gazebo/ROS_BUILD_BLACKLIST 2009-07-27 18:18:52 UTC (rev 19723)
@@ -1 +0,0 @@
-Depends on actionlib, which is blacklisted
Deleted: pkg/trunk/demos/test_2dnav_gazebo/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/ROS_BUILD_BLACKLIST 2009-07-27 18:15:14 UTC (rev 19722)
+++ pkg/trunk/demos/test_2dnav_gazebo/ROS_BUILD_BLACKLIST 2009-07-27 18:18:52 UTC (rev 19723)
@@ -1 +0,0 @@
-Depends on actionlib, which is blacklisted
Deleted: pkg/trunk/highlevel/move_base_stage/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/highlevel/move_base_stage/ROS_BUILD_BLACKLIST 2009-07-27 18:15:14 UTC (rev 19722)
+++ pkg/trunk/highlevel/move_base_stage/ROS_BUILD_BLACKLIST 2009-07-27 18:18:52 UTC (rev 19723)
@@ -1 +0,0 @@
-Depends on actionlib, which is blacklisted
Deleted: pkg/trunk/sandbox/annotated_map_builder/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/sandbox/annotated_map_builder/ROS_BUILD_BLACKLIST 2009-07-27 18:15:14 UTC (rev 19722)
+++ pkg/trunk/sandbox/annotated_map_builder/ROS_BUILD_BLACKLIST 2009-07-27 18:18:52 UTC (rev 19723)
@@ -1 +0,0 @@
-Depends on actionlib, which is blacklisted
Deleted: pkg/trunk/sandbox/object_modeler/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/sandbox/object_modeler/ROS_BUILD_BLACKLIST 2009-07-27 18:15:14 UTC (rev 19722)
+++ pkg/trunk/sandbox/object_modeler/ROS_BUILD_BLACKLIST 2009-07-27 18:18:52 UTC (rev 19723)
@@ -1 +0,0 @@
-Depends on actionlib, which is blacklisted
Deleted: pkg/trunk/stacks/nav/move_base/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/stacks/nav/move_base/ROS_BUILD_BLACKLIST 2009-07-27 18:15:14 UTC (rev 19722)
+++ pkg/trunk/stacks/nav/move_base/ROS_BUILD_BLACKLIST 2009-07-27 18:18:52 UTC (rev 19723)
@@ -1 +0,0 @@
-Depends on actionlib, which is blacklisted
Deleted: pkg/trunk/stacks/trex/trex_pr2_writing_test/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_test/ROS_BUILD_BLACKLIST 2009-07-27 18:15:14 UTC (rev 19722)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_test/ROS_BUILD_BLACKLIST 2009-07-27 18:18:52 UTC (rev 19723)
@@ -1 +0,0 @@
-Depends on actionlib, which is blacklisted
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-07-27 21:16:19
|
Revision: 19746
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19746&view=rev
Author: rdiankov
Date: 2009-07-27 21:16:00 +0000 (Mon, 27 Jul 2009)
Log Message:
-----------
several script fixes to openraveros, removed pr2 controller from orrosplanning due to it bringing in 25+ new packages, checkerboard detection header now uses image stamp
Modified Paths:
--------------
pkg/trunk/openrave_planning/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetAttachedSensors.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetManipulators.m
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
pkg/trunk/stacks/simulators/opende/Makefile
pkg/trunk/stacks/visual_feature_detectors/checkerboard_detector/checkerboard_detector.cpp
Modified: pkg/trunk/openrave_planning/openrave/Makefile
===================================================================
--- pkg/trunk/openrave_planning/openrave/Makefile 2009-07-27 20:55:44 UTC (rev 19745)
+++ pkg/trunk/openrave_planning/openrave/Makefile 2009-07-27 21:16:00 UTC (rev 19746)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 838
+SVN_REVISION = -r 839
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
#SVN_PATCH = fini_patch.patch
include $(shell rospack find mk)/svn_checkout.mk
@@ -13,7 +13,7 @@
BOOST_LIBRARY_DIRS=$(shell rosboost-cfg --lib_dirs)
installed: $(SVN_DIR) patched
- cd $(SVN_DIR) && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && mkdir -p build && cd build && export CPATH="$(shell rospack find bullet)/include:$(CPATH)" && export LD_LIBRARY_PATH=$(shell rospack find bullet)/lib:$(LD_LIBRARY_PATH) && BOOST_INCLUDEDIR=$(BOOST_INCLUDE_DIRS) BOOST_LIBRARYDIR=$(BOOST_LIBRARY_DIRS) cmake -DCMAKE_INSTALL_PREFIX=$(PWD) -DCMAKE_BUILD_TYPE=RelWithDebInfo -DBoost_INCLUDE_DIR=$(BOOST_INCLUDE_DIRS) -DBoost_LIBRARY_DIR=$(BOOST_LIBRARY_DIRS) .. && export PARALLEL_JOBS=ROS_PARALLEL_JOBS && make $(ROS_PARALLEL_JOBS) install
+ cd $(SVN_DIR) && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && mkdir -p build && cd build && BOOST_INCLUDEDIR=$(BOOST_INCLUDE_DIRS) BOOST_LIBRARYDIR=$(BOOST_LIBRARY_DIRS) cmake -DCMAKE_INSTALL_PREFIX=$(PWD) -DCMAKE_BUILD_TYPE=RelWithDebInfo -DBoost_INCLUDE_DIR=$(BOOST_INCLUDE_DIRS) -DBoost_LIBRARY_DIR=$(BOOST_LIBRARY_DIRS) .. && export PARALLEL_JOBS=ROS_PARALLEL_JOBS && make $(ROS_PARALLEL_JOBS) install
touch installed
clean:
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotGetAttachedSensors.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotGetAttachedSensors.m 2009-07-27 20:55:44 UTC (rev 19745)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotGetAttachedSensors.m 2009-07-27 21:16:00 UTC (rev 19746)
@@ -9,9 +9,9 @@
% Tglobal = Tlink * Trelative
% type - the xml id of the sensor that is attached
function sensors = orRobotGetAttachedSensors(robotid)
-robots = orEnvGetRobots(robotid);
-if( isempty(robots) )
+robot = orEnvGetRobots(robotid);
+if( isempty(robot) )
sensors = [];
else
- sensors = robots.sensors;
+ sensors = robot.sensors;
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotGetManipulators.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotGetManipulators.m 2009-07-27 20:55:44 UTC (rev 19745)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotGetManipulators.m 2009-07-27 21:16:00 UTC (rev 19746)
@@ -11,9 +11,9 @@
% effect on the end effector
% iksolvername - name of ik solver to use
function manipulators = orRobotGetManipulators(robotid)
-robots = orEnvGetRobots(robotid);
-if( isempty(robots) )
+robot = orEnvGetRobots(robotid);
+if( isempty(robot) )
manipulators = [];
else
- manipulators = robots{1}.manips;
+ manipulators = robot.manips;
end
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2009-07-27 20:55:44 UTC (rev 19745)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2009-07-27 21:16:00 UTC (rev 19746)
@@ -93,7 +93,7 @@
if( !pnode->advertiseService("openrave_session",&SessionServer::session_callback,this, 1) )
return false;
- _sessionname = pnode->mapName("openrave_session");
+ _sessionname = pnode->resolveName("openrave_session");
// advertise persistent services
if( !pnode->advertiseService("body_destroy",&SessionServer::body_destroy_srv,this,-1) )
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-07-27 20:55:44 UTC (rev 19745)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-07-27 21:16:00 UTC (rev 19746)
@@ -8,7 +8,6 @@
<depend package="openrave"/>
<depend package="openrave_robot_description"/>
<depend package="checkerboard_detector"/>
- <depend package="pr2_mechanism_controllers"/>
<depend package="tf"/>
<depend package="robot_msgs"/>
<depend package="mapping_msgs"/>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2009-07-27 20:55:44 UTC (rev 19745)
+++ pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2009-07-27 21:16:00 UTC (rev 19746)
@@ -28,7 +28,7 @@
#include "mocapsystem.h"
#include "objecttransformsystem.h"
#include "collisionmapsystem.h"
-#include "rosrobotcontroller.h"
+//#include "rosrobotcontroller.h"
#include "rosplanningproblem.h"
// declaring variables with stdcall can be a little complex
@@ -53,10 +53,10 @@
InterfaceBase* DECL_STDCALL(ORCreate, (PluginType type, wchar_t* name, EnvironmentBase* penv))
{
switch(type) {
- case PT_Controller:
- if( wcsicmp(name, L"ROSRobot") == 0 )
- return new ROSRobotController(penv);
- break;
+// case PT_Controller:
+// if( wcsicmp(name, L"ROSRobot") == 0 )
+// return new ROSRobotController(penv);
+// break;
case PT_ProblemInstance:
if( wcsicmp(name, L"ROSPlanning") == 0 )
return new ROSPlanningProblem(penv);
@@ -84,7 +84,7 @@
return false;
}
- pinfo->controllers.push_back(L"ROSRobot");
+ //pinfo->controllers.push_back(L"ROSRobot");
pinfo->problems.push_back(L"ROSPlanning");
pinfo->sensorsystems.push_back(L"ROSMocap");
pinfo->sensorsystems.push_back(L"ObjectTransform");
Modified: pkg/trunk/stacks/simulators/opende/Makefile
===================================================================
--- pkg/trunk/stacks/simulators/opende/Makefile 2009-07-27 20:55:44 UTC (rev 19745)
+++ pkg/trunk/stacks/simulators/opende/Makefile 2009-07-27 21:16:00 UTC (rev 19746)
@@ -1,7 +1,7 @@
# change the next flag to 'no' if drawstuff is not to be built
WITH_DRAWSTUFF = yes
-CFG_OPTIONS = --with-arch=nocona --enable-ou --enable-release --disable-asserts --with-pic #--enable-double-precision
+CFG_OPTIONS = --with-arch=nocona --enable-release --disable-asserts --with-pic #--enable-double-precision
SVN_REVISION = -r 1684
SVN_PATCH = opende_patch.diff
Modified: pkg/trunk/stacks/visual_feature_detectors/checkerboard_detector/checkerboard_detector.cpp
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/checkerboard_detector/checkerboard_detector.cpp 2009-07-27 20:55:44 UTC (rev 19745)
+++ pkg/trunk/stacks/visual_feature_detectors/checkerboard_detector/checkerboard_detector.cpp 2009-07-27 21:16:00 UTC (rev 19746)
@@ -284,6 +284,7 @@
}
_objdetmsg.set_objects_vec(vobjects);
+ _objdetmsg.header.stamp = _imagemsg.header.stamp;
if( frame_id.size() > 0 )
_objdetmsg.header.frame_id = frame_id;
else
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-28 02:59:03
|
Revision: 19815
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19815&view=rev
Author: isucan
Date: 2009-07-28 02:58:50 +0000 (Tue, 28 Jul 2009)
Log Message:
-----------
self filter removes points based on ray intersection
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicJoint.msg
pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg
pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg
pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h
pkg/trunk/util/geometric_shapes/src/bodies.cpp
pkg/trunk/util/geometric_shapes/test/test_point_inclusion.cpp
pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_see_filter.h
pkg/trunk/util/robot_self_filter/self_filter.launch
pkg/trunk/util/robot_self_filter/src/self_filter.cpp
pkg/trunk/util/robot_self_filter/src/self_mask.cpp
pkg/trunk/util/robot_self_filter/src/test_filter.cpp
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch 2009-07-28 02:58:50 UTC (rev 19815)
@@ -34,6 +34,7 @@
<remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
<remap from="cloud_out" to="tilt_scan_cloud_annotated" />
<param name="annotate" type="string" value="on_self" />
+ <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
</node>
<!-- assemble pointcloud into a full world view -->
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch 2009-07-28 02:58:50 UTC (rev 19815)
@@ -34,6 +34,7 @@
<remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
<remap from="cloud_out" to="tilt_scan_cloud_annotated" />
<param name="annotate" type="string" value="on_self" />
+ <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
</node>
<!-- assemble pointcloud into a full world view -->
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-07-28 02:58:50 UTC (rev 19815)
@@ -39,6 +39,8 @@
#include <ros/ros.h>
#include <robot_actions/action_client.h>
#include <mapping_msgs/ObjectInMap.h>
+#include <mapping_msgs/AttachedObject.h>
+
#include <pr2_robot_actions/MoveArmGoal.h>
#include <pr2_robot_actions/MoveArmState.h>
@@ -83,7 +85,8 @@
boost::thread th(&spinThread);
- ros::Publisher pub = nh.advertise<mapping_msgs::ObjectInMap>("object_in_map", 1);
+ ros::Publisher pubObj = nh.advertise<mapping_msgs::ObjectInMap>("object_in_map", 1);
+ ros::Publisher pubAttach = nh.advertise<mapping_msgs::AttachedObject>("attach_map", 1);
/*
mapping_msgs::ObjectInMap o1;
@@ -147,6 +150,30 @@
if (res.objects.size() > 0)
{
recognition_lambertian::TableTopObject obj = res.objects[0];
+
+ double dx = 0.055;
+ double dy = 0.02;
+ double dz = -0.04;
+
+ obj.object_pose.pose.position.x += dx;
+ obj.object_pose.pose.position.y += dy;
+ obj.object_pose.pose.position.z += dz;
+ obj.object_pose.pose.orientation.x = 0;
+ obj.object_pose.pose.orientation.y = 0;
+ obj.object_pose.pose.orientation.z = 0;
+ obj.object_pose.pose.orientation.w = 1;
+
+
+ obj.grasp_pose.pose.position.x += dx;
+ obj.grasp_pose.pose.position.y += dy;
+ obj.grasp_pose.pose.position.z += dz;
+ obj.grasp_pose.pose.orientation.x = 0;
+ obj.grasp_pose.pose.orientation.y = 0;
+ obj.grasp_pose.pose.orientation.z = 0;
+ obj.grasp_pose.pose.orientation.w = 1;
+
+
+
obj.grasp_pose.pose.position.x -= 0.16;
ROS_INFO("pose of object: %f %f %f", obj.object_pose.pose.position.x, obj.object_pose.pose.position.y, obj.object_pose.pose.position.z);
@@ -158,7 +185,7 @@
o1.action = mapping_msgs::ObjectInMap::ADD;
o1.object = obj.object;
o1.pose = obj.object_pose.pose;
- pub.publish(o1);
+ pubObj.publish(o1);
@@ -192,8 +219,19 @@
sendPoint(vmPub, obj.grasp_pose.header, obj.grasp_pose.pose.position.x, obj.grasp_pose.pose.position.y, obj.grasp_pose.pose.position.z);
- // if (move_arm.execute(goal, feedback, ros::Duration(60.0)) != robot_actions::SUCCESS)
- // ROS_ERROR("failed achieving goal");
+ /*
+ if (move_arm.execute(goal, feedback, ros::Duration(60.0)) != robot_actions::SUCCESS)
+ ROS_ERROR("failed achieving goal");
+ else
+ {
+ mapping_msgs::AttachedObject ao;
+ ao.header = obj.object_pose.header;
+ ao.link_name = "r_wrist_roll_link";
+ ao.objects.push_back(obj.object);
+ ao.poses.push_back(obj.object_pose.pose);
+ pubAttach.publish(ao);
+ }
+ */
}
}
else
Modified: pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
===================================================================
--- pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-07-28 02:58:50 UTC (rev 19815)
@@ -101,6 +101,7 @@
bool getControlJointNames(std::vector<std::string> &joint_names);
void contactFound(collision_space::EnvironmentModel::Contact &contact);
void printPath(const motion_planning_msgs::KinematicPath &path);
+ bool fixState(planning_models::StateParams &sp, bool atGoal);
bool fillStartState(std::vector<motion_planning_msgs::KinematicJoint> &start);
void fillTrajectoryPath(const motion_planning_msgs::KinematicPath &path, manipulation_msgs::JointTraj &traj);
bool alterRequestUsingIK(motion_planning_srvs::MotionPlan::Request &req);
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-28 02:58:50 UTC (rev 19815)
@@ -163,12 +163,12 @@
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0.0;
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1.0;
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.003;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.002;
goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.003;
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.003;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.003;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.008;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.002;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.003;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.003;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.008;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.005;
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-07-28 02:58:50 UTC (rev 19815)
@@ -137,13 +137,14 @@
// do not spend more than this amount of time
req.allowed_time = 1.0;
- if (perform_ik_)
- alterRequestUsingIK(req);
-
// tell the planning monitor about the constraints we will be following
planningMonitor_->setPathConstraints(req.path_constraints);
planningMonitor_->setGoalConstraints(req.goal_constraints);
+ if (perform_ik_)
+ alterRequestUsingIK(req);
+
+
ResultStatus result = robot_actions::SUCCESS;
feedback = pr2_robot_actions::MoveArmState::PLANNING;
@@ -475,21 +476,16 @@
return true;
}
- bool MoveArm::fillStartState(std::vector<motion_planning_msgs::KinematicJoint> &start_state)
+ bool MoveArm::fixState(planning_models::StateParams &st, bool atGoal)
{
bool result = true;
- // get the current state
- planning_models::StateParams st(*planningMonitor_->getRobotState());
-
// just in case the system is a bit outside bounds, we enforce the bounds
st.enforceBounds();
// if the state is not valid, we try to fix it
- if (!planningMonitor_->isStateValidOnPath(&st))
+ if (atGoal ? !planningMonitor_->isStateValidAtGoal(&st) : !planningMonitor_->isStateValidOnPath(&st))
{
- ROS_DEBUG("Start state not valid, fixing it!");
-
// try 2% change in each component
planning_models::StateParams temp(st);
int count = 0;
@@ -498,10 +494,10 @@
temp = st;
temp.perturbStateGroup(0.02, arm_);
count++;
- } while (!planningMonitor_->isStateValidOnPath(&temp) && count < 50);
+ } while ((atGoal ? !planningMonitor_->isStateValidAtGoal(&temp) : !planningMonitor_->isStateValidOnPath(&temp)) && count < 50);
// try 10% change in each component
- if (!planningMonitor_->isStateValidOnPath(&temp))
+ if (atGoal ? !planningMonitor_->isStateValidAtGoal(&temp) : !planningMonitor_->isStateValidOnPath(&temp))
{
count = 0;
do
@@ -509,18 +505,27 @@
temp = st;
temp.perturbStateGroup(0.1, arm_);
count++;
- } while (!planningMonitor_->isStateValidOnPath(&temp) && count < 50);
+ } while ((atGoal ? !planningMonitor_->isStateValidAtGoal(&temp) : !planningMonitor_->isStateValidOnPath(&temp)) && count < 50);
}
- if (planningMonitor_->isStateValidOnPath(&temp))
+ if (atGoal ? !planningMonitor_->isStateValidAtGoal(&temp) : !planningMonitor_->isStateValidOnPath(&temp))
st = temp;
else
- {
result = false;
- ROS_ERROR("Starting state for the robot is in collision and attempting to fix it failed");
- }
}
+ return result;
+ }
+
+ bool MoveArm::fillStartState(std::vector<motion_planning_msgs::KinematicJoint> &start_state)
+ {
+ // get the current state
+ planning_models::StateParams st(*planningMonitor_->getRobotState());
+ bool result = fixState(st, false);
+
+ if (!result)
+ ROS_ERROR("Starting state for the robot is in collision and attempting to fix it failed");
+
// fill in start state with current one
std::vector<planning_models::KinematicModel::Joint*> joints;
planningMonitor_->getKinematicModel()->getJoints(joints);
@@ -558,7 +563,7 @@
{
// we can do ik can turn the pose constraint into a joint one
ROS_INFO("Converting pose constraint to joint constraint using IK...");
-
+
planningMonitor_->getEnvironmentModel()->setVerbose(false);
ros::ServiceClient client = node_handle_.serviceClient<manipulation_srvs::IKService>(ARM_IK_NAME, true);
for (int t = 0 ; t < 20 ; ++t)
@@ -594,6 +599,10 @@
req.goal_constraints.joint_constraint.push_back(jc);
}
req.goal_constraints.pose_constraint.clear();
+
+ // update the goal constraints for the planning monitor as well
+ planningMonitor_->setGoalConstraints(req.goal_constraints);
+
result = true;
}
}
@@ -649,7 +658,7 @@
}
for(unsigned int i = 0; i < solution.size() ; ++i)
- ROS_DEBUG("%f", solution[i]);
+ ROS_DEBUG("IK[%d] = %f", (int)i, solution[i]);
// if IK did not fail, check if the state is valid
planning_models::StateParams spTest(*planningMonitor_->getRobotState());
@@ -664,10 +673,23 @@
}
n += u;
}
+
+ // if state is not valid, we try to fix it
+ fixState(spTest, true);
+
if (planningMonitor_->isStateValidAtGoal(&spTest))
+ {
validSolution = true;
- else
- request.data.positions = solution;
+
+ // update solution
+ solution.clear();
+ for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
+ {
+ std::vector<double> params;
+ spTest.copyParamsJoint(params, arm_joint_names_[i]);
+ solution.insert(solution.end(), params.begin(), params.end());
+ }
+ }
}
else
{
Modified: pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-07-28 02:58:50 UTC (rev 19815)
@@ -222,7 +222,7 @@
void computeCloudMask(const robot_msgs::PointCloud &cloud, std::vector<int> &mask)
{
if (cloud_annotation_.empty())
- sf_.mask(cloud, mask);
+ sf_.maskContainment(cloud, mask);
else
{
int c = -1;
@@ -235,7 +235,7 @@
if (c < 0)
{
ROS_WARN("Cloud annotation channel '%s' is missing", cloud_annotation_.c_str());
- sf_.mask(cloud, mask);
+ sf_.maskContainment(cloud, mask);
}
else
{
@@ -560,7 +560,7 @@
else
{
// this is the point in the robotFrame_; check if it is currently inside the robot
- bool out = sf_.getMask(x * bi_.resolution + bi_.originX, y * bi_.resolution + bi_.originY, z * bi_.resolution + bi_.originZ);
+ bool out = sf_.getMaskContainment(x * bi_.resolution + bi_.originX, y * bi_.resolution + bi_.originY, z * bi_.resolution + bi_.originZ);
// if we are already inside the robot, we mark the fact we want to stop when we are outside
if (out == false)
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicJoint.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicJoint.msg 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicJoint.msg 2009-07-28 02:58:50 UTC (rev 19815)
@@ -1,3 +1,4 @@
+# The specification of one joint value
Header header
string joint_name
float64[] value
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg 2009-07-28 02:58:50 UTC (rev 19815)
@@ -8,7 +8,6 @@
string model_id
# The joint names, in the same order as the values in the state
-# and the same order reported by KinematicModel::StateParams
string[] names
# A list of states, each with the dimension of the requested model.
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg 2009-07-28 02:58:50 UTC (rev 19815)
@@ -2,20 +2,18 @@
# Since there are multiple types of constraints, the 'type' member is used
# to identify the different constraints
-# Constants that represent possible values for type. A position and an
-# orientation constant can be combined (by adding).
+# Constants that represent possible values for type. Each degree of freedom
+# can be constrained individually
+
int32 POSITION_X=1 # only x of position is considered
int32 POSITION_Y=2 # only y of position is considered
int32 POSITION_Z=4 # only z of position is considered
+
+int32 ORIENTATION_R=8 # only roll of orientation is considered
+int32 ORIENTATION_P=16 # only pitch of orientation is considered
+int32 ORIENTATION_Y=32 # only yaw of orientation is considered
-# the next values can be combined with one of the above, so they are offset by
-# 256, so we can use bit operations on them
-
-int32 ORIENTATION_R=8 # only roll, yaw of orientation is considered
-int32 ORIENTATION_P=16 # only roll, yaw of orientation is considered
-int32 ORIENTATION_Y=32 # only roll, yaw of orientation is considered
-
int32 type
# The robot link this constraint refers to
Modified: pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h
===================================================================
--- pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h 2009-07-28 02:58:50 UTC (rev 19815)
@@ -39,6 +39,7 @@
#include "geometric_shapes/shapes.h"
#include <LinearMath/btTransform.h>
+#include <vector>
/**
This set of classes allows quickly detecting whether a given point
@@ -137,6 +138,9 @@
return containsPoint(btVector3(btScalar(x), btScalar(y), btScalar(z)));
}
+ /** \brief Check is a ray intersects the body, and find the set of intersections, in order, along the ray */
+ virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL) = 0;
+
/** \brief Check is a point is inside the body */
virtual bool containsPoint(const btVector3 &p) const = 0;
@@ -182,7 +186,8 @@
virtual bool containsPoint(const btVector3 &p) const;
virtual double computeVolume(void) const;
virtual void computeBoundingSphere(BoundingSphere &sphere) const;
-
+ virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL);
+
protected:
virtual void useDimensions(const shapes::Shape *shape);
@@ -217,7 +222,8 @@
virtual bool containsPoint(const btVector3 &p) const;
virtual double computeVolume(void) const;
virtual void computeBoundingSphere(BoundingSphere &sphere) const;
-
+ virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL);
+
protected:
virtual void useDimensions(const shapes::Shape *shape);
@@ -228,6 +234,9 @@
btVector3 m_normalB1;
btVector3 m_normalB2;
+ btVector3 m_corner1;
+ btVector3 m_corner2;
+
double m_length;
double m_length2;
double m_radius;
@@ -259,7 +268,8 @@
virtual bool containsPoint(const btVector3 &p) const;
virtual double computeVolume(void) const;
virtual void computeBoundingSphere(BoundingSphere &sphere) const;
-
+ virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL);
+
protected:
virtual void useDimensions(const shapes::Shape *shape); // (x, y, z) = (length, width, height)
@@ -270,6 +280,9 @@
btVector3 m_normalW;
btVector3 m_normalH;
+ btVector3 m_corner1;
+ btVector3 m_corner2;
+
double m_length;
double m_width;
double m_height;
@@ -304,7 +317,8 @@
virtual bool containsPoint(const btVector3 &p) const;
virtual double computeVolume(void) const;
virtual void computeBoundingSphere(BoundingSphere &sphere) const;
-
+ virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL);
+
protected:
virtual void useDimensions(const shapes::Shape *shape);
@@ -315,6 +329,7 @@
std::vector<btVector4> m_planes;
std::vector<btVector3> m_vertices;
+ std::vector<btScalar> m_vertDists;
std::vector<unsigned int> m_triangles;
btTransform m_iPose;
Modified: pkg/trunk/util/geometric_shapes/src/bodies.cpp
===================================================================
--- pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-07-28 02:13:49 UTC (rev 19814)
+++ pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-07-28 02:58:50 UTC (rev 19815)
@@ -36,6 +36,7 @@
#include "geometric_shapes/bodies.h"
#include <LinearMath/btConvexHull.h>
+#include <algorithm>
#include <iostream>
#include <cmath>
@@ -97,6 +98,20 @@
}
}
+namespace bodies
+{
+ static const double ZERO = 1e-9;
+
+ /* Compute the square of the distance between a ray and a point */
+ /* Note: this requires 'dir' to be normalized */
+ static inline double distanceSQR(const btVector3& p, const btVector3& origin, const btVector3& dir)
+ {
+ btVector3 a = p - origin;
+ double d = dir.dot(a);
+ return a.length2() - d * d;
+ }
+}
+
bool bodies::Sphere::containsPoint(const btVector3 &p) const
{
return (m_center - p).length2() < m_radius2;
@@ -125,6 +140,57 @@
sphere.radius = m_radiusU;
}
+bool bodies::Sphere::intersectsRay(const btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections)
+{
+ if (distanceSQR(m_center, origin, dir) > m_radius2) return false;
+ bool result = false;
+
+ btVector3 cp = origin - m_center;
+ double dpcpv = cp.dot(dir);
+
+ btVector3 w = cp - dpcpv * dir;
+ btVector3 Q = m_center + w;
+ double x = m_radius2 - w.length2();
+
+ if (fabs(x) < ZERO)
+ {
+ w = Q - origin;
+ double dpQv = w.dot(dir);
+ if (dpQv > ZERO)
+ {
+ if (intersections)
+ intersections->push_back(Q);
+ result = true;
+ }
+ } else
+ if (x > 0.0)
+ {
+ x = sqrt(x);
+ w = dir * x;
+ btVector3 A = Q - w;
+ btVector3 B = Q + w;
+ w = A - origin;
+ double dpAv = w.dot(dir);
+ w = B - origin;
+ double dpBv = w.dot(dir);
+
+ if (dpAv > ZERO)
+ {
+ if (intersections)
+ intersections->push_back(A);
+ result = true;
+ }
+
+ if (dpBv > ZERO)
+ {
+ if (intersections)
+ intersections->push_back(B);
+ result = true;
+ }
+ }
+ return result;
+}
+
bool bodies::Cylinder::containsPoint(const btVector3 &p) const
{
btVector3 v = p - m_center;
@@ -157,12 +223,16 @@
m_radius2 = m_radiusU * m_radiusU;
m_length2 = m_scale * m_length / 2.0 + m_padding;
m_center = m_pose.getOrigin();
- m_radiusB = std::max(m_radiusU, m_length2);
+ m_radiusB = sqrt(m_length2 * m_length2 + m_radius2);
const btMatrix3x3& basis = m_pose.getBasis();
m_normalB1 = basis.getColumn(0);
m_normalB2 = basis.getColumn(1);
m_normalH = basis.getColumn(2);
+
+ btVector3 tmp = (m_normalB1 + m_normalB2) * m_radiusU;
+ m_corner1 = m_center - tmp - m_normalH * m_length2;
+ m_corner2 = m_center + tmp + m_normalH * m_length2;
}
double bodies::Cylinder::computeVolume(void) const
@@ -176,6 +246,85 @@
sphere.radius = m_radiusB;
}
+bool bodies::Cylinder::intersectsRay(const btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections)
+{
+ if (distanceSQR(m_center, origin, dir) > m_radiusB) return false;
+
+ double t_near = -INFINITY;
+ double t_far = INFINITY;
+
+ for (int i = 0; i < 3; i++)
+ {
+ btVector3 &vN = i == 0 ? m_normalB1 : (i == 1 ? m_normalB2 : m_normalH);
+ double dp = vN.dot(dir);
+
+ if (fabs(dp) > ZERO)
+ {
+ double t1 = vN.dot(m_corner1 - origin) / dp;
+ double t2 = vN.dot(m_corner2 - origin) / dp;
+
+ if (t1 > t2)
+ std::swap(t1, t2);
+
+ if (t1 > t_near)
+ t_near = t1;
+
+ if (t2 < t_far)
+ t_far = t2;
+
+ if (t_near > t_far)
+ return false;
+
+ if (t_far < 0.0)
+ return false;
+ }
+ else
+ {
+ if (i == 0)
+ {
+ if ((std::min(m_corner1.y(), m_corner2.y()) > origin.y() ||
+ std::max(m_corner1.y(), m_corner2.y()) < origin.y()) &&
+ (std::min(m_corner1.z(), m_corner2.z()) > origin.z() ||
+ std::max(m_corner1.z(), m_corner2.z()) < origin.z()))
+ return false;
+ }
+ else
+ {
+ if (i == 1)
+ {
+ if ((std::min(m_corner1.x(), m_corner2.x()) > origin.x() ||
+ std::max(m_corner1.x(), m_corner2.x()) < origin.x()) &&
+ (std::min(m_corner1.z(), m_corner2.z()) > origin.z() ||
+ std::max(m_corner1.z(), m_corner2.z()) < origin.z()))
+ return false;
+ }
+ else
+ if ((std::min(m_corner1.x(), m_corner2.x()) > origin.x() ||
+ std::max(m_corner1.x(), m_corner2.x()) < origin.x()) &&
+ (std::min(m_corner1.y(), m_corner2.y()) > origin.y() ||
+ std::max(m_corner1.y(), m_corner2.y()) < origin.y()))
+ return false;
+ }
+ }
+ }
+
+ // at this point we know there is intersection with the box around the cylinder
+ // \todo we need to figure out if this intersection is on the cylinder as well
+
+ if (intersections)
+ {
+ if (t_far - t_near > ZERO)
+ {
+ intersections->push_back(t_near * dir + origin);
+ intersections->push_back(t_far * dir + origin);
+ }
+ else
+ intersections->push_back(t_far * dir + origin);
+ }
+
+ return true;
+}
+
bool bodies::Box::containsPoint(const btVector3 &p) const
{
btVector3 v = p - m_center;
@@ -220,6 +369,9 @@
m_normalL = basis.getColumn(0);
m_normalW = basis.getColumn(1);
m_normalH = basis.getColumn(2);
+
+ m_corner1 = m_center - m_normalL * m_length2 - m_normalW * m_width2 - m_normalH * m_height2;
+ m_corner2 = m_center + m_normalL * m_length2 + m_normalW * m_width2 + m_normalH * m_height2;
}
double bodies::Box::computeVolume(void) const
@@ -233,6 +385,82 @@
sphere.radius = m_radiusB;
}
+bool bodies::Box::intersectsRay(const btVector3& origin, const btVector3& dir, std::vector<btVector3> *intersections)
+{
+ if (distanceSQR(m_center, origin, dir) > m_radiusB) return false;
+
+ double t_near = -INFINITY;
+ double t_far = INFINITY;
+
+ for (int i = 0; i < 3; i++)
+ {
+ btVector3 &vN = i == 0 ? m_normalL : (i == 1 ? m_normalW : m_normalH);
+ double dp = vN.dot(dir);
+
+ if (fabs(dp) > ZERO)
+ {
+ double t1 = vN.dot(m_corner1 - origin) / dp;
+ double t2 = vN.dot(m_corner2 - origin) / dp;
+
+ if (t1 > t2)
+ std::swap(t1, t2);
+
+ if (t1 > t_near)
+ t_near = t1;
+
+ if (t2 < t_far)
+ t_far = t2;
+
+ if (t_near > t_far)
+ return false;
+
+ if (t_far < 0.0)
+ return false;
+ }
+ else
+ {
+ if (i == 0)
+ {
+ if ((std::min(m_corner1.y(), m_corner2.y()) > origin.y() ||
+ std::max(m_corner1.y(), m_corner2.y()) < origin.y()) &&
+ (std::min(m_corner1.z(), m_corner2.z()) > origin.z() ||
+ std::max(m_corner1.z(), m_corner2.z()) < origin.z()))
+ return false;
+ }
+ else
+ {
+ if (i == 1)
+ {
+ if ((std::min(m_corner1.x(), m_corner2.x()) > origin.x() ||
+ std::max(m_corner1.x(), m_corner2.x()) < origin.x()) &&
+ (std::min(m_corner1.z(), m_corner2.z()) > origin.z() ||
+ std::max(m_corner1.z(), m_corner2.z()) < origin.z()))
+ return false;
+ }
+ else
+ if ((std::min(m_corner1.x(), m_corner2.x()) > origin.x() ||
+ std::max(m_corner1.x(), m_corner2.x()) < origin.x()) &&
+ (std::min(m_corner1.y(), m_corner2.y()) > origin.y() ||
+ std::max(m_corner1.y(), m_corner2.y()) < origin.y()))
+ return false;
+ }
+ }
+ }
+
+ if (intersections)
+ {
+ if (t_far - t_near > ZERO)
+ {
+ intersections->push_back(t_near * dir + origin);
+ intersections->push_back(t_far * dir + origin);
+ }
+ else
+ intersections->push_back(t_far * dir + origin);
+ }
+
+ return true;
+}
+
bool bodies::ConvexMesh::containsPoint(const btVector3 &p) const
{
btVector3 ip = (m_iPose * p) / m_scale;
@@ -245,6 +473,7 @@
m_planes.clear();
m_triangles.clear();
m_vertices.clear();
+ m_vertDists.clear();
m_meshRadiusB = 0.0;
m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
@@ -264,10 +493,12 @@
std::cout << "Convex hull has " << hr.m_OutputVertices.size() << " vertices (down from " << mesh->vertexCount << "), " << hr.mNumFaces << " faces" << std::endl;
m_vertices.reserve(hr.m_OutputVertices.size());
- btVector3 sum(btScalar(0), btScalar(0), btScalar(0));
+ btVector3 sum(0, 0, 0);
+
for (int j = 0 ; j < hr.m_OutputVertices.size() ; ++j)
{
m_vertices.push_back(hr.m_OutputVertices[j]);
+ m_vertDists.push_back(hr.m_OutputVertices[j].length());
sum = sum + hr.m_OutputVertices[j];
}
@@ -281,9 +512,6 @@
m_meshRadiusB = sqrt(m_meshRadiusB);
m_triangles.reserve(hr.m_Indices.size());
- for (int j = 0 ; j < hr.m_Indices.size() ; ++j)
- m_triangles.push_back(hr.m_Indices[j]);
-
for (unsigned int j = 0 ; j < hr.mNumFaces ; ++j)
{
const btVector3 &p1 = hr.m_OutputVertices[hr.m_Indices[j * 3 ]];
@@ -319,6 +547,10 @@
std::cerr << "Approximate plane: " << behindPlane << " of " << m_vertices.size() << " points are behind the plane" << std::endl;
m_planes.push_back(planeEquation);
+
+ m_triangles.push_back(hr.m_Indices[j * 3 + 0]);
+ m_triangles.push_back(hr.m_Indices[j * 3 + 1]);
+ m_triangles.push_back(hr.m_Indices[j * 3 + 2]);
}
}
}
@@ -380,3 +612,105 @@
}
return fabs(volume)/6.0;
}
+
+namespace bodies
+{
+
+ // temp structure for intersection points (used for ordering them)
+ struct intersc
+ {
+ intersc(const btVector3 &_pt, const double _tm) : pt(_pt), time(_tm) {}
+
+ btVector3 pt;
+ double time;
+ };
+
+ struct interscOrder
+ {
+ bool ope...
[truncated message content] |
|
From: <vij...@us...> - 2009-07-28 05:17:46
|
Revision: 19826
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19826&view=rev
Author: vijaypradeep
Date: 2009-07-28 05:17:36 +0000 (Tue, 28 Jul 2009)
Log Message:
-----------
Switching all robot launch files from LaserScannerControllerNode to LaserScannerTrajControllerNode. LaserScannerControllerNode is now deprecated
Modified Paths:
--------------
pkg/trunk/demos/door_demos/launch/2dnav_pr2_door.launch
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
pkg/trunk/openrave_planning/ormanipulation/startlasermap.ros.xml
pkg/trunk/pr2/life_test/laser_tilt_test/life_test/controllers.xml
pkg/trunk/pr2/life_test/laser_tilt_test/life_test/life_test.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/laser_tilt_controller.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/slow.launch
pkg/trunk/stacks/pr2/pr2_alpha/tilt_laser.launch
Added Paths:
-----------
pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller_deprecated.xml
Removed Paths:
-------------
pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller.xml
Modified: pkg/trunk/demos/door_demos/launch/2dnav_pr2_door.launch
===================================================================
--- pkg/trunk/demos/door_demos/launch/2dnav_pr2_door.launch 2009-07-28 04:53:01 UTC (rev 19825)
+++ pkg/trunk/demos/door_demos/launch/2dnav_pr2_door.launch 2009-07-28 05:17:36 UTC (rev 19826)
@@ -8,7 +8,7 @@
<!-- node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" />
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 2 .75 .30" />-->
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 2 .75 .30" />-->
<!-- Filter for tilt laser shadowing/veiling -->
<!-- node pkg="laser_scan" type="scan_shadows_filter_node" respawn="true" machine="three" /-->
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch 2009-07-28 04:53:01 UTC (rev 19825)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch 2009-07-28 05:17:36 UTC (rev 19826)
@@ -5,7 +5,7 @@
<!-- start controller -->
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 8 .75 .25" />
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 8 .75 .25" />
<!-- convert laser scan to pointcloud -->
<node pkg="laser_scan" type="scan_to_cloud" output="screen">
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch 2009-07-28 04:53:01 UTC (rev 19825)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch 2009-07-28 05:17:36 UTC (rev 19826)
@@ -5,7 +5,7 @@
<!-- start controller -->
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 15 .75 .25" />
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 15 .75 .25" />
<!-- convert laser scan to pointcloud -->
<node pkg="laser_scan" type="scan_to_cloud" output="screen">
Modified: pkg/trunk/openrave_planning/ormanipulation/startlasermap.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startlasermap.ros.xml 2009-07-28 04:53:01 UTC (rev 19825)
+++ pkg/trunk/openrave_planning/ormanipulation/startlasermap.ros.xml 2009-07-28 05:17:36 UTC (rev 19826)
@@ -3,7 +3,7 @@
<machine name="localhost-lasermap" address="localhost" default="true"/>
<node pkg="mechanism_control" type="spawner.py" args="$(find wg_robot_description)/laser_tilt/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 10 .75 .25" />
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 10 .75 .25" />
<!-- <group ns="laser"> -->
<node pkg="laser_scan" type="scan_shadows_filter_node"/>
Modified: pkg/trunk/pr2/life_test/laser_tilt_test/life_test/controllers.xml
===================================================================
--- pkg/trunk/pr2/life_test/laser_tilt_test/life_test/controllers.xml 2009-07-28 04:53:01 UTC (rev 19825)
+++ pkg/trunk/pr2/life_test/laser_tilt_test/life_test/controllers.xml 2009-07-28 05:17:36 UTC (rev 19826)
@@ -1,10 +1,12 @@
<controllers>
-<controller name="laser_controller" topic="laser_controller" type="LaserScannerControllerNode">
- <filter smoothing_factor="0.1" />
+ <controller name="laser_tilt_controller" topic="laser_controller" type="LaserScannerTrajControllerNode">
+ <max_acc value="1.0" />
+ <max_rate value="100.0" />
+ <filter name="d_error_filter" type="TransferFunctionFilter">
+ <params a="1.0 -.1" b=".9" />
+ </filter>
<joint name="laser_tilt_mount_joint">
- <pid p="12" i=".1" d="1" iClamp="0.5" />
+ <pid p="8" i=".1" d="0.2" iClamp="0.5" />
</joint>
- </controller>
-
-
+ </controller>
</controllers>
Modified: pkg/trunk/pr2/life_test/laser_tilt_test/life_test/life_test.launch
===================================================================
--- pkg/trunk/pr2/life_test/laser_tilt_test/life_test/life_test.launch 2009-07-28 04:53:01 UTC (rev 19825)
+++ pkg/trunk/pr2/life_test/laser_tilt_test/life_test/life_test.launch 2009-07-28 05:17:36 UTC (rev 19826)
@@ -1,5 +1,5 @@
<launch>
<node pkg="mechanism_control" type="spawner.py" args="$(find life_test)/laser_tilt_test/life_test/controllers.xml" />
-
- <node pkg="life_test" type="run_laser_tilt_test.py" />
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 0.25 1.25 0.25" />
+ <!-- node pkg="life_test" type="run_laser_tilt_test.py" / -->
</launch>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/laser_tilt_controller.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/laser_tilt_controller.launch 2009-07-28 04:53:01 UTC (rev 19825)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/laser_tilt_controller.launch 2009-07-28 05:17:36 UTC (rev 19826)
@@ -1,4 +1,4 @@
<launch>
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 10 .75 .25" />
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 10 .75 .25" />
</launch>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/slow.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/slow.launch 2009-07-28 04:53:01 UTC (rev 19825)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/slow.launch 2009-07-28 05:17:36 UTC (rev 19826)
@@ -1,5 +1,5 @@
<launch>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_experimental_controllers)/laser_tilt/laser_tilt_controller.xml" />
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
<!--<node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 10 .75 .25" />-->
<node pkg="pr2_mechanism_controllers" type="send_laser_traj_cmd_ms2.py" args="laser_tilt_controller" />
</launch>
Modified: pkg/trunk/stacks/pr2/pr2_alpha/tilt_laser.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/tilt_laser.launch 2009-07-28 04:53:01 UTC (rev 19825)
+++ pkg/trunk/stacks/pr2/pr2_alpha/tilt_laser.launch 2009-07-28 05:17:36 UTC (rev 19826)
@@ -1,4 +1,4 @@
<launch>
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" output="screen"/>
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 2 0.78 0.3" />
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 2 0.78 0.3" />
</launch>
Deleted: pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller.xml 2009-07-28 04:53:01 UTC (rev 19825)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller.xml 2009-07-28 05:17:36 UTC (rev 19826)
@@ -1,8 +0,0 @@
-<controllers>
- <controller name="laser_tilt_controller" topic="laser_tilt_controller" type="LaserScannerControllerNode">
- <filter smoothing_factor="0.1" />
- <joint name="laser_tilt_mount_joint">
- <pid p="12" i=".1" d="1" iClamp="0.5" />
- </joint>
- </controller>
-</controllers>
Copied: pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller.xml (from rev 19820, pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml)
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller.xml (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller.xml 2009-07-28 05:17:36 UTC (rev 19826)
@@ -0,0 +1,12 @@
+<controllers>
+ <controller name="laser_tilt_controller" topic="laser_controller" type="LaserScannerTrajControllerNode">
+ <max_acc value="1.0" />
+ <max_rate value="100.0" />
+ <filter name="d_error_filter" type="TransferFunctionFilter">
+ <params a="1.0 -.1" b=".9" />
+ </filter>
+ <joint name="laser_tilt_mount_joint">
+ <pid p="8" i=".1" d="0.2" iClamp="0.5" />
+ </joint>
+ </controller>
+</controllers>
Property changes on: pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller.xml
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller_deprecated.xml (from rev 19820, pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller.xml)
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller_deprecated.xml (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller_deprecated.xml 2009-07-28 05:17:36 UTC (rev 19826)
@@ -0,0 +1,8 @@
+<controllers>
+ <controller name="laser_tilt_controller" topic="laser_tilt_controller" type="LaserScannerControllerNode">
+ <filter smoothing_factor="0.1" />
+ <joint name="laser_tilt_mount_joint">
+ <pid p="12" i=".1" d="1" iClamp="0.5" />
+ </joint>
+ </controller>
+</controllers>
Property changes on: pkg/trunk/stacks/pr2/pr2_default_controllers/laser_tilt_controller_deprecated.xml
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/robot_descriptions/pr2/pr2_default_controllers/laser_tilt_controller.xml:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-07-28 05:31:53
|
Revision: 19827
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19827&view=rev
Author: vijaypradeep
Date: 2009-07-28 05:31:45 +0000 (Tue, 28 Jul 2009)
Log Message:
-----------
Updating Gazebo launch files to match changes in r19826
Modified Paths:
--------------
pkg/trunk/calibration/laser_camera_calibration/gatherdata.ros.xml
pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
pkg/trunk/demos/pr2_gazebo/pr2_trajectory_controllers.launch
pkg/trunk/demos/pr2_gazebo/prototype1_default_controllers.launch
Modified: pkg/trunk/calibration/laser_camera_calibration/gatherdata.ros.xml
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/gatherdata.ros.xml 2009-07-28 05:17:36 UTC (rev 19826)
+++ pkg/trunk/calibration/laser_camera_calibration/gatherdata.ros.xml 2009-07-28 05:31:45 UTC (rev 19827)
@@ -18,7 +18,7 @@
<!-- start nodding laser -->
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 10 .4 .1" />
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 10 .4 .1" />
<!-- start the filtering node -->
<node machine="localhost_cameras" pkg="laser_camera_calibration" type="gatherdata.py" output="screen">
Modified: pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2009-07-28 05:17:36 UTC (rev 19826)
+++ pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2009-07-28 05:31:45 UTC (rev 19827)
@@ -14,10 +14,10 @@
<!--include file="$(find pr2_default_controllers)/prototype1_calibration_controller.launch" /-->
<!-- send laser tilt motor a command -->
- <!--node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 20 0.872 0.3475" respawn="false" output="screen" /-->
- <!--node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 1 .45 .40" /--> <!-- this one will pick up shoulders at lowest position -->
+ <!--node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 20 0.872 0.3475" respawn="false" output="screen" /-->
+ <!--node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 1 .45 .40" /--> <!-- this one will pick up shoulders at lowest position -->
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 2 .45 .40" />
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 2 .45 .40" />
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_trajectory_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_trajectory_controllers.launch 2009-07-28 05:17:36 UTC (rev 19826)
+++ pkg/trunk/demos/pr2_gazebo/pr2_trajectory_controllers.launch 2009-07-28 05:31:45 UTC (rev 19827)
@@ -12,7 +12,7 @@
<!-- tilting laser controller -->
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 2 .45 .40" />
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 2 .45 .40" />
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/prototype1_default_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/prototype1_default_controllers.launch 2009-07-28 05:17:36 UTC (rev 19826)
+++ pkg/trunk/demos/pr2_gazebo/prototype1_default_controllers.launch 2009-07-28 05:31:45 UTC (rev 19827)
@@ -6,8 +6,8 @@
<!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
for details of the profile, rates, see controller::LaserScannerControllerNode. -->
- <!--node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 20 0.872 0.3475" respawn="false" output="screen" /-->
+ <!--node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 20 0.872 0.3475" respawn="false" output="screen" /-->
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 2 .45 .40" />
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 2 .45 .40" />
</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-28 06:53:24
|
Revision: 19833
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19833&view=rev
Author: isucan
Date: 2009-07-28 06:53:08 +0000 (Tue, 28 Jul 2009)
Log Message:
-----------
fix deprecation warnings
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-07-28 06:24:19 UTC (rev 19832)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-07-28 06:53:08 UTC (rev 19833)
@@ -399,7 +399,7 @@
visualization_msgs::Marker mk;
mk.header.stamp = planningMonitor_->lastMapUpdate();
mk.header.frame_id = planningMonitor_->getFrameId();
- mk.ns = node_handle_.getName();
+ mk.ns = ros::this_node::getName();
mk.id = count++;
mk.type = visualization_msgs::Marker::SPHERE;
mk.action = visualization_msgs::Marker::ADD;
@@ -566,7 +566,7 @@
planningMonitor_->getEnvironmentModel()->setVerbose(false);
ros::ServiceClient client = node_handle_.serviceClient<manipulation_srvs::IKService>(ARM_IK_NAME, true);
- for (int t = 0 ; t < 20 ; ++t)
+ for (int t = 0 ; t < 2 ; ++t)
{
robot_msgs::PoseStamped tpose = req.goal_constraints.pose_constraint[0].pose;
if (t > 0)
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-28 06:24:19 UTC (rev 19832)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-28 06:53:08 UTC (rev 19833)
@@ -476,7 +476,7 @@
visualization_msgs::Marker mk;
mk.header.stamp = psetup->model->planningMonitor->lastMechanismStateUpdate();
mk.header.frame_id = psetup->model->planningMonitor->getFrameId();
- mk.ns = nh_.getName();
+ mk.ns = ros::this_node::getName();
mk.id = 1;
mk.type = visualization_msgs::Marker::POINTS;
mk.action = visualization_msgs::Marker::ADD;
Modified: pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp 2009-07-28 06:24:19 UTC (rev 19832)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp 2009-07-28 06:53:08 UTC (rev 19833)
@@ -93,7 +93,7 @@
void objectInMapUpdate(const mapping_msgs::ObjectInMapConstPtr &objectInMap)
{
visualization_msgs::Marker mk;
- mk.ns = nh_.getName() + "_" + objectInMap->id;
+ mk.ns = ros::this_node::getName() + "_" + objectInMap->id;
mk.id = 0;
mk.header = objectInMap->header;
@@ -122,7 +122,7 @@
{
const robot_msgs::Point32 &point = collisionMap->boxes[i].center;
const robot_msgs::Point32 &extents = collisionMap->boxes[i].extents;
- sendPoint(i, nh_.getName(), point.x, point.y, point.z,
+ sendPoint(i, ros::this_node::getName(), point.x, point.y, point.z,
std::max(std::max(extents.x, extents.y), extents.z) * 0.867 + collisionSpaceMonitor_->getPointCloudPadd(),
collisionMap->header);
}
@@ -136,7 +136,7 @@
for (unsigned int i = 0 ; i < link->attachedBodies.size() ; ++i)
{
visualization_msgs::Marker mk;
- mk.ns = nh_.getName() + "_attached_" + link->name;
+ mk.ns = ros::this_node::getName() + "_attached_" + link->name;
mk.id = i;
mk.header = header;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-07-29 00:30:00
|
Revision: 19941
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19941&view=rev
Author: gerkey
Date: 2009-07-29 00:29:52 +0000 (Wed, 29 Jul 2009)
Log Message:
-----------
Deprecated mux package
Added Paths:
-----------
pkg/trunk/deprecated/mux/
Removed Paths:
-------------
pkg/trunk/util/mux/
Property changes on: pkg/trunk/deprecated/mux
___________________________________________________________________
Added: svn:ignore
+ mux
switch
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/util/mux:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-29 01:58:53
|
Revision: 19960
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19960&view=rev
Author: isucan
Date: 2009-07-29 01:58:46 +0000 (Wed, 29 Jul 2009)
Log Message:
-----------
moved launch files
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
Added Paths:
-----------
pkg/trunk/sandbox/3dnav_pr2/launch/collision_map_occ.launch
pkg/trunk/sandbox/3dnav_pr2/launch/laser+stereo-perception.launch
pkg/trunk/sandbox/3dnav_pr2/launch/laser-perception.launch
pkg/trunk/sandbox/3dnav_pr2/launch/narrow_stereoproc.launch
pkg/trunk/sandbox/3dnav_pr2/launch/stereo-perception.launch
Removed Paths:
-------------
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/stereo-perception.launch
pkg/trunk/mapping/collision_map/collision_map_occ.launch
Deleted: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch 2009-07-29 01:53:44 UTC (rev 19959)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser+stereo-perception.launch 2009-07-29 01:58:46 UTC (rev 19960)
@@ -1,97 +0,0 @@
-<launch>
-
- <!-- send additional description parameters -->
- <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
-
- <!-- start controller -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 8 .75 .25" />
-
- <!-- convert laser scan to pointcloud -->
- <node pkg="laser_scan" type="scan_to_cloud" output="screen">
- <param name="scan_topic" type="string" value="tilt_scan" />
- <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
- <param name="high_fidelity" type="bool" value="true" />
- <param name="cloud_topic" type="string" value="tilt_scan_cloud" />
- </node>
-
- <!-- remove points corresponding to known objects -->
- <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
- <remap from="robot_description" to="robot_description" />
-
-<!-- define a frame that stays fixed for the known objects -->
-<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
-
- <remap from="cloud_in" to="tilt_scan_cloud" />
- <remap from="cloud_out" to="tilt_scan_cloud_without_known_objects" />
- <param name="object_scale" type="double" value="1.0" />
- <param name="object_padd" type="double" value="0.05" />
- </node>
-
- <!-- add a channel that marks points that are on the robot -->
- <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description" />
- <remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
- <remap from="cloud_out" to="tilt_scan_cloud_annotated" />
- <param name="annotate" type="string" value="on_self" />
- <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
- </node>
-
- <!-- assemble pointcloud into a full world view -->
- <node pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
- <remap from="scan_in" to="tilt_scan_cloud_annotated"/>
- <param name="tf_cache_time_secs" type="double" value="10.0" />
- <param name="tf_tolerance_secs" type="double" value="0.0" />
- <param name="max_scans" type="int" value="400" />
- <param name="ignore_laser_skew" type="bool" value="true" />
- <param name="fixed_frame" type="string" value="torso_lift_link" />
- <param name="downsample_factor" type="int" value="2" />
- </node>
-
- <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
- <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
- <remap from="build_cloud" to="point_cloud_assembler/build_cloud" />
- <remap from="full_cloud" to="full_laser_cloud_annotated" />
- </node>
-
- <include file="$(find stereo_image_proc)/narrow_stereoproc.launch" />
-
- <!-- remove points corresponding to known objects -->
- <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
- <remap from="robot_description" to="robot_description" />
-
-<!-- define a frame that stays fixed for the known objects -->
-<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
-
- <remap from="cloud_in" to="/narrow_stereo/cloud" />
- <remap from="cloud_out" to="stereo_cloud_without_known_objects" />
- </node>
-
- <!-- add a channel that marks points that are on the robot -->
- <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description" />
- <remap from="cloud_in" to="stereo_cloud_without_known_objects" />
- <remap from="cloud_out" to="full_stereo_cloud_annotated" />
- <param name="annotate" type="string" value="on_self" />
- </node>
-
- <node pkg="point_cloud_assembler" type="merge_clouds" output="screen">
-
- <!-- first input cloud -->
- <remap from="cloud_in1" to="full_stereo_cloud_annotated"/>
-
- <!-- second input cloud -->
- <remap from="cloud_in2" to="full_laser_cloud_annotated"/>
-
- <!-- output cloud -->
- <remap from="cloud_out" to="full_cloud_annotated"/>
-
- <!-- the frame in which the output cloud should be published; this parameter must ALWAYS be specified -->
- <param name="output_frame" type="string" value="base_link"/>
- </node>
-
-
- <!-- start collision map -->
- <include file="$(find collision_map)/collision_map_occ.launch" />
-
-</launch>
Deleted: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch 2009-07-29 01:53:44 UTC (rev 19959)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch 2009-07-29 01:58:46 UTC (rev 19960)
@@ -1,60 +0,0 @@
-<launch>
-
- <!-- send additional description parameters -->
- <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
-
- <!-- start controller -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 15 .75 .25" />
-
- <!-- convert laser scan to pointcloud -->
- <node pkg="laser_scan" type="scan_to_cloud" output="screen">
- <param name="scan_topic" type="string" value="tilt_scan" />
- <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
- <param name="high_fidelity" type="bool" value="true" />
- <param name="cloud_topic" type="string" value="tilt_scan_cloud" />
- </node>
-
- <!-- remove points corresponding to known objects -->
- <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
- <remap from="robot_description" to="robot_description" />
-
-<!-- define a frame that stays fixed for the known objects -->
-<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
-
- <remap from="cloud_in" to="tilt_scan_cloud" />
- <remap from="cloud_out" to="tilt_scan_cloud_without_known_objects" />
- <param name="object_scale" type="double" value="1.0" />
- <param name="object_padd" type="double" value="0.05" />
- </node>
-
- <!-- add a channel that marks points that are on the robot -->
- <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description" />
- <remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
- <remap from="cloud_out" to="tilt_scan_cloud_annotated" />
- <param name="annotate" type="string" value="on_self" />
- <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
- </node>
-
- <!-- assemble pointcloud into a full world view -->
- <node pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
- <remap from="scan_in" to="tilt_scan_cloud_annotated"/>
- <param name="tf_cache_time_secs" type="double" value="10.0" />
- <param name="tf_tolerance_secs" type="double" value="0.0" />
- <param name="max_scans" type="int" value="400" />
- <param name="ignore_laser_skew" type="bool" value="true" />
- <param name="fixed_frame" type="string" value="torso_lift_link" />
- <param name="downsample_factor" type="int" value="2" />
- </node>
-
- <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
- <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
- <remap from="build_cloud" to="point_cloud_assembler/build_cloud" />
- <remap from="full_cloud" to="full_cloud_annotated" />
- </node>
-
- <!-- start collision map -->
- <include file="$(find collision_map)/collision_map_occ.launch" />
-
-</launch>
Deleted: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/stereo-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/stereo-perception.launch 2009-07-29 01:53:44 UTC (rev 19959)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/stereo-perception.launch 2009-07-29 01:58:46 UTC (rev 19960)
@@ -1,29 +0,0 @@
-<launch>
-
- <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
- <include file="$(find stereo_image_proc)/narrow_stereoproc.launch" />
-
- <!-- remove points corresponding to known objects -->
- <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
- <remap from="robot_description" to="robot_description" />
-
-<!-- define a frame that stays fixed for the known objects -->
-<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
-
- <remap from="cloud_in" to="/narrow_stereo/cloud" />
- <remap from="cloud_out" to="full_cloud_without_known_objects" />
- <param name="object_scale" type="double" value="1.0" />
- <param name="object_padd" type="double" value="0.025" />
- </node>
-
- <!-- add a channel that marks points that are on the robot -->
- <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description" />
- <remap from="cloud_in" to="full_cloud_without_known_objects" />
- <remap from="cloud_out" to="full_cloud_annotated" />
- <param name="annotate" type="string" value="on_self" />
- </node>
-
- <include file="$(find collision_map)/collision_map_occ.launch" />
-
-</launch>
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-29 01:53:44 UTC (rev 19959)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-29 01:58:46 UTC (rev 19960)
@@ -163,19 +163,19 @@
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0.0;
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1.0;
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.002;
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.003;
- goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.008;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.002;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.003;
- goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.008;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.01;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.01;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.015;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.01;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.01;
+ goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.015;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.005;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.005;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.005;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.005;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.005;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.005;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.15;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.15;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.15;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.15;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.15;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.15;
goal.goal_constraints.pose_constraint[0].orientation_importance = 0.01;
}
Deleted: pkg/trunk/mapping/collision_map/collision_map_occ.launch
===================================================================
--- pkg/trunk/mapping/collision_map/collision_map_occ.launch 2009-07-29 01:53:44 UTC (rev 19959)
+++ pkg/trunk/mapping/collision_map/collision_map_occ.launch 2009-07-29 01:58:46 UTC (rev 19960)
@@ -1,42 +0,0 @@
-<launch>
- <node pkg="collision_map" type="collision_map_occ_node" name="collision_map_occ" respawn="true" output="screen">
- <remap from="cloud_in" to="full_cloud_annotated" />
- <remap from="cloud_incremental_in" to="tilt_scan_cloud_annotated" />
-
- <param name="publish_occlusion" type="bool" value="false" />
- <param name="mark_self_occlusion" type="bool" value="false" />
-
- <!-- tell the collision map there may be an annotation channel telling which points are on the robot -->
- <param name="cloud_annotation" type="string" value="on_self" />
-
- <!-- a frame that does not change as the robot moves -->
-<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
- <param name="fixed_frame" type="string" value="base_link" />
-
- <!-- define a box of size 2x3x4 around (1.1, 0, 0) in the robot frame -->
- <param name="robot_frame" type="string" value="base_link" />
-
- <param name="origin_x" type="double" value="1.1" />
- <param name="origin_y" type="double" value="0.0" />
- <param name="origin_z" type="double" value="0.0" />
-
- <param name="dimension_x" type="double" value="1.0" />
- <param name="dimension_y" type="double" value="1.5" />
- <param name="dimension_z" type="double" value="2.0" />
-
- <!-- set the resolution (1.0 cm) -->
- <param name="resolution" type="double" value="0.01" />
-
- <!-- the amount of time to remember an occluded point for -->
- <param name="keep_occluded" type="double" value="30.0" />
-
- <!-- when occluded obstacles are raytraced, keep boxes from occluded space within a given radius -->
- <param name="radius" type="int" value="5" />
-
- <!-- define sensor location in robot frame -->
- <param name="sensor_x" type="double" value="0.05" />
- <param name="sensor_y" type="double" value="0.0" />
- <param name="sensor_z" type="double" value="1.0" />
-
- </node>
-</launch>
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-29 01:53:44 UTC (rev 19959)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-07-29 01:58:46 UTC (rev 19960)
@@ -39,7 +39,6 @@
#include <robot_msgs/PointStamped.h>
#include <robot_msgs/PoseStamped.h>
#include <boost/bind.hpp>
-#include <sstream>
#include <climits>
namespace planning_environment
Added: pkg/trunk/sandbox/3dnav_pr2/launch/collision_map_occ.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/collision_map_occ.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/collision_map_occ.launch 2009-07-29 01:58:46 UTC (rev 19960)
@@ -0,0 +1,45 @@
+<launch>
+ <node pkg="collision_map" type="collision_map_occ_node" name="collision_map_occ" respawn="true" output="screen">
+ <remap from="cloud_in" to="full_cloud_annotated" />
+ <remap from="cloud_incremental_in" to="tilt_scan_cloud_annotated" />
+
+ <!-- we do not want a separate map with occlusions alone -->
+ <param name="publish_occlusion" type="bool" value="false" />
+
+ <!-- we do not want to assume initially occluded space (when we start the robot) is in collision -->
+ <param name="mark_self_occlusion" type="bool" value="false" />
+
+ <!-- tell the collision map there may be an annotation channel telling which points are on the robot -->
+ <param name="cloud_annotation" type="string" value="on_self" />
+
+ <!-- a frame that does not change as the robot moves -->
+<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
+ <param name="fixed_frame" type="string" value="base_link" />
+
+ <!-- define a box of size 2x3x4 around (1.1, 0, 0) in the robot frame -->
+ <param name="robot_frame" type="string" value="base_link" />
+
+ <param name="origin_x" type="double" value="1.1" />
+ <param name="origin_y" type="double" value="0.0" />
+ <param name="origin_z" type="double" value="0.0" />
+
+ <param name="dimension_x" type="double" value="1.0" />
+ <param name="dimension_y" type="double" value="1.5" />
+ <param name="dimension_z" type="double" value="2.0" />
+
+ <!-- set the resolution (1.0 cm) -->
+ <param name="resolution" type="double" value="0.01" />
+
+ <!-- the amount of time to remember an occluded point for -->
+ <param name="keep_occluded" type="double" value="30.0" />
+
+ <!-- when occluded obstacles are raytraced, keep boxes from occluded space within a given radius -->
+ <param name="radius" type="int" value="5" />
+
+ <!-- define sensor location in robot frame -->
+ <param name="sensor_x" type="double" value="0.05" />
+ <param name="sensor_y" type="double" value="0.0" />
+ <param name="sensor_z" type="double" value="1.0" />
+
+ </node>
+</launch>
Added: pkg/trunk/sandbox/3dnav_pr2/launch/laser+stereo-perception.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/laser+stereo-perception.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/laser+stereo-perception.launch 2009-07-29 01:58:46 UTC (rev 19960)
@@ -0,0 +1,97 @@
+<launch>
+
+ <!-- send additional description parameters -->
+ <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
+
+ <!-- start controller -->
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 8 .75 .25" />
+
+ <!-- convert laser scan to pointcloud -->
+ <node pkg="laser_scan" type="scan_to_cloud" output="screen">
+ <param name="scan_topic" type="string" value="tilt_scan" />
+ <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
+ <param name="high_fidelity" type="bool" value="true" />
+ <param name="cloud_topic" type="string" value="tilt_scan_cloud" />
+ </node>
+
+ <!-- remove points corresponding to known objects -->
+ <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
+ <remap from="robot_description" to="robot_description" />
+
+<!-- define a frame that stays fixed for the known objects -->
+<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
+
+ <remap from="cloud_in" to="tilt_scan_cloud" />
+ <remap from="cloud_out" to="tilt_scan_cloud_without_known_objects" />
+ <param name="object_scale" type="double" value="1.0" />
+ <param name="object_padd" type="double" value="0.05" />
+ </node>
+
+ <!-- add a channel that marks points that are on the robot -->
+ <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
+ <remap from="robot_description" to="robot_description" />
+ <remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
+ <remap from="cloud_out" to="tilt_scan_cloud_annotated" />
+ <param name="annotate" type="string" value="on_self" />
+ <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
+ </node>
+
+ <!-- assemble pointcloud into a full world view -->
+ <node pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
+ <remap from="scan_in" to="tilt_scan_cloud_annotated"/>
+ <param name="tf_cache_time_secs" type="double" value="10.0" />
+ <param name="tf_tolerance_secs" type="double" value="0.0" />
+ <param name="max_scans" type="int" value="400" />
+ <param name="ignore_laser_skew" type="bool" value="true" />
+ <param name="fixed_frame" type="string" value="torso_lift_link" />
+ <param name="downsample_factor" type="int" value="2" />
+ </node>
+
+ <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
+ <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
+ <remap from="build_cloud" to="point_cloud_assembler/build_cloud" />
+ <remap from="full_cloud" to="full_laser_cloud_annotated" />
+ </node>
+
+ <include file="$(find 3dnav_pr2)/launch/narrow_stereoproc.launch" />
+
+ <!-- remove points corresponding to known objects -->
+ <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
+ <remap from="robot_description" to="robot_description" />
+
+<!-- define a frame that stays fixed for the known objects -->
+<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
+
+ <remap from="cloud_in" to="/narrow_stereo/cloud" />
+ <remap from="cloud_out" to="stereo_cloud_without_known_objects" />
+ </node>
+
+ <!-- add a channel that marks points that are on the robot -->
+ <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
+ <remap from="robot_description" to="robot_description" />
+ <remap from="cloud_in" to="stereo_cloud_without_known_objects" />
+ <remap from="cloud_out" to="full_stereo_cloud_annotated" />
+ <param name="annotate" type="string" value="on_self" />
+ </node>
+
+ <node pkg="point_cloud_assembler" type="merge_clouds" output="screen">
+
+ <!-- first input cloud -->
+ <remap from="cloud_in1" to="full_stereo_cloud_annotated"/>
+
+ <!-- second input cloud -->
+ <remap from="cloud_in2" to="full_laser_cloud_annotated"/>
+
+ <!-- output cloud -->
+ <remap from="cloud_out" to="full_cloud_annotated"/>
+
+ <!-- the frame in which the output cloud should be published; this parameter must ALWAYS be specified -->
+ <param name="output_frame" type="string" value="base_link"/>
+ </node>
+
+
+ <!-- start collision map -->
+ <include file="$(find 3dnav_pr2)/launch/collision_map_occ.launch" />
+
+</launch>
Added: pkg/trunk/sandbox/3dnav_pr2/launch/laser-perception.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/laser-perception.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/laser-perception.launch 2009-07-29 01:58:46 UTC (rev 19960)
@@ -0,0 +1,61 @@
+<launch>
+
+ <!-- send additional description parameters -->
+ <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
+
+ <!-- start controller -->
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
+ <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 10 .75 .25" />
+
+ <!-- convert laser scan to pointcloud -->
+ <node pkg="laser_scan" type="scan_to_cloud" output="screen">
+ <param name="scan_topic" type="string" value="tilt_scan" />
+ <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
+ <param name="high_fidelity" type="bool" value="true" />
+ <param name="cloud_topic" type="string" value="tilt_scan_cloud" />
+ </node>
+
+ <!-- remove points corresponding to known objects -->
+ <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
+ <remap from="robot_description" to="robot_description" />
+
+<!-- define a frame that stays fixed for the known objects -->
+<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
+
+ <remap from="cloud_in" to="tilt_scan_cloud" />
+ <remap from="cloud_out" to="tilt_scan_cloud_without_known_objects" />
+ <param name="object_scale" type="double" value="1.0" />
+ <param name="object_padd" type="double" value="0.05" />
+ </node>
+
+ <!-- add a channel that marks points that are on the robot -->
+ <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
+ <remap from="robot_description" to="robot_description" />
+ <remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
+ <remap from="cloud_out" to="tilt_scan_cloud_annotated" />
+ <param name="annotate" type="string" value="on_self" />
+ <!-- we also want to remove shadow points -->
+ <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
+ </node>
+
+ <!-- assemble pointcloud into a full world view -->
+ <node pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
+ <remap from="scan_in" to="tilt_scan_cloud_annotated"/>
+ <param name="tf_cache_time_secs" type="double" value="10.0" />
+ <param name="tf_tolerance_secs" type="double" value="0.0" />
+ <param name="max_scans" type="int" value="400" />
+ <param name="ignore_laser_skew" type="bool" value="true" />
+ <param name="fixed_frame" type="string" value="torso_lift_link" />
+ <param name="downsample_factor" type="int" value="2" />
+ </node>
+
+ <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
+ <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
+ <remap from="build_cloud" to="point_cloud_assembler/build_cloud" />
+ <remap from="full_cloud" to="full_cloud_annotated" />
+ </node>
+
+ <!-- start collision map -->
+ <include file="$(find 3dnav_pr2)/launch/collision_map_occ.launch" />
+
+</launch>
Added: pkg/trunk/sandbox/3dnav_pr2/launch/narrow_stereoproc.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/narrow_stereoproc.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/narrow_stereoproc.launch 2009-07-29 01:58:46 UTC (rev 19960)
@@ -0,0 +1,41 @@
+<launch>
+ <!-- videre_mode should be one of the following:
+ disparity: Disparity and rectification done on chip.
+ Provides: Disparity and left mono image
+ disparity_raw: Disparity done on chip (debayer and rectification in software).
+ Provides: disparity and left color image.
+ none: No stereo on chip (all processing done in software).
+ Provides: all 3 images available
+ rectified: Rectification on chip
+ Provides: all 3 images available but no color
+ -->
+ <!-- stereo processing parameters
+ texture_thresh: Threshold for removing bad disparities based on texture
+ Default value: 30
+ unique_thresh: Threshold for removing bad disparities based on ambiguity
+ Default value: 36
+ speckle_diff: Threshold for region-growing (1/16 pixel disparity)
+ Default value: 10
+ speckle_size: Threshold for disparity region size (pixels)
+ Default value: 100
+ horopter: X offset for close-in stereo (pixels)
+ Default value: 0
+ corr_size: Correlation window size (pixels)
+ Default value: 15
+ num_disp: Number of disparities (pixels)
+ Default value: 64
+ -->
+ <group ns="narrow_stereo">
+ <node pkg="stereo_image_proc" type="stereoproc" respawn="false" output="screen" name="stereoproc">
+ <param name="videre_mode" type="str" value="none"/>
+ <param name="do_colorize" type="bool" value="True"/>
+ <param name="do_rectify" type="bool" value="True"/>
+ <param name="do_stereo" type="bool" value="True"/>
+ <param name="do_calc_points" type="bool" value="True"/>
+ <param name="do_keep_coords" type="bool" value="True"/>
+ <param name="num_disp" type="int" value="128"/>
+ <param name="corr_size" type="int" value="7"/>
+ </node>
+ </group>
+</launch>
+
Added: pkg/trunk/sandbox/3dnav_pr2/launch/stereo-perception.launch
===============================...
[truncated message content] |
|
From: <hsu...@us...> - 2009-07-29 03:05:52
|
Revision: 19971
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19971&view=rev
Author: hsujohnhsu
Date: 2009-07-29 03:05:44 +0000 (Wed, 29 Jul 2009)
Log Message:
-----------
moving physsim to sandbox for now.
Added Paths:
-----------
pkg/trunk/sandbox/physsim/
Removed Paths:
-------------
pkg/trunk/sandbox/physsim/ROS_BUILD_BLACKLIST
pkg/trunk/stacks/simulators/physsim/
Property changes on: pkg/trunk/sandbox/physsim
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/simulators/physsim:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-29 04:31:29
|
Revision: 19975
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19975&view=rev
Author: isucan
Date: 2009-07-29 04:31:22 +0000 (Wed, 29 Jul 2009)
Log Message:
-----------
implemented collision_map_self_occ based on the raytracing abilities added earlier this week; switched perception to use collision_map_self_occ instead of collision_map_occ;
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/CMakeLists.txt
pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/util/robot_self_filter/src/self_mask.cpp
Added Paths:
-----------
pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
pkg/trunk/sandbox/3dnav_pr2/launch/perception/
pkg/trunk/sandbox/3dnav_pr2/launch/perception/collision_map_occ.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/collision_map_self_occ.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser+stereo-perception.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/narrow_stereoproc.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/stereo-perception.launch
Removed Paths:
-------------
pkg/trunk/sandbox/3dnav_pr2/launch/collision_map_occ.launch
pkg/trunk/sandbox/3dnav_pr2/launch/laser+stereo-perception.launch
pkg/trunk/sandbox/3dnav_pr2/launch/laser-perception.launch
pkg/trunk/sandbox/3dnav_pr2/launch/narrow_stereoproc.launch
pkg/trunk/sandbox/3dnav_pr2/launch/stereo-perception.launch
Modified: pkg/trunk/mapping/collision_map/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-07-29 04:31:22 UTC (rev 19975)
@@ -82,3 +82,7 @@
rospack_add_executable(collision_map_occ_node src/collision_map_occ.cpp)
rospack_add_openmp_flags(collision_map_occ_node)
rospack_link_boost(collision_map_occ_node thread)
+
+rospack_add_executable(collision_map_self_occ_node src/collision_map_self_occ.cpp)
+rospack_add_openmp_flags(collision_map_self_occ_node)
+rospack_link_boost(collision_map_self_occ_node thread)
Modified: pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-07-29 04:31:22 UTC (rev 19975)
@@ -828,7 +828,7 @@
int main (int argc, char** argv)
{
- ros::init(argc, argv, "collision_map_occ");
+ ros::init(argc, argv, "collision_map_occ", ros::init_options::AnonymousName);
CollisionMapperOcc cm;
Copied: pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp (from rev 19967, pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp)
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp (rev 0)
+++ pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp 2009-07-29 04:31:22 UTC (rev 19975)
@@ -0,0 +1,411 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#include <ros/ros.h>
+#include <robot_msgs/PointCloud.h>
+#include <mapping_msgs/CollisionMap.h>
+#include <tf/transform_listener.h>
+#include <tf/message_notifier.h>
+#include <robot_self_filter/self_mask.h>
+#include <boost/thread/mutex.hpp>
+#include <boost/bind.hpp>
+#include <vector>
+#include <algorithm>
+#include <set>
+#include <iterator>
+#include <cstdlib>
+
+class CollisionMapperOcc
+{
+public:
+
+ CollisionMapperOcc(void) : sf_(tf_)
+ {
+ // read ROS params
+ loadParams();
+
+ // advertise our topics: full map and updates
+ cmapPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_occ", 1);
+ cmapUpdPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_occ_update", 1);
+ if (publishOcclusion_)
+ occPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_occ_occlusion", 1);
+
+ // create a message notifier (and enable subscription) for both the full map and for the updates
+ mnCloud_ = new tf::MessageNotifier<robot_msgs::PointCloud>(tf_, boost::bind(&CollisionMapperOcc::cloudCallback, this, _1), "cloud_in", "", 1);
+ mnCloudIncremental_ = new tf::MessageNotifier<robot_msgs::PointCloud>(tf_, boost::bind(&CollisionMapperOcc::cloudIncrementalCallback, this, _1), "cloud_incremental_in", "", 1);
+
+ // configure the self mask and the message notifier
+ std::vector<std::string> frames;
+ sf_.getLinkNames(frames);
+ if (std::find(frames.begin(), frames.end(), robotFrame_) == frames.end())
+ frames.push_back(robotFrame_);
+ mnCloud_->setTargetFrame(frames);
+ mnCloudIncremental_->setTargetFrame(frames);
+ }
+
+ ~CollisionMapperOcc(void)
+ {
+ delete mnCloud_;
+ delete mnCloudIncremental_;
+ }
+
+ void run(void)
+ {
+ if (bi_.sensor_frame.empty())
+ ROS_ERROR("No sensor frame specified. Cannot perform raytracing");
+ else
+ ros::spin();
+ }
+
+private:
+
+ struct CollisionPoint
+ {
+ CollisionPoint(void) {}
+ CollisionPoint(int _x, int _y, int _z) : x(_x), y(_y), z(_z) {}
+
+ int x, y, z;
+ };
+
+ // define an order on points
+ struct CollisionPointOrder
+ {
+ bool operator()(const CollisionPoint &a, const CollisionPoint &b) const
+ {
+ if (a.x < b.x)
+ return true;
+ if (a.x > b.x)
+ return false;
+ if (a.y < b.y)
+ return true;
+ if (a.y > b.y)
+ return false;
+ return a.z < b.z;
+ }
+ };
+
+ typedef std::set<CollisionPoint, CollisionPointOrder> CMap;
+
+ // parameters & precomputed values for the box that represents the collision map
+ // around the robot
+ struct BoxInfo
+ {
+ double dimensionX, dimensionY, dimensionZ;
+ double originX, originY, originZ;
+ std::string sensor_frame;
+ double resolution;
+ double real_minX, real_minY, real_minZ;
+ double real_maxX, real_maxY, real_maxZ;
+ };
+
+ void loadParams(void)
+ {
+ // a frame that does not move with the robot
+ nh_.param<std::string>("~fixed_frame", fixedFrame_, "odom");
+
+ // a frame that moves with the robot
+ nh_.param<std::string>("~robot_frame", robotFrame_, "base_link");
+
+ // bounds of collision map in robot frame
+ nh_.param<double>("~dimension_x", bi_.dimensionX, 1.0);
+ nh_.param<double>("~dimension_y", bi_.dimensionY, 1.5);
+ nh_.param<double>("~dimension_z", bi_.dimensionZ, 2.0);
+
+ // origin of collision map in the robot frame
+ nh_.param<double>("~origin_x", bi_.originX, 1.1);
+ nh_.param<double>("~origin_y", bi_.originY, 0.0);
+ nh_.param<double>("~origin_z", bi_.originZ, 0.0);
+
+ // sensor frame
+ nh_.param<std::string>("~sensor_frame", bi_.sensor_frame, std::string());
+
+ // resolution
+ nh_.param<double>("~resolution", bi_.resolution, 0.015);
+
+ ROS_INFO("Maintaining occlusion map in frame '%s', with origin at (%f, %f, %f) and dimension (%f, %f, %f), resolution of %f; "
+ "sensor is in frame '%s', fixed fame is '%s'.",
+ robotFrame_.c_str(), bi_.dimensionX, bi_.dimensionY, bi_.dimensionZ, bi_.originX, bi_.originY, bi_.originZ,
+ bi_.resolution, bi_.sensor_frame.c_str(), fixedFrame_.c_str());
+
+ nh_.param<bool>("~publish_occlusion", publishOcclusion_, false);
+
+ // compute some useful values
+ bi_.real_minX = -bi_.dimensionX + bi_.originX;
+ bi_.real_maxX = bi_.dimensionX + bi_.originX;
+ bi_.real_minY = -bi_.dimensionY + bi_.originY;
+ bi_.real_maxY = bi_.dimensionY + bi_.originY;
+ bi_.real_minZ = -bi_.dimensionZ + bi_.originZ;
+ bi_.real_maxZ = bi_.dimensionZ + bi_.originZ;
+ }
+
+ void cloudIncrementalCallback(const robot_msgs::PointCloudConstPtr &cloud)
+ {
+ if (!mapProcessing_.try_lock())
+ return;
+
+ ROS_DEBUG("Got pointcloud update that is %f seconds old", (ros::Time::now() - cloud->header.stamp).toSec());
+
+ robot_msgs::PointCloud out;
+ ros::WallTime tm = ros::WallTime::now();
+
+ // transform the pointcloud to the robot frame
+ // since we need the points in this frame (around the robot)
+ // to compute the collision map
+ tf_.transformPointCloud(robotFrame_, *cloud, out);
+
+ CMap obstacles;
+ constructCollisionMap(out, obstacles);
+
+ CMap diff;
+ set_difference(obstacles.begin(), obstacles.end(), currentMap_.begin(), currentMap_.end(),
+ std::inserter(diff, diff.begin()), CollisionPointOrder());
+ mapProcessing_.unlock();
+
+ if (!diff.empty())
+ publishCollisionMap(diff, out.header, cmapUpdPublisher_);
+ }
+
+ 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();
+
+ robot_msgs::PointCloud out;
+ ros::WallTime tm = ros::WallTime::now();
+
+ CMap obstacles;
+
+ // transform the pointcloud to the robot frame
+ // since we need the points in this frame (around the robot)
+ // to compute the collision map
+ tf_.transformPointCloud(robotFrame_, *cloud, out);
+ constructCollisionMap(out, obstacles);
+
+ // try to transform the previous map (if it exists) to the new frame
+ if (!currentMap_.empty())
+ if (!transformMap(currentMap_, header_, out.header))
+ currentMap_.clear();
+ header_ = out.header;
+
+ // update map
+ updateMap(obstacles);
+
+ double sec = (ros::WallTime::now() - tm).toSec();
+ ROS_INFO("Updated collision map with %d points at %f Hz", currentMap_.size(), 1.0/sec);
+
+ publishCollisionMap(currentMap_, header_, cmapPublisher_);
+ mapProcessing_.unlock();
+ }
+
+ void updateMap(CMap &obstacles)
+ {
+ if (currentMap_.empty())
+ currentMap_ = obstacles;
+ else
+ {
+ CMap diff;
+
+ // find the points from the old map that are no longer visible
+ set_difference(currentMap_.begin(), currentMap_.end(), obstacles.begin(), obstacles.end(),
+ std::inserter(diff, diff.begin()), CollisionPointOrder());
+
+ // the current map will at least contain the new info
+ currentMap_ = obstacles;
+
+ // find out which of these points are now occluded
+ sf_.assumeFrame(header_, bi_.sensor_frame);
+
+ // OpenMP need an int as the lookup variable, but for set,
+ // this is not possible, so we copy to a vector
+ int n = diff.size();
+ std::vector<CollisionPoint> pts(n);
+ std::copy(diff.begin(), diff.end(), pts.begin());
+
+ // add points occluded by self
+ if (publishOcclusion_)
+ {
+ CMap keep;
+
+#pragma omp parallel for
+ for (int i = 0 ; i < n ; ++i)
+ {
+ btVector3 p(((double)pts[i].x - 0.5) * bi_.resolution + bi_.originX,
+ ((double)pts[i].y - 0.5) * bi_.resolution + bi_.originY,
+ ((double)pts[i].z - 0.5) * bi_.resolution + bi_.originZ);
+ if (sf_.getMaskIntersection(p) == robot_self_filter::SHADOW)
+ {
+#pragma omp critical
+ {
+ keep.insert(pts[i]);
+ currentMap_.insert(pts[i]);
+ }
+ }
+ }
+
+ publishCollisionMap(keep, header_, occPublisher_);
+ }
+ else
+ {
+#pragma omp parallel for
+ for (int i = 0 ; i < n ; ++i)
+ {
+ btVector3 p(((double)pts[i].x - 0.5) * bi_.resolution + bi_.originX,
+ ((double)pts[i].y - 0.5) * bi_.resolution + bi_.originY,
+ ((double)pts[i].z - 0.5) * bi_.resolution + bi_.originZ);
+ if (sf_.getMaskIntersection(p) == robot_self_filter::SHADOW)
+ {
+#pragma omp critical
+ {
+ currentMap_.insert(pts[i]);
+ }
+ }
+ }
+
+ }
+
+ }
+ }
+
+ bool transformMap(CMap &map, const roslib::Header &from, const roslib::Header &to)
+ {
+ if (tf_.canTransform(to.frame_id, to.stamp, from.frame_id, from.stamp, fixedFrame_))
+ {
+ tf::Stamped<btTransform> transf;
+ tf_.lookupTransform(to.frame_id, to.stamp, from.frame_id, from.stamp, fixedFrame_, transf);
+
+ // copy data to temporary location
+ const int n = map.size();
+ std::vector<CollisionPoint> pts(n);
+ std::copy(map.begin(), map.end(), pts.begin());
+ map.clear();
+
+#pragma omp parallel for
+ for (int i = 0 ; i < n ; ++i)
+ {
+ btVector3 p(((double)pts[i].x - 0.5) * bi_.resolution + bi_.originX,
+ ((double)pts[i].y - 0.5) * bi_.resolution + bi_.originY,
+ ((double)pts[i].z - 0.5) * bi_.resolution + bi_.originZ);
+ p = transf * p;
+ if (p.x() > bi_.real_minX && p.x() < bi_.real_maxX && p.y() > bi_.real_minY && p.y() < bi_.real_maxY && p.z() > bi_.real_minZ && p.z() < bi_.real_maxZ)
+ {
+ CollisionPoint c((int)(0.5 + (p.x() - bi_.originX) / bi_.resolution),
+ (int)(0.5 + (p.y() - bi_.originY) / bi_.resolution),
+ (int)(0.5 + (p.z() - bi_.originZ) / bi_.resolution));
+ map.insert(c);
+ }
+ }
+
+ return true;
+ }
+ else
+ {
+ ROS_WARN("Unable to transform previous collision map into new frame");
+ return false;
+ }
+ }
+
+ /** Construct an axis-aligned collision map from a point cloud assumed to be in the robot frame */
+ void constructCollisionMap(const robot_msgs::PointCloud &cloud, CMap &map)
+ {
+ const unsigned int n = cloud.pts.size();
+ CollisionPoint c;
+
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ const robot_msgs::Point32 &p = cloud.pts[i];
+ if (p.x > bi_.real_minX && p.x < bi_.real_maxX && p.y > bi_.real_minY && p.y < bi_.real_maxY && p.z > bi_.real_minZ && p.z < bi_.real_maxZ)
+ {
+ c.x = (int)(0.5 + (p.x - bi_.originX) / bi_.resolution);
+ c.y = (int)(0.5 + (p.y - bi_.originY) / bi_.resolution);
+ c.z = (int)(0.5 + (p.z - bi_.originZ) / bi_.resolution);
+ map.insert(c);
+ }
+ }
+ }
+
+ void publishCollisionMap(const CMap &map, const roslib::Header &header, ros::Publisher &pub) const
+ {
+ mapping_msgs::CollisionMap cmap;
+ cmap.header = header;
+ const unsigned int ms = map.size();
+
+ for (CMap::const_iterator it = map.begin() ; it != map.end() ; ++it)
+ {
+ const CollisionPoint &cp = *it;
+ mapping_msgs::OrientedBoundingBox box;
+ box.extents.x = box.extents.y = box.extents.z = bi_.resolution;
+ box.axis.x = box.axis.y = 0.0; box.axis.z = 1.0;
+ box.angle = 0.0;
+ box.center.x = cp.x * bi_.resolution + bi_.originX;
+ box.center.y = cp.y * bi_.resolution + bi_.originY;
+ box.center.z = cp.z * bi_.resolution + bi_.originZ;
+ cmap.boxes.push_back(box);
+ }
+ pub.publish(cmap);
+
+ ROS_DEBUG("Published collision map with %u boxes", ms);
+ }
+
+ tf::TransformListener tf_;
+ robot_self_filter::SelfMask sf_;
+ tf::MessageNotifier<robot_msgs::PointCloud> *mnCloud_;
+ tf::MessageNotifier<robot_msgs::PointCloud> *mnCloudIncremental_;
+ ros::NodeHandle nh_;
+ ros::Publisher cmapPublisher_;
+ ros::Publisher cmapUpdPublisher_;
+ ros::Publisher occPublisher_;
+ roslib::Header header_;
+ bool publishOcclusion_;
+
+ boost::mutex mapProcessing_;
+ CMap currentMap_;
+
+ BoxInfo bi_;
+ std::string fixedFrame_;
+ std::string robotFrame_;
+
+};
+
+int main (int argc, char** argv)
+{
+ ros::init(argc, argv, "collision_map_self_occ", ros::init_options::AnonymousName);
+
+ CollisionMapperOcc cm;
+ cm.run();
+
+ return 0;
+}
Property changes on: pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/mapping/collision_map/src/collision_map_occ.cpp:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Deleted: pkg/trunk/sandbox/3dnav_pr2/launch/collision_map_occ.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/collision_map_occ.launch 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/collision_map_occ.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -1,45 +0,0 @@
-<launch>
- <node pkg="collision_map" type="collision_map_occ_node" name="collision_map_occ" respawn="true" output="screen">
- <remap from="cloud_in" to="full_cloud_annotated" />
- <remap from="cloud_incremental_in" to="tilt_scan_cloud_annotated" />
-
- <!-- we do not want a separate map with occlusions alone -->
- <param name="publish_occlusion" type="bool" value="false" />
-
- <!-- we do not want to assume initially occluded space (when we start the robot) is in collision -->
- <param name="mark_self_occlusion" type="bool" value="false" />
-
- <!-- tell the collision map there may be an annotation channel telling which points are on the robot -->
- <param name="cloud_annotation" type="string" value="on_self" />
-
- <!-- a frame that does not change as the robot moves -->
-<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
- <param name="fixed_frame" type="string" value="base_link" />
-
- <!-- define a box of size 2x3x4 around (1.1, 0, 0) in the robot frame -->
- <param name="robot_frame" type="string" value="base_link" />
-
- <param name="origin_x" type="double" value="1.1" />
- <param name="origin_y" type="double" value="0.0" />
- <param name="origin_z" type="double" value="0.0" />
-
- <param name="dimension_x" type="double" value="1.0" />
- <param name="dimension_y" type="double" value="1.5" />
- <param name="dimension_z" type="double" value="2.0" />
-
- <!-- set the resolution (1.0 cm) -->
- <param name="resolution" type="double" value="0.01" />
-
- <!-- the amount of time to remember an occluded point for -->
- <param name="keep_occluded" type="double" value="30.0" />
-
- <!-- when occluded obstacles are raytraced, keep boxes from occluded space within a given radius -->
- <param name="radius" type="int" value="5" />
-
- <!-- define sensor location in robot frame -->
- <param name="sensor_x" type="double" value="0.05" />
- <param name="sensor_y" type="double" value="0.0" />
- <param name="sensor_z" type="double" value="1.0" />
-
- </node>
-</launch>
Deleted: pkg/trunk/sandbox/3dnav_pr2/launch/laser+stereo-perception.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/laser+stereo-perception.launch 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/laser+stereo-perception.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -1,97 +0,0 @@
-<launch>
-
- <!-- send additional description parameters -->
- <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
-
- <!-- start controller -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 8 .75 .25" />
-
- <!-- convert laser scan to pointcloud -->
- <node pkg="laser_scan" type="scan_to_cloud" output="screen">
- <param name="scan_topic" type="string" value="tilt_scan" />
- <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
- <param name="high_fidelity" type="bool" value="true" />
- <param name="cloud_topic" type="string" value="tilt_scan_cloud" />
- </node>
-
- <!-- remove points corresponding to known objects -->
- <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
- <remap from="robot_description" to="robot_description" />
-
-<!-- define a frame that stays fixed for the known objects -->
-<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
-
- <remap from="cloud_in" to="tilt_scan_cloud" />
- <remap from="cloud_out" to="tilt_scan_cloud_without_known_objects" />
- <param name="object_scale" type="double" value="1.0" />
- <param name="object_padd" type="double" value="0.05" />
- </node>
-
- <!-- add a channel that marks points that are on the robot -->
- <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description" />
- <remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
- <remap from="cloud_out" to="tilt_scan_cloud_annotated" />
- <param name="annotate" type="string" value="on_self" />
- <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
- </node>
-
- <!-- assemble pointcloud into a full world view -->
- <node pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
- <remap from="scan_in" to="tilt_scan_cloud_annotated"/>
- <param name="tf_cache_time_secs" type="double" value="10.0" />
- <param name="tf_tolerance_secs" type="double" value="0.0" />
- <param name="max_scans" type="int" value="400" />
- <param name="ignore_laser_skew" type="bool" value="true" />
- <param name="fixed_frame" type="string" value="torso_lift_link" />
- <param name="downsample_factor" type="int" value="2" />
- </node>
-
- <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
- <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
- <remap from="build_cloud" to="point_cloud_assembler/build_cloud" />
- <remap from="full_cloud" to="full_laser_cloud_annotated" />
- </node>
-
- <include file="$(find 3dnav_pr2)/launch/narrow_stereoproc.launch" />
-
- <!-- remove points corresponding to known objects -->
- <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
- <remap from="robot_description" to="robot_description" />
-
-<!-- define a frame that stays fixed for the known objects -->
-<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
-
- <remap from="cloud_in" to="/narrow_stereo/cloud" />
- <remap from="cloud_out" to="stereo_cloud_without_known_objects" />
- </node>
-
- <!-- add a channel that marks points that are on the robot -->
- <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description" />
- <remap from="cloud_in" to="stereo_cloud_without_known_objects" />
- <remap from="cloud_out" to="full_stereo_cloud_annotated" />
- <param name="annotate" type="string" value="on_self" />
- </node>
-
- <node pkg="point_cloud_assembler" type="merge_clouds" output="screen">
-
- <!-- first input cloud -->
- <remap from="cloud_in1" to="full_stereo_cloud_annotated"/>
-
- <!-- second input cloud -->
- <remap from="cloud_in2" to="full_laser_cloud_annotated"/>
-
- <!-- output cloud -->
- <remap from="cloud_out" to="full_cloud_annotated"/>
-
- <!-- the frame in which the output cloud should be published; this parameter must ALWAYS be specified -->
- <param name="output_frame" type="string" value="base_link"/>
- </node>
-
-
- <!-- start collision map -->
- <include file="$(find 3dnav_pr2)/launch/collision_map_occ.launch" />
-
-</launch>
Deleted: pkg/trunk/sandbox/3dnav_pr2/launch/laser-perception.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/laser-perception.launch 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/laser-perception.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -1,61 +0,0 @@
-<launch>
-
- <!-- send additional description parameters -->
- <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
-
- <!-- start controller -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="send_periodic_cmd_srv.py" args="laser_tilt_controller linear 10 .75 .25" />
-
- <!-- convert laser scan to pointcloud -->
- <node pkg="laser_scan" type="scan_to_cloud" output="screen">
- <param name="scan_topic" type="string" value="tilt_scan" />
- <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
- <param name="high_fidelity" type="bool" value="true" />
- <param name="cloud_topic" type="string" value="tilt_scan_cloud" />
- </node>
-
- <!-- remove points corresponding to known objects -->
- <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
- <remap from="robot_description" to="robot_description" />
-
-<!-- define a frame that stays fixed for the known objects -->
-<!-- <param name="fixed_frame" type="string" value="/odom_combined" /> -->
-
- <remap from="cloud_in" to="tilt_scan_cloud" />
- <remap from="cloud_out" to="tilt_scan_cloud_without_known_objects" />
- <param name="object_scale" type="double" value="1.0" />
- <param name="object_padd" type="double" value="0.05" />
- </node>
-
- <!-- add a channel that marks points that are on the robot -->
- <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
- <remap from="robot_description" to="robot_description" />
- <remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
- <remap from="cloud_out" to="tilt_scan_cloud_annotated" />
- <param name="annotate" type="string" value="on_self" />
- <!-- we also want to remove shadow points -->
- <param name="sensor_frame" type="string" value="laser_tilt_mount_link" />
- </node>
-
- <!-- assemble pointcloud into a full world view -->
- <node pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
- <remap from="scan_in" to="tilt_scan_cloud_annotated"/>
- <param name="tf_cache_time_secs" type="double" value="10.0" />
- <param name="tf_tolerance_secs" type="double" value="0.0" />
- <param name="max_scans" type="int" value="400" />
- <param name="ignore_laser_skew" type="bool" value="true" />
- <param name="fixed_frame" type="string" value="torso_lift_link" />
- <param name="downsample_factor" type="int" value="2" />
- </node>
-
- <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
- <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
- <remap from="build_cloud" to="point_cloud_assembler/build_cloud" />
- <remap from="full_cloud" to="full_cloud_annotated" />
- </node>
-
- <!-- start collision map -->
- <include file="$(find 3dnav_pr2)/launch/collision_map_occ.launch" />
-
-</launch>
Deleted: pkg/trunk/sandbox/3dnav_pr2/launch/narrow_stereoproc.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/narrow_stereoproc.launch 2009-07-29 03:49:39 UTC (rev 19974)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/narrow_stereoproc.launch 2009-07-29 04:31:22 UTC (rev 19975)
@@ -1,41 +0,0 @@
-<launch>
- <!-- videre_mode should be one of the following:
- disparity: Disparity and rectification done on chip.
- Provides: Disparity and left mono image
- disparity_raw: Disparity done on chip (debayer and rectification in software).
- Provides: disparity and left color image.
- none: No stereo on chip (all processing done in software).
- Provides: all 3 images available
- rectified: Rectification on chip
- Provides: all 3 images available but no color
- -->
- <!-- stereo processing parameters
- texture_thresh: Threshold for removing bad disparities based on texture
- Default value: 30
- unique_thresh: Threshold for removing bad disparities based on ambiguity
- Default value: 36
- speckle_diff: Threshold for region-growing (1/16 pixel disparity)
- Default value: 10
- speckle_size: Threshold for disparity region size (pixels)
- Default value: 100
- horopt...
[truncated message content] |
|
From: <is...@us...> - 2009-07-30 00:50:35
|
Revision: 20094
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20094&view=rev
Author: isucan
Date: 2009-07-30 00:50:20 +0000 (Thu, 30 Jul 2009)
Log Message:
-----------
bugfixes for planning environment, gaik; removed motion_planning_srvs package ; added service for searching for valid states using GAIK ; using this service in move_arm (seeded with ik solutions) to find valid states
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
pkg/trunk/highlevel/move_arm/launch/move_larm.launch
pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
pkg/trunk/highlevel/move_arm/manifest.xml
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/motion_planning/motion_planning_msgs/CMakeLists.txt
pkg/trunk/motion_planning/motion_planning_msgs/manifest.xml
pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/GAIK.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/src/GAIK.cpp
pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/Model.h
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/PlannerSetup.h
pkg/trunk/motion_planning/ompl_planning/manifest.xml
pkg/trunk/motion_planning/ompl_planning/src/helpers/Model.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/GoalDefinitions.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/plan_ompl_path/manifest.xml
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_planner_node.h
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp
pkg/trunk/motion_planning/planning_research/sbpl_pm_wrapper/include/sbpl_pm_wrapper/pm_wrapper.h
pkg/trunk/motion_planning/planning_research/sbpl_pm_wrapper/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_pm_wrapper/src/pm_wrapper.cpp
pkg/trunk/sandbox/3dnav_pr2/manifest.xml
pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml
pkg/trunk/sandbox/chomp_motion_planner/include/chomp_motion_planner/chomp_planner_node.h
pkg/trunk/sandbox/chomp_motion_planner/manifest.xml
pkg/trunk/sandbox/chomp_motion_planner/src/chomp_planner_node.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/motion_planning_msgs/srv/
pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertPoseToJointConstraint.srv
pkg/trunk/motion_planning/motion_planning_msgs/srv/GetMotionPlan.srv
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/IKSetup.cpp
pkg/trunk/motion_planning/ompl_planning/src/request_handler/
pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h
pkg/trunk/motion_planning/plan_ompl_path/ROS_BUILD_BLACKLIST
Removed Paths:
-------------
pkg/trunk/motion_planning/motion_planning_srvs/
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/RequestHandler.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
Modified: pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
===================================================================
--- pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-07-30 00:50:20 UTC (rev 20094)
@@ -44,7 +44,7 @@
#include <manipulation_msgs/JointTraj.h>
#include <pr2_robot_actions/MoveArmState.h>
#include <pr2_robot_actions/MoveArmGoal.h>
-#include <motion_planning_srvs/MotionPlan.h>
+#include <motion_planning_msgs/GetMotionPlan.h>
#include <ros/ros.h>
#include <planning_environment/monitors/planning_monitor.h>
@@ -104,8 +104,8 @@
bool fixState(planning_models::StateParams &sp, bool atGoal);
bool fillStartState(std::vector<motion_planning_msgs::KinematicJoint> &start);
void fillTrajectoryPath(const motion_planning_msgs::KinematicPath &path, manipulation_msgs::JointTraj &traj);
- bool alterRequestUsingIK(motion_planning_srvs::MotionPlan::Request &req);
- bool computeIK(ros::ServiceClient &client, const robot_msgs::PoseStamped &pose_stamped_msg, int attempts, std::vector<double> &solution);
+ bool alterRequestUsingIK(motion_planning_msgs::GetMotionPlan::Request &req);
+ bool computeIK(ros::ServiceClient &client, const robot_msgs::PoseStamped &pose_stamped_msg, std::vector<double> &solution);
};
}
Modified: pkg/trunk/highlevel/move_arm/launch/move_larm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_larm.launch 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/highlevel/move_arm/launch/move_larm.launch 2009-07-30 00:50:20 UTC (rev 20094)
@@ -6,13 +6,14 @@
<remap from="collision_map_update" to="collision_map_occ_update" />
<remap from="arm_ik" to="pr2_ik_left_arm/ik_service" />
- <remap from="arm_ik_query" to="pr2_ik_left_arm/ik_query" />
+
+ <remap from="get_valid_state" to="find_valid_state" />
<remap from="controller_start" to="l_arm_joint_trajectory_controller/TrajectoryStart" />
<remap from="controller_query" to="l_arm_joint_trajectory_controller/TrajectoryQuery" />
<remap from="controller_cancel" to="l_arm_joint_trajectory_controller/TrajectoryCancel" />
- <remap from="motion_plan" to="plan_kinematic_path" />
+ <remap from="get_motion_plan" to="plan_kinematic_path" />
<param name="arm" type="string" value="left_arm" />
<param name="show_collisions" type="bool" value="true" />
Modified: pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-07-30 00:50:20 UTC (rev 20094)
@@ -6,13 +6,14 @@
<remap from="collision_map_update" to="collision_map_occ_update" />
<remap from="arm_ik" to="pr2_ik_right_arm/ik_service" />
- <remap from="arm_ik_query" to="pr2_ik_right_arm/ik_query" />
+
+ <remap from="get_valid_state" to="find_valid_state" />
<remap from="controller_start" to="r_arm_joint_trajectory_controller/TrajectoryStart" />
<remap from="controller_query" to="r_arm_joint_trajectory_controller/TrajectoryQuery" />
<remap from="controller_cancel" to="r_arm_joint_trajectory_controller/TrajectoryCancel" />
- <remap from="motion_plan" to="plan_kinematic_path" />
+ <remap from="get_motion_plan" to="plan_kinematic_path" />
<param name="arm" type="string" value="right_arm" />
<param name="show_collisions" type="bool" value="true" />
Modified: pkg/trunk/highlevel/move_arm/manifest.xml
===================================================================
--- pkg/trunk/highlevel/move_arm/manifest.xml 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/highlevel/move_arm/manifest.xml 2009-07-30 00:50:20 UTC (rev 20094)
@@ -14,7 +14,6 @@
<depend package="std_msgs"/>
<depend package="robot_msgs"/>
<depend package="motion_planning_msgs"/>
- <depend package="motion_planning_srvs"/>
<depend package="manipulation_srvs"/>
<depend package="visualization_msgs"/>
<depend package="planning_models" />
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-07-30 00:50:20 UTC (rev 20094)
@@ -43,6 +43,7 @@
#include <manipulation_srvs/IKService.h>
#include <manipulation_srvs/IKQuery.h>
+#include <motion_planning_msgs/ConvertPoseToJointConstraint.h>
#include <visualization_msgs/Marker.h>
#include <cstdlib>
@@ -53,14 +54,13 @@
{
// these are the strings used internally to access services
// they should be remaped in the launch file
- static const std::string CONTROL_START_NAME = "controller_start";
- static const std::string CONTROL_QUERY_NAME = "controller_query";
- static const std::string CONTROL_CANCEL_NAME = "controller_cancel";
- static const std::string MOTION_PLAN_NAME = "motion_plan";
+ static const std::string CONTROL_START_NAME = "controller_start";
+ static const std::string CONTROL_QUERY_NAME = "controller_query";
+ static const std::string CONTROL_CANCEL_NAME = "controller_cancel";
+ static const std::string MOTION_PLAN_NAME = "get_motion_plan";
+ static const std::string SEARCH_VALID_STATE_NAME = "get_valid_state";
+ static const std::string ARM_IK_NAME = "arm_ik";
- static const std::string ARM_IK_NAME = "arm_ik";
- static const std::string ARM_IK_QUERY_NAME = "arm_ik_query";
-
MoveArm::MoveArm(const::std::string &arm_name) : Action<pr2_robot_actions::MoveArmGoal, int32_t>("move_" + arm_name)
{
valid_ = true;
@@ -120,8 +120,8 @@
return robot_actions::ABORTED;
}
- motion_planning_srvs::MotionPlan::Request req;
- motion_planning_srvs::MotionPlan::Response res;
+ motion_planning_msgs::GetMotionPlan::Request req;
+ motion_planning_msgs::GetMotionPlan::Response res;
req.params.model_id = arm_; // the model to plan for (should be defined in planning.yaml)
@@ -140,17 +140,19 @@
// tell the planning monitor about the constraints we will be following
planningMonitor_->setPathConstraints(req.path_constraints);
planningMonitor_->setGoalConstraints(req.goal_constraints);
+
+ // fill the staring state
+ fillStartState(req.start_state);
if (perform_ik_)
alterRequestUsingIK(req);
-
ResultStatus result = robot_actions::SUCCESS;
feedback = pr2_robot_actions::MoveArmState::PLANNING;
update(feedback);
- ros::ServiceClient clientPlan = node_handle_.serviceClient<motion_planning_srvs::MotionPlan>(MOTION_PLAN_NAME, true);
+ ros::ServiceClient clientPlan = node_handle_.serviceClient<motion_planning_msgs::GetMotionPlan>(MOTION_PLAN_NAME, true);
ros::ServiceClient clientStart = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryStart>(CONTROL_START_NAME, true);
ros::ServiceClient clientQuery = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryQuery>(CONTROL_QUERY_NAME, true);
ros::ServiceClient clientCancel = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryCancel>(CONTROL_CANCEL_NAME, true);
@@ -369,19 +371,28 @@
void MoveArm::fillTrajectoryPath(const motion_planning_msgs::KinematicPath &path, manipulation_msgs::JointTraj &traj)
{
- /// \todo Joint controller does not take joint names; make sure we set them when the controller is updated
traj.names = arm_joint_names_;
traj.points.resize(path.states.size());
+ planning_models::StateParams *sp = planningMonitor_->getKinematicModel()->newStateParams();
for (unsigned int i = 0 ; i < path.states.size() ; ++i)
{
- traj.points[i].positions = path.states[i].vals;
traj.points[i].time = path.times[i];
+ traj.points[i].positions.reserve(path.states[i].vals.size());
+
+ sp->setParamsGroup(path.states[i].vals, arm_);
+ for (unsigned int j = 0 ; j < arm_joint_names_.size() ; ++j)
+ {
+ planning_models::KinematicModel::Joint *joint = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[j]);
+ const double *params = sp->getParamsJoint(arm_joint_names_[j]);
+ for (unsigned int k = 0 ; k < joint->usedParams ; ++k)
+ traj.points[i].positions.push_back(params[k]);
+ }
}
+ delete sp;
}
void MoveArm::printPath(const motion_planning_msgs::KinematicPath &path)
{
- ROS_DEBUG("Received path with %d states", (int)path.states.size());
for (unsigned int i = 0 ; i < path.states.size() ; ++i)
{
std::stringstream ss;
@@ -426,17 +437,17 @@
ros::ServiceClient client_query = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryQuery>(CONTROL_QUERY_NAME);
pr2_mechanism_controllers::TrajectoryQuery::Request req_query;
pr2_mechanism_controllers::TrajectoryQuery::Response res_query;
- req_query.trajectoryid = -1;
+ req_query.trajectoryid = pr2_mechanism_controllers::TrajectoryQuery::Request::Query_Joint_Names;
bool result = client_query.call(req_query, res_query);
if (!result)
{
- ROS_WARN("Unable to retrieve controller joint names from control query service. Waiting a bit and retrying...");
+ ROS_INFO("Querying controller for joint names ...");
ros::Duration(5.0).sleep();
result = client_query.call(req_query, res_query);
if (result)
- ROS_WARN("Retrieved controller joints on second attempt");
+ ROS_INFO("Joint names received");
}
if (!result)
@@ -467,13 +478,10 @@
planningMonitor_->getKinematicModel()->getJointsInGroup(groupNames, arm_);
if (groupNames.size() != joint_names.size())
{
- ROS_ERROR("The group '%s' has more joints than the controller can handle", arm_.c_str());
+ ROS_ERROR("The group '%s' does not have the same number of joints as the controller can handle", arm_.c_str());
return false;
}
- if (groupNames != joint_names)
- ROS_WARN("The group joints are not in the same order as the controller expects");
-
return true;
}
@@ -548,7 +556,7 @@
return (upper_bound - lower_bound) * drand48() + lower_bound;
}
- bool MoveArm::alterRequestUsingIK(motion_planning_srvs::MotionPlan::Request &req)
+ bool MoveArm::alterRequestUsingIK(motion_planning_msgs::GetMotionPlan::Request &req)
{
bool result = false;
@@ -566,28 +574,24 @@
ROS_INFO("Converting pose constraint to joint constraint using IK...");
planningMonitor_->getEnvironmentModel()->setVerbose(false);
- ros::ServiceClient client = node_handle_.serviceClient<manipulation_srvs::IKService>(ARM_IK_NAME, true);
- for (int t = 0 ; t < 2 ; ++t)
+ ros::ServiceClient ik_client = node_handle_.serviceClient<manipulation_srvs::IKService>(ARM_IK_NAME, true);
+
+ // find an IK solution
+ std::vector<double> solution;
+ if (computeIK(ik_client, req.goal_constraints.pose_constraint[0].pose, solution))
{
- robot_msgs::PoseStamped tpose = req.goal_constraints.pose_constraint[0].pose;
- if (t > 0)
+ // check if it is a valid state
+ planning_models::StateParams spTest(*planningMonitor_->getRobotState());
+ spTest.setParamsJoints(solution, arm_joint_names_);
+
+ if (planningMonitor_->isStateValidAtGoal(&spTest))
{
- tpose.pose.position.x = uniformDouble(tpose.pose.position.x - req.goal_constraints.pose_constraint[0].position_tolerance_below.x,
- tpose.pose.position.x + req.goal_constraints.pose_constraint[0].position_tolerance_above.x);
- tpose.pose.position.y = uniformDouble(tpose.pose.position.y - req.goal_constraints.pose_constraint[0].position_tolerance_below.y,
- tpose.pose.position.y + req.goal_constraints.pose_constraint[0].position_tolerance_above.y);
- tpose.pose.position.z = uniformDouble(tpose.pose.position.z - req.goal_constraints.pose_constraint[0].position_tolerance_below.z,
- tpose.pose.position.z + req.goal_constraints.pose_constraint[0].position_tolerance_above.z);
- }
- std::vector<double> solution;
- if (computeIK(client, tpose, 5, solution))
- {
unsigned int n = 0;
for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
{
motion_planning_msgs::JointConstraint jc;
jc.joint_name = arm_joint_names_[i];
- jc.header.frame_id = tpose.header.frame_id;
+ jc.header.frame_id = req.goal_constraints.pose_constraint[0].pose.header.frame_id;
jc.header.stamp = planningMonitor_->lastMechanismStateUpdate();
unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
for (unsigned int j = 0 ; j < u ; ++j)
@@ -600,22 +604,92 @@
req.goal_constraints.joint_constraint.push_back(jc);
}
req.goal_constraints.pose_constraint.clear();
-
+
// update the goal constraints for the planning monitor as well
planningMonitor_->setGoalConstraints(req.goal_constraints);
result = true;
}
+ else
+ {
+ // if it is not, we try to fix it
+ motion_planning_msgs::ConvertPoseToJointConstraint::Request c_req;
+ c_req.params = req.params;
+ c_req.start_state = req.start_state;
+ c_req.constraints = req.goal_constraints;
+ c_req.names = arm_joint_names_;
+ c_req.states.resize(1);
+ c_req.states[0].vals = solution;
+ c_req.allowed_time = 0.3;
+
+ // add a few more guesses
+ for (int t = 0 ; t < 4 ; ++t)
+ {
+ robot_msgs::PoseStamped tpose = req.goal_constraints.pose_constraint[0].pose;
+ tpose.pose.position.x = uniformDouble(tpose.pose.position.x - req.goal_constraints.pose_constraint[0].position_tolerance_below.x,
+ tpose.pose.position.x + req.goal_constraints.pose_constraint[0].position_tolerance_above.x);
+ tpose.pose.position.y = uniformDouble(tpose.pose.position.y - req.goal_constraints.pose_constraint[0].position_tolerance_below.y,
+ tpose.pose.position.y + req.goal_constraints.pose_constraint[0].position_tolerance_above.y);
+ tpose.pose.position.z = uniformDouble(tpose.pose.position.z - req.goal_constraints.pose_constraint[0].position_tolerance_below.z,
+ tpose.pose.position.z + req.goal_constraints.pose_constraint[0].position_tolerance_above.z);
+ std::vector<double> sol;
+ if (computeIK(ik_client, tpose, sol))
+ {
+ unsigned int sz = c_req.states.size();
+ c_req.states.resize(sz + 1);
+ c_req.states[sz].vals = sol;
+ }
+ }
+
+ motion_planning_msgs::ConvertPoseToJointConstraint::Response c_res;
+ ros::ServiceClient s_client = node_handle_.serviceClient<motion_planning_msgs::ConvertPoseToJointConstraint>(SEARCH_VALID_STATE_NAME);
+ if (s_client.call(c_req, c_res))
+ {
+ if (!c_res.joint_constraint.empty())
+ {
+ req.goal_constraints.joint_constraint = c_res.joint_constraint;
+ req.goal_constraints.pose_constraint.clear();
+
+ // update the goal constraints for the planning monitor as well
+ planningMonitor_->setGoalConstraints(req.goal_constraints);
+ result = true;
+ }
+ }
+ else
+ ROS_ERROR("Service for searching for valid states failed");
+ }
}
- planningMonitor_->getEnvironmentModel()->setVerbose(true);
- if (!result)
- ROS_WARN("Unable to compute IK");
+ else
+ {
+ motion_planning_msgs::ConvertPoseToJointConstraint::Request c_req;
+ c_req.params = req.params;
+ c_req.start_state = req.start_state;
+ c_req.constraints = req.goal_constraints;
+ c_req.names = arm_joint_names_;
+ c_req.allowed_time = 0.3;
+ motion_planning_msgs::ConvertPoseToJointConstraint::Response c_res;
+ ros::ServiceClient s_client = node_handle_.serviceClient<motion_planning_msgs::ConvertPoseToJointConstraint>(SEARCH_VALID_STATE_NAME);
+ if (s_client.call(c_req, c_res))
+ {
+ if (!c_res.joint_constraint.empty())
+ {
+ req.goal_constraints.joint_constraint = c_res.joint_constraint;
+ req.goal_constraints.pose_constraint.clear();
+
+ // update the goal constraints for the planning monitor as well
+ planningMonitor_->setGoalConstraints(req.goal_constraints);
+ result = true;
+ }
+ }
+ else
+ ROS_ERROR("Service for searching for valid states failed");
+ }
}
}
return result;
}
- bool MoveArm::computeIK(ros::ServiceClient &client, const robot_msgs::PoseStamped &pose_stamped_msg, int attempts, std::vector<double> &solution)
+ bool MoveArm::computeIK(ros::ServiceClient &client, const robot_msgs::PoseStamped &pose_stamped_msg, std::vector<double> &solution)
{
// define the service messages
manipulation_srvs::IKService::Request request;
@@ -624,82 +698,36 @@
request.data.pose_stamped = pose_stamped_msg;
request.data.joint_names = arm_joint_names_;
- bool validSolution = false;
- int ikSteps = 0;
- while (ikSteps < attempts && !validSolution)
+ planning_models::StateParams *sp = planningMonitor_->getKinematicModel()->newStateParams();
+ sp->randomStateGroup(arm_);
+ for(unsigned int i = 0; i < arm_joint_names_.size() ; ++i)
{
- request.data.positions.clear();
- planning_models::StateParams *sp = NULL;
- if (ikSteps == 0)
- sp = new planning_models::StateParams(*planningMonitor_->getRobotState());
- else
+ const double *params = sp->getParamsJoint(arm_joint_names_[i]);
+ const unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
+ for (unsigned int j = 0 ; j < u ; ++j)
+ request.data.positions.push_back(params[j]);
+ }
+ delete sp;
+
+ if (client.call(request, response))
+ {
+ ROS_DEBUG("Obtained IK solution");
+ solution = response.solution;
+ if (solution.size() != request.data.positions.size())
{
- sp = planningMonitor_->getKinematicModel()->newStateParams();
- sp->randomStateGroup(arm_);
- }
- ikSteps++;
-
- for(unsigned int i = 0; i < arm_joint_names_.size() ; ++i)
- {
- const double *params = sp->getParamsJoint(arm_joint_names_[i]);
- const unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
- for (unsigned int j = 0 ; j < u ; ++j)
- request.data.positions.push_back(params[j]);
- }
- delete sp;
-
- if (client.call(request, response))
- {
- ROS_DEBUG("Obtained IK solution");
- solution = response.solution;
- if (solution.size() != request.data.positions.size())
- {
- ROS_ERROR("Incorrect number of elements in IK output");
- return false;
- }
-
- for(unsigned int i = 0; i < solution.size() ; ++i)
- ROS_DEBUG("IK[%d] = %f", (int)i, solution[i]);
-
- // if IK did not fail, check if the state is valid
- planning_models::StateParams spTest(*planningMonitor_->getRobotState());
- unsigned int n = 0;
- for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
- {
- unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
- for (unsigned int j = 0 ; j < u ; ++j)
- {
- std::vector<double> params(solution.begin() + n, solution.begin() + n + u);
- spTest.setParamsJoint(params, arm_joint_names_[i]);
- }
- n += u;
- }
-
- // if state is not valid, we try to fix it
- fixState(spTest, true);
-
- if (planningMonitor_->isStateValidAtGoal(&spTest))
- {
- validSolution = true;
-
- // update solution
- solution.clear();
- for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
- {
- std::vector<double> params;
- spTest.copyParamsJoint(params, arm_joint_names_[i]);
- solution.insert(solution.end(), params.begin(), params.end());
- }
- }
- }
- else
- {
- ROS_ERROR("IK service failed");
+ ROS_ERROR("Incorrect number of elements in IK output");
return false;
}
+ for(unsigned int i = 0; i < solution.size() ; ++i)
+ ROS_DEBUG("IK[%d] = %f", (int)i, solution[i]);
}
-
- return validSolution;
+ else
+ {
+ ROS_ERROR("IK service failed");
+ return false;
+ }
+
+ return true;
}
}
Modified: pkg/trunk/motion_planning/motion_planning_msgs/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/CMakeLists.txt 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/motion_planning_msgs/CMakeLists.txt 2009-07-30 00:50:20 UTC (rev 20094)
@@ -2,3 +2,4 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(motion_planning_msgs)
genmsg()
+gensrv()
Modified: pkg/trunk/motion_planning/motion_planning_msgs/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/manifest.xml 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/motion_planning_msgs/manifest.xml 2009-07-30 00:50:20 UTC (rev 20094)
@@ -13,7 +13,7 @@
<depend package="robot_msgs"/>
<export>
- <cpp cflags="-I${prefix}/msg/cpp"/>
+ <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/srv/cpp"/>
</export>
</package>
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg 2009-07-30 00:50:20 UTC (rev 20094)
@@ -5,12 +5,6 @@
# for which the motion planner should plan for
string model_id
-
-# The name of the motion planner to use. If no name is specified,
-# a default motion planner will be used
-string planner_id
-
-
# A string that identifies which distance metric the planner should use
string distance_metric
Added: pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertPoseToJointConstraint.srv
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertPoseToJointConstraint.srv (rev 0)
+++ pkg/trunk/motion_planning/motion_planning_msgs/srv/ConvertPoseToJointConstraint.srv 2009-07-30 00:50:20 UTC (rev 20094)
@@ -0,0 +1,30 @@
+# Parameters for the state space
+motion_planning_msgs/KinematicSpaceParameters params
+
+
+# Starting state updates. If certain joints should be considered
+# at positions other than the current ones, these positions should
+# be set here
+motion_planning_msgs/KinematicJoint[] start_state
+
+
+# The joint names, in the same order as the values in the state
+string[] names
+
+# A list of states, each with the dimension of the requested model.
+# The dimension of the model may be different from the number of joints
+# (when there are joints that use more than one parameter)
+# Each state is to be used as a hint (initialization) for the search
+# if the valid state
+motion_planning_msgs/KinematicState[] states
+
+# The input constraints, to be converted to a set of joint constraints
+motion_planning_msgs/KinematicConstraints constraints
+
+# The maximum amount of time the search is to be run for
+float64 allowed_time
+
+---
+
+# The set of joint constraints that correspond to the pose constraint
+motion_planning_msgs/JointConstraint[] joint_constraint
Added: pkg/trunk/motion_planning/motion_planning_msgs/srv/GetMotionPlan.srv
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/srv/GetMotionPlan.srv (rev 0)
+++ pkg/trunk/motion_planning/motion_planning_msgs/srv/GetMotionPlan.srv 2009-07-30 00:50:20 UTC (rev 20094)
@@ -0,0 +1,40 @@
+# This service contains the definition for a request to the motion
+# planner and the output it provides
+
+# Parameters for the state space
+motion_planning_msgs/KinematicSpaceParameters params
+
+# Starting state updates. If certain joints should be considered
+# at positions other than the current ones, these positions should
+# be set here
+motion_planning_msgs/KinematicJoint[] start_state
+
+# The goal state for the model to plan for. The goal is achieved
+# if all constraints are satisfied
+motion_planning_msgs/KinematicConstraints goal_constraints
+
+# No state in the produced motion plan will violate these constraints
+motion_planning_msgs/KinematicConstraints path_constraints
+
+# The name of the motion planner to use. If no name is specified,
+# a default motion planner will be used
+string planner_id
+
+# The number of times this plan is to be computed. Shortest solution
+# will be reported.
+int32 times
+
+# The maximum amount of time the motion planner is allowed to plan for
+float64 allowed_time
+
+---
+
+# A solution path, if found
+motion_planning_msgs/KinematicPath path
+
+# distance to goal as computed by internal planning heuristic
+# this should be 0.0 if the solution was achieved exaclty
+float64 distance
+
+# set to 1 if the computed path was approximate
+byte approximate
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/GAIK.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/GAIK.h 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/GAIK.h 2009-07-30 00:50:20 UTC (rev 20094)
@@ -122,7 +122,7 @@
}
/** \brief Get the number of individuals to add to the population in each generation */
- unsigned int getPoolExtensionSize(void) const
+ unsigned int getPoolExpansionSize(void) const
{
return m_poolExpansion;
}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/src/GAIK.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/src/GAIK.cpp 2009-07-30 00:20:08 UTC (rev 20093)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/kinematic/extension/ik/src/GAIK.cpp 2009-07-30 00:50:20 UTC (rev 20094)
@@...
[truncated message content] |
|
From: <ei...@us...> - 2009-07-30 03:03:00
|
Revision: 20118
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20118&view=rev
Author: eitanme
Date: 2009-07-30 03:02:41 +0000 (Thu, 30 Jul 2009)
Log Message:
-----------
Merging tha actionlib branch back into trunk
r29135@att (orig r19792): eitanme | 2009-07-27 18:30:30 -0700
Creating a branch for actionlib.... hopefully for the last time
r29137@att (orig r19794): eitanme | 2009-07-27 18:32:49 -0700
Changing ParticleCloud to PoseArray
r29139@att (orig r19796): eitanme | 2009-07-27 18:33:42 -0700
Adding action definition to the rep
r29148@att (orig r19805): eitanme | 2009-07-27 18:47:39 -0700
Some fixes... almost compiling
r29165@att (orig r19822): eitanme | 2009-07-27 20:41:07 -0700
Macro version of the typedefs that compiles
r29213@att (orig r19869): eitanme | 2009-07-28 11:49:10 -0700
Compling version of the ActionServer re-write complete with garbage collection, be default it will keep goals without goal handles for 5 seconds
r29220@att (orig r19876): eitanme | 2009-07-28 12:06:06 -0700
Fix to make sure that transitions into preempting and recalling states actually happen
r29254@att (orig r19888): eitanme | 2009-07-28 13:27:40 -0700
Forgot to actually call the cancel callback... addind a subscriber on the cancel topic
r29267@att (orig r19901): eitanme | 2009-07-28 14:41:09 -0700
Adding text field to GoalStatus to allow users to throw some debugging information into the GoalStatus messages
r29275@att (orig r19909): eitanme | 2009-07-28 15:43:49 -0700
Using tf remapping as I should've been doing for awhile
r29277@att (orig r19911): eitanme | 2009-07-28 15:48:48 -0700
The navigation stack can now handle goals that aren't in the global frame. However, these goals will be transformed to the global frame at the time of reception, so for achieving them accurately the global frame of move_base should really be set to match the goals.
r29299@att (orig r19933): stuglaser | 2009-07-28 17:08:10 -0700
Created genaction.py script to create the various messages that an action needs
r29376@att (orig r20003): vijaypradeep | 2009-07-29 02:45:24 -0700
ActionClient is running. MoveBase ActionServer seems to be crashing
r29409@att (orig r20033): vijaypradeep | 2009-07-29 11:57:54 -0700
Fixing bug with adding status trackers
r29410@att (orig r20034): vijaypradeep | 2009-07-29 11:58:18 -0700
Changing from Release to Debug
r29432@att (orig r20056): vijaypradeep | 2009-07-29 14:07:30 -0700
No longer building goal_manager_test.cpp
r29472@att (orig r20090): vijaypradeep | 2009-07-29 17:04:14 -0700
Lots of Client-Side doxygen
r29484@att (orig r20101): vijaypradeep | 2009-07-29 18:35:01 -0700
Adding to mainpage.dox
r29487@att (orig r20104): eitanme | 2009-07-29 18:55:06 -0700
Removing file to help resolve merge I hope
r29489@att (orig r20106): eitanme | 2009-07-29 19:00:07 -0700
Removing another file to try to resolve the branch
r29492@att (orig r20108): eitanme | 2009-07-29 19:14:25 -0700
Again removing a file to get the merge working
r29493@att (orig r20109): eitanme | 2009-07-29 19:34:45 -0700
Removing yet another file on which ssl negotiation fails
r29500@att (orig r20116): eitanme | 2009-07-29 19:54:18 -0700
Fixing bug in genaction
Modified Paths:
--------------
pkg/trunk/sandbox/actionlib/CMakeLists.txt
pkg/trunk/sandbox/actionlib/include/actionlib/action_server.h
pkg/trunk/sandbox/actionlib/include/actionlib/goal_id_generator.h
pkg/trunk/sandbox/actionlib/include/actionlib/single_goal_action_server.h
pkg/trunk/sandbox/actionlib/mainpage.dox
pkg/trunk/sandbox/actionlib/msg/GoalStatus.msg
pkg/trunk/sandbox/actionlib/msg/MoveBaseAction.msg
pkg/trunk/sandbox/actionlib/src/move_base_client.cpp
pkg/trunk/sandbox/actionlib/src/move_base_server.cpp
pkg/trunk/stacks/nav/amcl/src/amcl_node.cpp
pkg/trunk/stacks/nav/fake_localization/fake_localization.cpp
pkg/trunk/stacks/nav/move_base/CMakeLists.txt
pkg/trunk/stacks/nav/move_base/include/move_base/move_base.h
pkg/trunk/stacks/nav/move_base/msg/MoveBaseAction.msg
pkg/trunk/stacks/nav/move_base/src/move_base.cpp
pkg/trunk/stacks/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/stacks/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/stacks/nav/navfn/src/navfn_ros.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/particle_cloud_2d_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/particle_cloud_2d_display.h
Added Paths:
-----------
pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c
pkg/trunk/sandbox/actionlib/genaction.py
pkg/trunk/sandbox/actionlib/include/actionlib/action_definition.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/
pkg/trunk/sandbox/actionlib/include/actionlib/client/action_client.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/comm_state.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/comm_state_machine.cpp
pkg/trunk/sandbox/actionlib/include/actionlib/client/comm_state_machine.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_handle.cpp
pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.cpp
pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_action_client.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/terminal_state.h
pkg/trunk/sandbox/actionlib/include/actionlib/managed_list.h
pkg/trunk/sandbox/actionlib/include/actionlib/old_action_client.h_old
pkg/trunk/sandbox/actionlib/include/actionlib/older_action_client.h_old
pkg/trunk/sandbox/descriptors_2d/descriptors.cpp
pkg/trunk/sandbox/descriptors_2d/descriptors.h
pkg/trunk/sandbox/descriptors_2d_gpl/image_descriptors_gpl.cpp
pkg/trunk/sandbox/descriptors_2d_gpl/include/image_descriptors_gpl/
pkg/trunk/sandbox/descriptors_2d_gpl/include/image_descriptors_gpl/image_descriptors_gpl.h
pkg/trunk/stacks/common_msgs/nav_msgs/msg/PoseArray.msg
pkg/trunk/stacks/trex/trex_pr2/test/doors/domain_tests/open_door.0/run_real.launch
pkg/trunk/stacks/trex/trex_pr2/test/doors/domain_tests/open_door.0/run_sim.launch
pkg/trunk/stacks/trex/trex_pr2/test/doors/domain_tests/open_door.1/run_real.launch
pkg/trunk/stacks/trex/trex_pr2/test/doors/domain_tests/open_door.1/run_sim.launch
pkg/trunk/stacks/trex/trex_pr2/test/doors/domain_tests/open_door.1/run_stubs.launch
pkg/trunk/stacks/trex/trex_pr2/test/plugs/domain_tests/recharge.0/run_real.launch
pkg/trunk/stacks/trex/trex_pr2/test/plugs/domain_tests/recharge.0/run_sim.launch
pkg/trunk/stacks/trex/trex_pr2/test/plugs/domain_tests/recharge.1/run_real.launch
pkg/trunk/stacks/trex/trex_pr2/test/plugs/domain_tests/recharge.1/run_sim.launch
pkg/trunk/stacks/trex/trex_pr2/test/plugs/domain_tests/recharge.1/run_stubs.launch
Removed Paths:
-------------
pkg/trunk/sandbox/actionlib/include/actionlib/action_client.h
pkg/trunk/sandbox/actionlib/include/actionlib/old_action_client.h
pkg/trunk/stacks/common_msgs/nav_msgs/msg/ParticleCloud.msg
pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/vision/place_recognition/test/directed.py
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/nav_rework:21482
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/action_branch:19631
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/milestone2:16602
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/nav_rework:17337
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/tags/milestone2_tags/milestone2_final:16577
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/nav_rework:21482
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/action_branch:20116
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/milestone2:16602
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/nav_rework:17337
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/tags/milestone2_tags/milestone2_final:16577
Added: pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c (rev 0)
+++ pkg/trunk/drivers/cam/forearm_cam/src/fcamlib/pr2lib.c 2009-07-30 03:02:41 UTC (rev 20118)
@@ -0,0 +1,1289 @@
+#include "pr2lib.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/time.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdbool.h>
+#include <net/if.h>
+
+#include "host_netutil.h"
+#include "ipcam_packet.h"
+
+/// Amount of time in microseconds that the host should wait for packet replies
+#define STD_REPLY_TIMEOUT SEC_TO_USEC(0.2)
+
+#define VMODEDEF(width, height, fps, hblank, vblank) { #width"x"#height"x"#fps, width, height, fps, hblank, vblank }
+const struct MT9VMode MT9VModes[MT9V_NUM_MODES] = {
+ VMODEDEF(752,480,15, 974, 138),
+ VMODEDEF(752,480,12.5, 848, 320),
+ VMODEDEF(640,480,30, 372, 47),
+ VMODEDEF(640,480,25, 543, 61),
+ VMODEDEF(640,480,15, 873, 225),
+ VMODEDEF(640,480,12.5, 960, 320),
+ VMODEDEF(320,240,60, 106, 73),
+ VMODEDEF(320,240,50, 180, 80),
+ VMODEDEF(320,240,30, 332, 169),
+ VMODEDEF(320,240,25, 180, 400)
+};
+
+/**
+ * Returns the version information for the library
+ *
+ *
+ * @return Returns the the library as an integer 0x0000MMNN, where MM = Major version and NN = Minor version
+ */
+int pr2LibVersion() {
+ return PR2LIB_VERSION;
+}
+
+
+/**
+ * Waits for a PR2 StatusPacket on the specified socket for a specified duration.
+ *
+ * The Status type and code will be reported back to the called via the 'type' & 'code'
+ * arguments. If the timeout expires before a valid status packet is received, then
+ * the function will still return zero but will indicate that a TIMEOUT error has occurred.
+ *
+ * @param s The open, bound & 'connected' socket to listen on
+ * @param wait_us The number of microseconds to wait before timing out
+ * @param type Points to the location to store the type of status packet
+ * @param code Points to the location to store the subtype/error code
+ *
+ * @return Returns 0 if no system errors occured. -1 with errno set otherwise.
+ */
+int pr2StatusWait( int s, uint32_t wait_us, uint32_t *type, uint32_t *code ) {
+ if( !wgWaitForPacket(s, PKTT_STATUS, sizeof(PacketStatus), &wait_us) && (wait_us != 0) ) {
+ PacketStatus sPkt;
+ if( recvfrom( s, &sPkt, sizeof(PacketStatus), 0, NULL, NULL ) == -1 ) {
+ perror("wgDiscover unable to receive from socket");
+ *type = PKT_STATUST_ERROR;
+ *code = PKT_ERROR_SYSERR;
+ return -1;
+ }
+
+ *type = ntohl(sPkt.status_type);
+ *code = ntohl(sPkt.status_code);
+ return 0;
+ }
+
+ *type = PKT_STATUST_ERROR;
+ *code = PKT_ERROR_TIMEOUT;
+ return 0;
+}
+
+
+/**
+ * Discovers all PR2 cameras that are connected to the 'ifName' ethernet interface and
+ * adds new ones to the 'ipCamList' list.
+ *
+ * @param ifName The ethernet interface name to use. Null terminated string (e.g., "eth0").
+ * @param ipCamList The list to which the new cameras should be added
+ * @param wait_us The number of microseconds to wait for replies
+ *
+ * @return Returns -1 with errno set for system call errors. Otherwise returns number of new
+ * cameras found.
+ */
+int pr2Discover(const char *ifName, IpCamList *ipCamList, const char *ipAddress, unsigned wait_us) {
+ // Create and initialize a new Discover packet
+ PacketDiscover dPkt;
+ dPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ dPkt.hdr.type = htonl(PKTT_DISCOVER);
+ strncpy(dPkt.hdr.hrt, "DISCOVER", sizeof(dPkt.hdr.hrt));
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(ifName, &dPkt.hdr.reply_to);
+ if(s == -1) {
+ perror("Unable to create socket\n");
+ return -1;
+ }
+
+ //// Determine the broadcast address for ifName. This is the address we
+ //// will tell the camera to send from.
+
+ struct in_addr newIP;
+ if (ipAddress) // An IP was specified
+ {
+ inet_aton(ipAddress, &newIP);
+ dPkt.ip_addr = newIP.s_addr;
+ }
+ else // We guess an IP by flipping the host bits of the local IP.
+ {
+ struct in_addr localip;
+ struct in_addr netmask;
+ wgIpGetLocalAddr(ifName, &localip);
+ wgIpGetLocalNetmask(ifName, &netmask);
+ dPkt.ip_addr = localip.s_addr ^ netmask.s_addr ^ ~0;
+ }
+
+ if( wgSendUDPBcast(s, ifName, &dPkt,sizeof(dPkt)) == -1) {
+ perror("Unable to send broadcast\n");
+ }
+
+ // For the old API
+ if( wgSendUDPBcast(s, ifName, &dPkt,sizeof(dPkt)-sizeof(dPkt.ip_addr)) == -1) {
+ perror("Unable to send broadcast\n");
+ }
+
+ // Count the number of new cameras found
+ int newCamCount = 0;
+ do {
+ // Wait in the loop for replies. wait_us is updated each time through the loop.
+ if( !wgWaitForPacket(s, PKTT_ANNOUNCE, sizeof(PacketAnnounce), &wait_us) && (wait_us != 0) ) {
+ // We've received an Announce packet, so pull it out of the receive queue
+ PacketAnnounce aPkt;
+ struct sockaddr_in fromaddr;
+ fromaddr.sin_family = AF_INET;
+ socklen_t fromlen = sizeof(fromaddr);
+
+ if( recvfrom( s, &aPkt, sizeof(PacketAnnounce), 0, (struct sockaddr *) &fromaddr, &fromlen) == -1 ) {
+ perror("wgDiscover unable to receive from socket");
+ close(s);
+ return -1;
+ }
+
+ // Create a new list entry and initialize it
+ IpCamList *tmpListItem;
+ if( (tmpListItem = (IpCamList *)malloc(sizeof(IpCamList))) == NULL ) {
+ perror("Malloc failed");
+ close(s);
+ return -1;
+ }
+ pr2CamListInit( tmpListItem );
+
+ // Initialize the new list item's data fields (byte order corrected)
+ tmpListItem->hw_version = ntohl(aPkt.hw_version);
+ tmpListItem->fw_version = ntohl(aPkt.fw_version);
+ tmpListItem->ip = fromaddr.sin_addr.s_addr;
+ tmpListItem->serial = ntohl(aPkt.ser_no);
+ memcpy(&tmpListItem->mac, aPkt.mac, sizeof(aPkt.mac));
+ strncpy(tmpListItem->ifName, ifName, sizeof(tmpListItem->ifName));
+ tmpListItem->status = CamStatusDiscovered;
+ char pcb_rev = 0x0A + (0x0000000F & ntohl(aPkt.hw_version));
+ int hdl_rev = 0x00000FFF & (ntohl(aPkt.hw_version)>>4);
+ snprintf(tmpListItem->hwinfo, PR2_CAMINFO_LEN, "PCB rev %X : HDL rev %3X : FW rev %3X", pcb_rev, hdl_rev, ntohl(aPkt.fw_version));
+
+ // If this camera is already in the list, we don't want to add another copy
+ if( pr2CamListAdd( ipCamList, tmpListItem ) == CAMLIST_ADD_DUP) {
+ free(tmpListItem);
+ } else {
+ debug("MAC Address: %02X:%02X:%02X:%02X:%02X:%02X\n", aPkt.mac[0], aPkt.mac[1], aPkt.mac[2], aPkt.mac[3], aPkt.mac[4], aPkt.mac[5]);
+ debug("Product #%07u : Unit #%04u\n", ntohl(aPkt.product_id), ntohl(aPkt.ser_no));
+ debug("%s\n", tmpListItem->hwinfo);
+ newCamCount++;
+ }
+ }
+ } while(wait_us > 0);
+
+ close(s);
+ return newCamCount;
+}
+
+
+/**
+ * Configures the IP address of one specific camera.
+ *
+ * @param camInfo Structure describing the camera to configure
+ * @param ipAddress An ASCII string containing the new IP address to assign (e.g., "192.168.0.5")
+ * @param wait_us The number of microseconds to wait for a reply from the camera
+ *
+ * @return Returns -1 with errno set for system call failures.
+ * Returns 0 for success
+ * Returns ERR_TIMEOUT if no reply is received before the timeout expires
+ * Returns ERR_CONFIG_ARPFAIL if the camera was configured successfully but
+ * the host system could not update its arp table. This is normally because
+ * the library is not running as root.
+ */
+int pr2Configure( IpCamList *camInfo, const char *ipAddress, unsigned wait_us) {
+ // Create and initialize a new Configure packet
+ PacketConfigure cPkt;
+
+ cPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ cPkt.hdr.type = htonl(PKTT_CONFIGURE);
+ cPkt.product_id = htonl(CONFIG_PRODUCT_ID);
+ strncpy(cPkt.hdr.hrt, "CONFIGURE", sizeof(cPkt.hdr.hrt));
+
+ cPkt.ser_no = htonl(camInfo->serial);
+
+ struct in_addr newIP;
+ inet_aton(ipAddress, &newIP);
+ cPkt.ip_addr = newIP.s_addr;
+
+
+ // Create and send the Configure broadcast packet. It is sent broadcast
+ // because the camera does not yet have an IP address assigned.
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &cPkt.hdr.reply_to);
+ if(s == -1) {
+ perror("pr2Configure socket creation failed");
+ return -1;
+ }
+
+ if(wgSendUDPBcast(s, camInfo->ifName, &cPkt, sizeof(cPkt)) == -1) {
+ debug("Unable to send broadcast\n");
+ close(s);
+ return -1;
+ }
+
+
+ // 'Connect' insures we will only receive datagram replies from the camera's new IP
+ IPAddress camIP = newIP.s_addr;
+ if( wgSocketConnect(s, &camIP) ) {
+ close(s);
+ return -1;
+ }
+
+ // Wait up to wait_us for a valid packet to be received on s
+ do {
+ if( !wgWaitForPacket(s, PKTT_ANNOUNCE, sizeof(PacketAnnounce), &wait_us) && (wait_us != 0) ) {
+ PacketAnnounce aPkt;
+
+ if( recvfrom( s, &aPkt, sizeof(PacketAnnounce), 0, NULL, NULL ) == -1 ) {
+ perror("wgDiscover unable to receive from socket");
+ close(s);
+ return -1;
+ }
+
+ // Need to have a valid packet from a camera with the expected serial number
+ if( ntohl(aPkt.ser_no) == camInfo->serial ) {
+ camInfo->status = CamStatusConfigured;
+ memcpy(&camInfo->ip, &cPkt.ip_addr, sizeof(IPAddress));
+ // Add the IP/MAC mapping to the system ARP table
+ if( wgArpAdd(camInfo) ) {
+ close(s);
+ return ERR_CONFIG_ARPFAIL;
+ }
+ break;
+ }
+ }
+ } while(wait_us > 0);
+ close(s);
+
+ if(wait_us != 0) {
+ return 0;
+ } else {
+ return ERR_TIMEOUT;
+ }
+}
+
+/**
+ * Instructs one camera to begin streaming video to the host/port specified.
+ *
+ * @param camInfo Structure that describes the camera to contact
+ * @param mac Contains the MAC address of the host that will receive the video
+ * @param ipAddress An ASCII string in dotted quad form containing the IP address of the host
+ * that will receive the video (e.g., "192.168.0.5")
+ * @param port The port number that the video should be sent to. Host byte order.
+ *
+ * @return Returns -1 with errno set for system call failures
+ * Returns 0 for success
+ * Returns 1 for protocol failures (timeout, etc)
+ */
+int pr2StartVid( const IpCamList *camInfo, const uint8_t mac[6], const char *ipAddress, uint16_t port ) {
+ PacketVidStart vPkt;
+
+ // Initialize the packet
+ vPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ vPkt.hdr.type = htonl(PKTT_VIDSTART);
+ strncpy(vPkt.hdr.hrt, "Start Video", sizeof(vPkt.hdr.hrt));
+
+
+ // Convert the ipAddress string into a binary value in network byte order
+ inet_aton(ipAddress, (struct in_addr*)&vPkt.receiver.addr);
+
+ // Convert the receiver port into network byte order
+ vPkt.receiver.port = htons(port);
+
+ // Copy the MAC address into the vPkt
+ memcpy(&vPkt.receiver.mac, mac, 6);
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &vPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if(wgSendUDP(s, &camInfo->ip, &vPkt, sizeof(vPkt)) == -1) {
+ goto err_out;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ goto err_out;
+ }
+
+ // Wait for a status reply
+ uint32_t type, code;
+ if( pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code ) == -1) {
+ goto err_out;
+ }
+
+ close(s);
+ if(type == PKT_STATUST_OK) {
+ return 0;
+ } else {
+ debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
+ return 1;
+ }
+
+err_out:
+ close(s);
+ return -1;
+}
+
+/**
+ * Instructs one camera to stop sending video.
+ *
+ * @param camInfo Describes the camera to send the stop to.
+ * @return Returns 0 for success
+ * Returns -1 with errno set for system errors
+ * Returns 1 for protocol errors
+ */
+int pr2StopVid( const IpCamList *camInfo ) {
+ PacketVidStop vPkt;
+
+ // Initialize the packet
+ vPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ vPkt.hdr.type = htonl(PKTT_VIDSTOP);
+ strncpy(vPkt.hdr.hrt, "Stop Video", sizeof(vPkt.hdr.hrt));
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &vPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &vPkt, sizeof(vPkt)) == -1 ) {
+ goto err_out;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) == -1) {
+ goto err_out;
+ }
+
+ uint32_t type, code;
+ if(pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code ) == -1) {
+ goto err_out;
+ }
+
+ close(s);
+ if(type == PKT_STATUST_OK) {
+ return 0;
+ } else {
+ debug("Error: pr2StatusWait returned status %d, code %d\n", type, code);
+ return 1;
+ }
+
+err_out:
+ close(s);
+ return -1;
+}
+
+/**
+ * Instructs one camera to execute a soft reset.
+ *
+ * @param camInfo Describes the camera to reset.
+ * @return Returns 0 for success
+ * Returns -1 for system errors
+ */
+int pr2Reset( IpCamList *camInfo ) {
+ PacketReset gPkt;
+
+ // Initialize the packet
+ gPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ gPkt.hdr.type = htonl(PKTT_RESET);
+ strncpy(gPkt.hdr.hrt, "Reset", sizeof(gPkt.hdr.hrt));
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &gPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &gPkt, sizeof(gPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+
+ close(s);
+
+ // Camera is no longer configured after a reset
+ camInfo->status = CamStatusDiscovered;
+
+ // There is no reply to a reset packet
+
+ return 0;
+}
+
+/**
+ * Gets the system time of a specified camera.
+ *
+ * In the camera, system time is tracked as a number of clock 'ticks' since
+ * the last hard reset. This function receives the number of 'ticks' as well as
+ * a 'ticks_per_sec' conversion factor to return a 64-bit time result in microseconds.
+ *
+ * @param camInfo Describes the camera to connect to
+ * @param time_us Points to the location to store the system time in us
+ *
+ * @return Returns 0 for success, -1 for system error, or 1 for protocol error.
+ */
+int pr2GetTimer( const IpCamList *camInfo, uint64_t *time_us ) {
+ PacketTimeRequest gPkt;
+
+ // Initialize the packet
+ gPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ gPkt.hdr.type = htonl(PKTT_TIMEREQ);
+ strncpy(gPkt.hdr.hrt, "Time Req", sizeof(gPkt.hdr.hrt));
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &gPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &gPkt, sizeof(gPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ close(s);
+ return -1;
+ }
+
+ uint32_t wait_us = STD_REPLY_TIMEOUT;
+ do {
+ if( !wgWaitForPacket(s, PKTT_TIMEREPLY, sizeof(PacketTimer), &wait_us) && (wait_us != 0) ) {
+ PacketTimer tPkt;
+ if( recvfrom( s, &tPkt, sizeof(PacketTimer), 0, NULL, NULL ) == -1 ) {
+ perror("GetTime unable to receive from socket");
+ close(s);
+ return -1;
+ }
+
+ *time_us = (uint64_t)ntohl(tPkt.ticks_hi) << 32;
+ *time_us += ntohl(tPkt.ticks_lo);
+
+ // Need to multiply final result by 1E6 to get us from sec
+ // Try to do this to limit loss of precision without overflow
+ // (We will overflow the 64-bit type with this approach
+ // after the camera has been up for about 11 continuous years)
+ //FIXME: Review this algorithm for possible improvements.
+ *time_us *= 1000;
+ *time_us /= (ntohl(tPkt.ticks_per_sec)/1000);
+ //debug("Got time of %llu us since last reset\n", *time_us);
+ close(s);
+ return 0;
+ }
+ } while(wait_us > 0);
+
+ debug("Timed out waiting for time value\n");
+ close(s);
+ return 1;
+}
+
+/**
+ * Reads one FLASH_PAGE_SIZE byte page of the camera's onboard Atmel dataflash.
+ *
+ * @param camInfo Describes the camera to connect to.
+ * @param address Specifies the 12-bit flash page address to read (0-4095)
+ * @param pageDataOut Points to at least FLASH_PAGE_SIZE bytes of storage in which to place the flash data.
+ *
+ * @return Returns 0 for success
+ * Returns -1 for system error
+ * Returns 1 for protocol error
+ *
+ */
+
+int pr2FlashRead( const IpCamList *camInfo, uint32_t address, uint8_t *pageDataOut ) {
+ PacketFlashRequest rPkt;
+
+ // Initialize the packet
+ rPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ rPkt.hdr.type = htonl(PKTT_FLASHREAD);
+ if(address > FLASH_MAX_PAGENO) {
+ return 1;
+ }
+
+ // The page portion of the address is 12-bits wide, range (bit9..bit21)
+ rPkt.address = htonl(address<<9);
+
+ strncpy(rPkt.hdr.hrt, "Flash read", sizeof(rPkt.hdr.hrt));
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &rPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &rPkt, sizeof(rPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ close(s);
+ return -1;
+ }
+
+ uint32_t wait_us = STD_REPLY_TIMEOUT;
+ do {
+ if( !wgWaitForPacket(s, PKTT_FLASHDATA, sizeof(PacketFlashPayload), &wait_us) && (wait_us != 0) ) {
+ PacketFlashPayload fPkt;
+ if( recvfrom( s, &fPkt, sizeof(PacketFlashPayload), 0, NULL, NULL ) == -1 ) {
+ perror("GetTime unable to receive from socket");
+ close(s);
+ return -1;
+ }
+
+ // Copy the received data into the space pointed to by pageDataOut
+ memcpy(pageDataOut, fPkt.data, FLASH_PAGE_SIZE);
+ close(s);
+ return 0;
+ }
+ } while(wait_us > 0);
+
+ debug("Timed out waiting for flash value\n");
+ close(s);
+ return 1;
+}
+
+/**
+ * Writes one FLASH_PAGE_SIZE byte page to the camera's onboard Atmel dataflash.
+ *
+ * @param camInfo Describes the camera to connect to.
+ * @param address Specifies the 12-bit flash page address to write (0-4095)
+ * @param pageDataOut Points to at least FLASH_PAGE_SIZE bytes of storage from which to get the flash data.
+ *
+ * @return Returns 0 for success
+ * Returns -1 for system error
+ * Returns 1 for protocol error
+ *
+ */
+int pr2FlashWrite( const IpCamList *camInfo, uint32_t address, const uint8_t *pageDataIn ) {
+ PacketFlashPayload rPkt;
+
+ // Initialize the packet
+ rPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ rPkt.hdr.type = htonl(PKTT_FLASHWRITE);
+ if(address > FLASH_MAX_PAGENO) {
+ return 1;
+ }
+
+ // The page portion of the address is 12-bits wide, range (bit9..bit21)
+ rPkt.address = htonl(address<<9);
+ strncpy(rPkt.hdr.hrt, "Flash write", sizeof(rPkt.hdr.hrt));
+
+ memcpy(rPkt.data, pageDataIn, FLASH_PAGE_SIZE);
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back to the same socket.
+ */
+ int s = wgCmdSocketCreate(camInfo->ifName, &rPkt.hdr.reply_to);
+ if( s == -1 ) {
+ return -1;
+ }
+
+ if( wgSendUDP(s, &camInfo->ip, &rPkt, sizeof(rPkt)) == -1 ) {
+ close(s);
+ return -1;
+ }
+
+ // 'Connect' insures we will only receive datagram replies from the camera we've specified
+ if( wgSocketConnect(s, &camInfo->ip) ) {
+ close(s);
+ return -1;
+ }
+
+ // Wait for response - once we get an OK, we're clear to send the next page.
+ uint32_t type, code;
+ pr2StatusWait( s, STD_REPLY_TIMEOUT, &type, &code );
+
+ close(s);
+ if(type == PKT_STATUST_OK) {
+ return 0;
+ } else {
+ debug("Error: wgStatusWait returned status %d, code %d\n", type, code);
+ return 1;
+ }
+}
+
+/**
+ * Sets the trigger type (internal or external) for one camera.
+ *
+ * @param camInfo Describes the camera to connect to
+ * @param triggerType Can be either TRIG_STATE_INTERNAL or TRIG_STATE_EXTERNAL
+ *
+ * @return Returns 0 for success
+ * Returns -1 for system error
+ * Returns 1 for protocol error
+ */
+int pr2TriggerControl( const IpCamList *camInfo, uint32_t triggerType ) {
+ PacketTrigControl tPkt;
+
+ // Initialize the packet
+ tPkt.hdr.magic_no = htonl(WG_MAGIC_NO);
+ tPkt.hdr.type = htonl(PKTT_TRIGCTRL);
+ tPkt.trig_state = htonl(triggerType);
+
+ if(triggerType == TRIG_STATE_INTERNAL ) {
+ strncpy(tPkt.hdr.hrt, "Int. Trigger", sizeof(tPkt.hdr.hrt));
+ } else {
+ strncpy(tPkt.hdr.hrt, "Ext. Trigger", sizeof(tPkt.hdr.hrt));
+ }
+
+ /* Create a new socket bound to a local ephemeral port, and get our local connection
+ * info so we can request a reply back...
[truncated message content] |
|
From: <mr...@us...> - 2009-07-30 04:07:09
|
Revision: 20133
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20133&view=rev
Author: mrinal
Date: 2009-07-30 04:06:52 +0000 (Thu, 30 Jul 2009)
Log Message:
-----------
We use the spline smoothed controller for move arm now.
Also moved some launch files to 3dnav_pr2
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/launch/move_larm.launch
pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
pkg/trunk/sandbox/joint_waypoint_controller/launch/filters.xml
Added Paths:
-----------
pkg/trunk/sandbox/3dnav_pr2/launch/actions/
pkg/trunk/sandbox/3dnav_pr2/launch/actions/left_arm.launch
pkg/trunk/sandbox/3dnav_pr2/launch/actions/right_arm.launch
pkg/trunk/sandbox/joint_waypoint_controller/launch/l_arm_joint_waypoint_controller.launch
pkg/trunk/sandbox/joint_waypoint_controller/launch/r_arm_joint_waypoint_controller.launch
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_trajectory_controller.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_trajectory_controller.yaml
Removed Paths:
-------------
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/larm.launch
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/rarm.launch
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/rarm_splines.launch
pkg/trunk/highlevel/move_arm/launch/move_rarm_splines.launch
pkg/trunk/sandbox/joint_waypoint_controller/launch/joint_waypoint_controller.launch
pkg/trunk/stacks/pr2/pr2_default_controllers/trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/trajectory_controller.yaml
Deleted: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/larm.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/larm.launch 2009-07-30 04:05:47 UTC (rev 20132)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/larm.launch 2009-07-30 04:06:52 UTC (rev 20133)
@@ -1,6 +0,0 @@
-<launch>
- <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
- <include file="$(find pr2_ik)/pr2_ik_larm_node.launch"/>
- <include file="$(find move_arm)/launch/move_larm.launch"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/l_arm_joint_trajectory_controller.xml" />
-</launch>
Deleted: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/rarm.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/rarm.launch 2009-07-30 04:05:47 UTC (rev 20132)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/rarm.launch 2009-07-30 04:06:52 UTC (rev 20133)
@@ -1,25 +0,0 @@
-<launch>
-
- <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
- <include file="$(find pr2_ik)/pr2_ik_rarm_node.launch"/>
- <include file="$(find move_arm)/launch/move_rarm.launch"/>
-
- <param name="r_arm_joint_trajectory_controller/r_shoulder_pan_joint/goal_reached_threshold" value="0.001" />
- <param name="r_arm_joint_trajectory_controller/r_shoulder_lift_joint/goal_reached_threshold" value="0.001" />
- <param name="r_arm_joint_trajectory_controller/r_upper_arm_roll_joint/goal_reached_threshold" value="0.001" />
- <param name="r_arm_joint_trajectory_controller/r_elbow_flex_joint/goal_reached_threshold" value="0.001" />
- <param name="r_arm_joint_trajectory_controller/r_forearm_roll_joint/goal_reached_threshold" value="0.001" />
- <param name="r_arm_joint_trajectory_controller/r_wrist_flex_joint/goal_reached_threshold" value="0.001" />
- <param name="r_arm_joint_trajectory_controller/r_wrist_roll_joint/goal_reached_threshold" value="0.001" />
-
- <param name="r_arm_joint_trajectory_controller/r_shoulder_pan_joint/joint_error_threshold" value="0.1" />
- <param name="r_arm_joint_trajectory_controller/r_shoulder_lift_joint/joint_error_threshold" value="0.1" />
- <param name="r_arm_joint_trajectory_controller/r_upper_arm_roll_joint/joint_error_threshold" value="0.1" />
- <param name="r_arm_joint_trajectory_controller/r_elbow_flex_joint/joint_error_threshold" value="0.1" />
- <param name="r_arm_joint_trajectory_controller/r_forearm_roll_joint/joint_error_threshold" value="0.1" />
- <param name="r_arm_joint_trajectory_controller/r_wrist_flex_joint/joint_error_threshold" value="0.1" />
- <param name="r_arm_joint_trajectory_controller/r_wrist_roll_joint/joint_error_threshold" value="0.1" />
-
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/r_arm_joint_trajectory_controller.xml" />
-
-</launch>
Deleted: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/rarm_splines.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/rarm_splines.launch 2009-07-30 04:05:47 UTC (rev 20132)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/rarm_splines.launch 2009-07-30 04:06:52 UTC (rev 20133)
@@ -1,15 +0,0 @@
-<launch>
-
- <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
- <include file="$(find pr2_ik)/pr2_ik_rarm_node.launch"/>
-
- <rosparam file="$(find pr2_default_controllers)/trajectory_controller.yaml" command="load" ns="trajectory_controller"/>
-
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/trajectory_controller.xml" output="screen"/>
-
-
- <include file="$(find move_arm)/launch/move_rarm_splines.launch"/>
-
- <include file="$(find joint_waypoint_controller)/launch/joint_waypoint_controller.launch"/>
-
-</launch>
Modified: pkg/trunk/highlevel/move_arm/launch/move_larm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_larm.launch 2009-07-30 04:05:47 UTC (rev 20132)
+++ pkg/trunk/highlevel/move_arm/launch/move_larm.launch 2009-07-30 04:06:52 UTC (rev 20133)
@@ -9,16 +9,16 @@
<remap from="get_valid_state" to="find_valid_state" />
- <remap from="controller_start" to="l_arm_joint_trajectory_controller/TrajectoryStart" />
- <remap from="controller_query" to="l_arm_joint_trajectory_controller/TrajectoryQuery" />
- <remap from="controller_cancel" to="l_arm_joint_trajectory_controller/TrajectoryCancel" />
+ <remap from="controller_start" to="/l_arm_joint_waypoint_controller/TrajectoryStart" />
+ <remap from="controller_query" to="/l_arm_joint_waypoint_controller/TrajectoryQuery" />
+ <remap from="controller_cancel" to="/l_arm_joint_waypoint_controller/TrajectoryCancel" />
<remap from="get_motion_plan" to="plan_kinematic_path" />
<param name="arm" type="string" value="left_arm" />
<param name="show_collisions" type="bool" value="true" />
- <param name="refresh_interval_collision_map" type="double" value="5.0" />
+ <param name="refresh_interval_collision_map" type="double" value="20.0" />
<param name="refresh_interval_kinematic_state" type="double" value="1.0" />
<param name="bounding_planes" type="string" value="0 0 1 -0.01" />
Modified: pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-07-30 04:05:47 UTC (rev 20132)
+++ pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-07-30 04:06:52 UTC (rev 20133)
@@ -9,16 +9,16 @@
<remap from="get_valid_state" to="find_valid_state" />
- <remap from="controller_start" to="r_arm_joint_trajectory_controller/TrajectoryStart" />
- <remap from="controller_query" to="r_arm_joint_trajectory_controller/TrajectoryQuery" />
- <remap from="controller_cancel" to="r_arm_joint_trajectory_controller/TrajectoryCancel" />
+ <remap from="controller_start" to="/r_arm_joint_waypoint_controller/TrajectoryStart" />
+ <remap from="controller_query" to="/r_arm_joint_waypoint_controller/TrajectoryQuery" />
+ <remap from="controller_cancel" to="/r_arm_joint_waypoint_controller/TrajectoryCancel" />
<remap from="get_motion_plan" to="plan_kinematic_path" />
<param name="arm" type="string" value="right_arm" />
<param name="show_collisions" type="bool" value="true" />
- <param name="refresh_interval_collision_map" type="double" value="5.0" />
+ <param name="refresh_interval_collision_map" type="double" value="20.0" />
<param name="refresh_interval_kinematic_state" type="double" value="1.0" />
<param name="bounding_planes" type="string" value="0 0 1 -0.01" />
</node>
Deleted: pkg/trunk/highlevel/move_arm/launch/move_rarm_splines.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_rarm_splines.launch 2009-07-30 04:05:47 UTC (rev 20132)
+++ pkg/trunk/highlevel/move_arm/launch/move_rarm_splines.launch 2009-07-30 04:06:52 UTC (rev 20133)
@@ -1,28 +0,0 @@
-<launch>
- <node pkg="move_arm" type="move_arm_action" output="screen">
-
- <remap from="robot_description" to="robot_description" />
- <remap from="collision_map" to="collision_map_occ" />
- <remap from="collision_map_update" to="collision_map_occ_update" />
-
- <remap from="arm_ik" to="pr2_ik_right_arm/ik_service" />
-
- <remap from="get_valid_state" to="find_valid_state" />
-
- <remap from="controller_start" to="/joint_waypoint_controller/TrajectoryStart" />
- <remap from="controller_query" to="/joint_waypoint_controller/TrajectoryQuery" />
- <remap from="controller_cancel" to="/joint_waypoint_controller/TrajectoryCancel" />
-
- <remap from="get_motion_plan" to="plan_kinematic_path" />
-
- <param name="arm" type="string" value="right_arm" />
- <param name="show_collisions" type="bool" value="true" />
-
- <param name="refresh_interval_collision_map" type="double" value="20.0" />
- <param name="refresh_interval_kinematic_state" type="double" value="1.0" />
- <param name="bounding_planes" type="string" value="0 0 1 -0.01" />
- </node>
-
- <include file="$(find move_arm)/launch/gripper_rarm.launch" />
-
-</launch>
Added: pkg/trunk/sandbox/3dnav_pr2/launch/actions/left_arm.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/actions/left_arm.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/actions/left_arm.launch 2009-07-30 04:06:52 UTC (rev 20133)
@@ -0,0 +1,13 @@
+<launch>
+
+ <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
+ <include file="$(find pr2_ik)/pr2_ik_larm_node.launch"/>
+
+ <rosparam file="$(find pr2_default_controllers)/l_arm_trajectory_controller.yaml" command="load" ns="l_arm_trajectory_controller"/>
+
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/l_arm_trajectory_controller.xml" output="screen"/>
+
+ <include file="$(find joint_waypoint_controller)/launch/l_arm_joint_waypoint_controller.launch"/>
+
+ <include file="$(find move_arm)/launch/move_larm.launch"/>
+</launch>
Added: pkg/trunk/sandbox/3dnav_pr2/launch/actions/right_arm.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/actions/right_arm.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/actions/right_arm.launch 2009-07-30 04:06:52 UTC (rev 20133)
@@ -0,0 +1,13 @@
+<launch>
+
+ <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
+ <include file="$(find pr2_ik)/pr2_ik_rarm_node.launch"/>
+
+ <rosparam file="$(find pr2_default_controllers)/r_arm_trajectory_controller.yaml" command="load" ns="r_arm_trajectory_controller"/>
+
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/r_arm_trajectory_controller.xml" output="screen"/>
+
+ <include file="$(find joint_waypoint_controller)/launch/r_arm_joint_waypoint_controller.launch"/>
+
+ <include file="$(find move_arm)/launch/move_rarm.launch"/>
+</launch>
Modified: pkg/trunk/sandbox/joint_waypoint_controller/launch/filters.xml
===================================================================
--- pkg/trunk/sandbox/joint_waypoint_controller/launch/filters.xml 2009-07-30 04:05:47 UTC (rev 20132)
+++ pkg/trunk/sandbox/joint_waypoint_controller/launch/filters.xml 2009-07-30 04:06:52 UTC (rev 20133)
@@ -1,5 +1,6 @@
<filters>
<filter type="FixWraparoundJoints" name="fix_wraparound_joints"/>
<filter type="ClampedCubicSplineSmoother" name="clamped_cubic_spline_smoother"/>
+<!-- <filter type="NumericalDifferentiationSplineSmoother" name="clamped_cubic_spline_smoother"/> -->
</filters>
Deleted: pkg/trunk/sandbox/joint_waypoint_controller/launch/joint_waypoint_controller.launch
===================================================================
--- pkg/trunk/sandbox/joint_waypoint_controller/launch/joint_waypoint_controller.launch 2009-07-30 04:05:47 UTC (rev 20132)
+++ pkg/trunk/sandbox/joint_waypoint_controller/launch/joint_waypoint_controller.launch 2009-07-30 04:06:52 UTC (rev 20133)
@@ -1,7 +0,0 @@
-<launch>
- <node pkg="joint_waypoint_controller" name="joint_waypoint_controller" type="joint_waypoint_controller" output="screen" clear_params="true">
- <param name="filters" textfile="$(find joint_waypoint_controller)/launch/filters.xml"/>
- <param name="trajectory_type" value="cubic"/>
- <param name="spline_controller_prefix" value="/trajectory_controller/"/>
- </node>
-</launch>
Added: pkg/trunk/sandbox/joint_waypoint_controller/launch/l_arm_joint_waypoint_controller.launch
===================================================================
--- pkg/trunk/sandbox/joint_waypoint_controller/launch/l_arm_joint_waypoint_controller.launch (rev 0)
+++ pkg/trunk/sandbox/joint_waypoint_controller/launch/l_arm_joint_waypoint_controller.launch 2009-07-30 04:06:52 UTC (rev 20133)
@@ -0,0 +1,7 @@
+<launch>
+ <node pkg="joint_waypoint_controller" name="l_arm_joint_waypoint_controller" type="joint_waypoint_controller" output="screen" clear_params="true">
+ <param name="filters" textfile="$(find joint_waypoint_controller)/launch/filters.xml"/>
+ <param name="trajectory_type" value="cubic"/>
+ <param name="spline_controller_prefix" value="/l_arm_trajectory_controller/"/>
+ </node>
+</launch>
Copied: pkg/trunk/sandbox/joint_waypoint_controller/launch/r_arm_joint_waypoint_controller.launch (from rev 20100, pkg/trunk/sandbox/joint_waypoint_controller/launch/joint_waypoint_controller.launch)
===================================================================
--- pkg/trunk/sandbox/joint_waypoint_controller/launch/r_arm_joint_waypoint_controller.launch (rev 0)
+++ pkg/trunk/sandbox/joint_waypoint_controller/launch/r_arm_joint_waypoint_controller.launch 2009-07-30 04:06:52 UTC (rev 20133)
@@ -0,0 +1,7 @@
+<launch>
+ <node pkg="joint_waypoint_controller" name="r_arm_joint_waypoint_controller" type="joint_waypoint_controller" output="screen" clear_params="true">
+ <param name="filters" textfile="$(find joint_waypoint_controller)/launch/filters.xml"/>
+ <param name="trajectory_type" value="cubic"/>
+ <param name="spline_controller_prefix" value="/r_arm_trajectory_controller/"/>
+ </node>
+</launch>
Property changes on: pkg/trunk/sandbox/joint_waypoint_controller/launch/r_arm_joint_waypoint_controller.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/sandbox/joint_waypoint_controller/launch/joint_waypoint_controller.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Added: pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_trajectory_controller.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_trajectory_controller.xml (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_trajectory_controller.xml 2009-07-30 04:06:52 UTC (rev 20133)
@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<controller name="dynamic_loader" type="DynamicLoaderController"
+ package="experimental_controllers" lib="libexperimental_controllers">
+<controllers>
+ <controller name="l_arm_trajectory_controller" type="TrajectoryController"/>
+</controllers>
+</controller>
Added: pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_trajectory_controller.yaml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_trajectory_controller.yaml (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_trajectory_controller.yaml 2009-07-30 04:06:52 UTC (rev 20133)
@@ -0,0 +1,58 @@
+joint_names: l_shoulder_pan_joint l_shoulder_lift_joint l_upper_arm_roll_joint l_elbow_flex_joint l_forearm_roll_joint l_wrist_flex_joint l_wrist_roll_joint
+
+l_shoulder_pan_joint:
+ joint: l_shoulder_pan_joint
+ p: 140.0
+ d: 30.0
+ i: 100.0
+ i_clamp: 3.0
+ goal_reached_threshold: 0.01
+
+l_shoulder_lift_joint:
+ joint: l_shoulder_lift_joint
+ p: 70.0
+ d: 20.0
+ i: 300.0
+ i_clamp: 3.0
+ goal_reached_threshold: 0.01
+
+l_upper_arm_roll_joint:
+ joint: l_upper_arm_roll_joint
+ p: 50.0
+ d: 5.0
+ i: 100.0
+ i_clamp: 3.0
+ goal_reached_threshold: 0.01
+
+l_elbow_flex_joint:
+ joint: l_elbow_flex_joint
+ p: 100.0
+ d: 5.0
+ i: 100.0
+ i_clamp: 1.0
+ goal_reached_threshold: 0.01
+
+l_forearm_roll_joint:
+ joint: l_forearm_roll_joint
+ p: 10.0
+ d: 2.0
+ i: 100.0
+ i_clamp: 1.0
+ goal_reached_threshold: 0.01
+
+l_wrist_flex_joint:
+ joint: l_wrist_flex_joint
+ p: 20.0
+ d: 0.5
+ i: 100.0
+ i_clamp: 1.0
+ goal_reached_threshold: 0.01
+
+l_wrist_roll_joint:
+ joint: l_wrist_roll_joint
+ p: 10.0
+ d: 0.5
+ i: 100.0
+ i_clamp: 1.0
+ goal_reached_threshold: 0.01
+
Copied: pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_trajectory_controller.xml (from rev 20100, pkg/trunk/stacks/pr2/pr2_default_controllers/trajectory_controller.xml)
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_trajectory_controller.xml (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_trajectory_controller.xml 2009-07-30 04:06:52 UTC (rev 20133)
@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<controller name="dynamic_loader" type="DynamicLoaderController"
+ package="experimental_controllers" lib="libexperimental_controllers">
+<controllers>
+ <controller name="r_arm_trajectory_controller" type="TrajectoryController"/>
+</controllers>
+</controller>
Property changes on: pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_trajectory_controller.xml
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/robot_descriptions/pr2/pr2_default_controllers/trajectory_controller.xml:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_trajectory_controller.yaml (from rev 20113, pkg/trunk/stacks/pr2/pr2_default_controllers/trajectory_controller.yaml)
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_trajectory_controller.yaml (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_trajectory_controller.yaml 2009-07-30 04:06:52 UTC (rev 20133)
@@ -0,0 +1,58 @@
+joint_names: r_shoulder_pan_joint r_shoulder_lift_joint r_upper_arm_roll_joint r_elbow_flex_joint r_forearm_roll_joint r_wrist_flex_joint r_wrist_roll_joint
+
+r_shoulder_pan_joint:
+ joint: r_shoulder_pan_joint
+ p: 140.0
+ d: 30.0
+ i: 100.0
+ i_clamp: 3.0
+ goal_reached_threshold: 0.01
+
+r_shoulder_lift_joint:
+ joint: r_shoulder_lift_joint
+ p: 70.0
+ d: 20.0
+ i: 300.0
+ i_clamp: 3.0
+ goal_reached_threshold: 0.01
+
+r_upper_arm_roll_joint:
+ joint: r_upper_arm_roll_joint
+ p: 50.0
+ d: 5.0
+ i: 100.0
+ i_clamp: 3.0
+ goal_reached_threshold: 0.01
+
+r_elbow_flex_joint:
+ joint: r_elbow_flex_joint
+ p: 100.0
+ d: 5.0
+ i: 100.0
+ i_clamp: 1.0
+ goal_reached_threshold: 0.01
+
+r_forearm_roll_joint:
+ joint: r_forearm_roll_joint
+ p: 10.0
+ d: 2.0
+ i: 100.0
+ i_clamp: 1.0
+ goal_reached_threshold: 0.01
+
+r_wrist_flex_joint:
+ joint: r_wrist_flex_joint
+ p: 20.0
+ d: 0.5
+ i: 100.0
+ i_clamp: 1.0
+ goal_reached_threshold: 0.01
+
+r_wrist_roll_joint:
+ joint: r_wrist_roll_joint
+ p: 10.0
+ d: 0.5
+ i: 100.0
+ i_clamp: 1.0
+ goal_reached_threshold: 0.01
+
Property changes on: pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_trajectory_controller.yaml
___________________________________________________________________
Added: svn:mergeinfo
+
Deleted: pkg/trunk/stacks/pr2/pr2_default_controllers/trajectory_controller.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/trajectory_controller.xml 2009-07-30 04:05:47 UTC (rev 20132)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/trajectory_controller.xml 2009-07-30 04:06:52 UTC (rev 20133)
@@ -1,7 +0,0 @@
-<?xml version="1.0"?>
-<controller name="dynamic_loader" type="DynamicLoaderController"
- package="experimental_controllers" lib="libexperimental_controllers">
-<controllers>
- <controller name="trajectory_controller" type="TrajectoryController"/>
-</controllers>
-</controller>
\ No newline at end of file
Deleted: pkg/trunk/stacks/pr2/pr2_default_controllers/trajectory_controller.yaml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/trajectory_controller.yaml 2009-07-30 04:05:47 UTC (rev 20132)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/trajectory_controller.yaml 2009-07-30 04:06:52 UTC (rev 20133)
@@ -1,58 +0,0 @@
-joint_names: r_shoulder_pan_joint r_shoulder_lift_joint r_upper_arm_roll_joint r_elbow_flex_joint r_forearm_roll_joint r_wrist_flex_joint r_wrist_roll_joint
-
-r_shoulder_pan_joint:
- joint: r_shoulder_pan_joint
- p: 140.0
- d: 30.0
- i: 100.0
- i_clamp: 3.0
- goal_reached_threshold: 0.1
-
-r_shoulder_lift_joint:
- joint: r_shoulder_lift_joint
- p: 70.0
- d: 20.0
- i: 300.0
- i_clamp: 3.0
- goal_reached_threshold: 0.1
-
-r_upper_arm_roll_joint:
- joint: r_upper_arm_roll_joint
- p: 50.0
- d: 5.0
- i: 100.0
- i_clamp: 3.0
- goal_reached_threshold: 0.1
-
-r_elbow_flex_joint:
- joint: r_elbow_flex_joint
- p: 100.0
- d: 5.0
- i: 100.0
- i_clamp: 1.0
- goal_reached_threshold: 0.1
-
-r_forearm_roll_joint:
- joint: r_forearm_roll_joint
- p: 10.0
- d: 2.0
- i: 100.0
- i_clamp: 1.0
- goal_reached_threshold: 0.1
-
-r_wrist_flex_joint:
- joint: r_wrist_flex_joint
- p: 20.0
- d: 0.5
- i: 100.0
- i_clamp: 1.0
- goal_reached_threshold: 0.1
-
-r_wrist_roll_joint:
- joint: r_wrist_roll_joint
- p: 10.0
- d: 0.5
- i: 100.0
- i_clamp: 1.0
- goal_reached_threshold: 0.1
-
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-07-30 18:57:20
|
Revision: 20170
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20170&view=rev
Author: eitanme
Date: 2009-07-30 18:57:04 +0000 (Thu, 30 Jul 2009)
Log Message:
-----------
Moving StaticMap.srv to GetMap.srv and moving Plan.srv to GetPlan.srv, also moving them to nav_msgs and removing the nav_srvs package
Modified Paths:
--------------
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_check_path.h
pkg/trunk/highlevel/doors/doors_core/manifest.xml
pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h
pkg/trunk/nav/people_aware_nav/manifest.xml
pkg/trunk/nav/wavefront/manifest.xml
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
pkg/trunk/sandbox/webteleop/manifest.xml
pkg/trunk/sandbox/webteleop/src/server.py
pkg/trunk/stacks/common_msgs/nav_msgs/CMakeLists.txt
pkg/trunk/stacks/common_msgs/nav_msgs/manifest.xml
pkg/trunk/stacks/map_building/slam_gmapping/manifest.xml
pkg/trunk/stacks/map_building/slam_gmapping/src/slam_gmapping.cpp
pkg/trunk/stacks/map_building/slam_gmapping/src/slam_gmapping.h
pkg/trunk/stacks/nav/amcl/manifest.xml
pkg/trunk/stacks/nav/amcl/src/amcl_node.cpp
pkg/trunk/stacks/nav/costmap_2d/manifest.xml
pkg/trunk/stacks/nav/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/stacks/nav/costmap_2d/src/costmap_test.cpp
pkg/trunk/stacks/nav/map_server/include/map_server/image_loader.h
pkg/trunk/stacks/nav/map_server/manifest.xml
pkg/trunk/stacks/nav/map_server/src/image_loader.cpp
pkg/trunk/stacks/nav/map_server/src/main.cpp
pkg/trunk/stacks/nav/map_server/src/map_saver.cpp
pkg/trunk/stacks/nav/map_server/test/consumer.py
pkg/trunk/stacks/nav/map_server/test/rtest.cpp
pkg/trunk/stacks/nav/map_server/test/utest.cpp
pkg/trunk/stacks/nav/move_base/include/move_base/move_base.h
pkg/trunk/stacks/nav/move_base/include/move_base/move_base_old.h
pkg/trunk/stacks/nav/move_base/manifest.xml
pkg/trunk/stacks/nav/move_base/src/move_base.cpp
pkg/trunk/stacks/nav/move_base/src/move_base_old.cpp
pkg/trunk/stacks/nav/nav_view/manifest.xml
pkg/trunk/stacks/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/stacks/visualization/rviz/manifest.xml
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/map_display.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/map_display.h
Removed Paths:
-------------
pkg/trunk/stacks/common/nav_srvs/
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_check_path.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_check_path.h 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_check_path.h 2009-07-30 18:57:04 UTC (rev 20170)
@@ -42,7 +42,7 @@
#include <ros/node.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
-#include <nav_srvs/Plan.h>
+#include <nav_msgs/GetPlan.h>
#include <robot_actions/action.h>
namespace door_handle_detector{
@@ -60,8 +60,8 @@
private:
tf::TransformListener& tf_;
- nav_srvs::Plan::Request req_plan;
- nav_srvs::Plan::Response res_plan;
+ nav_msgs::GetPlan::Request req_plan;
+ nav_msgs::GetPlan::Response res_plan;
};
}
Modified: pkg/trunk/highlevel/doors/doors_core/manifest.xml
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/manifest.xml 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/highlevel/doors/doors_core/manifest.xml 2009-07-30 18:57:04 UTC (rev 20170)
@@ -19,7 +19,7 @@
<depend package="robot_srvs" />
<depend package="deprecated_srvs" />
- <depend package="nav_srvs" />
+ <depend package="nav_msgs" />
<depend package="robot_actions" />
<depend package="nav_robot_actions" />
Modified: pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h
===================================================================
--- pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h 2009-07-30 18:57:04 UTC (rev 20170)
@@ -49,7 +49,7 @@
#include <base_local_planner/trajectory_planner_ros.h>
#include <vector>
#include <string>
-#include <nav_srvs/Plan.h>
+#include <nav_msgs/GetPlan.h>
namespace people_aware_nav {
/**
@@ -86,7 +86,7 @@
* @param resp The plan request
* @return True if planning succeeded, false otherwise
*/
- bool planService(nav_srvs::Plan::Request &req, nav_srvs::Plan::Response &resp);
+ bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
/**
* @brief Make a new global plan
Modified: pkg/trunk/nav/people_aware_nav/manifest.xml
===================================================================
--- pkg/trunk/nav/people_aware_nav/manifest.xml 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/nav/people_aware_nav/manifest.xml 2009-07-30 18:57:04 UTC (rev 20170)
@@ -14,7 +14,7 @@
<depend package="costmap_2d"/>
<depend package="angles"/>
<depend package="navfn"/>
-<depend package="nav_srvs"/>
+<depend package="nav_msgs"/>
<depend package="robot_msgs" />
<depend package="robot_actions" />
<depend package="deprecated_msgs" />
Modified: pkg/trunk/nav/wavefront/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront/manifest.xml 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/nav/wavefront/manifest.xml 2009-07-30 18:57:04 UTC (rev 20170)
@@ -5,7 +5,7 @@
<review status="unreviewed" notes=""/>
<depend package="roscpp" />
<depend package="laser_scan" />
- <depend package="nav_srvs" />
+ <depend package="nav_msgs" />
<depend package="sensor_msgs" />
<depend package="robot_msgs" />
<depend package="robot_actions" />
Modified: pkg/trunk/nav/wavefront/src/wavefront_node.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-07-30 18:57:04 UTC (rev 20170)
@@ -106,7 +106,7 @@
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/PointCloud.h>
#include <sensor_msgs/LaserScan.h>
-#include <nav_srvs/StaticMap.h>
+#include <nav_msgs/GetMap.h>
// For GUI debug
#include <visualization_msgs/Polyline.h>
@@ -295,8 +295,8 @@
gui_publish_rate = ros::Duration(1.0/tmp);
// get map via RPC
- nav_srvs::StaticMap::Request req;
- nav_srvs::StaticMap::Response resp;
+ nav_msgs::GetMap::Request req;
+ nav_msgs::GetMap::Response resp;
puts("Requesting the map...");
while(!ros::service::call("/static_map", req, resp))
{
Modified: pkg/trunk/sandbox/webteleop/manifest.xml
===================================================================
--- pkg/trunk/sandbox/webteleop/manifest.xml 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/sandbox/webteleop/manifest.xml 2009-07-30 18:57:04 UTC (rev 20170)
@@ -11,7 +11,6 @@
<depend package="std_msgs"/>
<depend package="robot_msgs"/>
<depend package="robot_srvs"/>
- <depend package="nav_srvs"/>
<depend package="nav_msgs"/>
<depend package="bullet_python"/>
<depend package="tf"/>
Modified: pkg/trunk/sandbox/webteleop/src/server.py
===================================================================
--- pkg/trunk/sandbox/webteleop/src/server.py 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/sandbox/webteleop/src/server.py 2009-07-30 18:57:04 UTC (rev 20170)
@@ -22,7 +22,7 @@
import bullet
from tf.msg import tfMessage
-from nav_srvs.srv import *
+from nav_msgs.srv import *
from nav_msgs.msg import *
from std_msgs.msg import *
from robot_msgs.msg import *
@@ -314,7 +314,7 @@
data = ''
try:
#staticMap = rospy.ServiceProxy(topic, service_class)
- #staticMap = rospy.ServiceProxy('/static_map', StaticMap)
+ #staticMap = rospy.ServiceProxy('/static_map', GetMap)
map = service_proxy()
mapW = map.map.info.width
Modified: pkg/trunk/stacks/common_msgs/nav_msgs/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/common_msgs/nav_msgs/CMakeLists.txt 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/common_msgs/nav_msgs/CMakeLists.txt 2009-07-30 18:57:04 UTC (rev 20170)
@@ -19,7 +19,7 @@
#uncomment if you have defined messages
genmsg()
#uncomment if you have defined services
-#gensrv()
+gensrv()
#common commands for building c++ executables and libraries
#rospack_add_library(${PROJECT_NAME} src/example.cpp)
Modified: pkg/trunk/stacks/common_msgs/nav_msgs/manifest.xml
===================================================================
--- pkg/trunk/stacks/common_msgs/nav_msgs/manifest.xml 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/common_msgs/nav_msgs/manifest.xml 2009-07-30 18:57:04 UTC (rev 20170)
@@ -10,7 +10,7 @@
<url>http://pr.willowgarage.com/wiki/nav_msgs</url>
<depend package="robot_msgs"/>
<export>
- <cpp cflags="-I${prefix}/msg/cpp"/>
+ <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/srv/cpp"/>
</export>
</package>
Modified: pkg/trunk/stacks/map_building/slam_gmapping/manifest.xml
===================================================================
--- pkg/trunk/stacks/map_building/slam_gmapping/manifest.xml 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/map_building/slam_gmapping/manifest.xml 2009-07-30 18:57:04 UTC (rev 20170)
@@ -7,7 +7,7 @@
<depend package="rosconsole"/>
<depend package="std_msgs"/>
<depend package="laser_scan"/>
- <depend package="nav_srvs"/>
+ <depend package="nav_msgs"/>
<depend package="gmapping"/>
<depend package="tf"/>
</package>
Modified: pkg/trunk/stacks/map_building/slam_gmapping/src/slam_gmapping.cpp
===================================================================
--- pkg/trunk/stacks/map_building/slam_gmapping/src/slam_gmapping.cpp 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/map_building/slam_gmapping/src/slam_gmapping.cpp 2009-07-30 18:57:04 UTC (rev 20170)
@@ -452,8 +452,8 @@
}
bool
-SlamGMapping::map_cb(nav_srvs::StaticMap::Request &req,
- nav_srvs::StaticMap::Response &res)
+SlamGMapping::map_cb(nav_msgs::GetMap::Request &req,
+ nav_msgs::GetMap::Response &res)
{
if(got_map_ && map_.map.info.width && map_.map.info.height)
{
Modified: pkg/trunk/stacks/map_building/slam_gmapping/src/slam_gmapping.h
===================================================================
--- pkg/trunk/stacks/map_building/slam_gmapping/src/slam_gmapping.h 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/map_building/slam_gmapping/src/slam_gmapping.h 2009-07-30 18:57:04 UTC (rev 20170)
@@ -31,7 +31,7 @@
#include "ros/node.h"
#include "sensor_msgs/LaserScan.h"
-#include "nav_srvs/StaticMap.h"
+#include "nav_msgs/GetMap.h"
#include "tf/transform_listener.h"
#include "tf/transform_broadcaster.h"
#include <tf/message_notifier.h>
@@ -48,8 +48,8 @@
void main_loop();
void laser_cb(const tf::MessageNotifier<sensor_msgs::LaserScan>::MessagePtr& message);
- bool map_cb(nav_srvs::StaticMap::Request &req,
- nav_srvs::StaticMap::Response &res);
+ bool map_cb(nav_msgs::GetMap::Request &req,
+ nav_msgs::GetMap::Response &res);
private:
ros::Node* node_;
@@ -65,7 +65,7 @@
bool got_first_scan_;
bool got_map_;
- nav_srvs::StaticMap::Response map_;
+ nav_msgs::GetMap::Response map_;
ros::Duration map_update_interval_;
tf::Transform map_to_odom_;
Modified: pkg/trunk/stacks/nav/amcl/manifest.xml
===================================================================
--- pkg/trunk/stacks/nav/amcl/manifest.xml 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/amcl/manifest.xml 2009-07-30 18:57:04 UTC (rev 20170)
@@ -10,7 +10,6 @@
<depend package="robot_srvs" />
<depend package="robot_msgs" />
<depend package="nav_msgs" />
- <depend package="nav_srvs" />
<depend package="std_srvs" />
<depend package="laser_scan" />
<depend package="visualization_msgs"/>
Modified: pkg/trunk/stacks/nav/amcl/src/amcl_node.cpp
===================================================================
--- pkg/trunk/stacks/nav/amcl/src/amcl_node.cpp 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/amcl/src/amcl_node.cpp 2009-07-30 18:57:04 UTC (rev 20170)
@@ -160,7 +160,7 @@
#include "robot_msgs/PoseWithCovariance.h"
#include "nav_msgs/PoseArray.h"
#include "robot_msgs/Pose.h"
-#include "nav_srvs/StaticMap.h"
+#include "nav_msgs/GetMap.h"
#include "std_srvs/Empty.h"
#include "visualization_msgs/Polyline.h"
@@ -462,8 +462,8 @@
ROS_ASSERT(map);
// get map via RPC
- nav_srvs::StaticMap::Request req;
- nav_srvs::StaticMap::Response resp;
+ nav_msgs::GetMap::Request req;
+ nav_msgs::GetMap::Response resp;
ROS_INFO("Requesting the map...");
while(!ros::service::call("static_map", req, resp))
{
Modified: pkg/trunk/stacks/nav/costmap_2d/manifest.xml
===================================================================
--- pkg/trunk/stacks/nav/costmap_2d/manifest.xml 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/costmap_2d/manifest.xml 2009-07-30 18:57:04 UTC (rev 20170)
@@ -20,7 +20,7 @@
<depend package="laser_scan" />
<depend package="tf" />
<depend package="voxel_grid" />
-<depend package="nav_srvs" />
+<depend package="nav_msgs" />
<depend package="visualization_msgs" />
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lcostmap_2d"/>
Modified: pkg/trunk/stacks/nav/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/stacks/nav/costmap_2d/src/costmap_2d_ros.cpp 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/costmap_2d/src/costmap_2d_ros.cpp 2009-07-30 18:57:04 UTC (rev 20170)
@@ -36,7 +36,7 @@
*********************************************************************/
#include <costmap_2d/costmap_2d_ros.h>
-#include <nav_srvs/StaticMap.h>
+#include <nav_msgs/GetMap.h>
namespace costmap_2d {
@@ -155,8 +155,8 @@
map_height = (unsigned int)(map_height_meters / map_resolution);
if(static_map){
- nav_srvs::StaticMap::Request map_req;
- nav_srvs::StaticMap::Response map_resp;
+ nav_msgs::GetMap::Request map_req;
+ nav_msgs::GetMap::Response map_resp;
ROS_INFO("Requesting the map...\n");
while(!ros::service::call("/static_map", map_req, map_resp))
{
Modified: pkg/trunk/stacks/nav/costmap_2d/src/costmap_test.cpp
===================================================================
--- pkg/trunk/stacks/nav/costmap_2d/src/costmap_test.cpp 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/costmap_2d/src/costmap_test.cpp 2009-07-30 18:57:04 UTC (rev 20170)
@@ -38,7 +38,7 @@
#include <ros/console.h>
#include <new_costmap/costmap_2d.h>
#include <new_costmap/observation_buffer.h>
-#include <nav_srvs/StaticMap.h>
+#include <nav_msgs/GetMap.h>
#include <visualization_msgs/Polyline.h>
#include <map>
#include <vector>
@@ -71,8 +71,8 @@
boost::bind(&CostmapTester::baseScanCallback, this, _1, (int) 1),
"base_scan", global_frame_, 50);
- nav_srvs::StaticMap::Request map_req;
- nav_srvs::StaticMap::Response map_resp;
+ nav_msgs::GetMap::Request map_req;
+ nav_msgs::GetMap::Response map_resp;
ROS_INFO("Requesting the map...\n");
while(!ros::service::call("static_map", map_req, map_resp))
{
Modified: pkg/trunk/stacks/nav/map_server/include/map_server/image_loader.h
===================================================================
--- pkg/trunk/stacks/nav/map_server/include/map_server/image_loader.h 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/map_server/include/map_server/image_loader.h 2009-07-30 18:57:04 UTC (rev 20170)
@@ -33,7 +33,7 @@
* Author: Brian Gerkey
*/
-#include "nav_srvs/StaticMap.h"
+#include "nav_msgs/GetMap.h"
namespace map_server
{
@@ -51,7 +51,7 @@
*
* @throws std::runtime_error If the image file can't be loaded
* */
-void loadMapFromFile(nav_srvs::StaticMap::Response* resp,
+void loadMapFromFile(nav_msgs::GetMap::Response* resp,
const char* fname, double res, bool negate,
double occ_th, double free_th);
}
Modified: pkg/trunk/stacks/nav/map_server/manifest.xml
===================================================================
--- pkg/trunk/stacks/nav/map_server/manifest.xml 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/map_server/manifest.xml 2009-07-30 18:57:04 UTC (rev 20170)
@@ -11,7 +11,7 @@
<depend package="rosconsole"/>
<depend package="roscpp"/>
<depend package="rospy" />
- <depend package="nav_srvs"/>
+ <depend package="nav_msgs"/>
<depend package="bullet"/>
<export>
<cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread`"/>
Modified: pkg/trunk/stacks/nav/map_server/src/image_loader.cpp
===================================================================
--- pkg/trunk/stacks/nav/map_server/src/image_loader.cpp 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/map_server/src/image_loader.cpp 2009-07-30 18:57:04 UTC (rev 20170)
@@ -51,7 +51,7 @@
{
void
-loadMapFromFile(nav_srvs::StaticMap::Response* resp,
+loadMapFromFile(nav_msgs::GetMap::Response* resp,
const char* fname, double res, bool negate,
double occ_th, double free_th)
{
Modified: pkg/trunk/stacks/nav/map_server/src/main.cpp
===================================================================
--- pkg/trunk/stacks/nav/map_server/src/main.cpp 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/map_server/src/main.cpp 2009-07-30 18:57:04 UTC (rev 20170)
@@ -70,7 +70,7 @@
@section services ROS services
Offers (name/type):
-- @b "static_map"/nav_srvs::StaticMap : Retrieve the map via this service
+- @b "static_map"/nav_msgs::GetMap : Retrieve the map via this service
@section parameters ROS parameters
@@ -144,8 +144,8 @@
ros::ServiceServer service;
/** Callback invoked when someone requests our service */
- bool mapCallback(nav_srvs::StaticMap::Request &req,
- nav_srvs::StaticMap::Response &res )
+ bool mapCallback(nav_msgs::GetMap::Request &req,
+ nav_msgs::GetMap::Response &res )
{
// request is empty; we ignore it
@@ -157,7 +157,7 @@
/** The map response is cached here, to be sent out to service callers
*/
- nav_srvs::StaticMap::Response map_resp_;
+ nav_msgs::GetMap::Response map_resp_;
void metadataSubscriptionCallback(const ros::SingleSubscriberPublisher& pub)
{
Modified: pkg/trunk/stacks/nav/map_server/src/map_saver.cpp
===================================================================
--- pkg/trunk/stacks/nav/map_server/src/map_saver.cpp 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/map_server/src/map_saver.cpp 2009-07-30 18:57:04 UTC (rev 20170)
@@ -57,14 +57,14 @@
@section topic ROS services
Uses (name type):
-- @b static_map nav_srvs/StaticMap : map service.
+- @b static_map nav_msgs/GetMap : map service.
**/
#include <cstdio>
#include "ros/node.h"
#include "ros/console.h"
-#include "nav_srvs/StaticMap.h"
+#include "nav_msgs/GetMap.h"
#include "LinearMath/btMatrix3x3.h"
using namespace std;
@@ -84,8 +84,8 @@
ros::Node n("map_generator");
const static std::string servname = "static_map";
ROS_INFO("Requesting the map from %s...", n.mapName(servname).c_str());
- nav_srvs::StaticMap::Request req;
- nav_srvs::StaticMap::Response resp;
+ nav_msgs::GetMap::Request req;
+ nav_msgs::GetMap::Response resp;
while(n.ok() && !ros::service::call(servname, req, resp))
{
ROS_WARN("request to %s failed; trying again...", n.mapName(servname).c_str());
Modified: pkg/trunk/stacks/nav/map_server/test/consumer.py
===================================================================
--- pkg/trunk/stacks/nav/map_server/test/consumer.py 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/map_server/test/consumer.py 2009-07-30 18:57:04 UTC (rev 20170)
@@ -41,7 +41,7 @@
import sys, unittest, time
import rospy, rostest
-from nav_srvs.srv import StaticMap
+from nav_msgs.srv import GetMap
class TestConsumer(unittest.TestCase):
def __init__(self, *args):
@@ -55,7 +55,7 @@
def test_consumer(self):
rospy.wait_for_service('static_map')
- mapsrv = rospy.ServiceProxy('static_map', StaticMap)
+ mapsrv = rospy.ServiceProxy('static_map', GetMap)
resp = mapsrv()
self.success = True
print resp
Modified: pkg/trunk/stacks/nav/map_server/test/rtest.cpp
===================================================================
--- pkg/trunk/stacks/nav/map_server/test/rtest.cpp 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/map_server/test/rtest.cpp 2009-07-30 18:57:04 UTC (rev 20170)
@@ -32,7 +32,7 @@
#include <gtest/gtest.h>
#include <ros/service.h>
#include <ros/node.h>
-#include <nav_srvs/StaticMap.h>
+#include <nav_msgs/GetMap.h>
#include "test_constants.h"
@@ -65,8 +65,8 @@
{
try
{
- nav_srvs::StaticMap::Request req;
- nav_srvs::StaticMap::Response resp;
+ nav_msgs::GetMap::Request req;
+ nav_msgs::GetMap::Response resp;
// Try a few times, because the server may not be up yet.
int i=10;
bool call_result;
Modified: pkg/trunk/stacks/nav/map_server/test/utest.cpp
===================================================================
--- pkg/trunk/stacks/nav/map_server/test/utest.cpp 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/map_server/test/utest.cpp 2009-07-30 18:57:04 UTC (rev 20170)
@@ -43,7 +43,7 @@
{
try
{
- nav_srvs::StaticMap::Response map_resp;
+ nav_msgs::GetMap::Response map_resp;
map_server::loadMapFromFile(&map_resp, g_valid_png_file, g_valid_image_res, false, 0.65, 0.1);
EXPECT_FLOAT_EQ(map_resp.map.info.resolution, g_valid_image_res);
EXPECT_EQ(map_resp.map.info.width, g_valid_image_width);
@@ -63,7 +63,7 @@
{
try
{
- nav_srvs::StaticMap::Response map_resp;
+ nav_msgs::GetMap::Response map_resp;
map_server::loadMapFromFile(&map_resp, g_valid_bmp_file, g_valid_image_res, false, 0.65, 0.1);
EXPECT_FLOAT_EQ(map_resp.map.info.resolution, g_valid_image_res);
EXPECT_EQ(map_resp.map.info.width, g_valid_image_width);
@@ -83,7 +83,7 @@
{
try
{
- nav_srvs::StaticMap::Response map_resp;
+ nav_msgs::GetMap::Response map_resp;
map_server::loadMapFromFile(&map_resp, "foo", 0.1, false, 0.65, 0.1);
}
catch(std::runtime_error &e)
Modified: pkg/trunk/stacks/nav/move_base/include/move_base/move_base.h
===================================================================
--- pkg/trunk/stacks/nav/move_base/include/move_base/move_base.h 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/move_base/include/move_base/move_base.h 2009-07-30 18:57:04 UTC (rev 20170)
@@ -48,7 +48,7 @@
#include <costmap_2d/costmap_2d.h>
#include <vector>
#include <string>
-#include <nav_srvs/Plan.h>
+#include <nav_msgs/GetPlan.h>
#include <visualization_msgs/Marker.h>
#include <robot_msgs/PoseDot.h>
@@ -109,7 +109,7 @@
* @param resp The plan request
* @return True if planning succeeded, false otherwise
*/
- bool planService(nav_srvs::Plan::Request &req, nav_srvs::Plan::Response &resp);
+ bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
/**
* @brief Make a new global plan
Modified: pkg/trunk/stacks/nav/move_base/include/move_base/move_base_old.h
===================================================================
--- pkg/trunk/stacks/nav/move_base/include/move_base/move_base_old.h 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/move_base/include/move_base/move_base_old.h 2009-07-30 18:57:04 UTC (rev 20170)
@@ -48,7 +48,7 @@
#include <costmap_2d/rate.h>
#include <vector>
#include <string>
-#include <nav_srvs/Plan.h>
+#include <nav_msgs/GetPlan.h>
#include <visualization_msgs/Marker.h>
#include <robot_msgs/PoseDot.h>
@@ -99,7 +99,7 @@
* @param resp The plan request
* @return True if planning succeeded, false otherwise
*/
- bool planService(nav_srvs::Plan::Request &req, nav_srvs::Plan::Response &resp);
+ bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
/**
* @brief Make a new global plan
Modified: pkg/trunk/stacks/nav/move_base/manifest.xml
===================================================================
--- pkg/trunk/stacks/nav/move_base/manifest.xml 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/move_base/manifest.xml 2009-07-30 18:57:04 UTC (rev 20170)
@@ -19,7 +19,7 @@
<depend package="costmap_2d"/>
<depend package="tf"/>
<depend package="roslib"/>
- <depend package="nav_srvs"/>
+ <depend package="nav_msgs"/>
<!--These deps should go away once the plugin model is implemented -->
<depend package="base_local_planner"/>
Modified: pkg/trunk/stacks/nav/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/stacks/nav/move_base/src/move_base.cpp 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/move_base/src/move_base.cpp 2009-07-30 18:57:04 UTC (rev 20170)
@@ -190,7 +190,7 @@
controller_costmap_ros_->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
}
- bool MoveBase::planService(nav_srvs::Plan::Request &req, nav_srvs::Plan::Response &resp){
+ bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp){
if(as_.isActive()){
ROS_ERROR("move_base must be in an inactive state to make a plan for an external user");
return false;
Modified: pkg/trunk/stacks/nav/move_base/src/move_base_old.cpp
===================================================================
--- pkg/trunk/stacks/nav/move_base/src/move_base_old.cpp 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/move_base/src/move_base_old.cpp 2009-07-30 18:57:04 UTC (rev 20170)
@@ -173,7 +173,7 @@
controller_costmap_ros_->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
}
- bool MoveBase::planService(nav_srvs::Plan::Request &req, nav_srvs::Plan::Response &resp){
+ bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp){
if(isActive()){
ROS_ERROR("move_base must be in an inactive state to make a plan for an external user");
return false;
Modified: pkg/trunk/stacks/nav/nav_view/manifest.xml
===================================================================
--- pkg/trunk/stacks/nav/nav_view/manifest.xml 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/nav_view/manifest.xml 2009-07-30 18:57:04 UTC (rev 20170)
@@ -5,7 +5,6 @@
<review status="proposal cleared" notes=""/>
<url>http://pr.willowgarage.com/wiki/nav_view</url>
<depend package="roscpp"/>
- <depend package="nav_srvs"/>
<depend package="robot_srvs"/>
<depend package="nav_msgs"/>
<depend package="tf"/>
Modified: pkg/trunk/stacks/nav/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/stacks/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-07-30 18:57:04 UTC (rev 20170)
@@ -31,7 +31,7 @@
#include "ogre_tools/wx_ogre_render_window.h"
-#include "nav_srvs/StaticMap.h"
+#include "nav_msgs/GetMap.h"
#include "nav_msgs/PoseArray.h"
#include <tf/transform_listener.h>
@@ -193,8 +193,8 @@
void NavViewPanel::loadMap()
{
- nav_srvs::StaticMap::Request req;
- nav_srvs::StaticMap::Response resp;
+ nav_msgs::GetMap::Request req;
+ nav_msgs::GetMap::Response resp;
ROS_INFO("Requesting the map...\n");
if( !ros::service::call("/static_map", req, resp) )
{
Modified: pkg/trunk/stacks/visualization/rviz/manifest.xml
===================================================================
--- pkg/trunk/stacks/visualization/rviz/manifest.xml 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/visualization/rviz/manifest.xml 2009-07-30 18:57:04 UTC (rev 20170)
@@ -17,7 +17,6 @@
<depend package="robot_msgs"/>
<depend package="sensor_msgs"/>
<depend package="nav_msgs"/>
- <depend package="nav_srvs"/>
<depend package="mechanism_msgs" />
<depend package="deprecated_msgs"/>
<depend package="tf"/>
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/map_display.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/map_display.cpp 2009-07-30 18:53:07 UTC (rev 20169)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/map_display.cpp 2009-07-30 18:57:04 UTC (rev 20170)
@@ -188,9 +188,9 @@
{
if (!new_map_ && (!loaded_ || reload_))
{
- nav_srvs::StaticMap srv;
+ nav_msgs::GetMap srv;
ROS_DEBUG("Requesting the map...");
- ros::ServiceClient client = update_nh_.serviceClient<nav_srvs::StaticMap>(service_);
+ ros::ServiceClient client = update_nh_.serviceClient<nav_msgs::GetMap>(service_);
if(client.call(srv) )
{
{
@@ -442,7 +442,7 @@
const char* MapDisplay::getDescription()
{
- return "Displays an image of a map gotten through a nav_srvs::StaticMap service.";
+ return "Displays an image of a map gotten through a nav_msgs::GetMap service.";
}
} // namespace rviz
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/map_display.h
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz...
[truncated message content] |
|
From: <stu...@us...> - 2009-07-30 21:06:33
|
Revision: 20185
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20185&view=rev
Author: stuglaser
Date: 2009-07-30 21:06:22 +0000 (Thu, 30 Jul 2009)
Log Message:
-----------
Merging in new controller spawning changes.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller_effort.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_position_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_qualification.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_gripper_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
pkg/trunk/controllers/qualification_controllers/dynamic_verification_controllers/include/dynamic_verification_controllers/constant_torque_controller.h
pkg/trunk/controllers/qualification_controllers/dynamic_verification_controllers/include/dynamic_verification_controllers/freq_resp_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/backlash_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/checkout_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_tff_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_trajectory_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/dynamic_loader_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_autotuner.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_sine_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_effort_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_inverse_dynamics_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_pd_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_smoothing_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/probe.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/trigger_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py
pkg/trunk/controllers/robot_mechanism_controllers/scripts/position.py
pkg/trunk/controllers/robot_mechanism_controllers/scripts/velocity.py
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_tff_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller_ik.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/dynamic_loader_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_effort_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_inverse_dynamics_controller.cpp
pkg/trunk/demos/handhold/run.launch
pkg/trunk/demos/milestone2/milestone2_actions/config/controllers.xml
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
pkg/trunk/highlevel/move_arm/launch/gripper_larm.launch
pkg/trunk/highlevel/move_arm/launch/gripper_rarm.launch
pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_trajectory_left.launch
pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_trajectory_right.launch
pkg/trunk/pr2/qualification/scripts/full_arm_test_spawner.py
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hca.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcb.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.launch
pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/r_arm_hybrid_controller.xml
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_trajectory_controller2.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/trajectory_controller.h
pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate.py
pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_pr2.py
pkg/trunk/stacks/mechanism/mechanism_control/CMakeLists.txt
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/stacks/mechanism/mechanism_control/scripts/mech.py
pkg/trunk/stacks/mechanism/mechanism_control/scripts/spawner.py
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control/mechanism.py
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/stacks/mechanism/mechanism_msgs/srv/SpawnController.srv
pkg/trunk/stacks/mechanism/mechanism_msgs/srv/SwitchController.srv
pkg/trunk/stacks/mechanism/robot_state_publisher/src/joint_state_listener.cpp
pkg/trunk/stacks/pr2/pr2_alpha/prf_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_alpha/prg_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_alpha/teleop_joystick.launch
pkg/trunk/stacks/pr2/pr2_alpha/teleop_joystick_new.launch
pkg/trunk/stacks/pr2/pr2_alpha/teleop_keyboard.launch
pkg/trunk/stacks/pr2/pr2_alpha/wheel_odometry_calibrate.launch
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_controller.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_controller_odom.launch
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_calibration_controllers.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_odometry.yaml
pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/one_way_outlets.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_detector.cpp
pkg/trunk/vision/cv_mech_turk/mainpage.dox
Added Paths:
-----------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
pkg/trunk/demos/pr2_gazebo/coffee_cup.launch
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/controller.h
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/controller_handle.h
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/controller_spec.h
pkg/trunk/stacks/mechanism/mechanism_control/include/mechanism_control/scheduler.h
pkg/trunk/stacks/mechanism/mechanism_control/scripts/joints_to_diagnostics.py
pkg/trunk/stacks/mechanism/mechanism_control/src/controller_handle.cpp
pkg/trunk/stacks/mechanism/mechanism_control/src/scheduler.cpp
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_joint_effort_controllers.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_joint_position_controllers.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_joint_velocity_controllers.yaml
pkg/trunk/stacks/trex/trex_pr2/nddl/nav/
pkg/trunk/stacks/trex/trex_pr2/nddl/nav/nav.nddl
Removed Paths:
-------------
pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_pose_right.launch
pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_pose_right_forearm.launch
pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_twist_right.launch
pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_wrench_right.launch
pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/controller.h
pkg/trunk/stacks/mechanism/mechanism_msgs/srv/KillAndSpawnControllers.srv
pkg/trunk/stacks/pr2/pr2_default_controllers/arm_joint_effort_controllers.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/arm_joint_position_controllers.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/arm_joint_velocity_controllers.yaml
pkg/trunk/stacks/pr2/pr2_default_controllers/base_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/base_controller_experimental.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/base_odometry.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/base_trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/gazebo_head_torso_lift_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/head_pan_joint_position_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/head_pan_tilt_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/head_position_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/head_servoing_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/head_tilt_joint_position_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_cartesian_pose_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_cartesian_tff_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_cartesian_trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_cartesian_twist_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_cartesian_twist_ik_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_cartesian_wrench_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_gripper_effort_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/l_gripper_position_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_odometry.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_cartesian_pose_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_cartesian_tff_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_cartesian_trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_cartesian_twist_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_cartesian_twist_ik_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_cartesian_wrench_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_constraint_cartesian_pose_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_constraint_cartesian_trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_constraint_cartesian_twist_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_constraint_cartesian_wrench_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_joint_position_controllers.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_joint_velocity_controllers.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_gripper_effort_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_gripper_position_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/torso_lift_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/torso_lift_vel_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/whole_body_controller.xml
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -37,7 +37,7 @@
#include <ros/node.h>
#include <boost/thread/mutex.hpp>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/joint_position_controller.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
#include <robot_mechanism_controllers/joint_effort_controller.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -36,7 +36,7 @@
*/
#include <mechanism_model/robot.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/Point.h>
#include <control_toolbox/filters.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -43,7 +43,7 @@
#include "ros/node_handle.h"
-#include "mechanism_model/controller.h"
+#include "mechanism_control/controller.h"
#include "mechanism_model/robot.h"
#include "control_toolbox/pid.h"
#include "robot_mechanism_controllers/joint_velocity_controller.h"
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller_effort.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller_effort.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller_effort.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -41,7 +41,7 @@
#ifndef CASTER_CONTROLLER_EFFORT_H
#define CASTER_CONTROLLER_EFFORT_H
-#include "mechanism_model/controller.h"
+#include "mechanism_control/controller.h"
#include "mechanism_model/robot.h"
#include "control_toolbox/pid.h"
#include "robot_mechanism_controllers/joint_velocity_controller.h"
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_position_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_position_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_position_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -41,7 +41,7 @@
#include <ros/node.h>
#include <ros/node_handle.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <tf/transform_listener.h>
#include <tf/message_notifier.h>
#include <tf/transform_datatypes.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -41,7 +41,7 @@
#include <boost/thread/mutex.hpp>
// Controllers
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <mechanism_model/joint.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -37,7 +37,7 @@
#include <ros/node.h>
#include <boost/thread/mutex.hpp>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/joint_pd_controller.h>
// Services
@@ -103,6 +103,8 @@
*/
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ bool init(mechanism::RobotState *robot, const ros::NodeHandle& n);
+
/*!
* @brief Issues commands to the joints and is called at regular intervals in realtime. This function is required
* to be realtime safe.
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -36,7 +36,7 @@
#include <ros/node.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/joint_position_smoothing_controller.h>
#include <realtime_tools/realtime_publisher.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_qualification.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_qualification.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_qualification.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -36,7 +36,7 @@
#include <ros/node.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
// Services
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -36,7 +36,7 @@
#include <ros/node.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/joint_position_smoothing_controller.h>
#include <realtime_tools/realtime_publisher.h>
@@ -70,6 +70,7 @@
~LaserScannerTrajController() ;
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+ bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
virtual void update() ;
@@ -134,6 +135,7 @@
void update() ;
bool initXml(mechanism::RobotState *robot, TiXmlElement *config) ;
+ bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
// Message Callbacks
void setPeriodicCmd() ;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -37,7 +37,7 @@
#include <ros/node.h>
#include <boost/thread/mutex.hpp>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <mechanism_model/joint.h>
#include <robot_mechanism_controllers/joint_position_controller.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_gripper_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_gripper_controller.h 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_gripper_controller.h 2009-07-30 21:06:22 UTC (rev 20185)
@@ -37,7 +37,7 @@
#include <ros/node.h>
#include <mechanism_model/robot.h>
-#include <mechanism_model/controller.h>
+#include <mechanism_control/controller.h>
#include <robot_mechanism_controllers/joint_effort_controller.h>
#include <pr2_mechanism_controllers/GripperControllerCmd.h>
#include <pr2_msgs/GripperControllerState.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp 2009-07-30 21:06:22 UTC (rev 20185)
@@ -91,11 +91,6 @@
robot_state_ = robot_state;
// get a pointer to the wrench controller
- MechanismControl* mc;
- if (!MechanismControl::Instance(mc)){
- ROS_ERROR("HeadPositionController: could not get instance of mechanism control");
- return false;
- }
std::string pan_output;
if (!node_.getParam("pan_output", pan_output))
{
@@ -103,7 +98,7 @@
node_.getNamespace().c_str());
return false;
}
- if (!mc->getControllerByName<JointPositionController>(pan_output, head_pan_controller_))
+ if (!getController<JointPositionController>(pan_output, AFTER_ME, head_pan_controller_))
{
ROS_ERROR("HeadPositionController: could not connect to the pan joint controller %s (namespace: %s)",
pan_output.c_str(), node_.getNamespace().c_str());
@@ -117,7 +112,7 @@
node_.getNamespace().c_str());
return false;
}
- if (!mc->getControllerByName<JointPositionController>(tilt_output, head_tilt_controller_))
+ if (!getController<JointPositionController>(tilt_output, AFTER_ME, head_tilt_controller_))
{
ROS_ERROR("HeadPositionController: could not connect to the tilt joint controller %s (namespace: %s)",
tilt_output.c_str(), node_.getNamespace().c_str());
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp 2009-07-30 21:06:22 UTC (rev 20185)
@@ -58,22 +58,46 @@
next_free_index_ = 0;
current_trajectory_index_ = 0;
trajectory_preempted_ = false;
+ joint_trajectory_ = NULL;
}
JointTrajectoryController::~JointTrajectoryController()
{
for(unsigned int i=0; i<joint_pv_controllers_.size();++i)
- delete joint_pv_controllers_[i];
+ if (joint_pv_controllers_[i])
+ delete joint_pv_controllers_[i];
stopPublishers();
unadvertiseServices();
unsubscribeTopics();
- delete joint_trajectory_;
+ if (joint_trajectory_)
+ delete joint_trajectory_;
}
+bool JointTrajectoryController::init(mechanism::RobotState *robot, const ros::NodeHandle& n)
+{
+ // get xml from parameter server
+ string xml_string;
+ if (!n.getParam("xml", xml_string)){
+ ROS_ERROR("Could not read xml from parameter server");
+ return false;
+ }
+
+ TiXmlDocument xml_doc;
+ xml_doc.Parse(xml_string.c_str());
+ TiXmlElement *xml = xml_doc.FirstChildElement("controller");
+ if (!xml){
+ ROS_ERROR("could not parse xml %s",xml_string.c_str());
+ return false;
+ }
+
+ return initXml(robot, xml);
+}
+
+
bool JointTrajectoryController::initXml(mechanism::RobotState * robot, TiXmlElement * config)
{
name_ = config->Attribute("name");
@@ -143,11 +167,10 @@
void JointTrajectoryController::stopPublishers()
{
- controller_state_publisher_->stop();
- diagnostics_publisher_->stop();
-
- delete controller_state_publisher_;
- delete diagnostics_publisher_;
+ if (controller_state_publisher_)
+ delete controller_state_publisher_;
+ if (diagnostics_publisher_)
+ delete diagnostics_publisher_;
}
void JointTrajectoryController::advertiseServices()
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-07-30 21:00:18 UTC (rev 20184)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-07-30 21:06:22 UTC (rev 20185)
@@ -144,6 +144,26 @@
return true ;
}
+bool LaserScannerTrajController::init(mechanism::RobotState *robot, const ros::NodeHandle &n)
+{
+ std::string xml;
+ if (!n.getParam("xml", xml))
+ {
+ ROS_ERROR("No XML given (namespace: %s)", n.getNamespace().c_str());
+ return false;
+ }
+
+ TiXmlDocument doc;
+ doc.Parse(xml.c_str());
+ if (!doc.RootElement())
+ {
+ ROS_ERROR("Error parsing XML (namespace: %s)", n.getNamespace().c_str());
+ return false;
+ }
+
+ return initXml(robot, doc.RootElement());
+}
+
void LaserScannerTrajController::update()
{
if (!joint_state_->calibrated_)
@@ -494,6 +514,28 @@
return true ;
}
+bool LaserScannerTrajControllerNode::init(mechanism::RobotState *robot,
+ const ros::NodeHandle &n)
+{
+ std::string xml;
+ if (!n.getParam("xml", xml))
+ {
+ ROS_ERROR("No XML given (namespace: %s)", n.getNamespace().c_str());
+ return false;
+ }
+
+ TiXmlDocument doc;
+ doc.Parse(xml.c_str());
+ if (!doc.RootElement())
+ {
+ ROS_ERROR("Error parsing XML (namespace: %s)", n.getNamespace().c_str());
+ return false;
+ }
+
+ return initXml(robot, doc.RootElement());
+}
+
+
bool LaserScannerTrajControllerNode::setPeriodicSrv(pr2_srvs::SetPeriodicCmd::Request &req,
pr2_srvs::SetPeriodicCmd::Response &res)
{
Modified: pkg/trunk/controllers/p...
[truncated message content] |
|
From: <is...@us...> - 2009-07-30 23:31:20
|
Revision: 20206
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20206&view=rev
Author: isucan
Date: 2009-07-30 23:31:13 +0000 (Thu, 30 Jul 2009)
Log Message:
-----------
working on the ability to edit collision worlds and re-construct collision maps
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/src/models/collision_models.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
pkg/trunk/world_models/collision_space/CMakeLists.txt
pkg/trunk/world_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/src/environment.cpp
pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
pkg/trunk/world_models/test_collision_space/CMakeLists.txt
pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp
Added Paths:
-----------
pkg/trunk/world_models/collision_space/include/collision_space/environment_objects.h
pkg/trunk/world_models/collision_space/src/environment_objects.cpp
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-07-30 23:31:13 UTC (rev 20206)
@@ -146,6 +146,8 @@
void setupCSM(void);
void updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear);
+ void collisionMapAsSpheres(const mapping_msgs::CollisionMapConstPtr &collisionMap,
+ std::vector<shapes::Shape*> &spheres, std::vector<btTransform> &poses);
void collisionMapCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
void collisionMapUpdateCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
void objectInMapCallback(const mapping_msgs::ObjectInMapConstPtr &objectInMap);
Modified: pkg/trunk/motion_planning/planning_environment/src/models/collision_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/models/collision_models.cpp 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/motion_planning/planning_environment/src/models/collision_models.cpp 2009-07-30 23:31:13 UTC (rev 20206)
@@ -59,7 +59,8 @@
for (unsigned int i = 0 ; i < boundingPlanes_.size() / 4 ; ++i)
{
- model->addPlane("bounds", boundingPlanes_[i * 4], boundingPlanes_[i * 4 + 1], boundingPlanes_[i * 4 + 2], boundingPlanes_[i * 4 + 3]);
+ shapes::Plane *plane = new shapes::Plane(boundingPlanes_[i * 4], boundingPlanes_[i * 4 + 1], boundingPlanes_[i * 4 + 2], boundingPlanes_[i * 4 + 3]);
+ model->addObject("bounds", plane);
ROS_INFO("Added static plane %fx + %fy + %fz + %f = 0 for model %p", boundingPlanes_[i * 4], boundingPlanes_[i * 4 + 1], boundingPlanes_[i * 4 + 2], boundingPlanes_[i * 4 + 3], model.get());
}
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-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-07-30 23:31:13 UTC (rev 20206)
@@ -143,23 +143,16 @@
updateCollisionSpace(collisionMap, true);
}
-void planning_environment::CollisionSpaceMonitor::updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear)
+void planning_environment::CollisionSpaceMonitor::collisionMapAsSpheres(const mapping_msgs::CollisionMapConstPtr &collisionMap,
+ std::vector<shapes::Shape*> &spheres, std::vector<btTransform> &poses)
{
- boost::mutex::scoped_lock lock(mapUpdateLock_);
-
- int n = collisionMap->get_boxes_size();
-
- 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);
-
// we want to make sure the frame the robot model is kept in is the same as the frame of the collisionMap
bool transform = !frame_id_.empty() && collisionMap->header.frame_id != frame_id_;
-
- ros::WallTime startTime = ros::WallTime::now();
- double *data = n > 0 ? new double[4 * n] : NULL;
-
+ const int n = collisionMap->get_boxes_size();
+
+ spheres.resize(n);
+ poses.resize(n);
+
if (transform)
{
std::string target = frame_id_;
@@ -168,7 +161,6 @@
#pragma omp parallel for
for (int i = 0 ; i < n ; ++i)
{
- int i4 = i * 4;
robot_msgs::PointStamped psi;
psi.header = collisionMap->header;
psi.point.x = collisionMap->boxes[i].center.x;
@@ -186,11 +178,9 @@
pso = psi;
}
- data[i4 ] = pso.point.x;
- data[i4 + 1] = pso.point.y;
- data[i4 + 2] = pso.point.z;
-
- data[i4 + 3] = maxCoord(collisionMap->boxes[i].extents) * 0.867 + pointcloud_padd_;
+ poses[i].setIdentity();
+ poses[i].setOrigin(btVector3(pso.point.x, pso.point.y, pso.point.z));
+ spheres[i] = new shapes::Sphere(maxCoord(collisionMap->boxes[i].extents) * 0.867 + pointcloud_padd_);
}
if (err)
@@ -202,27 +192,34 @@
#pragma omp parallel for
for (int i = 0 ; i < n ; ++i)
{
- int i4 = i * 4;
- data[i4 ] = collisionMap->boxes[i].center.x;
- data[i4 + 1] = collisionMap->boxes[i].center.y;
- data[i4 + 2] = collisionMap->boxes[i].center.z;
-
- data[i4 + 3] = maxCoord(collisionMap->boxes[i].extents) * 0.867 + pointcloud_padd_;
+ poses[i].setIdentity();
+ poses[i].setOrigin(btVector3(collisionMap->boxes[i].center.x, collisionMap->boxes[i].center.y, collisionMap->boxes[i].center.z));
+ spheres[i] = new shapes::Sphere(maxCoord(collisionMap->boxes[i].extents) * 0.867 + pointcloud_padd_);
}
}
+}
+
+void planning_environment::CollisionSpaceMonitor::updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear)
+{
+ boost::mutex::scoped_lock lock(mapUpdateLock_);
- collisionSpace_->lock();
+ ROS_DEBUG("Received collision map with %d points that is %f seconds old", collisionMap->get_boxes_size(), (ros::Time::now() - collisionMap->header.stamp).toSec());
+
+ if (onBeforeMapUpdate_ != NULL)
+ onBeforeMapUpdate_(collisionMap, clear);
- if (clear)
- collisionSpace_->clearObstacles("points");
- if (n > 0)
- collisionSpace_->addPointCloudSpheres("points", n, data);
+ ros::WallTime startTime = ros::WallTime::now();
+
+ std::vector<shapes::Shape*> spheres;
+ std::vector<btTransform> poses;
+ collisionMapAsSpheres(collisionMap, spheres, poses);
+ collisionSpace_->lock();
+ if (clear)
+ collisionSpace_->clearObjects("points");
+ collisionSpace_->addObjects("points", spheres, poses);
collisionSpace_->unlock();
- if (data)
- delete[] data;
-
double tupd = (ros::WallTime::now() - startTime).toSec();
ROS_DEBUG("Updated map model in %f seconds", tupd);
@@ -265,19 +262,18 @@
btTransform pose;
tf::poseMsgToTF(pso.pose, pose);
collisionSpace_->lock();
- collisionSpace_->clearObstacles(objectInMap->id);
+ collisionSpace_->clearObjects(objectInMap->id);
collisionSpace_->addObject(objectInMap->id, shape, pose);
collisionSpace_->unlock();
ROS_INFO("Added object '%s' to collision space", objectInMap->id.c_str());
}
- delete shape;
}
}
else
{
// remove the object from the map
collisionSpace_->lock();
- collisionSpace_->clearObstacles(objectInMap->id);
+ collisionSpace_->clearObjects(objectInMap->id);
collisionSpace_->unlock();
ROS_INFO("Removed object '%s' from collision space", objectInMap->id.c_str());
}
Modified: pkg/trunk/world_models/collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/collision_space/CMakeLists.txt 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/collision_space/CMakeLists.txt 2009-07-30 23:31:13 UTC (rev 20206)
@@ -6,7 +6,8 @@
rospack_add_boost_directories()
-rospack_add_library(collision_space src/environment.cpp
+rospack_add_library(collision_space src/environment_objects.cpp
+ src/environment.cpp
src/environmentBullet.cpp
src/environmentODE.cpp)
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-07-30 23:31:13 UTC (rev 20206)
@@ -37,6 +37,7 @@
#ifndef COLLISION_SPACE_ENVIRONMENT_MODEL_
#define COLLISION_SPACE_ENVIRONMENT_MODEL_
+#include "collision_space/environment_objects.h"
#include <planning_models/kinematic.h>
#include <planning_models/output.h>
#include <LinearMath/btVector3.h>
@@ -49,6 +50,7 @@
/** \brief Main namespace */
namespace collision_space
{
+
/** \brief A class describing an environment for a kinematic
robot. This is the base (abstract) definition. Different
implementations are possible. The class is aware of a set of
@@ -78,10 +80,13 @@
{
m_selfCollision = true;
m_verbose = false;
+ m_objects = new EnvironmentObjects();
}
virtual ~EnvironmentModel(void)
{
+ if (m_objects)
+ delete m_objects;
}
/**********************************************************************/
@@ -95,7 +100,7 @@
bool getSelfCollision(void) const;
/** \brief Add a group of links to be checked for self collision */
- virtual void addSelfCollisionGroup(std::vector<std::string> &links);
+ virtual void addSelfCollisionGroup(const std::vector<std::string> &links);
/** \brief Enable/Disable collision checking for specific links. Return the previous value of the state (1 or 0) if succesful; -1 otherwise */
virtual int setCollisionCheck(const std::string &link, bool state) = 0;
@@ -106,7 +111,7 @@
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual void setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
+ virtual void setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** \brief Get robot scale */
double getRobotScale(void) const
@@ -127,7 +132,7 @@
virtual void updateAttachedBodies(void) = 0;
/** \brief Get the robot model */
- boost::shared_ptr<planning_models::KinematicModel> getRobotModel(void) const
+ const boost::shared_ptr<const planning_models::KinematicModel>& getRobotModel(void) const
{
return m_robotModel;
}
@@ -152,20 +157,23 @@
/* Collision Bodies */
/**********************************************************************/
- /** \brief Remove all obstacles from collision model */
- virtual void clearObstacles(void) = 0;
+ /** \brief Remove all objects from collision model */
+ virtual void clearObjects(void) = 0;
- /** \brief Remove obstacles from a specific namespace in the collision model */
- virtual void clearObstacles(const std::string &ns) = 0;
+ /** \brief Remove objects from a specific namespace in the collision model */
+ virtual void clearObjects(const std::string &ns) = 0;
+
+ /** \brief Add a static collision object to the map. The user releases ownership of the passed object. */
+ virtual void addObject(const std::string &ns, const shapes::StaticShape *shape) = 0;
- /** \brief Add a point cloud to the collision space */
- virtual void addPointCloudSpheres(const std::string &ns, unsigned int n, const double* points) = 0;
+ /** \brief Add a collision object to the map. The user releases ownership of the passed object.*/
+ virtual void addObject(const std::string &ns, const shapes::Shape* shape, const btTransform &pose) = 0;
- /** \brief Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
- virtual void addPlane(const std::string &ns, double a, double b, double c, double d) = 0;
+ /** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. */
+ virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses) = 0;
- /** \brief Add a collision object to the map */
- virtual void addObject(const std::string &ns, const shapes::Shape *shape, const btTransform &pose) = 0;
+ /** \brief Get the objects currently contained in the model */
+ const EnvironmentObjects* getObjects(void) const;
/**********************************************************************/
/* Miscellaneous Routines */
@@ -188,22 +196,41 @@
protected:
- boost::mutex m_lock;
- std::vector<std::string> m_collisionLinks;
- std::map<std::string, unsigned int> m_collisionLinkIndex;
- std::vector< std::vector<bool> > m_selfCollisionTest;
+ /** \brief Mutex used to lock the datastructure */
+ boost::mutex m_lock;
+
+ /** \brief List of links (names) from the robot model that are considered for collision checking */
+ std::vector<std::string> m_collisionLinks;
+
+ /** \brief Map used internally to find the index of a link that we do collision checking for */
+ std::map<std::string, unsigned int> m_collisionLinkIndex;
+
+ /** \brief Matrix of booleans indicating whether pairs of links can self collide */
+ std::vector< std::vector<bool> > m_selfCollisionTest;
- bool m_selfCollision;
- bool m_verbose;
- planning_models::msg::Interface m_msg;
+ /** \brief Flag to indicate whether self collision checking is enabled */
+ bool m_selfCollision;
- /** \brief List of loaded robot models */
- boost::shared_ptr<planning_models::KinematicModel> m_robotModel;
- double m_robotScale;
- double m_robotPadd;
+ /** \brief Flag to indicate whether verbose mode is on */
+ bool m_verbose;
+
+ /** \brief Interface to printing information */
+ planning_models::msg::Interface m_msg;
+ /** \brief Loaded robot model */
+ boost::shared_ptr<const planning_models::KinematicModel> m_robotModel;
+
+ /** \brief List of objects contained in the environment */
+ EnvironmentObjects *m_objects;
+
+ /** \brief Scaling used for robot links */
+ double m_robotScale;
+
+ /** \brief Padding used for robot links */
+ double m_robotPadd;
+
};
}
#endif
-
+
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-07-30 23:31:13 UTC (rev 20206)
@@ -73,29 +73,29 @@
/** \brief Check if a model is in self collision */
virtual bool isSelfCollision(void);
+
+ /** \brief Remove all objects from collision model */
+ virtual void clearObjects(void);
+
+ /** \brief Remove objects from a specific namespace in the collision model */
+ virtual void clearObjects(const std::string &ns);
+
+ /** \brief Add a static collision object to the map. The user releases ownership of the passed object. */
+ virtual void addObject(const std::string &ns, const shapes::StaticShape *shape);
- /** \brief Remove all obstacles from collision model */
- virtual void clearObstacles(void);
+ /** \brief Add a collision object to the map. The user releases ownership of the passed object.*/
+ virtual void addObject(const std::string &ns, const shapes::Shape* shape, const btTransform &pose);
- /** \brief Remove obstacles from a specific namespace in the collision model */
- virtual void clearObstacles(const std::string &ns);
+ /** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. */
+ virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses);
- /** \brief Add a point cloud to the collision space */
- virtual void addPointCloudSpheres(const std::string &ns, unsigned int n, const double *points);
-
- /** \brief Add a collision object to the map */
- virtual void addObject(const std::string &ns, const shapes::Shape *shape, const btTransform &pose);
-
- /** \brief Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
- virtual void addPlane(const std::string &ns, double a, double b, double c, double d);
-
/** \brief Add a robot model. Ignore robot links if their name is not
specified in the string vector. The scale argument can be
used to increase or decrease the size of the robot's
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual void setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
+ virtual void setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** \brief Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(void);
@@ -232,6 +232,8 @@
};
btCollisionObject* createCollisionBody(const shapes::Shape *shape, double scale, double padding);
+ btCollisionObject* createCollisionBody(const shapes::StaticShape *shape);
+
void freeMemory(void);
SelfCollisionFilterCallback m_selfCollisionFilterCallback;
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-07-30 23:31:13 UTC (rev 20206)
@@ -63,29 +63,29 @@
/** \brief Check if a model is in self collision */
virtual bool isSelfCollision(void);
+
+ /** \brief Remove all objects from collision model */
+ virtual void clearObjects(void);
+
+ /** \brief Remove objects from a specific namespace in the collision model */
+ virtual void clearObjects(const std::string &ns);
+
+ /** \brief Add a static collision object to the map. The user releases ownership of the passed object. */
+ virtual void addObject(const std::string &ns, const shapes::StaticShape *shape);
- /** \brief Remove all obstacles from collision model */
- virtual void clearObstacles(void);
+ /** \brief Add a collision object to the map. The user releases ownership of the passed object.*/
+ virtual void addObject(const std::string &ns, const shapes::Shape* shape, const btTransform &pose);
- /** \brief Remove obstacles from a specific namespace in the collision model */
- virtual void clearObstacles(const std::string &ns);
+ /** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. */
+ virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses);
- /** \brief Add a point cloud to the collision space */
- virtual void addPointCloudSpheres(const std::string &ns, unsigned int n, const double *points);
-
- /** \brief Add a collision object to the map */
- virtual void addObject(const std::string &ns, const shapes::Shape *shape, const btTransform &pose);
-
- /** \brief Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
- virtual void addPlane(const std::string &ns, double a, double b, double c, double d);
-
/** \brief Add a robot model. Ignore robot links if their name is not
specified in the string vector. The scale argument can be
used to increase or decrease the size of the robot's
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual void setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
+ virtual void setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** \brief Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(void);
@@ -272,13 +272,13 @@
storage.clear();
}
- std::string name;
+ std::string name;
dSpaceID space;
std::vector<dGeomID> geoms;
ODECollide2 collide2;
ODEStorage storage;
};
-
+
struct CollisionData
{
CollisionData(void)
@@ -315,6 +315,7 @@
dGeomID copyGeom(dSpaceID space, ODEStorage &storage, dGeomID geom, ODEStorage &sourceStorage) const;
void createODERobotModel(void);
dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::Shape *shape, double scale, double padding);
+ dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::StaticShape *shape);
void updateGeom(dGeomID geom, const btTransform &pose) const;
/** \brief Check if thread-specific routines have been called */
Added: pkg/trunk/world_models/collision_space/include/collision_space/environment_objects.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment_objects.h (rev 0)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment_objects.h 2009-07-30 23:31:13 UTC (rev 20206)
@@ -0,0 +1,105 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#ifndef COLLISION_SPACE_ENVIRONMENT_OBJECTS_
+#define COLLISION_SPACE_ENVIRONMENT_OBJECTS_
+
+#include <vector>
+#include <string>
+#include <map>
+
+#include <geometric_shapes/shapes.h>
+#include <LinearMath/btTransform.h>
+
+namespace collision_space
+{
+
+ /** \brief List of objects contained in the environment (not including robot links) */
+ class EnvironmentObjects
+ {
+ public:
+ EnvironmentObjects(void)
+ {
+ }
+
+ ~EnvironmentObjects(void)
+ {
+ clearObjects();
+ }
+
+ struct NamespaceObjects
+ {
+ /** \brief An array of static shapes */
+ std::vector< const shapes::StaticShape* > staticShape;
+
+ /** \brief An array of shapes */
+ std::vector< const shapes::Shape* > shape;
+
+ /** \brief An array of shape poses */
+ std::vector< btTransform > shapePose;
+ };
+
+ /** \brief Get the list of namespaces */
+ std::vector<std::string> getNamespaces(void) const;
+
+ /** \brief Get the list of objects */
+ const NamespaceObjects& getObjects(const std::string &ns) const;
+
+ /** \brief Add a static object to the namespace. The user releases ownership of the object. */
+ void addObject(const std::string &ns, const shapes::StaticShape *shape);
+
+ /** \brief Add an object to the namespace. The user releases ownership of the object. */
+ void addObject(const std::string &ns, const shapes::Shape *shape, const btTransform &pose);
+
+ /** \brief Clear the objects in a specific namespace. Memory is freed. */
+ void clearObjects(const std::string &ns);
+
+ /** \brief Clear all objects. Memory is freed. */
+ void clearObjects(void);
+
+ /** \brief Clone this instance of the class */
+ EnvironmentObjects* clone(void) const;
+
+ private:
+
+ std::map<std::string, NamespaceObjects> m_objects;
+ NamespaceObjects m_empty;
+ };
+
+}
+
+#endif
+
Modified: pkg/trunk/world_models/collision_space/src/environment.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environment.cpp 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/collision_space/src/environment.cpp 2009-07-30 23:31:13 UTC (rev 20206)
@@ -46,8 +46,13 @@
m_verbose = verbose;
}
-void collision_space::EnvironmentModel::setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+const collision_space::EnvironmentObjects* collision_space::EnvironmentModel::getObjects(void) const
{
+ return m_objects;
+}
+
+void collision_space::EnvironmentModel::setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+{
m_robotModel = model;
m_collisionLinks = links;
m_selfCollisionTest.resize(links.size());
@@ -60,7 +65,7 @@
m_robotPadd = padding;
}
-void collision_space::EnvironmentModel::addSelfCollisionGroup(std::vector<std::string> &links)
+void collision_space::EnvironmentModel::addSelfCollisionGroup(const std::vector<std::string> &links)
{
for (unsigned int i = 0 ; i < links.size() ; ++i)
{
Modified: pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-07-30 23:27:29 UTC (rev 20205)
+++ pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-07-30 23:31:13 UTC (rev 20206)
@@ -59,7 +59,7 @@
delete m_config; */
}
-void collision_space::EnvironmentModelBullet::setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+void collision_space::EnvironmentModelBullet::setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
collision_space::EnvironmentModel::setRobotModel(model, links, scale, padding);
m_modelGeom.scale = scale;
@@ -145,6 +145,33 @@
return NULL;
}
+btCollisionObject* collision_space::EnvironmentModelBullet::createCollisionBody(const shapes::StaticShape *shape)
+{
+ btCollisionShape *btshape = NULL;
+
+ switch (shape->type)
+ {
+ case shapes::PLANE:
+ {
+ const shapes::Plane *p = static_cast<const shapes::Plane*>(shape);
+ btshape = new btStaticPlaneShape(btVector3(p->a, p->b, p->c), p->d);
+ }
+ break;
+
+ default:
+ break;
+ }
+
+ if (btshape)
+ {
+ btCollisionObject *object = new btCollisionObject();
+ object->setCollisionShape(btshape);
+ return object;
+ }
+ else
+ return NULL;
+}
+
void collision_space::EnvironmentModelBullet::updateAttachedBodies(void)
{
const unsigned int n = m_modelGeom.linkGeom.size();
@@ -265,7 +292,7 @@
return false;
}
-
+/*
void co...
[truncated message content] |
|
From: <is...@us...> - 2009-07-31 00:04:00
|
Revision: 20212
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20212&view=rev
Author: isucan
Date: 2009-07-31 00:03:50 +0000 (Fri, 31 Jul 2009)
Log Message:
-----------
proper fix for the const'ness issue
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/src/helpers/ModelBase.cpp
pkg/trunk/sandbox/3dnav_pr2/manifest.xml
pkg/trunk/world_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/src/environment.cpp
pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ModelBase.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ModelBase.cpp 2009-07-31 00:00:17 UTC (rev 20211)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ModelBase.cpp 2009-07-31 00:03:50 UTC (rev 20212)
@@ -57,7 +57,7 @@
{
result = new EnvironmentDescription();
result->collisionSpace = planningMonitor->getEnvironmentModel();
- result->kmodel = const_cast<planning_models::KinematicModel*>(result->collisionSpace->getRobotModel().get());
+ result->kmodel = result->collisionSpace->getRobotModel().get();
result->constraintEvaluator = &constraintEvaluator;
}
else
@@ -65,7 +65,7 @@
ROS_DEBUG("Cloning collision environment (%d total)", (int)ENVS.size() + 1);
result = new EnvironmentDescription();
result->collisionSpace = planningMonitor->getEnvironmentModel()->clone();
- result->kmodel = const_cast<planning_models::KinematicModel*>(result->collisionSpace->getRobotModel().get());
+ result->kmodel = result->collisionSpace->getRobotModel().get();
planning_environment::KinematicConstraintEvaluatorSet *kce = new planning_environment::KinematicConstraintEvaluatorSet();
kce->add(result->kmodel, constraintEvaluator.getPoseConstraints());
kce->add(result->kmodel, constraintEvaluator.getJointConstraints());
Modified: pkg/trunk/sandbox/3dnav_pr2/manifest.xml
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/manifest.xml 2009-07-31 00:00:17 UTC (rev 20211)
+++ pkg/trunk/sandbox/3dnav_pr2/manifest.xml 2009-07-31 00:03:50 UTC (rev 20212)
@@ -20,6 +20,11 @@
<depend package="manipulation_msgs" />
<depend package="visualization_msgs" />
+ <!-- controllers -->
+ <depent package="pr2_default_controllers" />
+ <depent package="pr2_mechanism_controllers" />
+ <depent package="pr2_experimental_controllers" />
+
<!-- mapping -->
<depend package="laser_scan"/>
<depend package="point_cloud_assembler"/>
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-07-31 00:00:17 UTC (rev 20211)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-07-31 00:03:50 UTC (rev 20212)
@@ -111,19 +111,13 @@
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual void setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
+ virtual void setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** \brief Get robot scale */
- double getRobotScale(void) const
- {
- return m_robotScale;
- }
-
+ double getRobotScale(void) const;
+
/** \brief Get robot padding */
- double getRobotPadding(void) const
- {
- return m_robotPadd;
- }
+ double getRobotPadding(void) const;
/** \brief Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(void) = 0;
@@ -132,12 +126,8 @@
virtual void updateAttachedBodies(void) = 0;
/** \brief Get the robot model */
- const boost::shared_ptr<const planning_models::KinematicModel>& getRobotModel(void) const
- {
- return m_robotModel;
- }
+ const boost::shared_ptr<planning_models::KinematicModel>& getRobotModel(void) const;
-
/**********************************************************************/
/* Collision Checking Routines */
/**********************************************************************/
@@ -218,7 +208,7 @@
planning_models::msg::Interface m_msg;
/** \brief Loaded robot model */
- boost::shared_ptr<const planning_models::KinematicModel> m_robotModel;
+ boost::shared_ptr<planning_models::KinematicModel> m_robotModel;
/** \brief List of objects contained in the environment */
EnvironmentObjects *m_objects;
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-07-31 00:00:17 UTC (rev 20211)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-07-31 00:03:50 UTC (rev 20212)
@@ -95,7 +95,7 @@
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual void setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
+ virtual void setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** \brief Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(void);
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-07-31 00:00:17 UTC (rev 20211)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-07-31 00:03:50 UTC (rev 20212)
@@ -85,7 +85,7 @@
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual void setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
+ virtual void setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** \brief Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(void);
Modified: pkg/trunk/world_models/collision_space/src/environment.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environment.cpp 2009-07-31 00:00:17 UTC (rev 20211)
+++ pkg/trunk/world_models/collision_space/src/environment.cpp 2009-07-31 00:03:50 UTC (rev 20212)
@@ -51,8 +51,23 @@
return m_objects;
}
-void collision_space::EnvironmentModel::setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+const boost::shared_ptr<planning_models::KinematicModel>& collision_space::EnvironmentModel::getRobotModel(void) const
{
+ return m_robotModel;
+}
+
+double collision_space::EnvironmentModel::getRobotScale(void) const
+{
+ return m_robotScale;
+}
+
+double collision_space::EnvironmentModel::getRobotPadding(void) const
+{
+ return m_robotPadd;
+}
+
+void collision_space::EnvironmentModel::setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+{
m_robotModel = model;
m_collisionLinks = links;
m_selfCollisionTest.resize(links.size());
Modified: pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-07-31 00:00:17 UTC (rev 20211)
+++ pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-07-31 00:03:50 UTC (rev 20212)
@@ -59,7 +59,7 @@
delete m_config; */
}
-void collision_space::EnvironmentModelBullet::setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+void collision_space::EnvironmentModelBullet::setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
collision_space::EnvironmentModel::setRobotModel(model, links, scale, padding);
m_modelGeom.scale = scale;
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-31 00:00:17 UTC (rev 20211)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-31 00:03:50 UTC (rev 20212)
@@ -100,7 +100,7 @@
delete it->second;
}
-void collision_space::EnvironmentModelODE::setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+void collision_space::EnvironmentModelODE::setRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
collision_space::EnvironmentModel::setRobotModel(model, links, scale, padding);
createODERobotModel();
@@ -781,7 +781,7 @@
env->m_verbose = m_verbose;
env->m_robotScale = m_robotScale;
env->m_robotPadd = m_robotPadd;
- env->m_robotModel = boost::shared_ptr<const planning_models::KinematicModel>(m_robotModel->clone());
+ env->m_robotModel = boost::shared_ptr<planning_models::KinematicModel>(m_robotModel->clone());
env->createODERobotModel();
for (unsigned int j = 0 ; j < m_modelGeom.linkGeom.size() ; ++j)
env->m_modelGeom.linkGeom[j]->enabled = m_modelGeom.linkGeom[j]->enabled;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-31 00:15:10
|
Revision: 20215
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20215&view=rev
Author: isucan
Date: 2009-07-31 00:12:30 +0000 (Fri, 31 Jul 2009)
Log Message:
-----------
moving launch file
Added Paths:
-----------
pkg/trunk/sandbox/3dnav_pr2/launch/planning/
pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch
Removed Paths:
-------------
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/ompl_planning.launch
Deleted: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/ompl_planning.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/ompl_planning.launch 2009-07-31 00:11:14 UTC (rev 20214)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/ompl_planning.launch 2009-07-31 00:12:30 UTC (rev 20215)
@@ -1,3 +0,0 @@
-<launch>
- <include file="$(find ompl_planning)/motion_planning.launch"/>
-</launch>
Copied: pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch (from rev 20207, pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/ompl_planning.launch)
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch (rev 0)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch 2009-07-31 00:12:30 UTC (rev 20215)
@@ -0,0 +1,3 @@
+<launch>
+ <include file="$(find ompl_planning)/motion_planning.launch"/>
+</launch>
Property changes on: pkg/trunk/sandbox/3dnav_pr2/launch/planning/ompl_planning.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/demos/tabletop_manipulation/launch/ompl_planning.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-07-31 01:02:30
|
Revision: 20228
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20228&view=rev
Author: vijaypradeep
Date: 2009-07-31 01:02:21 +0000 (Fri, 31 Jul 2009)
Log Message:
-----------
Working on simple action client. Moving move_base client code to a new package
Modified Paths:
--------------
pkg/trunk/sandbox/actionlib/CMakeLists.txt
pkg/trunk/sandbox/actionlib/include/actionlib/client/action_client.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_handle.cpp
pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.cpp
pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_action_client.h
pkg/trunk/stacks/navigation/move_base/manifest.xml
Added Paths:
-----------
pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_goal_state.h
pkg/trunk/sandbox/move_base_client/
pkg/trunk/sandbox/move_base_client/CMakeLists.txt
pkg/trunk/sandbox/move_base_client/Makefile
pkg/trunk/sandbox/move_base_client/mainpage.dox
pkg/trunk/sandbox/move_base_client/manifest.xml
pkg/trunk/sandbox/move_base_client/src/
pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp
pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp
Removed Paths:
-------------
pkg/trunk/sandbox/actionlib/src/move_base_client.cpp
Modified: pkg/trunk/sandbox/actionlib/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/actionlib/CMakeLists.txt 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/sandbox/actionlib/CMakeLists.txt 2009-07-31 01:02:21 UTC (rev 20228)
@@ -22,7 +22,8 @@
#rospack_add_executable(goal_manager_test src/goal_manager_test.cpp)
rospack_add_executable(move_base_server src/move_base_server.cpp)
-rospack_add_executable(move_base_client src/move_base_client.cpp)
+#rospack_add_executable(move_base_client src/move_base_client.cpp)
+#rospack_add_executable(simple_move_base_client src/simple_move_base_client.cpp)
Modified: pkg/trunk/sandbox/actionlib/include/actionlib/client/action_client.h
===================================================================
--- pkg/trunk/sandbox/actionlib/include/actionlib/client/action_client.h 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/sandbox/actionlib/include/actionlib/client/action_client.h 2009-07-31 01:02:21 UTC (rev 20228)
@@ -111,12 +111,11 @@
*/
void cancelAllGoals()
{
- ActionGoal cancel_msg;
+ GoalID cancel_msg;
// CancelAll policy encoded by stamp=0, id=0
- cancel_msg.goal_id.stamp = ros::Time(0,0);
- cancel_msg.goal_id.id = ros::Time(0,0);
- cancel_msg.request_type.type = RequestType::PREEMPT_REQUEST;
- goal_pub_.publish(cancel_msg);
+ cancel_msg.stamp = ros::Time(0,0);
+ cancel_msg.id = ros::Time(0,0);
+ cancel_pub_.publish(cancel_msg);
}
/**
@@ -137,6 +136,7 @@
ros::Subscriber feedback_sub_;
ros::Publisher goal_pub_;
+ ros::Publisher cancel_pub_;
ros::Subscriber status_sub_;
ros::Subscriber result_sub_;
@@ -147,11 +147,18 @@
goal_pub_.publish(action_goal);
}
+ void sendCancelFunc(const GoalID& cancel_msg)
+ {
+ cancel_pub_.publish(cancel_msg);
+ }
+
void initClient()
{
// Start publishers and subscribers
goal_pub_ = n_.advertise<ActionGoal>("goal", 1);
+ cancel_pub_ = n_.advertise<GoalID>("cancel", 1);
manager_.registerSendGoalFunc(boost::bind(&ActionClientT::sendGoalFunc, this, _1));
+ manager_.registerCancelFunc(boost::bind(&ActionClientT::sendCancelFunc, this, _1));
status_sub_ = n_.subscribe("status", 1, &ActionClientT::statusCb, this);
feedback_sub_ = n_.subscribe("feedback", 1, &ActionClientT::feedbackCb, this);
Modified: pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_handle.cpp
===================================================================
--- pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_handle.cpp 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_handle.cpp 2009-07-31 01:02:21 UTC (rev 20228)
@@ -154,13 +154,12 @@
ActionGoalConstPtr action_goal = list_handle_.getElem()->getActionGoal();
- boost::shared_ptr<ActionGoal> cancel_msg(new ActionGoal);
- cancel_msg->goal_id.stamp = ros::Time();
- cancel_msg->goal_id.id = action_goal->goal_id.id;
- cancel_msg->request_type.type = RequestType::PREEMPT_REQUEST;
+ GoalID cancel_msg;
+ cancel_msg.stamp = ros::Time(0,0);
+ cancel_msg.id = list_handle_.getElem()->getActionGoal()->goal_id.id;
- if (gm_->send_goal_func_)
- gm_->send_goal_func_(cancel_msg);
+ if (gm_->cancel_func_)
+ gm_->cancel_func_(cancel_msg);
list_handle_.getElem()->transitionToState(*this, CommState::WAITING_FOR_CANCEL_ACK);
}
Modified: pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.cpp
===================================================================
--- pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.cpp 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.cpp 2009-07-31 01:02:21 UTC (rev 20228)
@@ -46,6 +46,13 @@
}
template<class ActionSpec>
+void GoalManager<ActionSpec>::registerCancelFunc(CancelFunc cancel_func)
+{
+ cancel_func_ = cancel_func;
+}
+
+
+template<class ActionSpec>
GoalHandle<ActionSpec> GoalManager<ActionSpec>::initGoal(const Goal& goal,
TransitionCallback transition_cb,
FeedbackCallback feedback_cb )
Modified: pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.h
===================================================================
--- pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.h 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.h 2009-07-31 01:02:21 UTC (rev 20228)
@@ -83,10 +83,13 @@
typedef boost::function<void (GoalHandle<ActionSpec>) > TransitionCallback;
typedef boost::function<void (GoalHandle<ActionSpec>, const FeedbackConstPtr&) > FeedbackCallback;
typedef boost::function<void (const ActionGoalConstPtr)> SendGoalFunc;
+ typedef boost::function<void (const GoalID&)> CancelFunc;
GoalManager() { }
void registerSendGoalFunc(SendGoalFunc send_goal_func);
+ void registerCancelFunc(CancelFunc cancel_func);
+
GoalHandle<ActionSpec> initGoal( const Goal& goal,
TransitionCallback transition_cb = TransitionCallback(),
FeedbackCallback feedback_cb = FeedbackCallback() );
@@ -102,6 +105,8 @@
ManagedListT list_;
private:
SendGoalFunc send_goal_func_ ;
+ CancelFunc cancel_func_ ;
+
boost::recursive_mutex list_mutex_;
GoalIDGenerator id_generator_;
Modified: pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_action_client.h
===================================================================
--- pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_action_client.h 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_action_client.h 2009-07-31 01:02:21 UTC (rev 20228)
@@ -35,6 +35,9 @@
#ifndef ACTIONLIB_SIMPLE_ACTION_CLIENT_H_
#define ACTIONLIB_SIMPLE_ACTION_CLIENT_H_
+#include <boost/thread/condition.hpp>
+#include <boost/thread/mutex.hpp>
+
#include "ros/ros.h"
#include "actionlib/client/action_client.h"
#include "actionlib/client/simple_goal_state.h"
@@ -69,7 +72,7 @@
* Constructs a SingleGoalActionClient and sets up the necessary ros topics for the ActionInterface
* \param name The action name. Defines the namespace in which the action communicates
*/
- SimpleActionClient(const std::string& name) : ac_(name)
+ SimpleActionClient(const std::string& name) : ac_(name), cur_simple_state_(SimpleGoalState::PENDING)
{
initSimpleClient();
}
@@ -82,7 +85,7 @@
* \param n The node handle on top of which we want to namespace our action
* \param name The action name. Defines the namespace in which the action communicates
*/
- SimpleActionClient(const ros::NodeHandle& n, const std::string& name) : ac_(n, name)
+ SimpleActionClient(const ros::NodeHandle& n, const std::string& name) : ac_(n, name), cur_simple_state_(SimpleGoalState::PENDING)
{
initSimpleClient();
}
@@ -103,6 +106,7 @@
/**
* \brief Blocks until this goal transitions to done
+ * Note that this call exits only after the SimpleDoneCallback() is called.
*/
void waitForGoalToFinish();
@@ -154,6 +158,10 @@
SimpleGoalState cur_simple_state_;
+ // Signalling Stuff
+ boost::condition done_condition_;
+ boost::mutex done_mutex_;
+
// User Callbacks
SimpleDoneCallback done_cb_;
SimpleActiveCallback active_cb_;
@@ -163,6 +171,8 @@
void initSimpleClient();
void handleTransition(GoalHandleT gh);
void handleFeedback(GoalHandleT gh, const FeedbackConstPtr& feedback);
+ void setSimpleState(const SimpleGoalState::StateEnum& next_state);
+ void setSimpleState(const SimpleGoalState& next_state);
};
@@ -174,6 +184,21 @@
}
template<class ActionSpec>
+void SimpleActionClient<ActionSpec>::setSimpleState(const SimpleGoalState::StateEnum& next_state)
+{
+ setSimpleState( SimpleGoalState(next_state) );
+}
+
+template<class ActionSpec>
+void SimpleActionClient<ActionSpec>::setSimpleState(const SimpleGoalState& next_state)
+{
+ ROS_DEBUG("Transitioning SimpleState from [%s] to [%s]",
+ cur_simple_state_.toString().c_str(),
+ next_state.toString().c_str());
+ cur_simple_state_ = next_state;
+}
+
+template<class ActionSpec>
void SimpleActionClient<ActionSpec>::sendGoal(const Goal& goal,
SimpleDoneCallback done_cb,
SimpleActiveCallback active_cb,
@@ -187,6 +212,8 @@
active_cb_ = active_cb;
feedback_cb_ = feedback_cb;
+ cur_simple_state_ = SimpleGoalState::PENDING;
+
// Send the goal to the ActionServer
gh_ = ac_.sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1),
boost::bind(&SimpleActionClientT::handleFeedback, this, _1));
@@ -254,19 +281,127 @@
gh_.cancel();
}
+template<class ActionSpec>
+void SimpleActionClient<ActionSpec>::stopTrackingGoal()
+{
+ if (!gh_.isActive())
+ ROS_ERROR("Trying to stopTrackingGoal() when no goal is running. You are incorrectly using SimpleActionClient");
+ gh_.reset();
+}
+template<class ActionSpec>
+void SimpleActionClient<ActionSpec>::handleFeedback(GoalHandleT gh, const FeedbackConstPtr& feedback)
+{
+ if (gh_ != gh)
+ ROS_ERROR("Got a callback on a goalHandle that we're not tracking. \
+ This is an internal SimpleActionClient/ActionClient bug. \
+ This could also be a GoalID collision");
+ if (feedback_cb_)
+ feedback_cb_(feedback);
+}
+template<class ActionSpec>
+void SimpleActionClient<ActionSpec>::handleTransition(GoalHandleT gh)
+{
+ CommState comm_state_ = gh.getCommState();
+ switch (comm_state_.state_)
+ {
+ case CommState::WAITING_FOR_GOAL_ACK:
+ ROS_ERROR("BUG: Shouldn't ever get a transition callback for WAITING_FOR_GOAL_ACK");
+ break;
+ case CommState::PENDING:
+ ROS_ERROR_COND( cur_simple_state_ != SimpleGoalState::PENDING,
+ "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
+ comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
+ break;
+ case CommState::ACTIVE:
+ switch (cur_simple_state_.state_)
+ {
+ case SimpleGoalState::PENDING:
+ setSimpleState(SimpleGoalState::ACTIVE);
+ if (active_cb_)
+ active_cb_();
+ break;
+ case SimpleGoalState::ACTIVE:
+ break;
+ case SimpleGoalState::DONE:
+ ROS_ERROR("BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
+ comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
+ break;
+ default:
+ ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
+ break;
+ }
+ break;
+ case CommState::WAITING_FOR_RESULT:
+ break;
+ case CommState::WAITING_FOR_CANCEL_ACK:
+ break;
+ case CommState::RECALLING:
+ ROS_ERROR_COND( cur_simple_state_ != SimpleGoalState::PENDING,
+ "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
+ comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
+ break;
+ case CommState::PREEMPTING:
+ switch (cur_simple_state_.state_)
+ {
+ case SimpleGoalState::PENDING:
+ setSimpleState(SimpleGoalState::ACTIVE);
+ if (active_cb_)
+ active_cb_();
+ break;
+ case SimpleGoalState::ACTIVE:
+ break;
+ case SimpleGoalState::DONE:
+ ROS_ERROR("BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
+ comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
+ break;
+ default:
+ ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
+ break;
+ }
+ break;
+ case CommState::DONE:
+ switch (cur_simple_state_.state_)
+ {
+ case SimpleGoalState::PENDING:
+ case SimpleGoalState::ACTIVE:
+ done_mutex_.lock();
+ setSimpleState(SimpleGoalState::DONE);
+ done_mutex_.unlock();
+ if (done_cb_)
+ done_cb_(gh.getTerminalState(), gh.getResult());
+ done_condition_.notify_all();
+ break;
+ case SimpleGoalState::DONE:
+ ROS_ERROR("BUG: Got a second transition to DONE");
+ break;
+ default:
+ ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
+ break;
+ }
+ break;
+ default:
+ ROS_ERROR("Unknown CommState received [%u]", comm_state_.state_);
+ break;
+ }
+}
+template<class ActionSpec>
+void SimpleActionClient<ActionSpec>::waitForGoalToFinish()
+{
+ if (!gh_.isActive())
+ ROS_ERROR("Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient");
+ boost::mutex::scoped_lock lock(done_mutex_);
+ if (cur_simple_state_ != SimpleGoalState::DONE)
+ done_condition_.wait(lock);
+}
+}
-
-
-
-
-
#endif // ACTIONLIB_SINGLE_GOAL_ACTION_CLIENT_H_
Copied: pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_goal_state.h (from rev 20225, pkg/trunk/sandbox/actionlib/src/move_base_client.cpp)
===================================================================
--- pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_goal_state.h (rev 0)
+++ pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_goal_state.h 2009-07-31 01:02:21 UTC (rev 20228)
@@ -0,0 +1,107 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef ACTIONLIB_CLIENT_SIMPLE_GOAL_STATE_H_
+#define ACTIONLIB_CLIENT_SIMPLE_GOAL_STATE_H_
+
+#include <string>
+#include "ros/console.h"
+
+namespace actionlib
+{
+
+/**
+ * \brief Thin wrapper around an enum in order providing a simplified version of the
+ * communication state, but with less states than CommState
+ **/
+class SimpleGoalState
+{
+public:
+
+ //! \brief Defines the various states the SimpleGoalState can be in
+ enum StateEnum
+ {
+ PENDING,
+ ACTIVE,
+ DONE
+ } ;
+
+ SimpleGoalState(const StateEnum& state) : state_(state) { }
+
+ inline bool operator==(const SimpleGoalState& rhs) const
+ {
+ return (state_ == rhs.state_) ;
+ }
+
+ inline bool operator==(const SimpleGoalState::StateEnum& rhs) const
+ {
+ return (state_ == rhs);
+ }
+
+ inline bool operator!=(const SimpleGoalState::StateEnum& rhs) const
+ {
+ return !(*this == rhs);
+ }
+
+ inline bool operator!=(const SimpleGoalState& rhs) const
+ {
+ return !(*this == rhs);
+ }
+
+ std::string toString() const
+ {
+ switch(state_)
+ {
+ case PENDING:
+ return "PENDING";
+ case ACTIVE:
+ return "ACTIVE";
+ case DONE:
+ return "DONE";
+ default:
+ ROS_ERROR("BUG: Unhandled SimpleGoalState: %u", state_);
+ break;
+ }
+ return "BUG-UNKNOWN";
+ }
+
+ StateEnum state_;
+private:
+ SimpleGoalState();
+
+} ;
+
+}
+
+#endif
Deleted: pkg/trunk/sandbox/actionlib/src/move_base_client.cpp
===================================================================
--- pkg/trunk/sandbox/actionlib/src/move_base_client.cpp 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/sandbox/actionlib/src/move_base_client.cpp 2009-07-31 01:02:21 UTC (rev 20228)
@@ -1,112 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#include "ros/ros.h"
-
-#include "actionlib/MoveBaseAction.h"
-#include "actionlib/client/action_client.h"
-
-#include "boost/thread.hpp"
-
-using namespace actionlib;
-using namespace robot_msgs;
-
-typedef ActionClient<MoveBaseAction> MoveBaseClient;
-
-void goalCallback(GoalHandle<MoveBaseAction> gh)
-{
- ROS_DEBUG("In the goalCallback");
-
- ROS_DEBUG("Our final status is: [%s]", gh.getCommState().toString().c_str());
-
- if (gh.getResult())
- ROS_DEBUG("Got a Result!");
- else
- ROS_DEBUG("Got a NULL Result.");
-
-}
-
-void feedbackCallback(GoalHandle<MoveBaseAction> gh, const MoveBaseFeedbackConstPtr& fb)
-{
- ROS_INFO("Got Feedback!");
-}
-
-void spinThread()
-{
- ros::spin();
-}
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "move_base_action_client");
-
- ros::NodeHandle n;
-
- boost::thread spinthread = boost::thread(boost::bind(&spinThread)) ;
-
- MoveBaseClient ac("move_base");
-
- //ros::Duration sleep_duration = ros::Duration().fromSec(1.0);
- //sleep_duration.sleep();
- sleep(2.0);
-
- MoveBaseGoal goal;
-
- GoalHandle<MoveBaseAction> gh = ac.sendGoal(goal, &goalCallback, &feedbackCallback);
-
- sleep(1.0);
-
- gh.cancel();
-
- /*sleep(2.0);
-
- ROS_INFO("About to send a goal");
- gh.reset();
- gh = ac.sendGoal(goal, &goalCallback, &feedbackCallback, ros::Duration(10.0));
-
- sleep(2.0);
-
- ROS_INFO("About to preempt the goal");
- gh.preemptGoal();*/
-
- //ac.sendGoal(goal);
-
- while(n.ok())
- sleep(.1);
-
- sleep(3);
-
-
- return 0;
-}
Copied: pkg/trunk/sandbox/move_base_client/CMakeLists.txt (from rev 20225, pkg/trunk/sandbox/actionlib/CMakeLists.txt)
===================================================================
--- pkg/trunk/sandbox/move_base_client/CMakeLists.txt (rev 0)
+++ pkg/trunk/sandbox/move_base_client/CMakeLists.txt 2009-07-31 01:02:21 UTC (rev 20228)
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rospack(move_base_client)
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#genmsg()
+#uncomment if you have defined services
+#gensrv()
+
+rospack_add_executable(move_base_client src/move_base_client.cpp)
+rospack_add_executable(simple_move_base_client src/simple_move_base_client.cpp)
+
+#common commands for building c++ executables and libraries
+#rospack_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rospack_add_boost_directories()
+#rospack_link_boost(${PROJECT_NAME} thread)
+#rospack_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
Added: pkg/trunk/sandbox/move_base_client/Makefile
===================================================================
--- pkg/trunk/sandbox/move_base_client/Makefile (rev 0)
+++ pkg/trunk/sandbox/move_base_client/Makefile 2009-07-31 01:02:21 UTC (rev 20228)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/sandbox/move_base_client/mainpage.dox
===================================================================
--- pkg/trunk/sandbox/move_base_client/mainpage.dox (rev 0)
+++ pkg/trunk/sandbox/move_base_client/mainpage.dox 2009-07-31 01:02:21 UTC (rev 20228)
@@ -0,0 +1,119 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b move_base_client is ...
+
+<!--
+In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+\section rosapi ROS API
+
+<!--
+Names are very important in ROS because they can be remapped on the
+command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
+APPEAR IN THE CODE. You should list names of every topic, service and
+parameter used in your code. There is a template below that you can
+use to document each node separately.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+-->
+
+<!-- START: copy from here to 'END' for each node
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+END: copy for each node -->
+
+
+<!-- START: Uncomment if you have any command-line tools
+
+\section commandline Command-line tools
+
+This section is a catch-all for any additional tools that your package
+provides or uses that may be of use to the reader. For example:
+
+- tools/scripts (e.g. rospack, roscd)
+- roslaunch .launch files
+- xmlparam files
+
+\subsection script_name script_name
+
+Description of what this script/file does.
+
+\subsubsection Usage
+\verbatim
+$ ./script_name [args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ ./script_name foo bar
+\endverbatim
+
+END: Command-Line Tools Section -->
+
+*/
\ No newline at end of file
Added: pkg/trunk/sandbox/move_base_client/manifest.xml
===================================================================
--- pkg/trunk/sandbox/move_base_client/manifest.xml (rev 0)
+++ pkg/trunk/sandbox/move_base_client/manifest.xml 2009-07-31 01:02:21 UTC (rev 20228)
@@ -0,0 +1,15 @@
+<package>
+ <description brief="move_base_client">
+ move_base_client
+ </description>
+ <author>Vijay Pradeep</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/move_base_client</url>
+
+ <depend package="move_base" />
+ <depend package="actionlib" />
+
+</package>
+
+
Copied: pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp (from rev 20225, pkg/trunk/sandbox/actionlib/src/move_base_client.cpp)
===================================================================
--- pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp (rev 0)
+++ pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp 2009-07-31 01:02:21 UTC (rev 20228)
@@ -0,0 +1,112 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "ros/ros.h"
+
+#include "move_base/MoveBaseAction...
[truncated message content] |
|
From: <is...@us...> - 2009-07-31 04:11:10
|
Revision: 20244
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20244&view=rev
Author: isucan
Date: 2009-07-31 04:10:57 +0000 (Fri, 31 Jul 2009)
Log Message:
-----------
collision space editing is now possible
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
pkg/trunk/sandbox/3dnav_pr2/launch/actions/right_arm.launch
pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml
pkg/trunk/world_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/include/collision_space/environment_objects.h
pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
pkg/trunk/world_models/collision_space/src/environment_objects.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/planning_environment/launch/
pkg/trunk/motion_planning/planning_environment/launch/clear_known_objects.launch
pkg/trunk/motion_planning/planning_environment/launch/display_planner_collision_model.launch
pkg/trunk/motion_planning/planning_environment/launch/view_state_validity.launch
pkg/trunk/motion_planning/planning_environment/src/examples/
pkg/trunk/motion_planning/planning_environment/src/examples/remove_object_example.cpp
Removed Paths:
-------------
pkg/trunk/motion_planning/planning_environment/clear_known_objects.launch
pkg/trunk/motion_planning/planning_environment/display_planner_collision_model.launch
pkg/trunk/motion_planning/planning_environment/view_state_validity.launch
Modified: pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-07-31 04:10:57 UTC (rev 20244)
@@ -36,6 +36,12 @@
rospack_link_boost(display_planner_collision_model thread)
rospack_add_openmp_flags(display_planner_collision_model)
+# Examples
+rospack_add_executable(remove_object_example src/examples/remove_object_example.cpp)
+target_link_libraries(remove_object_example planning_environment)
+rospack_link_boost(remove_object_example thread)
+rospack_add_openmp_flags(remove_object_example)
+
# Tests
# Create a model of the PR2
Deleted: pkg/trunk/motion_planning/planning_environment/clear_known_objects.launch
===================================================================
--- pkg/trunk/motion_planning/planning_environment/clear_known_objects.launch 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/motion_planning/planning_environment/clear_known_objects.launch 2009-07-31 04:10:57 UTC (rev 20244)
@@ -1,10 +0,0 @@
-
-<launch>
- <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
- <remap from="cloud_in" to="full_cloud_annotated" />
- <remap from="cloud_out" to="full_cloud_without_known_objects" />
- <param name="fixed_frame" type="string" value="/base_link" />
- <param name="object_scale" type="double" value="1.0" />
- <param name="object_padd" type="double" value="0.03" />
- </node>
-</launch>
Deleted: pkg/trunk/motion_planning/planning_environment/display_planner_collision_model.launch
===================================================================
--- pkg/trunk/motion_planning/planning_environment/display_planner_collision_model.launch 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/motion_planning/planning_environment/display_planner_collision_model.launch 2009-07-31 04:10:57 UTC (rev 20244)
@@ -1,8 +0,0 @@
-
-<launch>
- <node pkg="planning_environment" type="display_planner_collision_model" respawn="false" output="screen">
- <remap from="collision_map" to="collision_map_occ" />
- <remap from="collision_map_update" to="collision_map_occ_update" />
- <param name="skip_collision_map" type="bool" value="true" />
- </node>
-</launch>
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-07-31 04:10:57 UTC (rev 20244)
@@ -142,12 +142,18 @@
return pointcloud_padd_;
}
+ /** \brief If the used modified some instance of an environment model, this function provides the means to obtain a collision map (the set of boxes)
+ * from that environment model */
+ void recoverCollisionMap(const collision_space::EnvironmentModel *env, mapping_msgs::CollisionMap &cmap);
+
protected:
void setupCSM(void);
void updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear);
void collisionMapAsSpheres(const mapping_msgs::CollisionMapConstPtr &collisionMap,
std::vector<shapes::Shape*> &spheres, std::vector<btTransform> &poses);
+ void collisionMapAsBoxes(const mapping_msgs::CollisionMapConstPtr &collisionMap,
+ std::vector<shapes::Shape*> &boxes, std::vector<btTransform> &poses);
void collisionMapCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
void collisionMapUpdateCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
void objectInMapCallback(const mapping_msgs::ObjectInMapConstPtr &objectInMap);
Copied: pkg/trunk/motion_planning/planning_environment/launch/clear_known_objects.launch (from rev 20207, pkg/trunk/motion_planning/planning_environment/clear_known_objects.launch)
===================================================================
--- pkg/trunk/motion_planning/planning_environment/launch/clear_known_objects.launch (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/launch/clear_known_objects.launch 2009-07-31 04:10:57 UTC (rev 20244)
@@ -0,0 +1,10 @@
+
+<launch>
+ <node pkg="planning_environment" type="clear_known_objects" respawn="false" output="screen">
+ <remap from="cloud_in" to="full_cloud_annotated" />
+ <remap from="cloud_out" to="full_cloud_without_known_objects" />
+ <param name="fixed_frame" type="string" value="/base_link" />
+ <param name="object_scale" type="double" value="1.0" />
+ <param name="object_padd" type="double" value="0.03" />
+ </node>
+</launch>
Property changes on: pkg/trunk/motion_planning/planning_environment/launch/clear_known_objects.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/motion_planning/planning_environment/clear_known_objects.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/motion_planning/planning_environment/launch/display_planner_collision_model.launch (from rev 20207, pkg/trunk/motion_planning/planning_environment/display_planner_collision_model.launch)
===================================================================
--- pkg/trunk/motion_planning/planning_environment/launch/display_planner_collision_model.launch (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/launch/display_planner_collision_model.launch 2009-07-31 04:10:57 UTC (rev 20244)
@@ -0,0 +1,8 @@
+
+<launch>
+ <node pkg="planning_environment" type="display_planner_collision_model" respawn="false" output="screen">
+ <remap from="collision_map" to="collision_map_occ" />
+ <remap from="collision_map_update" to="collision_map_occ_update" />
+ <param name="skip_collision_map" type="bool" value="true" />
+ </node>
+</launch>
Property changes on: pkg/trunk/motion_planning/planning_environment/launch/display_planner_collision_model.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/motion_planning/planning_environment/display_planner_collision_model.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/motion_planning/planning_environment/launch/view_state_validity.launch (from rev 20207, pkg/trunk/motion_planning/planning_environment/view_state_validity.launch)
===================================================================
--- pkg/trunk/motion_planning/planning_environment/launch/view_state_validity.launch (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/launch/view_state_validity.launch 2009-07-31 04:10:57 UTC (rev 20244)
@@ -0,0 +1,10 @@
+<launch>
+ <node pkg="planning_environment" type="view_state_validity" respawn="false" output="screen">
+ <remap from="collision_map" to="collision_map_occ" />
+ <remap from="collision_map_update" to="collision_map_occ_update" />
+ <param name="refresh_interval_collision_map" type="double" value="5.0" />
+ <param name="refresh_interval_kinematic_state" type="double" value="1.0" />
+ <param name="refresh_interval_pose" type="double" value="1.0" />
+ <param name="bounding_planes" type="string" value="0 0 1 -0.01" />
+ </node>
+</launch>
Copied: pkg/trunk/motion_planning/planning_environment/src/examples/remove_object_example.cpp (from rev 20207, pkg/trunk/motion_planning/planning_environment/src/tools/view_state_validity.cpp)
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/examples/remove_object_example.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/src/examples/remove_object_example.cpp 2009-07-31 04:10:57 UTC (rev 20244)
@@ -0,0 +1,134 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+/**
+
+@b RemoveObjectExample is a node that forwards a collision map after it removes a box from it
+
+**/
+
+#include "planning_environment/monitors/planning_monitor.h"
+#include <mapping_msgs/CollisionMap.h>
+
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <map>
+
+class RemoveObjectExample
+{
+public:
+
+ RemoveObjectExample(void)
+ {
+ collisionModels_ = new planning_environment::CollisionModels("robot_description");
+ if (collisionModels_->loadedModels())
+ {
+ collisionMapPublisher_ = nh_.advertise<mapping_msgs::CollisionMap>("collision_map_with_removed_box", 1);
+ planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, &tf_);
+ planningMonitor_->setOnAfterMapUpdateCallback(boost::bind(&RemoveObjectExample::afterWorldUpdate, this, _1, _2));
+ }
+ }
+
+ virtual ~RemoveObjectExample(void)
+ {
+ if (planningMonitor_)
+ delete planningMonitor_;
+ if (collisionModels_)
+ delete collisionModels_;
+ }
+
+protected:
+
+ void afterWorldUpdate(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear)
+ {
+ // we do not care about incremental updates, only re-writes of the map
+ if (!clear)
+ return;
+
+ // at this point, the environment model has the collision map inside it
+
+
+ // get exclusive access
+ planningMonitor_->getEnvironmentModel()->lock();
+
+ // get a copy of our own, to play with :)
+ collision_space::EnvironmentModel *env = planningMonitor_->getEnvironmentModel()->clone();
+
+ // release our hold
+ planningMonitor_->getEnvironmentModel()->unlock();
+
+
+ // create a box
+ shapes::Shape *box = new shapes::Box(2, 2, 2);
+ btTransform pose;
+ pose.setIdentity();
+
+ // remove the objects colliding with the box
+ env->removeCollidingObjects(box, pose);
+
+ // forward the updated map
+ mapping_msgs::CollisionMap cmap;
+ planningMonitor_->recoverCollisionMap(env, cmap);
+ collisionMapPublisher_.publish(cmap);
+
+ // throw away our copy
+ delete env;
+
+ ROS_INFO("Received collision map with %d points and published one with %d points",
+ (int)collisionMap->get_boxes_size(), (int)cmap.get_boxes_size());
+
+ }
+
+private:
+
+ ros::NodeHandle nh_;
+ tf::TransformListener tf_;
+
+ ros::Publisher collisionMapPublisher_;
+ planning_environment::CollisionModels *collisionModels_;
+ planning_environment::PlanningMonitor *planningMonitor_;
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "view_state_validity");
+
+ RemoveObjectExample example;
+ ros::spin();
+
+ return 0;
+}
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-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-07-31 04:10:57 UTC (rev 20244)
@@ -199,6 +199,64 @@
}
}
+void planning_environment::CollisionSpaceMonitor::collisionMapAsBoxes(const mapping_msgs::CollisionMapConstPtr &collisionMap,
+ std::vector<shapes::Shape*> &boxes, std::vector<btTransform> &poses)
+{
+ // we want to make sure the frame the robot model is kept in is the same as the frame of the collisionMap
+ bool transform = !frame_id_.empty() && collisionMap->header.frame_id != frame_id_;
+ const int n = collisionMap->get_boxes_size();
+
+ double pd = 2.0 * pointcloud_padd_;
+
+ boxes.resize(n);
+ poses.resize(n);
+
+ if (transform)
+ {
+ std::string target = frame_id_;
+ bool err = false;
+
+#pragma omp parallel for
+ for (int i = 0 ; i < n ; ++i)
+ {
+ robot_msgs::PointStamped psi;
+ psi.header = collisionMap->header;
+ psi.point.x = collisionMap->boxes[i].center.x;
+ psi.point.y = collisionMap->boxes[i].center.y;
+ psi.point.z = collisionMap->boxes[i].center.z;
+
+ robot_msgs::PointStamped pso;
+ try
+ {
+ tf_->transformPoint(target, psi, pso);
+ }
+ catch(...)
+ {
+ err = true;
+ pso = psi;
+ }
+
+ poses[i].setRotation(btQuaternion(btVector3(collisionMap->boxes[i].axis.x, collisionMap->boxes[i].axis.y, collisionMap->boxes[i].axis.z), collisionMap->boxes[i].angle));
+ poses[i].setOrigin(btVector3(pso.point.x, pso.point.y, pso.point.z));
+ boxes[i] = new shapes::Box(collisionMap->boxes[i].extents.x + pd, collisionMap->boxes[i].extents.y + pd, collisionMap->boxes[i].extents.z + pd);
+ }
+
+ if (err)
+ ROS_ERROR("Some errors encountered in transforming the collision map to frame '%s' from frame '%s'", target.c_str(), collisionMap->header.frame_id.c_str());
+ }
+ else
+ {
+
+#pragma omp parallel for
+ for (int i = 0 ; i < n ; ++i)
+ {
+ poses[i].setRotation(btQuaternion(btVector3(collisionMap->boxes[i].axis.x, collisionMap->boxes[i].axis.y, collisionMap->boxes[i].axis.z), collisionMap->boxes[i].angle));
+ poses[i].setOrigin(btVector3(collisionMap->boxes[i].center.x, collisionMap->boxes[i].center.y, collisionMap->boxes[i].center.z));
+ boxes[i] = new shapes::Box(collisionMap->boxes[i].extents.x + pd, collisionMap->boxes[i].extents.y + pd, collisionMap->boxes[i].extents.z + pd);
+ }
+ }
+}
+
void planning_environment::CollisionSpaceMonitor::updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear)
{
boost::mutex::scoped_lock lock(mapUpdateLock_);
@@ -212,7 +270,7 @@
std::vector<shapes::Shape*> spheres;
std::vector<btTransform> poses;
- collisionMapAsSpheres(collisionMap, spheres, poses);
+ collisionMapAsBoxes(collisionMap, spheres, poses);
collisionSpace_->lock();
if (clear)
@@ -307,3 +365,34 @@
return result;
}
+
+void planning_environment::CollisionSpaceMonitor::recoverCollisionMap(const collision_space::EnvironmentModel *env, mapping_msgs::CollisionMap &cmap)
+{
+ cmap.header.frame_id = getFrameId();
+ cmap.header.stamp = ros::Time::now();
+ cmap.boxes.clear();
+
+ const collision_space::EnvironmentObjects::NamespaceObjects &no = env->getObjects()->getObjects("points");
+ const unsigned int n = no.shape.size();
+ double pd = pointcloud_padd_ * 2.0;
+ for (unsigned int i = 0 ; i < n ; ++i)
+ if (no.shape[i]->type == shapes::BOX)
+ {
+ const shapes::Box* box = static_cast<const shapes::Box*>(no.shape[i]);
+ mapping_msgs::OrientedBoundingBox obb;
+ obb.extents.x = box->size[0] - pd;
+ obb.extents.y = box->size[1] - pd;
+ obb.extents.z = box->size[2] - pd;
+ const btVector3 &c = no.shapePose[i].getOrigin();
+ obb.center.x = c.x();
+ obb.center.y = c.y();
+ obb.center.z = c.z();
+ const btQuaternion q = no.shapePose[i].getRotation();
+ obb.angle = q.getAngle();
+ const btVector3 axis = q.getAxis();
+ obb.axis.x = axis.x();
+ obb.axis.y = axis.y();
+ obb.axis.z = axis.z();
+ cmap.boxes.push_back(obb);
+ }
+}
Deleted: pkg/trunk/motion_planning/planning_environment/view_state_validity.launch
===================================================================
--- pkg/trunk/motion_planning/planning_environment/view_state_validity.launch 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/motion_planning/planning_environment/view_state_validity.launch 2009-07-31 04:10:57 UTC (rev 20244)
@@ -1,10 +0,0 @@
-<launch>
- <node pkg="planning_environment" type="view_state_validity" respawn="false" output="screen">
- <remap from="collision_map" to="collision_map_occ" />
- <remap from="collision_map_update" to="collision_map_occ_update" />
- <param name="refresh_interval_collision_map" type="double" value="5.0" />
- <param name="refresh_interval_kinematic_state" type="double" value="1.0" />
- <param name="refresh_interval_pose" type="double" value="1.0" />
- <param name="bounding_planes" type="string" value="0 0 1 -0.01" />
- </node>
-</launch>
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/actions/right_arm.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/actions/right_arm.launch 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/actions/right_arm.launch 2009-07-31 04:10:57 UTC (rev 20244)
@@ -1,8 +1,8 @@
<launch>
<include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
+<!-- <include file="$(find pr2_ik)/pr2_ik_rarm_node.launch"/> -->
<include file="$(find pr2_ik)/pr2_ik_rarm_node.launch"/>
- <include file="$(find pr2_ik)/pr2_ik_wc_rarm_node.launch"/>
<rosparam file="$(find pr2_default_controllers)/r_arm_trajectory_controller.yaml" command="load" ns="r_arm_trajectory_controller"/>
Modified: pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml 2009-07-31 04:10:57 UTC (rev 20244)
@@ -28,7 +28,7 @@
l_wrist_flex_link
l_wrist_roll_link
planner_configs:
- RRTkConfig2 pRRTkConfig1 SBLkConfig2 KPIECEkConfig1 KPIECEkConfig2l LBKPIECEkConfig2l armIKConfig1
+ RRTkConfig2 SBLkConfig2 KPIECEkConfig1 KPIECEkConfig2l LBKPIECEkConfig2l armIKConfig1
right_arm:
links:
@@ -42,7 +42,7 @@
r_wrist_flex_link
r_wrist_roll_link
planner_configs:
- RRTkConfig2 pRRTkConfig1 SBLkConfig2 pSBLkConfig1 KPIECEkConfig1 KPIECEkConfig2r LBKPIECEkConfig2r armIKConfig1
+ RRTkConfig2 SBLkConfig2 KPIECEkConfig1 KPIECEkConfig2r LBKPIECEkConfig2r armIKConfig1
arms:
links:
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-07-31 04:10:57 UTC (rev 20244)
@@ -153,15 +153,21 @@
/** \brief Remove objects from a specific namespace in the collision model */
virtual void clearObjects(const std::string &ns) = 0;
- /** \brief Add a static collision object to the map. The user releases ownership of the passed object. */
- virtual void addObject(const std::string &ns, const shapes::StaticShape *shape) = 0;
+ /** \brief Add a static collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment. */
+ virtual void addObject(const std::string &ns, shapes::StaticShape *shape) = 0;
- /** \brief Add a collision object to the map. The user releases ownership of the passed object.*/
- virtual void addObject(const std::string &ns, const shapes::Shape* shape, const btTransform &pose) = 0;
+ /** \brief Add a collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment.*/
+ virtual void addObject(const std::string &ns, shapes::Shape* shape, const btTransform &pose) = 0;
- /** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. */
+ /** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. Memory allocated for the shapes is freed by the collision environment.*/
virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses) = 0;
+ /** \brief Remove objects in the collision space that are collising with the object supplied as argument. */
+ virtual void removeCollidingObjects(const shapes::StaticShape *shape) = 0;
+
+ /** \brief Remove objects in the collision space that are collising with the object supplied as argument. */
+ virtual void removeCollidingObjects(const shapes::Shape *shape, const btTransform &pose) = 0;
+
/** \brief Get the objects currently contained in the model */
const EnvironmentObjects* getObjects(void) const;
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-07-31 04:10:57 UTC (rev 20244)
@@ -45,7 +45,7 @@
namespace collision_space
{
- /** \brief A class describing an environment for a kinematic robot using bullet */
+ /** \brief A class describing an environment for a kinematic robot using bullet. This class is still experimental, and methos such as cloning are not implemented. */
class EnvironmentModelBullet : public EnvironmentModel
{
public:
@@ -80,15 +80,21 @@
/** \brief Remove objects from a specific namespace in the collision model */
virtual void clearObjects(const std::string &ns);
- /** \brief Add a static collision object to the map. The user releases ownership of the passed object. */
- virtual void addObject(const std::string &ns, const shapes::StaticShape *shape);
+ /** \brief Add a static collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment. */
+ virtual void addObject(const std::string &ns, shapes::StaticShape *shape);
- /** \brief Add a collision object to the map. The user releases ownership of the passed object.*/
- virtual void addObject(const std::string &ns, const shapes::Shape* shape, const btTransform &pose);
+ /** \brief Add a collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment. */
+ virtual void addObject(const std::string &ns, shapes::Shape* shape, const btTransform &pose);
- /** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. */
+ /** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. Memory allocated for the shapes is freed by the collision environment. */
virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses);
+ /** \brief Remove objects in the collision space that are collising with the object supplied as argument */
+ virtual void removeCollidingObjects(const shapes::StaticShape *shape);
+
+ /** \brief Remove objects in the collision space that are collising with the object supplied as argument */
+ virtual void removeCollidingObjects(const shapes::Shape *shape, const btTransform &pose);
+
/** \brief Add a robot model. Ignore robot links if their name is not
specified in the string vector. The scale argument can be
used to increase or decrease the size of the robot's
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-07-31 03:53:25 UTC (rev 20243)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-07-31 04:10:57 UTC (rev 20244)
@@ -70,15 +70,21 @@
/** \brief Remove objects from a specific namespace in the collision model */
virtual void clearObjects(const std::string &ns);
- /** \brief Add a static collision object to the map. The user releases ownership of the passed object. */
- virtual void addObject(const std::string &ns, const shapes::StaticShape *shape);
+ /** \brief Add a static collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment. */
+ virtual void addObject(const std::string &ns, shapes::StaticShape *shape);
- /** \brief Add a collision object to the map. The user releases ownership of the passed object.*/
- virtual void addObject(const std::string &ns, const shapes::Shape* shape, const btTransform &pose);
+ /** \brief Add a collision object to the map. The user releases ownership of the passed object. Memory allocated for the shape is freed by the collision environment. */
+ virtual void addObject(const std::string &ns, shapes::Shape* shape, const btTransform &pose);
- /** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. */
+ /** \brief Add a set of collision objects to the map. The user releases ownership of the passed objects. Memory allocated for the shapes is freed by the collision environment. */
virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<btTransform> &poses);
+ /** \brief Remove objects in the collision space that are collising with the object supplied as argument */
+ virtual void removeCollidingObjects(const shapes::StaticShape *shape);
+
+ /** \brief Remove objects in the collision space that are collising with the object supplied as argument */
+ virtual void removeCollidingObjects(const shapes::Shape *shape, const btTransform &pose);
+
/** \brief Add a robot model. Ignore robot links if their name is not
specified in the string vector. The scale argument can be
used to increase or decrease the size of the robot's
@@ -119,6 +125,7 @@
void registerSpace(dSpaceID space);
void registerGeom(dGeomID geom);
+ void unregisterGeom(dGeomID geom);
void clear(void);
void setup(void);
void collide(dGeomID geom, void *data, dNearCallback *nearCallback) const;
@@ -297,6 +304,7 @@
unsigned int max_contacts;
std::vector<EnvironmentModelODE::Contact> *contacts;
std::vector< std::vector<bool> > *selfCollisionTest;
+ dSpaceID selfSpace;
planning_mod...
[truncated message content] |
|
From: <ei...@us...> - 2009-07-31 04:50:29
|
Revision: 20249
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20249&view=rev
Author: eitanme
Date: 2009-07-31 04:50:18 +0000 (Fri, 31 Jul 2009)
Log Message:
-----------
Creating an applications directory and moving 2dnav_pr2 to the 2dnav_pr2_app application
Added Paths:
-----------
pkg/trunk/applications/
pkg/trunk/applications/2dnav_pr2_app/
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/
pkg/trunk/applications/2dnav_pr2_app/stack.xml
Removed Paths:
-------------
pkg/trunk/demos/2dnav_pr2/
Added: pkg/trunk/applications/2dnav_pr2_app/stack.xml
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/stack.xml (rev 0)
+++ pkg/trunk/applications/2dnav_pr2_app/stack.xml 2009-07-31 04:50:18 UTC (rev 20249)
@@ -0,0 +1,16 @@
+<stack name="2dnav_pr2_app" version="0.1">
+ <description brief="A 2D navigation application for the PR2 robot platform.">
+ The 2D navigation application for the PR2 robot
+ </description>
+ <version>0.1</version>
+ <author>Eitan ei...@wi...</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/2dnav_pr2_app</url>
+
+ <depend stack="ros" version="0.5"/>
+ <depend stack="ros-pkg_common" version="0.1"/>
+ <depend stack="ros-pkg_visualization_core" version="0.1"/>
+ <depend stack="navigation" version="0.1"/>
+</stack>
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|