|
From: <is...@us...> - 2009-07-06 20:23:38
|
Revision: 18336
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18336&view=rev
Author: isucan
Date: 2009-07-06 20:23:36 +0000 (Mon, 06 Jul 2009)
Log Message:
-----------
shapt type for bodies
Modified Paths:
--------------
pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h
pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h
pkg/trunk/util/geometric_shapes/src/bodies.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
Modified: pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h
===================================================================
--- pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h 2009-07-06 20:23:32 UTC (rev 18335)
+++ pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h 2009-07-06 20:23:36 UTC (rev 18336)
@@ -70,12 +70,19 @@
m_scale = 1.0;
m_padding = 0.0;
m_pose.setIdentity();
+ m_type = shapes::UNKNOWN;
}
virtual ~Body(void)
{
}
+ /** \brief Get the type of shape this body represents */
+ shapes::ShapeType getType(void) const
+ {
+ return m_type;
+ }
+
/** \brief If the dimension of the body should be scaled, this
method sets the scale. Default is 1.0 */
void setScale(double scale)
@@ -146,9 +153,10 @@
virtual void updateInternalData(void) = 0;
virtual void useDimensions(const shapes::Shape *shape) = 0;
- btTransform m_pose;
- double m_scale;
- double m_padding;
+ shapes::ShapeType m_type;
+ btTransform m_pose;
+ double m_scale;
+ double m_padding;
};
/** \brief Definition of a sphere */
@@ -158,10 +166,12 @@
Sphere(void) : Body()
{
m_radius = 0.0;
+ m_type = shapes::SPHERE;
}
Sphere(const shapes::Shape *shape) : Body()
{
+ m_type = shapes::SPHERE;
setDimensions(shape);
}
@@ -191,10 +201,12 @@
Cylinder(void) : Body()
{
m_length = m_radius = 0.0;
+ m_type = shapes::CYLINDER;
}
Cylinder(const shapes::Shape *shape) : Body()
{
+ m_type = shapes::CYLINDER;
setDimensions(shape);
}
@@ -231,10 +243,12 @@
Box(void) : Body()
{
m_length = m_width = m_height = 0.0;
+ m_type = shapes::BOX;
}
Box(const shapes::Shape *shape) : Body()
{
+ m_type = shapes::BOX;
setDimensions(shape);
}
@@ -271,12 +285,14 @@
public:
ConvexMesh(void) : Body()
- {
+ {
+ m_type = shapes::MESH;
m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
}
ConvexMesh(const shapes::Shape *shape) : Body()
- {
+ {
+ m_type = shapes::MESH;
m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
setDimensions(shape);
}
Modified: pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h
===================================================================
--- pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h 2009-07-06 20:23:32 UTC (rev 18335)
+++ pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h 2009-07-06 20:23:36 UTC (rev 18336)
@@ -48,6 +48,10 @@
namespace shapes
{
+ /** \brief A list of known shape types */
+ enum ShapeType { UNKNOWN, SPHERE, CYLINDER, BOX, MESH };
+
+
/** \brief A basic definition of a shape. Shapes are considered centered at origin */
class Shape
{
@@ -61,9 +65,7 @@
{
}
- enum { UNKNOWN, SPHERE, CYLINDER, BOX, MESH }
- type;
-
+ ShapeType type;
};
/** \brief Definition of a sphere */
Modified: pkg/trunk/util/geometric_shapes/src/bodies.cpp
===================================================================
--- pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-07-06 20:23:32 UTC (rev 18335)
+++ pkg/trunk/util/geometric_shapes/src/bodies.cpp 2009-07-06 20:23:36 UTC (rev 18336)
@@ -46,16 +46,16 @@
if (shape)
switch (shape->type)
{
- case shapes::Shape::BOX:
+ case shapes::BOX:
body = new bodies::Box(shape);
break;
- case shapes::Shape::SPHERE:
+ case shapes::SPHERE:
body = new bodies::Sphere(shape);
break;
- case shapes::Shape::CYLINDER:
+ case shapes::CYLINDER:
body = new bodies::Cylinder(shape);
break;
- case shapes::Shape::MESH:
+ case shapes::MESH:
body = new bodies::ConvexMesh(shape);
break;
default:
@@ -261,7 +261,7 @@
HullLibrary hl;
if (hl.CreateConvexHull(hd, hr) == QE_OK)
{
- std::cout << "Convex hull has " << hr.m_OutputVertices.size() << "(" << hr.mNumOutputVertices << ") vertices (down from " << mesh->vertexCount << "), " << hr.mNumFaces << " faces" << std::endl;
+ 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));
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-06 20:23:32 UTC (rev 18335)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-06 20:23:36 UTC (rev 18336)
@@ -96,24 +96,24 @@
dGeomID g = NULL;
switch (shape->type)
{
- case shapes::Shape::SPHERE:
+ case shapes::SPHERE:
{
g = dCreateSphere(space, static_cast<shapes::Sphere*>(shape)->radius * scale + padding);
}
break;
- case shapes::Shape::BOX:
+ case shapes::BOX:
{
const double *size = static_cast<shapes::Box*>(shape)->size;
g = dCreateBox(space, size[0] * scale + padding, size[1] * scale + padding, size[2] * scale + padding);
}
break;
- case shapes::Shape::CYLINDER:
+ case shapes::CYLINDER:
{
g = dCreateCylinder(space, static_cast<shapes::Cylinder*>(shape)->radius * scale + padding,
static_cast<shapes::Cylinder*>(shape)->length * scale + padding);
}
break;
- case shapes::Shape::MESH:
+ case shapes::MESH:
{
dTriMeshDataID data = dGeomTriMeshDataCreate();
shapes::Mesh *mesh = static_cast<shapes::Mesh*>(shape);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mp...@us...> - 2009-07-06 20:54:14
|
Revision: 18338
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18338&view=rev
Author: mppics
Date: 2009-07-06 20:30:57 +0000 (Mon, 06 Jul 2009)
Log Message:
-----------
Added teleop_anti_collision, updated controllers/odom and keyboard teleop gripper
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
pkg/trunk/sandbox/pr2_gripper_controller/src/teleop_gripper_keyboard.cpp
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/pr2_base_odometry.xml
Added Paths:
-----------
pkg/trunk/sandbox/teleop_anti_collision/
pkg/trunk/sandbox/teleop_anti_collision/.build_version
pkg/trunk/sandbox/teleop_anti_collision/CMakeLists.txt
pkg/trunk/sandbox/teleop_anti_collision/Makefile
pkg/trunk/sandbox/teleop_anti_collision/bin/
pkg/trunk/sandbox/teleop_anti_collision/launch/
pkg/trunk/sandbox/teleop_anti_collision/launch/base_odom_teleop.xml
pkg/trunk/sandbox/teleop_anti_collision/launch/run_teleop_anti_collision.xml
pkg/trunk/sandbox/teleop_anti_collision/launch/test_teleop_anti_collision.xml
pkg/trunk/sandbox/teleop_anti_collision/lib/
pkg/trunk/sandbox/teleop_anti_collision/manifest.xml
pkg/trunk/sandbox/teleop_anti_collision/src/
pkg/trunk/sandbox/teleop_anti_collision/src/teleop_goal_projection.cpp
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-06 20:23:46 UTC (rev 18337)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h 2009-07-06 20:30:57 UTC (rev 18338)
@@ -31,7 +31,7 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
- /*
+/*
* Author: Sachin Chitta and Matthew Piccoli
*/
@@ -43,245 +43,245 @@
namespace controller
{
- class Wheel;
- class Caster;
- class BaseKinematics;
- /*! \class
- \brief This class keeps track of the wheels
- */
- class Wheel
- {
- public:
-
- /*!
- * \brief JointState for this wheel joint
- */
- mechanism::JointState *joint_;
-
- /*!
- * \brief default offset from the parent caster before any transformations
- */
- robot_msgs::Point offset_;
-
- /*!
- * \brief name of the joint
- */
- std::string name_;
- /*!
- * \brief offset_ after rotation transformation from the parent caster's position
- */
- robot_msgs::Point position_;
-
- /*!
- * \brief the caster on which this wheel is mounted
- */
- Caster *parent_;
-
- /*!
- * \brief actual wheel speeds
- */
- double wheel_speed_actual_;
-
- /*!
- * \brief desired wheel speed
- */
- double wheel_speed_cmd_;
-
- /*!
- * \brief difference between desired and actual speed
- */
- double wheel_speed_error_;
-
- /*!
- * \brief wheel speed filtered with alpha
- */
- double wheel_speed_filtered_;
-
- /*!
- * \brief specifies the default direction of the wheel
- */
- int direction_multiplier_;
-
- /*!
- * \brief remembers if the wheel is stalled
- */
- int wheel_stuck_;
-
- /*!
- * \brief wheel radius scale (based on the default wheel radius in Basekinematics)
- */
- double wheel_radius_scaler_;
-
- /*!
- * \brief Loads wheel's information from the xml description file and param server
- * @param robot_state The robot's current state
- * @param config Tiny xml element pointing to this wheel
- */
- void initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
-
- /*!
- * \brief Computes 2d postion of the wheel relative to the center of the base
- */
- void updatePosition();
- };
-
- /*! \class
- \brief This class keeps track of the casters
- */
- class Caster
- {
- public:
-
- /*!
- * \brief JointState for this caster joint
- */
- mechanism::JointState *joint_;
-
- /*!
- * \brief offset from the center of the base
- */
- robot_msgs::Point offset_;
-
- /*!
- * \brief name of the joint
- */
- std::string name_;
- //robot_msgs::Point position_;
-
- /*!
- * \brief BaseKinematics to which this caster belongs
- */
- BaseKinematics *parent_;
-
- /*!
- * \brief the number of child wheels that are attached to this caster
- */
- int num_children_;
-
- /*!
- * \brief actual caster steer angles
- */
- double steer_angle_actual_;
-
- /*!
- * \brief vector of desired caster steer speeds
- */
- double steer_velocity_desired_;
-
- /*!
- * \brief stored caster steer angles
- */
- double steer_angle_stored_;
-
- /*!
- * \brief difference between desired and actual angles of the casters
- */
- double caster_position_error_;
-
- /*!
- * \brief difference between desired and actual speeds of the casters
- */
- double caster_speed_error_;
-
- /*!
- * \brief caster speed filtered with alpha
- */
- double caster_speed_filtered_;
+ class Wheel;
+ class Caster;
+ class BaseKinematics;
+ /*! \class
+ \brief This class keeps track of the wheels
+ */
+ class Wheel
+ {
+ public:
- /*!
- * \brief remembers the caster's current speed
- */
- double caster_speed_;
-
- /*!
- * \brief remembers if the caster is stalled
- */
- int caster_stuck_;
-
- /*!
- * \brief Loads caster's information from the xml description file and param server
- * @param robot_state The robot's current state
- * @param config Tiny xml element pointing to this caster
- */
- void initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
- };
-
- /*! \class
- \brief This class includes common functions used by the base controller and odometry
- */
- class BaseKinematics
- {
- public:
-
- /*!
- * \brief Loads BaseKinematic's information from the xml description file and param server
- * @param robot_state The robot's current state
- * @param config Tiny xml element pointing to its controller
- * @return Successful init
- */
- bool initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
-
- /*!
- * \brief Computes 2d postion of every wheel relative to the center of the base
- */
- void computeWheelPositions();
-
- /*!
- * \brief Computes 2d velocity of a point at relative distance pos to another point with velocity (and rotation (z)) vel
- * @param pos The position of the object relative to the center of rotation
- * @param vel Velocity of the center of rotation
- * @return Velocity at the given point
- */
- robot_msgs::PoseDot pointVel2D(const robot_msgs::Point& pos, const robot_msgs::PoseDot& vel);
-
- /*!
- * \brief remembers everything about the state of the robot
- */
- mechanism::RobotState *robot_state_;
-
- /*!
- * \brief number of wheels connected to the base
- */
- int num_wheels_;
-
- /*!
- * \brief number of casters connected to the base
- */
- int num_casters_;
-
- /*!
- * \brief vector of every wheel attached to the base
- */
- std::vector<Wheel> wheel_;
-
- /*!
- * \brief vector of every caster attached to the base
- */
- std::vector<Caster> caster_;
-
- /*!
- * \brief default radius of each wheel
- */
- double wheel_radius_;
-
- /*!
- * \brief the name of the casters in the xml file
- */
- std::string xml_caster_name_;
-
- /*!
- * \brief the name of the wheels in the xml file
- */
- std::string xml_wheel_name_;
-
- /*!
- * \brief name of this BaseKinematics (generally associated with whatever controller is using it)
- */
- std::string name_;
-
- /*!
- * \brief maximum dT used in computation of interpolated velocity command
- */
- double MAX_DT_;
- };
+ /*!
+ * \brief JointState for this wheel joint
+ */
+ mechanism::JointState *joint_;
+
+ /*!
+ * \brief default offset from the parent caster before any transformations
+ */
+ robot_msgs::Point offset_;
+
+ /*!
+ * \brief name of the joint
+ */
+ std::string name_;
+ /*!
+ * \brief offset_ after rotation transformation from the parent caster's position
+ */
+ robot_msgs::Point position_;
+
+ /*!
+ * \brief the caster on which this wheel is mounted
+ */
+ Caster *parent_;
+
+ /*!
+ * \brief actual wheel speeds
+ */
+ double wheel_speed_actual_;
+
+ /*!
+ * \brief desired wheel speed
+ */
+ double wheel_speed_cmd_;
+
+ /*!
+ * \brief difference between desired and actual speed
+ */
+ double wheel_speed_error_;
+
+ /*!
+ * \brief wheel speed filtered with alpha
+ */
+ double wheel_speed_filtered_;
+
+ /*!
+ * \brief specifies the default direction of the wheel
+ */
+ int direction_multiplier_;
+
+ /*!
+ * \brief remembers if the wheel is stalled
+ */
+ int wheel_stuck_;
+
+ /*!
+ * \brief wheel radius scale (based on the default wheel radius in Basekinematics)
+ */
+ double wheel_radius_scaler_;
+
+ /*!
+ * \brief Loads wheel's information from the xml description file and param server
+ * @param robot_state The robot's current state
+ * @param config Tiny xml element pointing to this wheel
+ */
+ void initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
+
+ /*!
+ * \brief Computes 2d postion of the wheel relative to the center of the base
+ */
+ void updatePosition();
+ };
+
+ /*! \class
+ \brief This class keeps track of the casters
+ */
+ class Caster
+ {
+ public:
+
+ /*!
+ * \brief JointState for this caster joint
+ */
+ mechanism::JointState *joint_;
+
+ /*!
+ * \brief offset from the center of the base
+ */
+ robot_msgs::Point offset_;
+
+ /*!
+ * \brief name of the joint
+ */
+ std::string name_;
+ //robot_msgs::Point position_;
+
+ /*!
+ * \brief BaseKinematics to which this caster belongs
+ */
+ BaseKinematics *parent_;
+
+ /*!
+ * \brief the number of child wheels that are attached to this caster
+ */
+ int num_children_;
+
+ /*!
+ * \brief actual caster steer angles
+ */
+ double steer_angle_actual_;
+
+ /*!
+ * \brief vector of desired caster steer speeds
+ */
+ double steer_velocity_desired_;
+
+ /*!
+ * \brief stored caster steer angles
+ */
+ double steer_angle_stored_;
+
+ /*!
+ * \brief difference between desired and actual angles of the casters
+ */
+ double caster_position_error_;
+
+ /*!
+ * \brief difference between desired and actual speeds of the casters
+ */
+ double caster_speed_error_;
+
+ /*!
+ * \brief caster speed filtered with alpha
+ */
+ double caster_speed_filtered_;
+
+ /*!
+ * \brief remembers the caster's current speed
+ */
+ double caster_speed_;
+
+ /*!
+ * \brief remembers if the caster is stalled
+ */
+ int caster_stuck_;
+
+ /*!
+ * \brief Loads caster's information from the xml description file and param server
+ * @param robot_state The robot's current state
+ * @param config Tiny xml element pointing to this caster
+ */
+ void initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
+ };
+
+ /*! \class
+ \brief This class includes common functions used by the base controller and odometry
+ */
+ class BaseKinematics
+ {
+ public:
+
+ /*!
+ * \brief Loads BaseKinematic's information from the xml description file and param server
+ * @param robot_state The robot's current state
+ * @param config Tiny xml element pointing to its controller
+ * @return Successful init
+ */
+ bool initXml(mechanism::RobotState *robot_state, TiXmlElement *config);
+
+ /*!
+ * \brief Computes 2d postion of every wheel relative to the center of the base
+ */
+ void computeWheelPositions();
+
+ /*!
+ * \brief Computes 2d velocity of a point at relative distance pos to another point with velocity (and rotation (z)) vel
+ * @param pos The position of the object relative to the center of rotation
+ * @param vel Velocity of the center of rotation
+ * @return Velocity at the given point
+ */
+ robot_msgs::PoseDot pointVel2D(const robot_msgs::Point& pos, const robot_msgs::PoseDot& vel);
+
+ /*!
+ * \brief remembers everything about the state of the robot
+ */
+ mechanism::RobotState *robot_state_;
+
+ /*!
+ * \brief number of wheels connected to the base
+ */
+ int num_wheels_;
+
+ /*!
+ * \brief number of casters connected to the base
+ */
+ int num_casters_;
+
+ /*!
+ * \brief vector of every wheel attached to the base
+ */
+ std::vector<Wheel> wheel_;
+
+ /*!
+ * \brief vector of every caster attached to the base
+ */
+ std::vector<Caster> caster_;
+
+ /*!
+ * \brief default radius of each wheel
+ */
+ double wheel_radius_;
+
+ /*!
+ * \brief the name of the casters in the xml file
+ */
+ std::string xml_caster_name_;
+
+ /*!
+ * \brief the name of the wheels in the xml file
+ */
+ std::string xml_wheel_name_;
+
+ /*!
+ * \brief name of this BaseKinematics (generally associated with whatever controller is using it)
+ */
+ std::string name_;
+
+ /*!
+ * \brief maximum dT used in computation of interpolated velocity command
+ */
+ double MAX_DT_;
+ };
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h 2009-07-06 20:23:46 UTC (rev 18337)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h 2009-07-06 20:30:57 UTC (rev 18338)
@@ -38,6 +38,7 @@
#include <ros/node.h>
#include <realtime_tools/realtime_publisher.h>
#include <pr2_msgs/BaseControllerState.h>
+#include <pr2_msgs/BaseCmdVelRad.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
#include <pr2_mechanism_controllers/base_kinematics.h>
#include <robot_msgs/PoseDDot.h>
@@ -263,11 +264,26 @@
void CmdBaseVelReceived();
/*!
+ * \brief deal with cmd_vel command from 2dnav stack
+ */
+ void CmdBaseVelRadReceived();
+
+ /*!
+ * \brief converts from a turn radius and velocity (along tangent) to velocities in vx, vy and ang_vz
+ */
+ robot_msgs::PoseDot convertBaseVel(pr2_msgs::BaseCmdVelRad cmd_vel_rad);
+
+ /*!
* \brief callback message, used to remember where the base is commanded to go
*/
robot_msgs::PoseDot baseVelMsg;
/*!
+ * \brief callback message, used to remember where the base is commanded to go
+ */
+ pr2_msgs::BaseCmdVelRad baseVelRadMsg;
+
+ /*!
* \brief generic epsilon value that is used as a minimum or maximum allowable input value
*/
double eps_;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp 2009-07-06 20:23:46 UTC (rev 18337)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp 2009-07-06 20:30:57 UTC (rev 18338)
@@ -31,14 +31,14 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
- /*
+/*
* Author: Sachin Chitta and Matthew Piccoli
*/
-
- #include <pr2_mechanism_controllers/base_kinematics.h>
-
- using namespace controller;
-
+
+#include <pr2_mechanism_controllers/base_kinematics.h>
+
+using namespace controller;
+
void Wheel::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
{
wheel_stuck_ = 0;
@@ -55,11 +55,9 @@
offset_.x = offset.getOrigin().x();
offset_.y = offset.getOrigin().y();
offset_.z = offset.getOrigin().z();
- ros::Node::instance()->param<double>(name_ + "/wheel_radius_scaler",wheel_radius_scaler_,1.0);
- #if 0
- ROS_DEBUG("Loading wheel: %s",name_.c_str());
- ROS_DEBUG("offset_.x: %f, offset_.y: %f, offset_.z: %f", offset_.x, offset_.y, offset_.z);
- #endif
+ ros::Node::instance()->param<double> (name_ + "/wheel_radius_scaler", wheel_radius_scaler_, 1.0);
+ ROS_DEBUG("Loading wheel: %s",name_.c_str());
+ ROS_DEBUG("offset_.x: %f, offset_.y: %f, offset_.z: %f", offset_.x, offset_.y, offset_.z);
}
void Caster::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
@@ -73,8 +71,7 @@
steer_velocity_desired_ = 0;
steer_angle_actual_ = 0;
num_children_ = 0;
-
- name_ = config->Attribute("name");
+
mechanism::Link *link = robot_state->model_->getLink(config->Attribute("name"));
name_ = link->joint_name_;
joint_ = robot_state->getJointState(name_);
@@ -86,10 +83,11 @@
ROS_DEBUG("Loading caster: %s: my parent is: %s",name_.c_str(), parent_->name_.c_str());
ROS_DEBUG("offset_.x: %f, offset_.y: %f, offset_.z: %f", offset_.x, offset_.y, offset_.z);
- while(elt){
+ while(elt)
+ {
Wheel tmp;
parent_->wheel_.push_back(tmp);
- parent_->wheel_[parent_->num_wheels_].initXml(robot_state,elt);
+ parent_->wheel_[parent_->num_wheels_].initXml(robot_state, elt);
elt = elt->NextSiblingElement(parent_->xml_wheel_name_);
parent_->num_wheels_++;
num_children_++;
@@ -103,29 +101,30 @@
MAX_DT_ = 0.01;
num_wheels_ = 0;
num_casters_ = 0;
-
+
//Add parameters
- ros::Node::instance()->param<std::string>("~xml_wheel_name",xml_wheel_name_,"wheel");
- ros::Node::instance()->param<std::string>("~xml_caster_name",xml_caster_name_,"caster");
- ros::Node::instance()->param<double>("~wheel_radius",wheel_radius_,0.074792);
+ ros::Node::instance()->param<std::string> ("~xml_wheel_name", xml_wheel_name_, "wheel");
+ ros::Node::instance()->param<std::string> ("~xml_caster_name", xml_caster_name_, "caster");
+ ros::Node::instance()->param<double> ("~wheel_radius", wheel_radius_, 0.074792);
double multiplier;
- ros::Node::instance()->param<double>(name_ + "/wheel_radius_multiplier",multiplier,1.0);
- wheel_radius_ = wheel_radius_*multiplier;
-
+ ros::Node::instance()->param<double> (name_ + "/wheel_radius_multiplier", multiplier, 1.0);
+ wheel_radius_ = wheel_radius_ * multiplier;
+
TiXmlElement *elt = config->FirstChildElement(xml_caster_name_);
- while(elt){
+ while(elt)
+ {
Caster tmp;
caster_.push_back(tmp);
caster_[num_casters_].parent_ = this;
- caster_[num_casters_].initXml(robot_state,elt);
+ caster_[num_casters_].initXml(robot_state, elt);
elt = elt->NextSiblingElement(xml_caster_name_);
num_casters_++;
}
int wheel_counter = 0;
- for(int j=0; j < num_casters_; j++)
+ for(int j = 0; j < num_casters_; j++)
{
- for(int i = 0; i< caster_[j].num_children_; i++)
+ for(int i = 0; i < caster_[j].num_children_; i++)
{
wheel_[wheel_counter].parent_ = &(caster_[j]);
wheel_counter++;
@@ -140,18 +139,18 @@
robot_msgs::Point result = parent_->offset_;
double costh = cos(parent_->joint_->position_);
double sinth = sin(parent_->joint_->position_);
- result.x += costh*offset_.x-sinth*offset_.y;
- result.y += sinth*offset_.x+costh*offset_.y;
+ result.x += costh * offset_.x - sinth * offset_.y;
+ result.y += sinth * offset_.x + costh * offset_.y;
result.z = 0.0;
position_ = result;
}
void BaseKinematics::computeWheelPositions()
{
- for(int i=0; i < num_wheels_; i++)
- {
- wheel_[i].updatePosition();
- }
+ for(int i = 0; i < num_wheels_; i++)
+ {
+ wheel_[i].updatePosition();
+ }
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-07-06 20:23:46 UTC (rev 18337)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-07-06 20:30:57 UTC (rev 18338)
@@ -101,27 +101,17 @@
ros::Node::instance()->param<double> (base_kin_.name_ + "/kp_wheel_steer_", kp_wheel_steer_, 2.0);
ros::Node::instance()->param<double> (base_kin_.name_ + "/alpha_stall", alpha_stall_, 0.5);
ros::Node::instance()->param<double> (base_kin_.name_ + "/kp_caster_steer_", kp_caster_steer_, 40.0);
- ros::Node::instance()->param<double> (base_kin_.name_ + "/timeout", timeout_, 1.0);
+ ros::Node::instance()->param<double> (base_kin_.name_ + "/timeout", timeout_, 0.1);
ros::Node::instance()->param<double> (base_kin_.name_ + "/eps", eps_, 1e-5);
ros::Node::instance()->param<double> (base_kin_.name_ + "/cmd_vel_trans_eps", cmd_vel_trans_eps_, 1e-5);
ros::Node::instance()->param<double> (base_kin_.name_ + "/cmd_vel_rot_eps", cmd_vel_rot_eps_, 1e-5);
-
- /*double kp_;
- double ki_;
- double kd_;
- double ki_clamp;*/
+ ROS_INFO("timeout is %f, base_kin_.name is %s", timeout_, base_kin_.name_.c_str());
//casters
caster_controller_.resize(base_kin_.num_casters_);
for(int i = 0; i < base_kin_.num_casters_; i++)
{
control_toolbox::Pid p_i_d;
state_publisher_->msg_.joint_name[i] = base_kin_.caster_[i].name_;
- /*ros::Node::instance()->param<double>(base_kin_.caster_[i].name_ + "/p",kp_,3.0);
- ros::Node::instance()->param<double>(base_kin_.caster_[i].name_ + "/i",ki_,0.1);
- ros::Node::instance()->param<double>(base_kin_.caster_[i].name_ + "/d",kd_,0.0);
- ros::Node::instance()->param<double>(base_kin_.caster_[i].name_ + "/i_clamp",ki_clamp,4.0);*/
- //tmp.init(base_kin_.robot_state_, base_kin_.caster_[i].name_ + "joint", control_toolbox::Pid(kp_,ki_,kd_,ki_clamp));
- //caster_controller_.push_back(tmp);//TODO::make this not copy!!!!!!!
p_i_d.initParam(base_kin_.name_ + "/" + base_kin_.caster_[i].name_ + "/");
caster_controller_[i] = new JointVelocityController();
caster_controller_[i]->init(base_kin_.robot_state_, base_kin_.caster_[i].name_, p_i_d);
@@ -132,12 +122,6 @@
{
control_toolbox::Pid p_i_d;
state_publisher_->msg_.joint_name[j + base_kin_.num_casters_] = base_kin_.wheel_[j].name_;
- /*ros::Node::instance()->param<double>(base_kin_.wheel_[j].name_ + "/kp",kp_,2.0);
- ros::Node::instance()->param<double>(base_kin_.wheel_[j].name_ + "/ki",ki_,0.01);
- ros::Node::instance()->param<double>(base_kin_.wheel_[j].name_ + "/kd",kd_,0.0);
- ros::Node::instance()->param<double>(base_kin_.wheel_[j].name_ + "/i_clamp",ki_clamp,0.4);*/
- //tmp.init(base_kin_.robot_state_, base_kin_.wheel_[j].name_ + "joint", control_toolbox::Pid(kp_,ki_,kd_,ki_clamp));
- //wheel_controller_.push_back(tmp);//TODO::make this not copy!!!!!!!
p_i_d.initParam(base_kin_.name_ + "/" + base_kin_.wheel_[j].name_ + "/");
wheel_controller_[j] = new JointVelocityController();
wheel_controller_[j]->init(base_kin_.robot_state_, base_kin_.wheel_[j].name_, p_i_d);
@@ -242,6 +226,7 @@
{
if(!base_kin_.caster_[i].joint_->calibrated_)
{
+ ROS_WARN("casters are not calibrated");
return; // Casters are not calibrated
}
}
Modified: pkg/trunk/sandbox/pr2_gripper_controller/src/teleop_gripper_keyboard.cpp
===================================================================
--- pkg/trunk/sandbox/pr2_gripper_controller/src/teleop_gripper_keyboard.cpp 2009-07-06 20:23:46 UTC (rev 18337)
+++ pkg/trunk/sandbox/pr2_gripper_controller/src/teleop_gripper_keyboard.cpp 2009-07-06 20:30:57 UTC (rev 18338)
@@ -40,7 +40,7 @@
#include <termios.h>
#include <pr2_mechanism_controllers/GripperControllerCmd.h>
-//TODO::would these #defines conflict with others? Change to const int?
+//TODO::#ifndef PACKAGE_PATH_FILE_H me!
#define KEYCODE_0 0x30
#define KEYCODE_1 0x31
#define KEYCODE_2 0x32
Added: pkg/trunk/sandbox/teleop_anti_collision/.build_version
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/.build_version (rev 0)
+++ pkg/trunk/sandbox/teleop_anti_collision/.build_version 2009-07-06 20:30:57 UTC (rev 18338)
@@ -0,0 +1 @@
+:
Added: pkg/trunk/sandbox/teleop_anti_collision/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/CMakeLists.txt (rev 0)
+++ pkg/trunk/sandbox/teleop_anti_collision/CMakeLists.txt 2009-07-06 20:30:57 UTC (rev 18338)
@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+set(ROS_BUILD_TYPE Release)
+rospack(teleop_anti_collision)
+genmsg()
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+
+#rospack_add_boost_directories()
+
+rospack_add_executable(teleop_goal_projection src/teleop_goal_projection.cpp)
Added: pkg/trunk/sandbox/teleop_anti_collision/Makefile
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/Makefile (rev 0)
+++ pkg/trunk/sandbox/teleop_anti_collision/Makefile 2009-07-06 20:30:57 UTC (rev 18338)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/sandbox/teleop_anti_collision/launch/base_odom_teleop.xml
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/launch/base_odom_teleop.xml (rev 0)
+++ pkg/trunk/sandbox/teleop_anti_collision/launch/base_odom_teleop.xml 2009-07-06 20:30:57 UTC (rev 18338)
@@ -0,0 +1,95 @@
+<launch>
+ <param name="base_controller/autostart" value="true"/>
+ <!--node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" output="screen"/-->
+ <param name="base_controller/odom_publish_rate" value="10" />
+ <param name="pr2_base_controller/timeout" value="0.2" />
+ <param name="pr2_base_controller/fl_caster_rotation_joint/p" value="3.0" />
+ <param name="pr2_base_controller/fr_caster_rotation_joint/p" value="3.0" />
+ <param name="pr2_base_controller/br_caster_rotation_joint/p" value="3.0" />
+ <param name="pr2_base_controller/bl_caster_rotation_joint/p" value="3.0" />
+ <param name="pr2_base_controller/fl_caster_rotation_joint/i" value="0.1" />
+ <param name="pr2_base_controller/fr_caster_rotation_joint/i" value="0.1" />
+ <param name="pr2_base_controller/br_caster_rotation_joint/i" value="0.1" />
+ <param name="pr2_base_controller/bl_caster_rotation_joint/i" value="0.1" />
+ <param name="pr2_base_controller/fl_caster_rotation_joint/d" value="0.0" />
+ <param name="pr2_base_controller/fr_caster_rotation_joint/d" value="0.0" />
+ <param name="pr2_base_controller/br_caster_rotation_joint/d" value="0.0" />
+ <param name="pr2_base_controller/bl_caster_rotation_joint/d" value="0.0" />
+ <param name="pr2_base_controller/fl_caster_rotation_joint/i_clamp" value="4.0" />
+ <param name="pr2_base_controller/fr_caster_rotation_joint/i_clamp" value="4.0" />
+ <param name="pr2_base_controller/br_caster_rotation_joint/i_clamp" value="4.0" />
+ <param name="pr2_base_controller/bl_caster_rotation_joint/i_clamp" value="4.0" />
+ <param name="pr2_base_controller/fl_caster_l_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/fl_caster_r_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/fr_caster_l_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/fr_caster_r_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/bl_caster_l_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/bl_caster_r_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/br_caster_l_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/br_caster_r_wheel_joint/p" value="2.0" />
+ <param name="pr2_base_controller/fl_caster_l_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/fl_caster_r_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/fr_caster_l_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/fr_caster_r_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/bl_caster_l_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/bl_caster_r_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/br_caster_l_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/br_caster_r_wheel_joint/i" value="0.01" />
+ <param name="pr2_base_controller/fl_caster_l_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/fl_caster_r_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/fr_caster_l_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/fr_caster_r_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/bl_caster_l_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/bl_caster_r_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/br_caster_l_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/br_caster_r_wheel_joint/d" value="0.0" />
+ <param name="pr2_base_controller/fl_caster_l_wheel_joint/i_clamp" value="0.4" />
+ <param name="pr2_base_controller/fl_caster_r_wheel_joint/i_clamp" value="0.4" />
+ <param name="pr2_base_controller/fr_caster_l_wheel_joint/i_clamp" value="0.4" />
+ <param name="pr2_base_controller/fr_caster_...
[truncated message content] |
|
From: <is...@us...> - 2009-07-06 21:32:45
|
Revision: 18345
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18345&view=rev
Author: isucan
Date: 2009-07-06 21:32:42 +0000 (Mon, 06 Jul 2009)
Log Message:
-----------
fixed the most awesome bug I have *ever* seen
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
pkg/trunk/util/robot_self_filter/CMakeLists.txt
pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/util/robot_self_filter/manifest.xml
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/mapping/collision_map/src/collision_map_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-07-06 21:31:39 UTC (rev 18344)
+++ pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-07-06 21:32:42 UTC (rev 18345)
@@ -190,7 +190,7 @@
bi_.real_maxZ = bi_.dimensionZ + bi_.originZ;
}
- void computeCloudMask(const robot_msgs::PointCloud &cloud, std::vector<bool> &mask)
+ void computeCloudMask(const robot_msgs::PointCloud &cloud, std::vector<int> &mask)
{
if (cloud_annotation_.empty())
sf_.mask(cloud, mask);
@@ -223,7 +223,7 @@
if (!mapProcessing_.try_lock())
return;
- std::vector<bool> mask;
+ std::vector<int> mask;
robot_msgs::PointCloud out;
ros::WallTime tm = ros::WallTime::now();
@@ -247,7 +247,7 @@
}
CMap obstacles;
- constructCollisionMap(out, mask, true, obstacles);
+ constructCollisionMap(out, mask, 1, obstacles);
CMap diff;
set_difference(obstacles.begin(), obstacles.end(), currentMap_.begin(), currentMap_.end(),
std::inserter(diff, diff.begin()), CollisionPointOrder());
@@ -260,7 +260,7 @@
{
mapProcessing_.lock();
- std::vector<bool> mask;
+ std::vector<int> mask;
robot_msgs::PointCloud out;
ros::WallTime tm = ros::WallTime::now();
@@ -292,11 +292,11 @@
#pragma omp section
{
- constructCollisionMap(out, mask, true, obstacles);
+ constructCollisionMap(out, mask, 1, obstacles);
}
#pragma omp section
{
- constructCollisionMap(out, mask, false, self);
+ constructCollisionMap(out, mask, 0, self);
}
#pragma omp section
{
@@ -553,7 +553,7 @@
}
/** Construct an axis-aligned collision map from a point cloud assumed to be in the robot frame */
- void constructCollisionMap(const robot_msgs::PointCloud &cloud, const std::vector<bool> &mask, bool keep, CMap &map)
+ void constructCollisionMap(const robot_msgs::PointCloud &cloud, const std::vector<int> &mask, int keep, CMap &map)
{
const unsigned int n = cloud.pts.size();
CollisionPoint c;
Modified: pkg/trunk/util/robot_self_filter/CMakeLists.txt
===================================================================
--- pkg/trunk/util/robot_self_filter/CMakeLists.txt 2009-07-06 21:31:39 UTC (rev 18344)
+++ pkg/trunk/util/robot_self_filter/CMakeLists.txt 2009-07-06 21:32:42 UTC (rev 18345)
@@ -12,13 +12,13 @@
rospack(robot_self_filter)
-rospack_add_library(new_robot_self_filter src/self_mask.cpp)
-rospack_add_openmp_flags(new_robot_self_filter)
+rospack_add_library(robot_self_filter src/self_mask.cpp)
+rospack_add_openmp_flags(robot_self_filter)
rospack_add_executable(test_filter src/test_filter.cpp)
rospack_add_openmp_flags(test_filter)
-target_link_libraries(test_filter new_robot_self_filter)
+target_link_libraries(test_filter robot_self_filter)
rospack_add_executable(self_filter src/self_filter.cpp)
rospack_add_openmp_flags(self_filter)
-target_link_libraries(self_filter new_robot_self_filter)
+target_link_libraries(self_filter robot_self_filter)
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-06 21:31:39 UTC (rev 18344)
+++ pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h 2009-07-06 21:32:42 UTC (rev 18345)
@@ -70,6 +70,12 @@
{
configure();
}
+
+ /** \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);
+ }
/** \brief Destructor to clean up
*/
@@ -83,14 +89,14 @@
/** \brief Compute the mask for a given pointcloud. If a mask element is true, the point
is outside the robot
*/
- void mask(const robot_msgs::PointCloud& data_in, std::vector<bool> &mask);
+ void mask(const robot_msgs::PointCloud& data_in, std::vector<int> &mask);
/** \brief Assume subsequent calls to getMask() will be in the frame passed to this function */
void assumeFrame(const roslib::Header& header);
/** \brief Get the mask value for an individual point. No
setup is performed, assumeFrame() should be called before use */
- bool getMask(double x, double y, double z) const;
+ 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;
@@ -100,11 +106,14 @@
/** \brief Configure the filter. */
bool configure(void);
+ /** \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. */
void computeBoundingSpheres(void);
/** \brief Perform the actual mask computation. */
- void maskSimple(const robot_msgs::PointCloud& data_in, std::vector<bool> &mask);
+ void maskAux(const robot_msgs::PointCloud& data_in, std::vector<int> &mask);
planning_environment::RobotModels rm_;
tf::TransformListener &tf_;
Modified: pkg/trunk/util/robot_self_filter/manifest.xml
===================================================================
--- pkg/trunk/util/robot_self_filter/manifest.xml 2009-07-06 21:31:39 UTC (rev 18344)
+++ pkg/trunk/util/robot_self_filter/manifest.xml 2009-07-06 21:32:42 UTC (rev 18345)
@@ -18,7 +18,7 @@
<depend package="planning_environment"/>
<export>
- <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lnew_robot_self_filter" />
+ <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lrobot_self_filter" />
</export>
</package>
Modified: pkg/trunk/util/robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_filter.cpp 2009-07-06 21:31:39 UTC (rev 18344)
+++ pkg/trunk/util/robot_self_filter/src/self_filter.cpp 2009-07-06 21:32:42 UTC (rev 18345)
@@ -58,7 +58,7 @@
void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
{
robot_msgs::PointCloud out;
- std::vector<bool> mask;
+ std::vector<int> mask;
ros::WallTime tm = ros::WallTime::now();
sf_.mask(*cloud, mask);
double sec = (ros::WallTime::now() - tm).toSec();
@@ -67,7 +67,7 @@
ROS_DEBUG("Self filter: reduced %d points to %d points in %f seconds", (int)cloud->pts.size(), (int)out.pts.size(), sec);
}
- void fillResult(const robot_msgs::PointCloud& data_in, const std::vector<bool> &keep, robot_msgs::PointCloud& data_out)
+ void fillResult(const robot_msgs::PointCloud& data_in, const std::vector<int> &keep, robot_msgs::PointCloud& data_out)
{
const unsigned int np = data_in.pts.size();
Modified: pkg/trunk/util/robot_self_filter/src/self_mask.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_mask.cpp 2009-07-06 21:31:39 UTC (rev 18344)
+++ pkg/trunk/util/robot_self_filter/src/self_mask.cpp 2009-07-06 21:32:42 UTC (rev 18345)
@@ -32,12 +32,8 @@
#include <climits>
#include <ros/console.h>
-bool robot_self_filter::SelfMask::configure(void)
+bool robot_self_filter::SelfMask::configure(const std::vector<std::string> &links, double scale, double padd)
{
- std::vector<std::string> links = rm_.getSelfSeeLinks();
- double scale = rm_.getSelfSeeScale();
- double padd = rm_.getSelfSeePadding();
-
// from the geometric model, find the shape of each link of interest
// and create a body from it, one that knows about poses and can
// check for point inclusion
@@ -72,22 +68,27 @@
for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
ROS_INFO("Self mask includes link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].body->computeVolume());
- return true;
+ return true;
}
+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
{
for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
frames.push_back(bodies_[i].name);
}
-void robot_self_filter::SelfMask::mask(const robot_msgs::PointCloud& data_in, std::vector<bool> &mask)
+void robot_self_filter::SelfMask::mask(const robot_msgs::PointCloud& data_in, std::vector<int> &mask)
{
mask.resize(data_in.pts.size());
if (bodies_.empty())
std::fill(mask.begin(), mask.end(), true);
else
- maskSimple(data_in, mask);
+ maskAux(data_in, mask);
}
void robot_self_filter::SelfMask::computeBoundingSpheres(void)
@@ -123,7 +124,7 @@
computeBoundingSpheres();
}
-void robot_self_filter::SelfMask::maskSimple(const robot_msgs::PointCloud& data_in, std::vector<bool> &mask)
+void robot_self_filter::SelfMask::maskAux(const robot_msgs::PointCloud& data_in, std::vector<int> &mask)
{
const unsigned int bs = bodies_.size();
const unsigned int np = data_in.pts.size();
@@ -136,25 +137,27 @@
btScalar radiusSquared = bound.radius * bound.radius;
// we now decide which points we keep
-#pragma omp parallel for schedule(dynamic)
+#pragma omp parallel for schedule(dynamic)
for (int i = 0 ; i < (int)np ; ++i)
{
btVector3 pt = btVector3(btScalar(data_in.pts[i].x), btScalar(data_in.pts[i].y), btScalar(data_in.pts[i].z));
- bool out = true;
+ int out = 1;
if (bound.center.distance2(pt) < radiusSquared)
for (unsigned int j = 0 ; out && j < bs ; ++j)
- out = !bodies_[j].body->containsPoint(pt);
+ if (bodies_[j].body->containsPoint(pt))
+ out = 0;
mask[i] = out;
}
}
-bool robot_self_filter::SelfMask::getMask(double x, double y, double z) const
+int robot_self_filter::SelfMask::getMask(double x, double y, double z) const
{
btVector3 pt = btVector3(btScalar(x), btScalar(y), btScalar(z));
const unsigned int bs = bodies_.size();
- bool out = true;
+ int out = 1;
for (unsigned int j = 0 ; out && j < bs ; ++j)
- out = !bodies_[j].body->containsPoint(pt);
+ if (bodies_[j].body->containsPoint(pt))
+ out = 0;
return out;
}
Modified: pkg/trunk/util/robot_self_filter/src/test_filter.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/test_filter.cpp 2009-07-06 21:31:39 UTC (rev 18344)
+++ pkg/trunk/util/robot_self_filter/src/test_filter.cpp 2009-07-06 21:32:42 UTC (rev 18345)
@@ -42,11 +42,18 @@
{
public:
- TestSelfFilter(void) : sf_(tf_)
+ TestSelfFilter(void) : rm_("robot_description")
{
id_ = 1;
vmPub_ = nodeHandle_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
+ std::vector<std::string> links = rm_.getCollisionCheckLinks();
+ sf_ = new robot_self_filter::SelfMask(tf_, links, 1.0, 0.0);
}
+
+ ~TestSelfFilter(void)
+ {
+ delete sf_;
+ }
void sendPoint(double x, double y, double z)
{
@@ -65,7 +72,7 @@
mk.pose.position.z = z;
mk.pose.orientation.w = 1.0;
- mk.scale.x = mk.scale.y = mk.scale.z = 0.02;
+ mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
mk.color.a = 1.0;
mk.color.r = 1.0;
@@ -84,7 +91,7 @@
in.chan.resize(1);
in.chan[0].name = "stamps";
- const unsigned int N = 100000;
+ const unsigned int N = 500000;
in.pts.resize(N);
in.chan[0].vals.resize(N);
for (unsigned int i = 0 ; i < N ; ++i)
@@ -102,18 +109,26 @@
}
ros::WallTime tm = ros::WallTime::now();
- std::vector<bool> mask;
- sf_.mask(in, mask);
+ std::vector<int> mask;
+ sf_->mask(in, mask);
printf("%f points per second\n", (double)N/(ros::WallTime::now() - tm).toSec());
+ sf_->assumeFrame(in.header);
+ int k = 0;
for (unsigned int i = 0 ; i < mask.size() ; ++i)
{
- if (!mask[i])
- sendPoint(in.pts[i].x, in.pts[i].y, in.pts[i].z);
+ bool v = sf_->getMask(in.pts[i].x, in.pts[i].y, in.pts[i].z);
+ if (v != mask[i])
+ ROS_ERROR("Mask does not match");
+ if (mask[i]) continue;
+ sendPoint(in.pts[i].x, in.pts[i].y, in.pts[i].z);
+ k++;
}
+ printf("%d points inside\n", k);
ros::spin();
}
+
protected:
double uniform(double magnitude)
@@ -121,11 +136,12 @@
return (2.0 * drand48() - 1.0) * magnitude;
}
- tf::TransformListener tf_;
- robot_self_filter::SelfMask sf_;
- ros::Publisher vmPub_;
- ros::NodeHandle nodeHandle_;
- int id_;
+ tf::TransformListener tf_;
+ robot_self_filter::SelfMask *sf_;
+ planning_environment::RobotModels rm_;
+ ros::Publisher vmPub_;
+ ros::NodeHandle nodeHandle_;
+ int id_;
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2009-07-06 22:29:09
|
Revision: 18349
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18349&view=rev
Author: jfaustwg
Date: 2009-07-06 22:29:05 +0000 (Mon, 06 Jul 2009)
Log Message:
-----------
Wrap boost::signals::connection with our own message_filters::Connection class for two reasons:
1) boost::signals is not thread-safe, so calling disconnect() directly on the connection is not safe in our case. boost::signals2 is thread-safe, but did not come out until 1.39
2) Niceness of not exposing the boost::signals API to users
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp
pkg/trunk/sandbox/message_filters/CMakeLists.txt
pkg/trunk/sandbox/message_filters/include/message_filters/cache.h
pkg/trunk/sandbox/message_filters/include/message_filters/subscriber.h
pkg/trunk/sandbox/message_filters/include/message_filters/time_synchronizer.h
pkg/trunk/sandbox/message_filters/manifest.xml
pkg/trunk/sandbox/message_filters/test/CMakeLists.txt
pkg/trunk/stacks/geometry/tf/include/tf/message_filter.h
pkg/trunk/stacks/geometry/tf/manifest.xml
Added Paths:
-----------
pkg/trunk/sandbox/message_filters/include/message_filters/connection.h
pkg/trunk/sandbox/message_filters/src/connection.cpp
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h 2009-07-06 22:24:45 UTC (rev 18348)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/dense_laser_msg_filter.h 2009-07-06 22:29:05 UTC (rev 18349)
@@ -42,6 +42,7 @@
#include "dense_laser_assembler/joint_pv_msg_filter.h"
#include "dense_laser_assembler/laser_scan_tagger.h"
#include "message_filters/cache.h"
+#include "message_filters/connection.h"
// Messages
@@ -137,7 +138,7 @@
* \brief Used to connect this MessageFilter to a callback
* \param callback Function to be called whenever a new TaggedLaserScan message is available
*/
- boost::signals::connection connect(const Callback& callback) ;
+ message_filters::Connection connect(const Callback& callback) ;
//! \brief Not yet implemented
void processLaserScan(const sensor_msgs::LaserScanConstPtr& msg) ;
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h 2009-07-06 22:24:45 UTC (rev 18348)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/joint_pv_msg_filter.h 2009-07-06 22:29:05 UTC (rev 18349)
@@ -44,6 +44,8 @@
#include <boost/thread.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/signals.hpp>
+#include <boost/bind.hpp>
+#include <message_filters/connection.h>
// Messages
#include "dense_laser_assembler/JointPVArray.h"
@@ -100,10 +102,10 @@
* \brief Called by user wants they want to connect this MessageFilter to their own code
* \param callback Function to be called whenever a JointPVArray message is available
*/
- boost::signals::connection connect(const Callback& callback)
+ message_filters::Connection connect(const Callback& callback)
{
boost::mutex::scoped_lock lock(signal_mutex_);
- return signal_.connect(callback);
+ return message_filters::Connection(boost::bind(&JointPVMsgFilter::disconnect, this, _1), signal_.connect(callback));
}
/**
@@ -154,11 +156,17 @@
}
protected:
+ void disconnect(const message_filters::Connection& c)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ signal_.disconnect(c.getBoostConnection());
+ }
+
boost::mutex joint_mapping_mutex_ ;
std::vector<std::string> joint_names_ ;
// Filter Connection Stuff
- boost::signals::connection incoming_connection_;
+ message_filters::Connection incoming_connection_;
Signal signal_;
boost::mutex signal_mutex_;
} ;
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h 2009-07-06 22:24:45 UTC (rev 18348)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/include/dense_laser_assembler/laser_scan_tagger.h 2009-07-06 22:29:05 UTC (rev 18349)
@@ -139,10 +139,10 @@
* \brief Connect this message filter's output to some callback
* \param callback Function to call after we've tagged a LaserScan
*/
- boost::signals::connection connect(const Callback& callback)
+ message_filters::Connection connect(const Callback& callback)
{
boost::mutex::scoped_lock lock(signal_mutex_);
- return signal_.connect(callback);
+ return message_filters::Connection(boost::bind(&LaserScanTagger::disconnect, this, _1), signal_.connect(callback));
}
/**
@@ -213,14 +213,20 @@
}
private:
+ void disconnect(const message_filters::Connection& c)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ signal_.disconnect(c.getBoostConnection());
+ }
+
std::deque<sensor_msgs::LaserScanConstPtr> queue_ ; //!< Incoming queue of laser scans
boost::mutex queue_mutex_ ; //!< Mutex for laser scan queue
unsigned int max_queue_size_ ; //!< Max # of laser scans to queue up for processing
message_filters::Cache<T>* tag_cache_ ; //!< Cache of the tags that we need to merge with laser data
Signal signal_;
- boost::signals::connection incoming_laser_scan_connection_;
- boost::signals::connection incoming_tag_connection_;
+ message_filters::Connection incoming_laser_scan_connection_;
+ message_filters::Connection incoming_tag_connection_;
boost::mutex signal_mutex_;
} ;
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp 2009-07-06 22:24:45 UTC (rev 18348)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/joint_extractor_display.cpp 2009-07-06 22:29:05 UTC (rev 18349)
@@ -85,7 +85,7 @@
JointPVMsgFilter joint_extractor(joint_names) ;
//JointPVDiagMsgFilter joint_extractor(diagnostic, joint_names) ;
- boost::signals::connection con = joint_extractor.connect( boost::bind(&display_joints, joint_names, _1) ) ;
+ message_filters::Connection con = joint_extractor.connect( boost::bind(&display_joints, joint_names, _1) ) ;
ros::Subscriber sub = nh.subscribe("mechanism_state", 1, &JointPVMsgFilter::processMechState, &joint_extractor) ;
Modified: pkg/trunk/sandbox/message_filters/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/message_filters/CMakeLists.txt 2009-07-06 22:24:45 UTC (rev 18348)
+++ pkg/trunk/sandbox/message_filters/CMakeLists.txt 2009-07-06 22:29:05 UTC (rev 18349)
@@ -2,7 +2,14 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(message_filters)
+include_directories(include/message_filters)
+
+rospack_add_boost_directories()
+rospack_add_library(message_filters src/connection.cpp)
+rospack_link_boost(message_filters thread signals)
+
#rospack_add_executable(filter_example src/filter_example.cpp)
+#target_link_libraries(filter_example message_filters)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
Modified: pkg/trunk/sandbox/message_filters/include/message_filters/cache.h
===================================================================
--- pkg/trunk/sandbox/message_filters/include/message_filters/cache.h 2009-07-06 22:24:45 UTC (rev 18348)
+++ pkg/trunk/sandbox/message_filters/include/message_filters/cache.h 2009-07-06 22:29:05 UTC (rev 18349)
@@ -46,6 +46,8 @@
#include <boost/noncopyable.hpp>
#include "ros/time.h"
+#include "connection.h"
+
namespace message_filters
{
@@ -104,10 +106,10 @@
cache_size_ = cache_size ;
}
- boost::signals::connection connect(const Callback& callback)
+ Connection connect(const Callback& callback)
{
boost::mutex::scoped_lock lock(signal_mutex_);
- return signal_.connect(callback);
+ return Connection(boost::bind(&Cache::disconnect, this, _1), signal_.connect(callback));
}
/**
@@ -252,11 +254,17 @@
}
private:
+ void disconnect(const Connection& c)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ signal_.disconnect(c.getBoostConnection());
+ }
+
boost::mutex cache_lock_ ; //!< Lock for cache_
std::deque<MConstPtr > cache_ ; //!< Cache for the messages
unsigned int cache_size_ ; //!< Maximum number of elements allowed in the cache.
- boost::signals::connection incoming_connection_;
+ Connection incoming_connection_;
Signal signal_;
boost::mutex signal_mutex_;
};
Added: pkg/trunk/sandbox/message_filters/include/message_filters/connection.h
===================================================================
--- pkg/trunk/sandbox/message_filters/include/message_filters/connection.h (rev 0)
+++ pkg/trunk/sandbox/message_filters/include/message_filters/connection.h 2009-07-06 22:29:05 UTC (rev 18349)
@@ -0,0 +1,76 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, 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 MESSAGE_FILTERS_CONNECTION_H
+#define MESSAGE_FILTERS_CONNECTION_H
+
+#include <boost/thread/mutex.hpp>
+#include <boost/function.hpp>
+#include <boost/signals.hpp>
+
+namespace message_filters
+{
+
+class Connection
+{
+public:
+ typedef boost::function<void(const Connection&)> DisconnectFunction;
+
+ Connection();
+ Connection(const DisconnectFunction& func, boost::signals::connection conn);
+ Connection(const Connection& rhs);
+ Connection& operator=(const Connection& rhs);
+
+ void disconnect();
+
+ boost::signals::connection getBoostConnection() const;
+
+private:
+ class Impl
+ {
+ public:
+ Impl();
+ ~Impl();
+
+ DisconnectFunction disconnect_;
+ boost::signals::connection connection_;
+ };
+ typedef boost::shared_ptr<Impl> ImplPtr;
+
+ ImplPtr impl_;
+};
+
+}
+
+#endif // MESSAGE_FILTERS_CONNECTION_H
Modified: pkg/trunk/sandbox/message_filters/include/message_filters/subscriber.h
===================================================================
--- pkg/trunk/sandbox/message_filters/include/message_filters/subscriber.h 2009-07-06 22:24:45 UTC (rev 18348)
+++ pkg/trunk/sandbox/message_filters/include/message_filters/subscriber.h 2009-07-06 22:29:05 UTC (rev 18349)
@@ -41,9 +41,17 @@
#include <boost/thread/mutex.hpp>
#include <boost/noncopyable.hpp>
+#include "connection.h"
+
namespace message_filters
{
+/**
+ * \brief ROS subscription filter.
+ *
+ * This class acts as a highest-level filter, simply passing messages from a ROS subscription through to the
+ * filters which have connected to it.
+ */
template<class M>
class Subscriber : public boost::noncopyable
{
@@ -52,22 +60,49 @@
typedef boost::function<void(const MConstPtr&)> Callback;
typedef boost::signal<void(const MConstPtr&)> Signal;
+ /**
+ * \brief Constructor
+ *
+ * See the ros::NodeHandle::subscribe() variants for more information on the parameters
+ *
+ * \param nh The ros::NodeHandle to use to subscribe.
+ * \param topic The topic to subscribe to.
+ * \param queue_size The subscription queue size
+ * \param transport_hints The transport hints to pass along
+ * \param callback_queue The callback queue to pass along
+ */
Subscriber(ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size, const ros::TransportHints& transport_hints = ros::TransportHints(), ros::CallbackQueueInterface* callback_queue = 0)
{
subscribe(nh, topic, queue_size, transport_hints, callback_queue);
}
+ /**
+ * \brief Empty constructor, use subscribe() to subscribe to a topic
+ */
Subscriber()
{
}
~Subscriber()
{
- sub_.shutdown();
+ unsubscribe();
}
+ /**
+ * \brief Subscribe to a topic.
+ *
+ * If this Subscriber is already subscribed to a topic, this function will first unsubscribe.
+ *
+ * \param nh The ros::NodeHandle to use to subscribe.
+ * \param topic The topic to subscribe to.
+ * \param queue_size The subscription queue size
+ * \param transport_hints The transport hints to pass along
+ * \param callback_queue The callback queue to pass along
+ */
void subscribe(ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size, const ros::TransportHints& transport_hints = ros::TransportHints(), ros::CallbackQueueInterface* callback_queue = 0)
{
+ unsubscribe();
+
ros::SubscribeOptions ops;
ops.init<M>(topic, queue_size, boost::bind(&Subscriber<M>::cb, this, _1));
ops.callback_queue = callback_queue;
@@ -80,13 +115,25 @@
sub_.shutdown();
}
- boost::signals::connection connect(const Callback& callback)
+ /**
+ * \brief Connect a callback to this filter. That callback will be called whenever new data arrives.
+ *
+ * \param callback A function of the same form as the message callbacks used in roscpp
+ * \return A connection object that allows you to disconnect from this
+ */
+ Connection connect(const Callback& callback)
{
boost::mutex::scoped_lock lock(signal_mutex_);
- return signal_.connect(callback);
+ return Connection(boost::bind(&Subscriber::disconnect, this, _1), signal_.connect(callback));
}
private:
+ void disconnect(const Connection& c)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ signal_.disconnect(c.getBoostConnection());
+ }
+
void cb(const MConstPtr& m)
{
boost::mutex::scoped_lock lock(signal_mutex_);
Modified: pkg/trunk/sandbox/message_filters/include/message_filters/time_synchronizer.h
===================================================================
--- pkg/trunk/sandbox/message_filters/include/message_filters/time_synchronizer.h 2009-07-06 22:24:45 UTC (rev 18348)
+++ pkg/trunk/sandbox/message_filters/include/message_filters/time_synchronizer.h 2009-07-06 22:29:05 UTC (rev 18349)
@@ -46,6 +46,8 @@
#include <roslib/Header.h>
+#include "connection.h"
+
#define TIME_SYNCHRONIZER_MAX_MESSAGES 9
namespace message_filters
@@ -64,9 +66,9 @@
public:
typedef boost::shared_ptr<M const> MConstPtr;
typedef boost::function<void(const MConstPtr&)> Callback;
- boost::signals::connection connect(const Callback& cb)
+ Connection connect(const Callback& cb)
{
- return boost::signals::connection();
+ return Connection();
}
};
@@ -228,10 +230,10 @@
}
template<class C>
- boost::signals::connection connect(const C& callback)
+ Connection connect(const C& callback)
{
boost::mutex::scoped_lock lock(signal_mutex_);
- return signal_.connect(boost::bind(callback, _1, _2, _3, _4, _5, _6, _7, _8, _9));
+ return Connection(boost::bind(&TimeSynchronizer::disconnect, this, _1), signal_.connect(boost::bind(callback, _1, _2, _3, _4, _5, _6, _7, _8, _9)));
}
void add0(const M0ConstPtr& msg)
@@ -492,6 +494,12 @@
}
}
+ void disconnect(const Connection& c)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ signal_.disconnect(c.getBoostConnection());
+ }
+
uint32_t queue_size_;
typedef std::map<ros::Time, Tuple> M_TimeToTuple;
@@ -502,7 +510,7 @@
boost::mutex signal_mutex_;
ros::Time last_signal_time_;
- boost::signals::connection input_connections_[TIME_SYNCHRONIZER_MAX_MESSAGES];
+ Connection input_connections_[TIME_SYNCHRONIZER_MAX_MESSAGES];
uint32_t real_type_count_;
};
Modified: pkg/trunk/sandbox/message_filters/manifest.xml
===================================================================
--- pkg/trunk/sandbox/message_filters/manifest.xml 2009-07-06 22:24:45 UTC (rev 18348)
+++ pkg/trunk/sandbox/message_filters/manifest.xml 2009-07-06 22:29:05 UTC (rev 18349)
@@ -8,7 +8,7 @@
<depend package="robot_msgs" />
<depend package="sensor_msgs" />
<export>
- <cpp cflags="-I${prefix}/include" />
+ <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmessage_filters `rosboost-cfg --lflags thread,signals`" />
</export>
<url>http://pr.willowgarage.com/wiki/topic_synchronizer</url>
</package>
Added: pkg/trunk/sandbox/message_filters/src/connection.cpp
===================================================================
--- pkg/trunk/sandbox/message_filters/src/connection.cpp (rev 0)
+++ pkg/trunk/sandbox/message_filters/src/connection.cpp 2009-07-06 22:29:05 UTC (rev 18349)
@@ -0,0 +1,89 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, 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 "connection.h"
+
+namespace message_filters
+{
+
+Connection::Impl::Impl()
+{
+}
+
+Connection::Impl::~Impl()
+{
+}
+
+Connection::Connection()
+{
+}
+
+Connection::Connection(const Connection& rhs)
+{
+ impl_ = rhs.impl_;
+}
+
+Connection::Connection(const DisconnectFunction& func, boost::signals::connection conn)
+: impl_(new Impl)
+{
+ impl_->disconnect_ = func;
+ impl_->connection_ = conn;
+}
+
+Connection& Connection::operator=(const Connection& rhs)
+{
+ impl_ = rhs.impl_;
+
+ return *this;
+}
+
+void Connection::disconnect()
+{
+ if (impl_)
+ {
+ impl_->disconnect_(*this);
+ }
+}
+
+boost::signals::connection Connection::getBoostConnection() const
+{
+ if (impl_)
+ {
+ return impl_->connection_;
+ }
+
+ return boost::signals::connection();
+}
+
+}
Modified: pkg/trunk/sandbox/message_filters/test/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/message_filters/test/CMakeLists.txt 2009-07-06 22:24:45 UTC (rev 18348)
+++ pkg/trunk/sandbox/message_filters/test/CMakeLists.txt 2009-07-06 22:29:05 UTC (rev 18349)
@@ -2,4 +2,7 @@
# ********** Tests **********
rospack_add_gtest(test/msg_cache_unittest msg_cache_unittest.cpp)
+target_link_libraries(test/msg_cache_unittest message_filters)
+
rospack_add_gtest(test/time_synchronizer_unittest time_synchronizer_unittest.cpp)
+target_link_libraries(test/time_synchronizer_unittest message_filters)
Modified: pkg/trunk/stacks/geometry/tf/include/tf/message_filter.h
===================================================================
--- pkg/trunk/stacks/geometry/tf/include/tf/message_filter.h 2009-07-06 22:24:45 UTC (rev 18348)
+++ pkg/trunk/stacks/geometry/tf/include/tf/message_filter.h 2009-07-06 22:29:05 UTC (rev 18349)
@@ -49,6 +49,8 @@
#include <ros/callback_queue.h>
+#include <message_filters/connection.h>
+
#define TF_MESSAGEFILTER_DEBUG(fmt, ...) \
ROS_DEBUG_NAMED("message_filter", "MessageFilter [target=%s]: "fmt, getTargetFramesString().c_str(), __VA_ARGS__)
@@ -101,10 +103,7 @@
template<class F>
void subscribeTo(F& f)
{
- if (message_connection_.connected())
- {
- message_connection_.disconnect();
- }
+ message_connection_.disconnect();
message_connection_ = f.connect(boost::bind(&MessageFilter::incomingMessage, this, _1));
}
@@ -208,10 +207,10 @@
++incoming_message_count_;
}
- boost::signals::connection connect(const Callback& callback)
+ message_filters::Connection connect(const Callback& callback)
{
boost::mutex::scoped_lock lock(signal_mutex_);
- return signal_.connect(callback);
+ return message_filters::Connection(boost::bind(&MessageFilter::disconnect, this, _1), signal_.connect(callback));
}
private:
@@ -452,6 +451,12 @@
}
}
+ void disconnect(const message_filters::Connection& c)
+ {
+ boost::mutex::scoped_lock lock(signal_mutex_);
+ signal_.disconnect(c.getBoostConnection());
+ }
+
Transformer& tf_; ///< The Transformer used to determine if transformation data is available
ros::NodeHandle nh_; ///< The node used to subscribe to the topic
ros::Duration min_rate_;
@@ -489,7 +494,7 @@
ros::Duration time_tolerance_; ///< Provide additional tolerance on time for messages which are stamped but can have associated duration
boost::signals::connection tf_connection_;
- boost::signals::connection message_connection_;
+ message_filters::Connection message_connection_;
};
} // namespace tf
Modified: pkg/trunk/stacks/geometry/tf/manifest.xml
===================================================================
--- pkg/trunk/stacks/geometry/tf/manifest.xml 2009-07-06 22:24:45 UTC (rev 18348)
+++ pkg/trunk/stacks/geometry/tf/manifest.xml 2009-07-06 22:29:05 UTC (rev 18349)
@@ -15,6 +15,7 @@
<depend package="rospy"/>
<depend package="rostest"/>
<depend package="roswtf"/>
+ <depend package="message_filters"/>
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -ltf `rosboost-cfg --lflags thread`"/>
<roswtf plugin="tf.tfwtf" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-06 22:41:18
|
Revision: 18352
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18352&view=rev
Author: isucan
Date: 2009-07-06 22:41:13 +0000 (Mon, 06 Jul 2009)
Log Message:
-----------
update frame names
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-06 22:38:51 UTC (rev 18351)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-06 22:41:13 UTC (rev 18352)
@@ -96,7 +96,7 @@
for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size(); ++i)
{
goal.goal_constraints.joint_constraint[i].header.stamp = ros::Time::now();
- goal.goal_constraints.joint_constraint[i].header.frame_id = "base_link";
+ goal.goal_constraints.joint_constraint[i].header.frame_id = "/base_link";
goal.goal_constraints.joint_constraint[i].joint_name = names[i];
goal.goal_constraints.joint_constraint[i].value.resize(1);
goal.goal_constraints.joint_constraint[i].toleranceAbove.resize(1);
@@ -113,7 +113,7 @@
goal.goal_constraints.pose_constraint[0].type = motion_planning_msgs::PoseConstraint::POSITION_XYZ + motion_planning_msgs::PoseConstraint::ORIENTATION_RPY;
goal.goal_constraints.pose_constraint[0].link_name = link;
goal.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
- goal.goal_constraints.pose_constraint[0].pose.header.frame_id = "base_link";
+ goal.goal_constraints.pose_constraint[0].pose.header.frame_id = "/base_link";
goal.goal_constraints.pose_constraint[0].pose.pose.position.x = x;
goal.goal_constraints.pose_constraint[0].pose.pose.position.y = y;
goal.goal_constraints.pose_constraint[0].pose.pose.position.z = z;
Modified: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-07-06 22:38:51 UTC (rev 18351)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-07-06 22:41:13 UTC (rev 18352)
@@ -64,7 +64,7 @@
if (dynamic_cast<planning_models::KinematicModel::FloatingJoint*>(kmodel_->getRobot(0)->chain))
floatingJoint_ = kmodel_->getRobot(0)->chain->name;
- robot_frame_ = kmodel_->getRobot(0)->chain->after->name;
+ robot_frame_ = "/" + kmodel_->getRobot(0)->chain->after->name;
ROS_DEBUG("Robot frame is '%s'", robot_frame_.c_str());
if (includePose_)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-07-06 23:43:59
|
Revision: 18356
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18356&view=rev
Author: hsujohnhsu
Date: 2009-07-06 23:43:57 +0000 (Mon, 06 Jul 2009)
Log Message:
-----------
fix gazebo demo launch scripts.
Modified Paths:
--------------
pkg/trunk/demos/erratic_gazebo/erratic.launch
pkg/trunk/demos/examples_gazebo/dual_link.launch
pkg/trunk/demos/examples_gazebo/multi_link.launch
pkg/trunk/demos/examples_gazebo/single_link.launch
pkg/trunk/demos/examples_gazebo/table_defs/table_defs.xml
pkg/trunk/demos/pr2_gazebo/cups.launch
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/coffee_cup.model
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/desk1.model
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/desk2.model
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/desk3.model
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/desk4.model
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/desk5.model
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/desk6.model
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/object1.model
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/balcony.world
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/simple.world
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/simple_office.world
pkg/trunk/stacks/simulators/gazebo/launch/empty_world_no_x.launch
Added Paths:
-----------
pkg/trunk/demos/2dnav_gazebo/pr2-empty-amcl.launch
pkg/trunk/demos/2dnav_gazebo/pr2-empty-fake_localization.launch
pkg/trunk/demos/2dnav_gazebo/pr2-simple-fake_localization.launch
pkg/trunk/demos/2dnav_gazebo/pr2-wg-amcl.launch
pkg/trunk/demos/2dnav_gazebo/prototype1-simple-amcl.launch
pkg/trunk/demos/2dnav_gazebo/prototype1-wg-fake_localization.launch
pkg/trunk/demos/pr2_gazebo/coffee_cup.launch
pkg/trunk/demos/pr2_gazebo/cups2.launch
pkg/trunk/demos/pr2_gazebo/pr2.launch
pkg/trunk/demos/pr2_gazebo/prototype1.launch
pkg/trunk/demos/pr2_gazebo/prototype1_balcony_world.launch
pkg/trunk/demos/pr2_gazebo/prototype1_simple_office.launch
pkg/trunk/demos/pr2_gazebo/tables.launch
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/coffee_cup_large.model
Removed Paths:
-------------
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-empty-amcl.launch
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-empty-fake_localization.launch
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-simple-amcl.launch
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.launch
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-fake_localization.launch
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-amcl.launch
pkg/trunk/demos/pr2_gazebo/pr2_balcony.launch
pkg/trunk/demos/pr2_gazebo/pr2_cups.launch
pkg/trunk/demos/pr2_gazebo/pr2_empty.launch
pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch
pkg/trunk/demos/pr2_gazebo/pr2_simple.launch
pkg/trunk/demos/pr2_gazebo/pr2_simple_office.launch
pkg/trunk/demos/pr2_gazebo/pr2_wg.launch
pkg/trunk/demos/pr2_gazebo/prototype1_empty.launch
pkg/trunk/demos/pr2_gazebo/prototype1_simple.launch
pkg/trunk/demos/pr2_gazebo/prototype1_tables.launch
pkg/trunk/demos/pr2_gazebo/prototype1_wg.launch
Deleted: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-empty-amcl.launch
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-empty-amcl.launch 2009-07-06 23:13:24 UTC (rev 18355)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-empty-amcl.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -1,24 +0,0 @@
-<launch>
-
- <!-- start up robot -->
- <include file="$(find pr2_gazebo)/pr2_empty.launch"/>
-
- <!-- Tug Arms For Navigation -->
- <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="l" output="screen" />
- <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="r" output="screen" />
-
- <!-- load map -->
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/map_blank.png 0.1" respawn="true" machine="three" />
-
- <!-- nav-stack -->
- <include file="$(find 2dnav_gazebo)/2dnav-stack-amcl.launch"/>
-
- <!-- for visualization -->
- <include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch"/>
-
- <!-- for manual control -->
- <!--node pkg="teleop_base" type="teleop_base_keyboard" respawn="false" output="screen" /-->
- <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
-
-</launch>
-
Deleted: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-empty-fake_localization.launch
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-empty-fake_localization.launch 2009-07-06 23:13:24 UTC (rev 18355)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-empty-fake_localization.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -1,29 +0,0 @@
-<launch>
-
- <!-- start up robot -->
- <include file="$(find pr2_gazebo)/pr2_empty.launch"/>
-
- <!-- Tug Arms For Navigation -->
- <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="l" output="screen" />
- <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="r" output="screen" />
-
- <!-- load map -->
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/map_blank.png 0.1" respawn="true" machine="three" />
-
- <!-- nav-stack -->
- <include file="$(find 2dnav_gazebo)/2dnav-stack-fake_localization.launch"/>
-
- <!-- for visualization -->
- <!--
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" >
- <remap from="goal" to="/move_base/activate" />
- </node>
- <include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch"/>
- -->
-
- <!-- for manual control -->
- <!--node pkg="teleop_base" type="teleop_base_keyboard" respawn="false" output="screen" /-->
- <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
-
-</launch>
-
Deleted: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-simple-amcl.launch
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-simple-amcl.launch 2009-07-06 23:13:24 UTC (rev 18355)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-simple-amcl.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -1,25 +0,0 @@
-<launch>
-
- <!-- start up robot -->
- <include file="$(find pr2_gazebo)/prototype1_simple.launch"/>
-
- <!-- load map -->
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/map3.png 0.1" respawn="true" machine="three" />
-
- <!-- nav-stack -->
- <include file="$(find 2dnav_gazebo)/2dnav-stack-amcl.launch"/>
-
- <!-- for visualization -->
- <!--
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" >
- <remap from="goal" to="/move_base/activate" />
- </node>
- <include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch"/>
- -->
-
- <!-- for manual control -->
- <!--node pkg="teleop_base" type="teleop_base_keyboard" respawn="false" output="screen" /-->
- <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
-
-</launch>
-
Deleted: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.launch
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.launch 2009-07-06 23:13:24 UTC (rev 18355)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -1,25 +0,0 @@
-<launch>
-
- <!-- start up robot -->
- <include file="$(find pr2_gazebo)/prototype1_wg.launch"/>
-
- <!-- load map -->
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/willowMap.png 0.1" respawn="true" machine="three" />
-
- <!-- nav-stack -->
- <include file="$(find 2dnav_gazebo)/2dnav-stack-fake_localization.launch"/>
-
- <!-- for visualization -->
- <!--
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" >
- <remap from="goal" to="/move_base/activate" />
- </node>
- <include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch"/>
- -->
-
- <!-- for manual control -->
- <!--node pkg="teleop_base" type="teleop_base_keyboard" respawn="false" output="screen" /-->
- <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
-
-</launch>
-
Deleted: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-fake_localization.launch
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-fake_localization.launch 2009-07-06 23:13:24 UTC (rev 18355)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-fake_localization.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -1,28 +0,0 @@
-<launch>
-
- <!-- start up robot -->
- <include file="$(find pr2_gazebo)/pr2_simple.launch"/>
-
- <!-- Tug Arms For Navigation -->
- <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="b" output="screen" />
-
- <!-- load map -->
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/map3.png 0.1" respawn="true" machine="three" />
-
- <!-- nav-stack -->
- <include file="$(find 2dnav_gazebo)/2dnav-stack-fake_localization.launch"/>
-
- <!-- for visualization -->
- <!--
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" >
- <remap from="goal" to="/move_base/activate" />
- </node>
- <include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch"/>
- -->
-
- <!-- for manual control -->
- <!--node pkg="teleop_base" type="teleop_base_keyboard" respawn="false" output="screen" /-->
- <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
-
-</launch>
-
Deleted: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-amcl.launch
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-amcl.launch 2009-07-06 23:13:24 UTC (rev 18355)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-amcl.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -1,26 +0,0 @@
-<launch>
-
- <!-- start up robot -->
- <include file="$(find pr2_gazebo)/pr2_wg.launch"/>
-
- <!-- load map -->
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/willowMap.png 0.1" respawn="true" machine="three" />
-
- <!-- Tug Arms For Navigation -->
- <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="l" output="screen" />
- <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="r" output="screen" />
-
- <!-- nav-stack -->
- <include file="$(find 2dnav_gazebo)/2dnav-stack-amcl.launch"/>
-
- <!-- for visualization -->
- <!--
- <include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch"/>
- -->
-
- <!-- for manual control -->
- <!--node pkg="teleop_base" type="teleop_base_keyboard" respawn="false" output="screen" /-->
- <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
-
-</launch>
-
Copied: pkg/trunk/demos/2dnav_gazebo/pr2-empty-amcl.launch (from rev 18333, pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-empty-amcl.launch)
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/pr2-empty-amcl.launch (rev 0)
+++ pkg/trunk/demos/2dnav_gazebo/pr2-empty-amcl.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -0,0 +1,27 @@
+<launch>
+
+ <!-- start up empty world -->
+ <include file="$(find gazebo)/launch/empty_world.launch"/>
+
+ <!-- start up robot -->
+ <include file="$(find pr2_gazebo)/pr2.launch"/>
+
+ <!-- Tug Arms For Navigation -->
+ <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="l" output="screen" />
+ <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="r" output="screen" />
+
+ <!-- load map -->
+ <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/map_blank.png 0.1" respawn="true" machine="three" />
+
+ <!-- nav-stack -->
+ <include file="$(find 2dnav_gazebo)/2dnav-stack-amcl.launch"/>
+
+ <!-- for visualization -->
+ <include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch"/>
+
+ <!-- for manual control -->
+ <!--node pkg="teleop_base" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
+
+</launch>
+
Property changes on: pkg/trunk/demos/2dnav_gazebo/pr2-empty-amcl.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/demos/2dnav_gazebo/2dnav-gazebo-empty-amcl.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/demos/2dnav_gazebo/pr2-empty-fake_localization.launch (from rev 18333, pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-empty-fake_localization.launch)
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/pr2-empty-fake_localization.launch (rev 0)
+++ pkg/trunk/demos/2dnav_gazebo/pr2-empty-fake_localization.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -0,0 +1,32 @@
+<launch>
+
+ <!-- start up empty world -->
+ <include file="$(find gazebo)/launch/empty_world.launch"/>
+
+ <!-- start up robot -->
+ <include file="$(find pr2_gazebo)/pr2.launch"/>
+
+ <!-- Tug Arms For Navigation -->
+ <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="l" output="screen" />
+ <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="r" output="screen" />
+
+ <!-- load map -->
+ <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/map_blank.png 0.1" respawn="true" machine="three" />
+
+ <!-- nav-stack -->
+ <include file="$(find 2dnav_gazebo)/2dnav-stack-fake_localization.launch"/>
+
+ <!-- for visualization -->
+ <!--
+ <node pkg="nav_view" type="nav_view" respawn="false" output="screen" >
+ <remap from="goal" to="/move_base/activate" />
+ </node>
+ <include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch"/>
+ -->
+
+ <!-- for manual control -->
+ <!--node pkg="teleop_base" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
+
+</launch>
+
Property changes on: pkg/trunk/demos/2dnav_gazebo/pr2-empty-fake_localization.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/demos/2dnav_gazebo/2dnav-gazebo-empty-fake_localization.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/demos/2dnav_gazebo/pr2-simple-fake_localization.launch (from rev 18333, pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-fake_localization.launch)
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/pr2-simple-fake_localization.launch (rev 0)
+++ pkg/trunk/demos/2dnav_gazebo/pr2-simple-fake_localization.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -0,0 +1,31 @@
+<launch>
+
+ <!-- start up simple world -->
+ <include file="$(find gazebo)/launch/simple_world.launch"/>
+
+ <!-- start up robot -->
+ <include file="$(find pr2_gazebo)/pr2.launch"/>
+
+ <!-- Tug Arms For Navigation -->
+ <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="b" output="screen" />
+
+ <!-- load map -->
+ <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/map3.png 0.1" respawn="true" machine="three" />
+
+ <!-- nav-stack -->
+ <include file="$(find 2dnav_gazebo)/2dnav-stack-fake_localization.launch"/>
+
+ <!-- for visualization -->
+ <!--
+ <node pkg="nav_view" type="nav_view" respawn="false" output="screen" >
+ <remap from="goal" to="/move_base/activate" />
+ </node>
+ <include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch"/>
+ -->
+
+ <!-- for manual control -->
+ <!--node pkg="teleop_base" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
+
+</launch>
+
Property changes on: pkg/trunk/demos/2dnav_gazebo/pr2-simple-fake_localization.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/demos/2dnav_gazebo/2dnav-gazebo-simple-fake_localization.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/demos/2dnav_gazebo/pr2-wg-amcl.launch (from rev 18333, pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-amcl.launch)
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/pr2-wg-amcl.launch (rev 0)
+++ pkg/trunk/demos/2dnav_gazebo/pr2-wg-amcl.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -0,0 +1,29 @@
+<launch>
+
+ <!-- start up wg world -->
+ <include file="$(find gazebo)/launch/wg_world.launch"/>
+
+ <!-- start up robot -->
+ <include file="$(find pr2_gazebo)/pr2.launch"/>
+
+ <!-- load map -->
+ <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/willowMap.png 0.1" respawn="true" machine="three" />
+
+ <!-- Tug Arms For Navigation -->
+ <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="l" output="screen" />
+ <node pkg="pr2_mechanism_controllers" type="tuckarm.py" args="r" output="screen" />
+
+ <!-- nav-stack -->
+ <include file="$(find 2dnav_gazebo)/2dnav-stack-amcl.launch"/>
+
+ <!-- for visualization -->
+ <!--
+ <include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch"/>
+ -->
+
+ <!-- for manual control -->
+ <!--node pkg="teleop_base" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
+
+</launch>
+
Property changes on: pkg/trunk/demos/2dnav_gazebo/pr2-wg-amcl.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/demos/2dnav_gazebo/2dnav-gazebo-wg-amcl.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/demos/2dnav_gazebo/prototype1-simple-amcl.launch (from rev 18333, pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-simple-amcl.launch)
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/prototype1-simple-amcl.launch (rev 0)
+++ pkg/trunk/demos/2dnav_gazebo/prototype1-simple-amcl.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -0,0 +1,28 @@
+<launch>
+
+ <!-- start up simple world -->
+ <include file="$(find gazebo)/launch/simple_world.launch"/>
+
+ <!-- start up robot -->
+ <include file="$(find pr2_gazebo)/prototype1.launch"/>
+
+ <!-- load map -->
+ <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/map3.png 0.1" respawn="true" machine="three" />
+
+ <!-- nav-stack -->
+ <include file="$(find 2dnav_gazebo)/2dnav-stack-amcl.launch"/>
+
+ <!-- for visualization -->
+ <!--
+ <node pkg="nav_view" type="nav_view" respawn="false" output="screen" >
+ <remap from="goal" to="/move_base/activate" />
+ </node>
+ <include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch"/>
+ -->
+
+ <!-- for manual control -->
+ <!--node pkg="teleop_base" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
+
+</launch>
+
Property changes on: pkg/trunk/demos/2dnav_gazebo/prototype1-simple-amcl.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/demos/2dnav_gazebo/2dnav-gazebo-prototype1-simple-amcl.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/demos/2dnav_gazebo/prototype1-wg-fake_localization.launch (from rev 18333, pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.launch)
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/prototype1-wg-fake_localization.launch (rev 0)
+++ pkg/trunk/demos/2dnav_gazebo/prototype1-wg-fake_localization.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -0,0 +1,28 @@
+<launch>
+
+ <!-- start up wg world -->
+ <include file="$(find gazebo)/launch/wg_world.launch"/>
+
+ <!-- start up robot -->
+ <include file="$(find pr2_gazebo)/prototype1.launch"/>
+
+ <!-- load map -->
+ <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/willowMap.png 0.1" respawn="true" machine="three" />
+
+ <!-- nav-stack -->
+ <include file="$(find 2dnav_gazebo)/2dnav-stack-fake_localization.launch"/>
+
+ <!-- for visualization -->
+ <!--
+ <node pkg="nav_view" type="nav_view" respawn="false" output="screen" >
+ <remap from="goal" to="/move_base/activate" />
+ </node>
+ <include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch"/>
+ -->
+
+ <!-- for manual control -->
+ <!--node pkg="teleop_base" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
+
+</launch>
+
Property changes on: pkg/trunk/demos/2dnav_gazebo/prototype1-wg-fake_localization.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Modified: pkg/trunk/demos/erratic_gazebo/erratic.launch
===================================================================
--- pkg/trunk/demos/erratic_gazebo/erratic.launch 2009-07-06 23:13:24 UTC (rev 18355)
+++ pkg/trunk/demos/erratic_gazebo/erratic.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -1,26 +1,19 @@
<launch>
- <param name="/use_sim_time" value="true" />
-
<!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/pr2/erratic_defs/robots for full erratic -->
- <group name="wg">
- <!-- Create a transform sender for linking these frames. -->
- <node pkg="tf" type="transform_sender" args="0 0 0 0 0 0 base_footprint base_link 40" />
- <!-- send pr2.xml to param server -->
- <param name="robotdesc/erratic" command="$(find xacro)/xacro.py '$(find erratic_defs)/robots/erratic.xml'" />
+ <!-- start gazebo -->
+ <include file="$(find gazebo)/launch/simple_world.launch" />
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/simple.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
+ <!-- Create a transform sender for linking these frames. -->
+ <node pkg="tf" type="transform_sender" args="0 0 0 0 0 0 base_footprint base_link 40" />
- <!-- push robotdesc/erratic to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/erratic" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <!-- send pr2.xml to param server -->
+ <param name="robotdesc/erratic" command="$(find xacro)/xacro.py '$(find erratic_defs)/robots/erratic.xml'" />
- <!-- load controllers -->
- <node pkg="gazebo_plugin" type="ros_diffdrive" respawn="true" />
- </group>
+ <!-- push robotdesc/erratic to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/erratic" respawn="false" output="screen" />
+
+ <!-- load controllers -->
+ <node pkg="gazebo_plugin" type="ros_diffdrive" respawn="true" />
</launch>
Modified: pkg/trunk/demos/examples_gazebo/dual_link.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/dual_link.launch 2009-07-06 23:13:24 UTC (rev 18355)
+++ pkg/trunk/demos/examples_gazebo/dual_link.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -5,9 +5,9 @@
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find examples_gazebo)/dual_link_defs/dual_link.xml'" />
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
- <!--node pkg="mechanism_control" type="mech.py" args="sp $(find examples_gazebo)/dual_link_defs/controllers_dual_link.xml" respawn="false" output="screen" /--> <!-- load default arm controller -->
+ <!--node pkg="mechanism_control" type="mech.py" args="sp $(find examples_gazebo)/dual_link_defs/controllers_dual_link.xml" respawn="false" output="screen" /-->
<!--node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /--> <!-- open gripper .5 radians -->
</launch>
Modified: pkg/trunk/demos/examples_gazebo/multi_link.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/multi_link.launch 2009-07-06 23:13:24 UTC (rev 18355)
+++ pkg/trunk/demos/examples_gazebo/multi_link.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -5,9 +5,9 @@
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find examples_gazebo)/multi_link_defs/multi_link.xml'" />
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
- <node pkg="mechanism_control" type="mech.py" args="sp $(find examples_gazebo)/multi_link_defs/controllers_multi_link_zero_gains.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find examples_gazebo)/multi_link_defs/controllers_multi_link_zero_gains.xml" respawn="false" output="screen" />
<!--node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /--> <!-- open gripper .5 radians -->
</launch>
Modified: pkg/trunk/demos/examples_gazebo/single_link.launch
===================================================================
--- pkg/trunk/demos/examples_gazebo/single_link.launch 2009-07-06 23:13:24 UTC (rev 18355)
+++ pkg/trunk/demos/examples_gazebo/single_link.launch 2009-07-06 23:43:57 UTC (rev 18356)
@@ -5,9 +5,10 @@
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find examples_gazebo)/single_link_defs/single_link.xml'" />
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
- <node pkg="mechanism_control" type="spawner.py" args="$(find examples_gazebo)/single_link_defs/controllers_single_link.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
- <node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
+ <node pkg="mechanism_control" type="spawner.py" args="$(find examples_gazebo)/single_link_defs/controllers_single_link.xml" respawn="false" output="screen" />
+
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" />
</launch>
Modified: pkg/trunk/demos/examples_gazebo/table_defs/table_defs.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/table_defs/table_defs.xml 2009-07-06 23:13:24 UTC (rev 18355)
+++ pkg/trunk/demos/examples_gazebo/table_defs/table_defs.xml 2009-07-06 23:43:57 UTC (rev 18356)
@@ -57,7 +57,7 @@
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0" />
<map name="gazebo_material" flag="gazebo">
- <elem key="material">Gazebo/Fish</elem>
+ <elem key="material">Gazebo/LightWood</elem>
</map>
<geometry name="floor_visual_geom">
<mesh scale="1 1 0.05" />
@@ -239,7 +239,7 @@
<visual>
<origin xyz="0.0 0.0 ${object_1_height/2}" rpy="0 0 0" />
<map name="gazebo_material" flag="gazebo">
- <elem key="material">Gazebo/Blue</elem>
+ <elem key="material">Gazebo/BumpyMetal</elem>
</map>
<geometry name="object_1_visual_geom">
<mesh scale="${object_1_radius*2} ${object_1_radius*2} ${object_1_height}" />
@@ -277,7 +277,7 @@
<visual>
<origin xyz="0.0 0.0 ${object_1_base_heigh...
[truncated message content] |
|
From: <hsu...@us...> - 2009-07-07 02:20:12
|
Revision: 18373
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18373&view=rev
Author: hsujohnhsu
Date: 2009-07-07 02:08:53 +0000 (Tue, 07 Jul 2009)
Log Message:
-----------
added cup to grasping example.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/r_arm_grasping_demo.launch
pkg/trunk/demos/arm_gazebo/scripts/grasp_preprogrammed.py
pkg/trunk/demos/examples_gazebo/table_defs/Makefile
pkg/trunk/demos/pr2_gazebo/coffee_cup.launch
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/coffee_cup.model
Modified: pkg/trunk/demos/arm_gazebo/r_arm_grasping_demo.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_grasping_demo.launch 2009-07-07 02:03:32 UTC (rev 18372)
+++ pkg/trunk/demos/arm_gazebo/r_arm_grasping_demo.launch 2009-07-07 02:08:53 UTC (rev 18373)
@@ -40,5 +40,6 @@
<param name="robotdesc/table" command="$(find xacro)/xacro.py '$(find examples_gazebo)/table_defs/table_defs.xml'" />
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/table 0 0 0.01 0 0 0 table_model" respawn="false" output="screen" />
+ <include file="$(find pr2_gazebo)/coffee_cup.launch" />
</launch>
Modified: pkg/trunk/demos/arm_gazebo/scripts/grasp_preprogrammed.py
===================================================================
--- pkg/trunk/demos/arm_gazebo/scripts/grasp_preprogrammed.py 2009-07-07 02:03:32 UTC (rev 18372)
+++ pkg/trunk/demos/arm_gazebo/scripts/grasp_preprogrammed.py 2009-07-07 02:08:53 UTC (rev 18373)
@@ -58,7 +58,7 @@
CMD_POS_5 = 0.0
CMD_POS_6 = 0.0
CMD_POS_7 = 0.0
-LOWER_Z = 0.2
+LOWER_Z = 0.1
PAN_RAD = 0.5
G_OPEN = 0.058;
G_CLOSE = 0.0199;
Modified: pkg/trunk/demos/examples_gazebo/table_defs/Makefile
===================================================================
--- pkg/trunk/demos/examples_gazebo/table_defs/Makefile 2009-07-07 02:03:32 UTC (rev 18372)
+++ pkg/trunk/demos/examples_gazebo/table_defs/Makefile 2009-07-07 02:08:53 UTC (rev 18373)
@@ -1,14 +1,14 @@
XACRO = `rospack find xacro`/xacro.py
-URDF2GAZEBO = `rospack find gazebo_robot_description`/urdf2gazebo
-MODEL_FILE = `rospack find gazebo_robot_description`
+URDF2GAZEBO = `rospack find gazebo_plugin`/bin/urdf2file
+MODEL_DIR = `pwd`
ROBOT = table
$(ROBOT).model: $(ROBOT).xml
$(XACRO) $^ > $^.expanded
- $(URDF2GAZEBO) $^.expanded $(MODEL_FILE)/$@
+ $(URDF2GAZEBO) $^.expanded $(MODEL_DIR)/$@
clean:
- $(RM) $(ROBOT).xml.expanded $(MODEL_FILE)/$(ROBOT).model
+ $(RM) $(ROBOT).xml.expanded $(MODEL_DIR)/$(ROBOT).model
$(RM) core.* Ogre.log
Modified: pkg/trunk/demos/pr2_gazebo/coffee_cup.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/coffee_cup.launch 2009-07-07 02:03:32 UTC (rev 18372)
+++ pkg/trunk/demos/pr2_gazebo/coffee_cup.launch 2009-07-07 02:08:53 UTC (rev 18373)
@@ -4,7 +4,7 @@
<param name="coffee_cup" textfile="$(find gazebo_robot_description)/gazebo_objects/coffee_cup.model" />
<!-- push urdf to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="xml2factory" args="coffee_cup 1 -.2 .84 0 0 0 coffee_cup" respawn="false" output="screen" />
+ <node pkg="gazebo_plugin" type="xml2factory" args="coffee_cup 1 -0.22 .84 0 0 0 coffee_cup" respawn="false" output="screen" />
</launch>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/coffee_cup.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/coffee_cup.model 2009-07-07 02:03:32 UTC (rev 18372)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/coffee_cup.model 2009-07-07 02:08:53 UTC (rev 18373)
@@ -5,7 +5,7 @@
<rpy> 0.0 0.0 0.0</rpy>
<body:trimesh name="cup1_body">
<xyz> 0.0 0.0 0.009</xyz>
- <rpy> 90.0 0.0 180.0</rpy>
+ <rpy> 90.0 0.0 90.0</rpy>
<massMatrix>true</massMatrix>
<mass>1.0</mass>
<ixx>0.01</ixx>
@@ -17,10 +17,10 @@
<cx>0.0</cx>
<cy>0.0</cy>
<cz>0.0</cz>
-
+ <dampingFactor>0.05</dampingFactor>
<geom:trimesh name="cup1_geom">
<laserRetro>2000.0</laserRetro>
- <kp>1000000.0</kp>
+ <kp>100000000.0</kp>
<kd>1.0</kd>
<scale>0.1 0.1 0.1</scale>
<mesh>cup.mesh</mesh>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <goo...@us...> - 2009-07-07 22:44:46
|
Revision: 18427
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18427&view=rev
Author: goodfellow
Date: 2009-07-07 22:44:41 +0000 (Tue, 07 Jul 2009)
Log Message:
-----------
Committing sandbox/stairvision so I can delete it (moved to sail)
Added app for converting SVL datasets to inria format
Modified Paths:
--------------
pkg/trunk/sandbox/stairvision/Makefile
pkg/trunk/sandbox/stairvision/manifest.xml
Added Paths:
-----------
pkg/trunk/vision/people/inria_stairvision/
pkg/trunk/vision/people/inria_stairvision/Makefile
pkg/trunk/vision/people/inria_stairvision/README
pkg/trunk/vision/people/inria_stairvision/bin/
pkg/trunk/vision/people/inria_stairvision/specifications
pkg/trunk/vision/people/inria_stairvision/svl2inria.cpp
Modified: pkg/trunk/sandbox/stairvision/Makefile
===================================================================
--- pkg/trunk/sandbox/stairvision/Makefile 2009-07-07 22:42:38 UTC (rev 18426)
+++ pkg/trunk/sandbox/stairvision/Makefile 2009-07-07 22:44:41 UTC (rev 18427)
@@ -1,4 +1,4 @@
-#Author: Ian Goodfellow
+#Author: Ian Goodfellow <goo...@wi...> (until Sep '09) <ia...@st...> (forever)
#File: Makefile
#Description: Makefile for stairvision ROS package. Checks out latest version of STAIR Vision Library and makes it. Based on the makefile for the opencv_latest package.
@@ -15,10 +15,16 @@
cd ${SVN_DIR} && make external && make svllibs && make svlapps && cd ../..
${EIGEN_LINK} :
+<<<<<<< .mine
+ ln -s `rospack find eigen`/build/eigen2/Eigen ${EIGEN_LINK}
+=======
-ln -s `rospack find eigen`/build/eigen2/Eigen ${EIGEN_LINK}
+>>>>>>> .r18425
${LOCAL_MAKEFILE} :
cp make.local ${LOCAL_MAKEFILE}
clean :
cd ${SVN_DIR} && make clean && cd ../..
+ rm ${EIGEN_LINK}
+ rm ${LOCAL_MAKEFILE}
Modified: pkg/trunk/sandbox/stairvision/manifest.xml
===================================================================
--- pkg/trunk/sandbox/stairvision/manifest.xml 2009-07-07 22:42:38 UTC (rev 18426)
+++ pkg/trunk/sandbox/stairvision/manifest.xml 2009-07-07 22:44:41 UTC (rev 18427)
@@ -12,5 +12,6 @@
<review status="3rdparty" notes=""/>
<url>http://stairvision.sourceforge.net</url>
<depend package="opencv_latest" />
+<depend package="eigen" />
</package>
Added: pkg/trunk/vision/people/inria_stairvision/Makefile
===================================================================
--- pkg/trunk/vision/people/inria_stairvision/Makefile (rev 0)
+++ pkg/trunk/vision/people/inria_stairvision/Makefile 2009-07-07 22:44:41 UTC (rev 18427)
@@ -0,0 +1,61 @@
+# Makefile for stairvision / inria conversion tools
+# Ian Goodfellow <ia...@cs...>
+
+LASIK_PATH := $(shell rospack find stairvision)/build/stairvision-svn
+
+USE_OPENCV = 1
+USE_GSL = 0
+USE_LAPACK = 0
+
+-include $(LASIK_PATH)/make.mk
+#-include $(LASIK_PATH)/projects/person/make.person_common
+
+MY_BIN_PATH=$(shell pwd)/bin
+
+#######################################################################
+# add application source files here
+#######################################################################
+APP_SRC = svl2inria.cpp
+
+OTHER_SRC =
+
+#######################################################################
+
+APP_PROG_NAMES = $(APP_SRC:.cpp=)
+APP_OBJ = $(APP_SRC:.cpp=.o)
+OTHER_OBJ = $(OTHER_SRC:.cpp=.o)
+
+LIB_CFLAGS = -I$(LASIK_PATH)/lib -I$(LASIK_PATH)/lib/core -I$(LASIK_PATH)/lib/ml -I$(LASIK_PATH)/lib/utils
+EXT_CFLAGS = -I$(EXT_PATH)/xmlParser -I$(EXT_PATH)
+EXT_LFLAGS = $(EXT_PATH)/xmlParser/xmlParser.a $(EXT_PATH)/lbfgs/lbfgs.a
+
+CFLAGS += $(WX_CFLAGS) $(EXT_CFLAGS) $(LIB_CFLAGS)
+LFLAGS += $(WX_LFLAGS) $(EXT_LFLAGS) $(LIBLASIK) $(LIBSVL) -lboost_filesystem-gcc42-mt-d-1_34_1
+
+.PHONY: clean
+.PRECIOUS: $(APP_OBJ)
+
+all: depend ${addprefix ${MY_BIN_PATH}/,$(APP_PROG_NAMES)}
+
+$(MY_BIN_PATH)/%: %.o $(OTHER_OBJ) $(LIBSVL)
+ ${CCC} $*.o -o $(@:.o=) $(OTHER_OBJ) $(LIBLASIK) $(LFLAGS)
+
+$(LIBSVL):
+ @echo "YOU NEED TO MAKE THE LIBLASIK LIBRARY FIRST"
+ false
+
+.cpp.o:
+ ${CCC} ${CFLAGS} -c $< -o $@
+
+depend:
+ echo ${LASIK_PATH}
+ g++ ${CFLAGS} -MM ${APP_SRC} ${OTHER_SRC} >depend
+
+clean:
+ -rm $(APP_OBJ) >& /dev/null
+ -rm $(OTHER_OBJ) >& /dev/null
+ -rm ${addprefix ${MY_BIN_PATH}/,$(APP_PROG_NAMES)} >& /dev/null
+ -rm depend >& /dev/null
+
+-include depend
+
Added: pkg/trunk/vision/people/inria_stairvision/README
===================================================================
--- pkg/trunk/vision/people/inria_stairvision/README (rev 0)
+++ pkg/trunk/vision/people/inria_stairvision/README 2009-07-07 22:44:41 UTC (rev 18427)
@@ -0,0 +1,6 @@
+INRIA/stairvision integration
+Author/maintainer: Ian Goodfellow <goo...@wi...> (until Sep '09) <ia...@cs...> (forever)
+
+This directory contains scripts and executables for integrating the INRIA classifier and dataset into the
+STAIR Vision Library.
+
Added: pkg/trunk/vision/people/inria_stairvision/specifications
===================================================================
--- pkg/trunk/vision/people/inria_stairvision/specifications (rev 0)
+++ pkg/trunk/vision/people/inria_stairvision/specifications 2009-07-07 22:44:41 UTC (rev 18427)
@@ -0,0 +1,13 @@
+Specifications of the INRIA dataset format (reverse engineered and based on comments at http://pascal.inrialpes.fr/data/human):
+
+Training data is stored in a directory of your choosing, ${TRAIN_DIR}.
+This contains two directories, "neg" and "pos", and two files, "neg.lst" and "pos.lst".
+
+Images in "pos":
+ file extension is always png, but this probably doesn't matter
+ filename doesn't seem to matter
+ all have same size, in this case 64x128 PLUS a border. for train the border is 16 for a total of 96x160. for test the border is 3 for a total of 70x134.
+ may contain multiple people
+
+Images in "neg":
+ don't contain people anywhere. can be any size. 64x128 patches will be sampled out of them, and this could cause people in the image to be used as negative samples.
Added: pkg/trunk/vision/people/inria_stairvision/svl2inria.cpp
===================================================================
--- pkg/trunk/vision/people/inria_stairvision/svl2inria.cpp (rev 0)
+++ pkg/trunk/vision/people/inria_stairvision/svl2inria.cpp 2009-07-07 22:44:41 UTC (rev 18427)
@@ -0,0 +1,496 @@
+//Author: Ian Goodfellow <goo...@wi...> (until Sep '09) <ia...@cs...> (forever)
+//Description: An application for converting an svl dataset to an inria dataset
+
+#include "svlVision.h"
+#include <sstream>
+using namespace std;
+
+void usage()
+{
+ cerr << SVL_USAGE_HEADER << endl;
+ cerr << "USAGE: ./svl2inria -is <svl image sequence> -il <svl labels> -od <directory of inria data to create>" << endl;
+ cerr << "Pass -t for test set, otherwise assumes train. (the INRIA format uses a different size for test positives than train positives) " << endl;
+
+}
+
+vector<IplImage *> getNegativeImages(svlLabeledFrame & f);
+vector<IplImage *> getPositiveImages(svlLabeledFrame & f, int w, int h, int xpad, int ypad);//for each positive example, grows the smaller dimension until it is proportional to a (w+2*xpad)x(h+2*ypad) rectangle. if this requires it to extend outside the image, fills in the extra pixels with black (makes sense for infrared, maybe not for other sensors).
+void removeImagesSmallerThan(vector<IplImage *> & images, int minWidth, int minHeight);
+vector<IplImage *> getImagesFromBoundingBoxes(const IplImage * im, const svlObject2dFrame & labels);
+vector<IplImage *> getImagesFromBoundingBoxesAllowingOutOfBounds(const IplImage * im, const svlObject2dFrame & boxes); //pixels outside the image are treated as black
+string makeDirectories(const char * outputDir); // returns outputDir as a full path
+void writeImages(const vector<IplImage *> & images, const string & baseDir, bool isPos);
+bool intersects(const svlObject2d & a, const svlObject2d & b);
+svlObject2dFrame growToProportion(const svlObject2dFrame & input, double wOverh);
+svlObject2dFrame rects2objects(const vector<CvRect> & rects);
+void scaleImages(vector<IplImage *> & v,int targW,int targH);
+
+int main(int argc, char ** argv)
+{
+ const char * inputSeq = NULL;
+ const char * inputLab = NULL;
+ const char * outputDir = NULL;
+ bool testSet = false;
+
+ SVL_BEGIN_CMDLINE_PROCESSING(argc,argv)
+ SVL_CMDLINE_STR_OPTION("-is",inputSeq)
+ SVL_CMDLINE_STR_OPTION("-il",inputLab)
+ SVL_CMDLINE_STR_OPTION("-od",outputDir)
+ SVL_CMDLINE_BOOL_OPTION("-t",testSet)
+ SVL_END_CMDLINE_PROCESSING(usage())
+
+ if (!inputSeq || !inputLab || !outputDir)
+ {
+ usage();
+ return -1;
+ }
+
+ int width = 64;
+ int height = 128;
+ int xpad = 0;
+ int ypad = 0;
+
+ if (testSet)
+ {
+ xpad = ypad = 3;
+ }
+ else
+ {
+ xpad = ypad = 16;
+ }
+
+
+ svlLabeledSequence s;
+ s.load(inputSeq, inputLab);
+
+ string fullPath = makeDirectories(outputDir);
+
+ for (unsigned i = 0; i < s.numImages(); i++)
+ {
+ //cout << "Processing frame "<<i<<endl;
+ svlLabeledFrame l = s.frame(i);
+
+ assert(l.image);
+
+ if (!l.labels)
+ {
+ l.labels = new svlObject2dFrame;
+ cerr << "WARNING: Adding empty label to unlabeled frame "<<s[i]<<endl;
+ continue;
+ }
+
+ vector<IplImage *> negImages = getNegativeImages(l);
+ removeImagesSmallerThan(negImages,width,height);
+ writeImages(negImages, fullPath, 0);
+ vector<IplImage *> posImages = getPositiveImages(l,width,height,xpad,ypad);
+ writeImages(posImages, fullPath, 1);
+ }
+
+ return 0;
+}
+
+vector<IplImage *> getNegativeImages(svlLabeledFrame & f)
+{
+ //Negative images may be any size but may not contain any positives at all
+ //The INRIA training code will extract random portions of them to make
+ //negative examples
+
+ //Most SVL datasets consist largely of images containing positives; the challenge
+ //is to correctly localize them
+
+ //Thus we need to make negative images by cropping out positives
+
+ assert(f.image);
+ assert(f.labels);//the main function should have filtered out frames with no label data structure associated (this is different from no labels)
+
+ vector<IplImage *> rval;
+
+ if (f.labels->size() == 0) //if there are no labels, then this whole frame can be used for negative examples
+ {
+ rval.push_back(cvCloneImage(f.image));
+ return rval;
+ }
+
+ svlObject2dFrame candidates;
+
+ //Initialize candidates with one rect filling the whole image
+ candidates.push_back(svlObject2d(0,0,f.image->width,f.image->height));
+
+
+ //for l in labels
+ for (unsigned l = 0; l < f.labels->size(); l++)
+ {
+ //cout << "\tLabel "<<l<<" of "<<f.labels->size()<< endl;
+ svlObject2d label = f.labels->operator[](l);
+
+
+ //for c in candidates
+ for (unsigned c = 0; c < candidates.size(); c++)
+ {
+ //cout << "\t\tCandidate "<<c<<" of "<<candidates.size()<<endl;
+ //if l intersects c
+ if (intersects(candidates[c],label) > 0)
+ {
+ //svlLabelVisualizer v("Intersect?");
+ //v.addLabel(candidates[c],SVL_DETECTION);
+ //v.addLabel(label, SVL_TRUTH);
+ //v.enableWindow();
+ //cvWaitKey(0);
+
+ //break c into part that is above, below, left, right of l;
+
+ //left
+ if (candidates[c].x < label.x)
+ {
+ candidates.push_back(svlObject2d(candidates[c].x, candidates[c].y, label.x - candidates[c].x, candidates[c].h));
+ assert(candidates.back().w > 0);
+ assert(candidates.back().h > 0);
+ }
+
+ //right
+ if (candidates[c].x + candidates[c].w > label.x + label.w)
+ {
+ candidates.push_back(
+ svlObject2d(label.x+label.w,
+ candidates[c].y,
+ candidates[c].x+candidates[c].w - label.x-label.w,
+ candidates[c].h));
+ assert(candidates.back().w > 0);
+ assert(candidates.back().h > 0);
+ }
+
+ //top
+ if (candidates[c].y < label.y)
+ {
+ candidates.push_back(svlObject2d(candidates[c].x, candidates[c].y, candidates[c].w, label.y-candidates[c].y));
+ assert(candidates.back().w > 0);
+ assert(candidates.back().h > 0);
+ }
+
+ //bottom
+ if (candidates[c].y + candidates[c].h > label.y + label.h)
+ {
+ candidates.push_back(svlObject2d(label.x, label.y + label.h, label.w, candidates[c].y+candidates[c].h - label.y-label.h));
+ assert(candidates.back().w > 0);
+ assert(candidates.back().h > 0);
+ }
+
+ candidates.erase(candidates.begin()+c);
+
+ c--; //prevent c index from advancing
+ }
+ }
+ }
+
+ removeOverlappingObjects(candidates,0.9);
+
+ rval = getImagesFromBoundingBoxes(f.image, candidates);
+
+ return rval;
+}
+
+
+vector<IplImage *> getPositiveImages(svlLabeledFrame & f, int w, int h, int xpad, int ypad)
+{
+ vector<IplImage *> rval;
+
+ if (f.labels)
+ {
+ int targW = w + 2*xpad;
+ int targH = h + 2*ypad;
+
+
+ svlObject2dFrame proportionalBoxes = growToProportion(*f.labels, double(targW)/double(targH));
+ rval = getImagesFromBoundingBoxesAllowingOutOfBounds(f.image, proportionalBoxes);
+ scaleImages(rval,targW,targH);
+ }
+
+ return rval;
+}
+
+
+void removeImagesSmallerThan(vector<IplImage *> & images, int minWidth, int minHeight)
+{
+ vector<IplImage *>::iterator i = images.begin();
+
+ while (i != images.end())
+ {
+ if ((*i)->width < minWidth || (*i)->height < minHeight)
+ i = images.erase(i);
+ else
+ ++i;
+ }
+}
+
+vector<IplImage *> getImagesFromBoundingBoxes(const IplImage * im, const svlObject2dFrame & labels)
+{
+ IplImage * hack = (IplImage *) im;
+
+ vector<IplImage *> rval;
+
+ for (unsigned i = 0; i < labels.size(); i++)
+ {
+ CvRect label = cvRect(labels[i].x, labels[i].y, labels[i].w, labels[i].h);
+
+ if (label.x < 0)
+ {
+ cerr << "getImagesFromBoundingBox: warning, label has x value of " << label.x << endl;
+ label.width -= label.x;
+ label.x = 0;
+ }
+
+ if (label.y < 0)
+ {
+ cerr << "getImagesFromBoundingBox: warning, label has y value of " << label.y << endl;
+ label.height -= label.y;
+ label.y = 0;
+ }
+
+ if (label.x + label.width > hack->width)
+ {
+ cerr << "getImagesFromBoundingBox: warning, label sticks out right of frame by "<<label.x+label.width - hack->width << " pixels" << endl;
+ label.width = hack->width - label.x;
+ }
+
+ if (label.y + label.height > hack->height)
+ {
+ cerr << "getImagesFromBoundingBox: warning, label sticks out top of frame by "<<label.y+label.height - hack->height << " pixels" << endl;
+ label.height = hack->height - label.y;
+ }
+
+ if (label.width <= 0)
+ {
+ cerr << "getImagesFromBoundingBox: warning, label has width of "<<label.width << " (skipping) " << endl;
+ continue;
+ }
+
+ if (label.height <= 0)
+ {
+ cerr << "getImagesFromBoundingBox: warning, label has height of "<<label.height<<" (skipping) " << endl;
+ continue;
+ }
+
+ //cerr << "image: " << hack->width << " "<< hack->height << endl;
+ //cerr << "label: " << label.x << " " << label.y << " " << label.width << " " << label.height << endl;
+
+ cvSetImageROI(hack, label);
+ IplImage * output = cvCreateImage(cvSize(labels[i].w, labels[i].h),hack->depth, hack->nChannels);
+ assert(hack->width > 0);
+ assert(hack->height > 0);
+ cvCopy(hack, output);
+ cvResetImageROI(hack);
+ rval.push_back(output);
+ }
+ return rval;
+}
+
+vector<IplImage *> getImagesFromBoundingBoxesAllowingOutOfBounds(const IplImage * im, const svlObject2dFrame & boxes)
+{
+ vector<CvRect> rects;
+ vector<int> xEdges;//x coords of all vertical box edges
+ vector<int> yEdges;//y coords of all horizontal box edges
+
+ for (unsigned i = 0; i < boxes.size(); i++)
+ {
+ rects.push_back(cvRect(boxes[i].x,boxes[i].y,boxes[i].w,boxes[i].h));
+ xEdges.push_back(rects.back().x);
+ xEdges.push_back(rects.back().x+rects.back().width-1);
+ yEdges.push_back(rects.back().y);
+ yEdges.push_back(rects.back().y+rects.back().height-1);
+ }
+
+ //Guarantee that we'll never shrink the image, this way we don't need to fiddle with ROI of im
+ xEdges.push_back(0);
+ xEdges.push_back(im->width-1);
+ yEdges.push_back(0);
+ yEdges.push_back(im->height-1);
+
+ int minX = minElem(xEdges);
+ int minY = minElem(yEdges);
+ int maxX = maxElem(xEdges);
+ int maxY = maxElem(yEdges);
+
+ int w = maxX-minX+1;
+ int h = maxY-minY+1;
+
+ IplImage * expanded = cvCreateImage(cvSize(w,h), im->depth, im->nChannels);
+
+ cvZero(expanded);
+ cvSetImageROI(expanded, cvRect(-minX,-minY,im->width,im->height));
+ cvCopyImage(im, expanded);
+
+ for (unsigned i = 0; i < rects.size(); i++)
+ {
+ rects[i].x -= minX;
+ rects[i].y -= minY;
+ }
+
+ return getImagesFromBoundingBoxes(expanded, rects2objects(rects));
+}
+
+string makeDirectories(const char * outputDir)
+{
+ string command0 = string("rm -r -f ")+outputDir;
+
+ FILE * output = popen(command0.c_str(), "r");
+
+ fclose(output);
+
+
+ stringstream command;
+
+ command << "mkdir -p " << outputDir;
+
+
+ output = popen(command.str().c_str(), "r");
+ fclose(output);
+
+ stringstream command2;
+
+ command2 << "mkdir -p " << outputDir << "/pos";
+
+
+ output = popen(command2.str().c_str(),"r");
+
+ fclose(output);
+
+ stringstream command3;
+
+ command3 << "mkdir -p " << outputDir << "/neg";
+
+
+ output = popen(command3.str().c_str(),"r");
+
+ fclose(output);
+
+ stringstream command4;
+
+ command4 << "cd " << outputDir << "; pwd";
+
+ output = popen(command4.str().c_str(),"r");
+
+
+ int x;
+
+ stringstream rval_stream;
+
+ while ( (x = fgetc(output)) != EOF)
+ {
+ if (x != '\n')
+ rval_stream << char(x);
+ }
+
+ return rval_stream.str();
+}
+
+void writeImages(const vector<IplImage *> & images, const string & baseDir, bool isPos)
+{
+ static int posCounter = 0;
+ static int negCounter = 0;
+
+ int * counter = NULL;
+
+ assert(baseDir.size() && baseDir[0] == '/'); //baseDir should be a full path
+
+ string set;
+ if (isPos)
+ {
+ set = "pos";
+ counter = & posCounter;
+ }
+ else
+ {
+ set = "neg";
+ counter = & negCounter;
+ }
+
+ string listFile = baseDir+"/"+set+".lst";
+
+ fstream ofs;
+ ofs.open(listFile.c_str(), ios::out | ios::app);
+
+ if (ofs.fail())
+ {
+ cerr << "Could not write to "<<listFile<< endl;
+ exit(1);
+ }
+
+ string imgDir = baseDir+"/"+set;
+
+ //cout << "Writing to "<<listFile << endl;
+
+ for (unsigned i = 0; i < images.size(); i++)
+ {
+ stringstream s;
+ s << imgDir << "/image_" << (*counter)++ << ".png";
+
+ string filepath = s.str();
+
+ ofs << filepath << endl;
+
+ cvSaveImage(filepath.c_str(), images[i]);
+
+ //cout << "\t" << filepath << endl;
+ }
+
+ ofs.close();
+}
+
+bool intersects(const svlObject2d & a, const svlObject2d & b)
+{
+ if (a.x+a.w <= b.x +0.1)
+ return false;
+
+ if (a.x + 0.1 >= b.x+b.w)
+ return false;
+
+ if (a.y + a.h <= b.y + 0.1 )
+ return false;
+
+ if (a.y + 0.1 >= b.y+b.h)
+ return false;
+
+
+
+ return true;
+}
+
+svlObject2dFrame growToProportion(const svlObject2dFrame & input, double wOverh)
+{
+ svlObject2dFrame rval = input;
+
+ for (unsigned i = 0; i < rval.size(); i++)
+ {
+ double p = rval[i].w / rval[i].h;
+ double centerX = rval[i].x + 0.5 * rval[i].w;
+ double centerY = rval[i].y + 0.5 * rval[i].h;
+
+
+ if (p < wOverh)
+ rval[i].w = wOverh * rval[i].h;
+ else if (p > wOverh)
+ rval[i].h = rval[i].w / wOverh;
+
+ rval[i].x = centerX - 0.5 * rval[i].w;
+ rval[i].y = centerY - 0.5 * rval[i].h;
+
+ }
+ return rval;
+}
+
+svlObject2dFrame rects2objects(const vector<CvRect> & rects)
+{
+ svlObject2dFrame rval;
+ for (unsigned i = 0; i < rects.size(); i++)
+ rval.push_back(svlObject2d(rects[i].x,rects[i].y,rects[i].width,rects[i].height));
+ return rval;
+}
+
+void scaleImages(vector<IplImage *> & v,int targW,int targH)
+{
+ for (unsigned i = 0; i < v.size(); i++)
+ {
+ //cout << "ratio: " << double(v[i]->width) / double(v[i]->height) << endl;
+ resizeInPlace(&v[i],targH,targW,CV_INTER_LINEAR);
+ }
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wa...@us...> - 2009-07-08 04:58:15
|
Revision: 18461
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18461&view=rev
Author: wattsk
Date: 2009-07-08 04:31:43 +0000 (Wed, 08 Jul 2009)
Log Message:
-----------
Changes to hot box deps, life test and hot box stuff for hot boxing
Modified Paths:
--------------
pkg/trunk/pr2/hot_box_test/base_shuffle.py
pkg/trunk/pr2/hot_box_test/manifest.xml
pkg/trunk/pr2/life_test/scripts/gripper_effort_controller.py
pkg/trunk/pr2/life_test/scripts/torso_life_test.py
pkg/trunk/stacks/pr2/pr2_alpha/pre.machine
pkg/trunk/stacks/pr2/pr2_default_controllers/scripts/tuckarm.py
Modified: pkg/trunk/pr2/hot_box_test/base_shuffle.py
===================================================================
--- pkg/trunk/pr2/hot_box_test/base_shuffle.py 2009-07-08 04:22:28 UTC (rev 18460)
+++ pkg/trunk/pr2/hot_box_test/base_shuffle.py 2009-07-08 04:31:43 UTC (rev 18461)
@@ -47,8 +47,11 @@
import rospy
from robot_msgs.msg import PoseDot
-from mechanism_control import mechanism
+from mechanism_msgs.srv import SpawnController, KillController
+spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
+kill_controller = rospy.ServiceProxy('kill_controller', KillController)
+
def main():
usage = "base_shuffle.py <dist>; Moves base in y dir side to side."
@@ -62,7 +65,7 @@
path = roslib.packages.get_pkg_dir("pr2_default_controllers")
xml_for_base = open(path + "/base_controller.xml")
- mechanism.spawn_controller(xml_for_base.read(), 1)
+ spawn_controller(xml_for_base.read(), 1)
# Publishes velocity every 0.05s, calculates number of publishes
num_publishes = int(distance * 20 * 2)
@@ -105,7 +108,7 @@
# Set velocity = 0
cmd_vel.vel.vy = float(0)
base_vel.publish(cmd_vel)
- mechanism.kill_controller('base_controller')
+ kill_controller('base_controller')
if __name__ == '__main__':
main()
Modified: pkg/trunk/pr2/hot_box_test/manifest.xml
===================================================================
--- pkg/trunk/pr2/hot_box_test/manifest.xml 2009-07-08 04:22:28 UTC (rev 18460)
+++ pkg/trunk/pr2/hot_box_test/manifest.xml 2009-07-08 04:31:43 UTC (rev 18461)
@@ -18,6 +18,7 @@
<depend package="pr2_default_controllers" />
<depend package="life_test" />
<depend package="ocean_battery_driver" />
- <depend package="mechanism_control" />
+ <depend package="robot_msgs" />
+ <depend package="mechanism_msgs" />
</package>
Modified: pkg/trunk/pr2/life_test/scripts/gripper_effort_controller.py
===================================================================
--- pkg/trunk/pr2/life_test/scripts/gripper_effort_controller.py 2009-07-08 04:22:28 UTC (rev 18460)
+++ pkg/trunk/pr2/life_test/scripts/gripper_effort_controller.py 2009-07-08 04:31:43 UTC (rev 18461)
@@ -35,7 +35,7 @@
import rospy
from mechanism_control import mechanism
from std_msgs.msg import Float64
-from robot_srvs.srv import SpawnController, KillController
+from mechanism_msgs.srv import SpawnController, KillController
import random
import sys
Modified: pkg/trunk/pr2/life_test/scripts/torso_life_test.py
===================================================================
--- pkg/trunk/pr2/life_test/scripts/torso_life_test.py 2009-07-08 04:22:28 UTC (rev 18460)
+++ pkg/trunk/pr2/life_test/scripts/torso_life_test.py 2009-07-08 04:31:43 UTC (rev 18461)
@@ -37,8 +37,7 @@
from time import sleep
from std_msgs.msg import Float64
-from mechanism_control import mechanism
-from robot_srvs.srv import SpawnController, KillController
+from mechanism_msgs.srv import SpawnController, KillController
from optparse import OptionParser
Modified: pkg/trunk/stacks/pr2/pr2_alpha/pre.machine
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/pre.machine 2009-07-08 04:22:28 UTC (rev 18460)
+++ pkg/trunk/stacks/pr2/pr2_alpha/pre.machine 2009-07-08 04:31:43 UTC (rev 18461)
@@ -1,8 +1,9 @@
<launch>
- <machine name="realtime_root" user="root" address="c1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="never"/>
+ <machine name="realtime_root" user="root" address="pre1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="never"/>
- <machine name="realtime" address="c1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="true"/>
- <machine name="two" address="c2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
- <machine name="two_root" user="root" address="c2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
- <machine name="stereo" address="c2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
+ <machine name="realtime" address="pre1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="true"/>
+ <machine name="two" address="pre2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
+
+ <machine name="two_root" user="root" address="pre2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
+ <machine name="stereo" address="pre2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
</launch>
Modified: pkg/trunk/stacks/pr2/pr2_default_controllers/scripts/tuckarm.py
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/scripts/tuckarm.py 2009-07-08 04:22:28 UTC (rev 18460)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/scripts/tuckarm.py 2009-07-08 04:31:43 UTC (rev 18461)
@@ -139,7 +139,7 @@
controllers.append('r_arm_joint_trajectory_controller')
controllers.append('l_arm_joint_trajectory_controller')
- positions_l = [[0.4,0.0,0.0,-2.25,0.0,0.0,0.0], [0.0,1.05,1.57,-2.25,0.0,0.0,0.0]]
+ positions_l = [[0.4,0.0,0.0,-2.25,0.0,0.0,0.0], [0.0,.90,1.57,-2.25,0.0,0.0,0.0]]
positions_r = [[-0.4,0.0,0.0,-2.25,0.0,0.0,0.0], [0.0,1.57,-1.57,-1.57,0.0,0.0,0.0]]
go('r', positions_r)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-07-08 04:59:13
|
Revision: 18444
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18444&view=rev
Author: hsujohnhsu
Date: 2009-07-08 00:26:45 +0000 (Wed, 08 Jul 2009)
Log Message:
-----------
rest of the files for pr2 and ikea cups demo.
Modified Paths:
--------------
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/000.580.67.model
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/001.327.79.model
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/desk3.model
pkg/trunk/robot_descriptions/ikea_objects/CMakeLists.txt
pkg/trunk/stacks/pr2/pr2_ogre/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/701.330.68.model
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/000.580.67.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/000.580.67.model 2009-07-08 00:11:42 UTC (rev 18443)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/000.580.67.model 2009-07-08 00:26:45 UTC (rev 18444)
@@ -1,34 +1,34 @@
<?xml version="1.0" ?>
-<model:physical name="wine_cup">
+<model:physical name="000_580_67">
<xyz> 0 0 0</xyz>
<rpy> 0.0 0.0 0.0</rpy>
- <static>true</static>
- <body:trimesh name="wine_cup_body">
- <geom:trimesh name="wine_cup_geom">
+ <static>false</static>
+ <body:trimesh name="000_580_67_body">
+
+ <massMatrix>true</massMatrix>
+ <mass>1.0</mass>
+ <ixx>0.1</ixx>
+ <ixy>0.0</ixy>
+ <ixz>0.0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0.0</iyz>
+ <izz>0.1</izz>
+ <cx>0.0</cx>
+ <cy>0.0</cy>
+ <cz>0.05</cz>
+
+ <geom:trimesh name="000_580_67_geom">
<xyz>0 0 0</xyz>
<laserRetro>2000.0</laserRetro>
<kp>1000000.0</kp>
<kd>1.0</kd>
- <scale>0.005 0.005 0.005</scale>
- <mesh>ikea_objects/000.580.67.ply_convex.stl.mesh</mesh>
-
- <massMatrix>true</massMatrix>
- <mass>0.1</mass>
- <ixx>0.01</ixx>
- <ixy>0.0</ixy>
- <ixz>0.0</ixz>
- <iyy>0.01</iyy>
- <iyz>0.0</iyz>
- <izz>0.001</izz>
- <cx>0.0</cx>
- <cy>0.0</cy>
- <cz>0.0</cz>
-
+ <scale>0.001 0.001 0.001</scale>
+ <mesh>ikea_objects/000.580.67.obj.stlb.mesh</mesh>
<visual>
<xyz>0 0 0.0</xyz>
- <size>0.45 0.45 1.030</size>
+ <size>0.09 0.09 0.206</size>
<material>PR2/White</material>
- <mesh>ikea_objects/000.580.67.ply_convex.stl.mesh</mesh>
+ <mesh>ikea_objects/000.580.67.obj.stlb.mesh</mesh>
</visual>
</geom:trimesh>
</body:trimesh>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/001.327.79.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/001.327.79.model 2009-07-08 00:11:42 UTC (rev 18443)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/001.327.79.model 2009-07-08 00:26:45 UTC (rev 18444)
@@ -1,34 +1,34 @@
<?xml version="1.0" ?>
-<model:physical name="bowl_cup">
+<model:physical name="001_327_79">
<xyz> 0 0 0</xyz>
<rpy> 0.0 0.0 0.0</rpy>
- <static>true</static>
- <body:trimesh name="bowl_cup_body">
- <geom:trimesh name="bowl_cup_geom">
+ <static>false</static>
+ <body:trimesh name="001_327_79_body">
+
+ <massMatrix>true</massMatrix>
+ <mass>1.0</mass>
+ <ixx>0.1</ixx>
+ <ixy>0.0</ixy>
+ <ixz>0.0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0.0</iyz>
+ <izz>0.1</izz>
+ <cx>0.0</cx>
+ <cy>0.0</cy>
+ <cz>0.05</cz>
+
+ <geom:trimesh name="001_327_79_geom">
<xyz>0 0 0</xyz>
<laserRetro>2000.0</laserRetro>
<kp>1000000.0</kp>
<kd>1.0</kd>
- <scale>0.005 0.005 0.005</scale>
- <mesh>ikea_objects/001.172.98.ply_convex.stl.mesh</mesh>
-
- <massMatrix>true</massMatrix>
- <mass>0.1</mass>
- <ixx>0.01</ixx>
- <ixy>0.0</ixy>
- <ixz>0.0</ixz>
- <iyy>0.01</iyy>
- <iyz>0.0</iyz>
- <izz>0.001</izz>
- <cx>0.0</cx>
- <cy>0.0</cy>
- <cz>0.0</cz>
-
+ <scale>0.001 0.001 0.001</scale>
+ <mesh>ikea_objects/001.327.79.obj.stlb.mesh</mesh>
<visual>
<xyz>0 0 0.0</xyz>
- <size>0.48 0.48 0.565</size>
+ <size>0.0816 0.0816 0.113</size>
<material>PR2/Green</material>
- <mesh>ikea_objects/001.172.98.ply_convex.stl.mesh</mesh>
+ <mesh>ikea_objects/001.327.79.obj.stlb.mesh</mesh>
</visual>
</geom:trimesh>
</body:trimesh>
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/701.330.68.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/701.330.68.model (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/701.330.68.model 2009-07-08 00:26:45 UTC (rev 18444)
@@ -0,0 +1,38 @@
+<?xml version="1.0" ?>
+<model:physical name="701_330_68">
+ <xyz> 0 0 0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>false</static>
+ <body:trimesh name="701_330_68_body">
+
+ <massMatrix>true</massMatrix>
+ <mass>1.0</mass>
+ <ixx>0.1</ixx>
+ <ixy>0.0</ixy>
+ <ixz>0.0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0.0</iyz>
+ <izz>0.1</izz>
+ <cx>0.0</cx>
+ <cy>0.0</cy>
+ <cz>0.05</cz>
+
+ <geom:trimesh name="701_330_68_geom">
+ <xyz>0 0 0</xyz>
+ <laserRetro>2000.0</laserRetro>
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
+ <scale>0.001 0.001 0.001</scale>
+ <mesh>ikea_objects/701.330.68.obj.stlb.mesh</mesh>
+ <visual>
+ <xyz>0 0 0.0</xyz>
+ <size>0.09 0.09 0.206</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>ikea_objects/701.330.68.obj.stlb.mesh</mesh>
+ </visual>
+ </geom:trimesh>
+ </body:trimesh>
+</model:physical>
+
+
+
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/desk3.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/desk3.model 2009-07-08 00:11:42 UTC (rev 18443)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_objects/desk3.model 2009-07-08 00:26:45 UTC (rev 18444)
@@ -3,129 +3,86 @@
<xyz>0 0 0</xyz>
<rpy>0 0 0</rpy>
<body:box name="desk3">
- <xyz>0 0 0.3</xyz>
+ <xyz>0 0 0</xyz>
<rpy>0 0 0</rpy>
+
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>0.1</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.1</iyy>
+ <iyz>0</iyz>
+ <izz>0.1</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
<geom:box name="desk3top_geom">
- <kp>100000000.0</kp>
+ <kp>1000000.0</kp>
<kd>1.0</kd>
- <xyz>0 0 0.6</xyz>
+ <xyz>0 0 0.5</xyz>
<rpy>0 0 0</rpy>
- <massMatrix>true</massMatrix>
- <mass>10.0</mass>
- <ixx>1.0</ixx>
- <ixy>0</ixy>
- <ixz>0</ixz>
- <iyy>1.0</iyy>
- <iyz>0</iyz>
- <izz>1.0</izz>
- <cx>0</cx>
- <cy>0</cy>
- <cz>0</cz>
<size>0.8 2.0 0.10</size>
<visual>
<xyz>0 0 0</xyz>
<rpy>0 0 0</rpy>
<scale>0.8 2.0 0.10</scale>
<mesh>unit_box</mesh>
- <material>Gazebo/Green</material>
+ <material>Gazebo/LightWood</material>
</visual>
</geom:box>
<geom:box name="desk3leg1_geom">
- <kp>100000000.0</kp>
+ <kp>1000000.0</kp>
<kd>1.0</kd>
- <xyz>-0.3 -0.8 .3</xyz>
+ <xyz>-0.3 -0.8 0.25</xyz>
<rpy>0 0 0</rpy>
- <massMatrix>true</massMatrix>
- <mass>10.0</mass>
- <ixx>1.0</ixx>
- <ixy>0</ixy>
- <ixz>0</ixz>
- <iyy>1.0</iyy>
- <iyz>0</iyz>
- <izz>1.0</izz>
- <cx>0</cx>
- <cy>0</cy>
- <cz>0</cz>
- <size>0.05 0.05 0.6</size>
+ <size>0.05 0.05 0.5</size>
<visual>
<xyz>0 0 0</xyz>
<rpy>0 0 0</rpy>
- <scale>0.05 0.05 0.6</scale>
+ <scale>0.05 0.05 0.5</scale>
<mesh>unit_box</mesh>
<material>Gazebo/Red</material>
</visual>
</geom:box>
<geom:box name="desk3leg2_geom">
- <kp>100000000.0</kp>
+ <kp>1000000.0</kp>
<kd>1.0</kd>
- <xyz>0.3 -0.8 .3</xyz>
+ <xyz>0.3 -0.8 0.25</xyz>
<rpy>0 0 0</rpy>
- <massMatrix>true</massMatrix>
- <mass>10.0</mass>
- <ixx>1.0</ixx>
- <ixy>0</ixy>
- <ixz>0</ixz>
- <iyy>1.0</iyy>
- <iyz>0</iyz>
- <izz>1.0</izz>
- <cx>0</cx>
- <cy>0</cy>
- <cz>0</cz>
- <size>0.05 0.05 0.6</size>
+ <size>0.05 0.05 0.5</size>
<visual>
<xyz>0 0 0</xyz>
<rpy>0 0 0</rpy>
- <scale>0.05 0.05 0.6</scale>
+ <scale>0.05 0.05 0.5</scale>
<mesh>unit_box</mesh>
<material>Gazebo/Red</material>
</visual>
</geom:box>
<geom:box name="desk3leg3_geom">
- <kp>100000000.0</kp>
+ <kp>1000000.0</kp>
<kd>1.0</kd>
- <xyz>-0.3 0.8 .3</xyz>
+ <xyz>-0.3 0.8 0.25</xyz>
<rpy>0 0 0</rpy>
- <massMatrix>true</massMatrix>
- <mass>10.0</mass>
- <ixx>1.0</ixx>
- <ixy>0</ixy>
- <ixz>0</ixz>
- <iyy>1.0</iyy>
- <iyz>0</iyz>
- <izz>1.0</izz>
- <cx>0</cx>
- <cy>0</cy>
- <cz>0</cz>
- <size>0.05 0.05 0.6</size>
+ <size>0.05 0.05 0.5</size>
<visual>
<xyz>0 0 0</xyz>
<rpy>0 0 0</rpy>
- <scale>0.05 0.05 0.6</scale>
+ <scale>0.05 0.05 0.5</scale>
<mesh>unit_box</mesh>
<material>Gazebo/Red</material>
</visual>
</geom:box>
<geom:box name="desk3leg4_geom">
- <kp>100000000.0</kp>
+ <kp>1000000.0</kp>
<kd>1.0</kd>
- <xyz>0.3 0.8 .3</xyz>
+ <xyz>0.3 0.8 0.25</xyz>
<rpy>0 0 0</rpy>
- <massMatrix>true</massMatrix>
- <mass>10.0</mass>
- <ixx>1.0</ixx>
- <ixy>0</ixy>
- <ixz>0</ixz>
- <iyy>1.0</iyy>
- <iyz>0</iyz>
- <izz>1.0</izz>
- <cx>0</cx>
- <cy>0</cy>
- <cz>0</cz>
- <size>0.05 0.05 0.6</size>
+ <size>0.05 0.05 0.5</size>
<visual>
<xyz>0 0 0</xyz>
<rpy>0 0 0</rpy>
- <scale>0.05 0.05 0.6</scale>
+ <scale>0.05 0.05 0.5</scale>
<mesh>unit_box</mesh>
<material>Gazebo/Red</material>
</visual>
Modified: pkg/trunk/robot_descriptions/ikea_objects/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/ikea_objects/CMakeLists.txt 2009-07-08 00:11:42 UTC (rev 18443)
+++ pkg/trunk/robot_descriptions/ikea_objects/CMakeLists.txt 2009-07-08 00:26:45 UTC (rev 18444)
@@ -11,7 +11,7 @@
#iv files
# iterate through all the obj files and generate *.mesh and *.iv files
-file(GLOB ikea_objects_obj_files ${CMAKE_CURRENT_SOURCE_DIR}/meshes/*.obj_dont_build)
+file(GLOB ikea_objects_obj_files ${CMAKE_CURRENT_SOURCE_DIR}/meshes/*.obj)
set(ikea_objects_gen_files "")
foreach(it ${ikea_objects_obj_files})
@@ -22,42 +22,46 @@
message("ignoring stale .._convex.* file:",${basename})
ELSE ( ${basename} MATCHES "_convex" )
- #convex directory
- add_custom_command(
- OUTPUT ${basepath}/convex
- COMMAND mkdir -p
- ARGS ${basepath}/convex)
+ #convex directory
+ add_custom_command(
+ OUTPUT ${basepath}/convex
+ COMMAND mkdir -p
+ ARGS ${basepath}/convex)
- #move obj files to convex directory
- add_custom_command(
- OUTPUT ${basepath}/convex/${basename}
- COMMAND cp
- ARGS ${it} ${basepath}/convex/${basename}
- DEPENDS ${it} ${basepath}/convex)
+ #create stl files from obj files
+ add_custom_command(
+ OUTPUT ${basepath}/convex/${basename}.stlb
+ COMMAND ${ivcon_PACKAGE_PATH}/bin/ivcon
+ ARGS ${it} ${basepath}/convex/${basename}.stlb
+ DEPENDS ${it})
+ set(ikea_objects_gen_files ${ikea_objects_gen_files} ${basepath}/convex/${basename}.stlb)
- set(ikea_objects_gen_files ${ikea_objects_gen_files} ${basepath}/convex/${basename})
+# #move obj files to convex directory
+# add_custom_command(
+# OUTPUT ${basepath}/convex/${basename}
+# COMMAND cp
+# ARGS ${it} ${basepath}/convex/${basename}
+# DEPENDS ${it} ${basepath}/convex)
+# set(ikea_objects_gen_files ${ikea_objects_gen_files} ${basepath}/convex/${basename})
+#
+# #convex decomp in convex directory
+# add_custom_command(
+# OUTPUT ${basepath}/convex/${basename}_convex.obj
+# COMMAND ${convex_decomposition_PACKAGE_PATH}/convex_decomposition/bin/convex_decomposition
+# ARGS ${basepath}/convex/${basename} -v64 -p0.01 -c0.01 -d7
+# DEPENDS ${basepath}/convex/${basename})
+# set(ikea_objects_gen_files ${ikea_objects_gen_files} ${basepath}/convex/${basename}_convex.obj)
+#
+#
+# #create convex stl files for convex decomposition obj files
+# add_custom_command(
+# OUTPUT ${basepath}/convex/${basename}_convex.stlb
+# COMMAND ${ivcon_PACKAGE_PATH}/bin/ivcon
+# ARGS ${basepath}/convex/${basename}_convex.obj ${basepath}/convex/${basename}_convex.stlb
+# DEPENDS ${basepath}/convex/${basename}_convex.obj)
+# set(ikea_objects_gen_files ${ikea_objects_gen_files} ${basepath}/convex/${basename}_convex.stlb)
- #convex decomp in convex directory
- add_custom_command(
- OUTPUT ${basepath}/convex/${basename}_convex.obj
- COMMAND ${convex_decomposition_PACKAGE_PATH}/convex_decomposition/bin/convex_decomposition
- ARGS ${basepath}/convex/${basename} -v64 -p0.01 -c0.01 -d7
- DEPENDS ${basepath}/convex/${basename})
- set(ikea_objects_gen_files ${ikea_objects_gen_files} ${basepath}/convex/${basename}_convex.obj)
-
- #create convex stl files for convex decomposition obj files
- add_custom_command(
- OUTPUT ${basepath}/convex/${basename}_convex.stlb
- #COMMAND /usr/bin/meshlabserver
- #ARGS -i ${basepath}/convex/${basename}_convex.obj -o ${basepath}/convex/${basename}_convex.stl
- COMMAND ${ivcon_PACKAGE_PATH}/bin/ivcon
- ARGS ${basepath}/convex/${basename}_convex.obj ${basepath}/convex/${basename}_convex.stlb
- DEPENDS ${basepath}/convex/${basename}_convex.obj)
-
- set(ikea_objects_gen_files ${ikea_objects_gen_files} ${basepath}/convex/${basename}_convex.stlb)
-
-
ENDIF ( ${basename} MATCHES "_convex" )
endforeach(it)
Modified: pkg/trunk/stacks/pr2/pr2_ogre/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/pr2/pr2_ogre/CMakeLists.txt 2009-07-08 00:11:42 UTC (rev 18443)
+++ pkg/trunk/stacks/pr2/pr2_ogre/CMakeLists.txt 2009-07-08 00:26:45 UTC (rev 18444)
@@ -52,9 +52,10 @@
find_ros_package(ikea_objects)
-if(EXISTS ikea_objects_PACKAGE_PATH)
+MESSAGE(STATUS "\nikea_objects path ${ikea_objects_PACKAGE_PATH}\n")
+if(EXISTS ${ikea_objects_PACKAGE_PATH})
# build the ogre mesh files from *.stl (and *.stlb from convex decomposition)
- file(GLOB ikea_objects_stl_files ${ikea_objects_PACKAGE_PATH}/meshes/convex/*_convex.stl)
+ file(GLOB ikea_objects_stl_files ${ikea_objects_PACKAGE_PATH}/meshes/convex/*.stlb)
set(ikea_objects_gen_files "")
set(ikea_objects_out_path ${CMAKE_CURRENT_SOURCE_DIR}/Media/models/ikea_objects)
@@ -77,5 +78,5 @@
endforeach(it)
add_custom_target(ikea_objects_media_files ALL DEPENDS ${ikea_objects_gen_files})
-endif(EXISTS ikea_objects_PACKAGE_PATH)
+endif(EXISTS ${ikea_objects_PACKAGE_PATH})
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-07-08 23:27:16
|
Revision: 18514
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18514&view=rev
Author: gerkey
Date: 2009-07-08 23:27:08 +0000 (Wed, 08 Jul 2009)
Log Message:
-----------
blacklisting openraveros and its dependents
Added Paths:
-----------
pkg/trunk/calibration/laser_camera_calibration/ROS_BUILD_BLACKLIST
pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST
pkg/trunk/openrave_planning/ormanipulation/ROS_BUILD_BLACKLIST
Added: pkg/trunk/calibration/laser_camera_calibration/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/calibration/laser_camera_calibration/ROS_BUILD_BLACKLIST 2009-07-08 23:27:08 UTC (rev 18514)
@@ -0,0 +1 @@
+Depends on openraveros, which is blacklisted
Added: pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST 2009-07-08 23:27:08 UTC (rev 18514)
@@ -0,0 +1,19 @@
+In file included from /home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/session.h:36,
+ from /home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/openraveros.cpp:27:
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/rosserver.h: In member function 'void ROSServer::FillRobotInfo(OpenRAVE::RobotBase*, openraveros::RobotInfo&, uint32_t)':
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/rosserver.h:829: error: cannot convert 'const char*' to 'const wchar_t*' for argument '1' to 'std::string _stdwcstombs(const wchar_t*)'
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/rosserver.h: In member function 'bool ROSServer::robot_sensorgetdata_srv(openraveros::robot_sensorgetdata::Request&, openraveros::robot_sensorgetdata::Response&)':
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/rosserver.h:1326: warning: enumeration value 'ST_Invalid' not handled in switch
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/rosserver.h:1326: warning: enumeration value 'ST_JointEncoder' not handled in switch
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/rosserver.h:1326: warning: enumeration value 'ST_Force6D' not handled in switch
+In file included from /home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/openraveros.cpp:27:
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/session.h: In member function 'bool SessionServer::Init()':
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/session.h:96: warning: 'mapName' is deprecated (declared at /home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/ros/core/roscpp/include/ros/node.h:1251)
+make[3]: *** [CMakeFiles/openraveros.dir/src/openraveros.o] Error 1
+make[3]: Leaving directory `/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/build'
+make[2]: Leaving directory `/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/build'
+make[1]: Leaving directory `/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/build'
+make[2]: *** [CMakeFiles/openraveros.dir/all] Error 2
+make[1]: *** [all] Error 2
+make: *** [all] Error 2
+[rosmakeall-buildfail] Build of openraveros failed
Added: pkg/trunk/openrave_planning/ormanipulation/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/ROS_BUILD_BLACKLIST 2009-07-08 23:27:08 UTC (rev 18514)
@@ -0,0 +1 @@
+Depends on openraveros, 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-08 23:35:37
|
Revision: 18516
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18516&view=rev
Author: rdiankov
Date: 2009-07-08 23:35:28 +0000 (Wed, 08 Jul 2009)
Log Message:
-----------
removed blacklists
Modified Paths:
--------------
pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m
pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
Removed Paths:
-------------
pkg/trunk/calibration/laser_camera_calibration/ROS_BUILD_BLACKLIST
pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST
pkg/trunk/openrave_planning/ormanipulation/ROS_BUILD_BLACKLIST
Deleted: pkg/trunk/calibration/laser_camera_calibration/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/ROS_BUILD_BLACKLIST 2009-07-08 23:28:18 UTC (rev 18515)
+++ pkg/trunk/calibration/laser_camera_calibration/ROS_BUILD_BLACKLIST 2009-07-08 23:35:28 UTC (rev 18516)
@@ -1 +0,0 @@
-Depends on openraveros, which is blacklisted
Deleted: pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST 2009-07-08 23:28:18 UTC (rev 18515)
+++ pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST 2009-07-08 23:35:28 UTC (rev 18516)
@@ -1,19 +0,0 @@
-In file included from /home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/session.h:36,
- from /home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/openraveros.cpp:27:
-/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/rosserver.h: In member function 'void ROSServer::FillRobotInfo(OpenRAVE::RobotBase*, openraveros::RobotInfo&, uint32_t)':
-/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/rosserver.h:829: error: cannot convert 'const char*' to 'const wchar_t*' for argument '1' to 'std::string _stdwcstombs(const wchar_t*)'
-/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/rosserver.h: In member function 'bool ROSServer::robot_sensorgetdata_srv(openraveros::robot_sensorgetdata::Request&, openraveros::robot_sensorgetdata::Response&)':
-/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/rosserver.h:1326: warning: enumeration value 'ST_Invalid' not handled in switch
-/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/rosserver.h:1326: warning: enumeration value 'ST_JointEncoder' not handled in switch
-/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/rosserver.h:1326: warning: enumeration value 'ST_Force6D' not handled in switch
-In file included from /home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/openraveros.cpp:27:
-/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/session.h: In member function 'bool SessionServer::Init()':
-/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/src/session.h:96: warning: 'mapName' is deprecated (declared at /home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/ros/core/roscpp/include/ros/node.h:1251)
-make[3]: *** [CMakeFiles/openraveros.dir/src/openraveros.o] Error 1
-make[3]: Leaving directory `/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/build'
-make[2]: Leaving directory `/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/build'
-make[1]: Leaving directory `/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/openrave_planning/openraveros/build'
-make[2]: *** [CMakeFiles/openraveros.dir/all] Error 2
-make[1]: *** [all] Error 2
-make: *** [all] Error 2
-[rosmakeall-buildfail] Build of openraveros failed
Deleted: pkg/trunk/openrave_planning/ormanipulation/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/ROS_BUILD_BLACKLIST 2009-07-08 23:28:18 UTC (rev 18515)
+++ pkg/trunk/openrave_planning/ormanipulation/ROS_BUILD_BLACKLIST 2009-07-08 23:35:28 UTC (rev 18516)
@@ -1 +0,0 @@
-Depends on openraveros, which is blacklisted
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m 2009-07-08 23:28:18 UTC (rev 18515)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m 2009-07-08 23:35:28 UTC (rev 18516)
@@ -64,11 +64,14 @@
orBodySetTransform(Target.id, [0 0 0], [1 0 0 0]); % identity
-standoffs = [0.01];
-rolls = [0 pi/2]; % hand is symmetric
+standoffs = [0.01 0.03];
+rolls = [0 pi/4 pi/2]; % hand is symmetric
+use_noise = 0;
+stepsize = 0.02;
+add_spherenorms = 1;
% start simulating grasps
-[GraspTable, GraspStats] = MakeGraspTable(robot,Target,preshapes, standoffs, rolls,0,0.02);
+[GraspTable, GraspStats] = MakeGraspTable(robot,Target,preshapes, standoffs, rolls,use_noise,stepsize,add_spherenorms);
if( ~isempty(GraspTable) && ~isempty(GraspStats) )
%% save the table
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-07-08 23:28:18 UTC (rev 18515)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-07-08 23:35:28 UTC (rev 18516)
@@ -21,4 +21,4 @@
orEnvSetOptions('wdims 800 600');
orEnvSetOptions('simulation timestep 0.001');
orEnvSetOptions('collision ode');
-%orEnvSetOptions('debug debug');
+orEnvSetOptions('debug debug');
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-07-09 00:54:20
|
Revision: 18528
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18528&view=rev
Author: vijaypradeep
Date: 2009-07-09 00:54:16 +0000 (Thu, 09 Jul 2009)
Log Message:
-----------
StereoCheckerboard detector now publishes cb corners with a more reasonable msg type
Working on stereo checkerboard capture action
Modified Paths:
--------------
pkg/trunk/vision/stereo_checkerboard_detector/manifest.xml
pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp
Added Paths:
-----------
pkg/trunk/calibration/calibration_msgs/msg/CbCamCorner.msg
pkg/trunk/calibration/calibration_msgs/msg/CbCamCorners.msg
pkg/trunk/calibration/calibration_msgs/msg/CbStereoCorners.msg
pkg/trunk/calibration/calibration_msgs/msg/PixelPoint.msg
pkg/trunk/calibration/sandbox/pr2_calibration_actions/
pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt
pkg/trunk/calibration/sandbox/pr2_calibration_actions/Makefile
pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/
pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/
pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/stereo_cb_action.h
pkg/trunk/calibration/sandbox/pr2_calibration_actions/manifest.xml
pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/
pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/CbCamCmd.msg
pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/StereoCbActionState.msg
pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/
pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/run_action_stereo_cb.cpp
pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/stereo_cb_action.cpp
pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/test_stereo_cb.cpp
Added: pkg/trunk/calibration/calibration_msgs/msg/CbCamCorner.msg
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/CbCamCorner.msg (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/msg/CbCamCorner.msg 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,4 @@
+# Stores a single checkerboard corner, as sensed by a camera
+int32 x_index # The x index of the checkerboard corner
+int32 y_index # The y index of the checkerboard corner
+PixelPoint point # The sensed corner location in pixel coordinates
\ No newline at end of file
Added: pkg/trunk/calibration/calibration_msgs/msg/CbCamCorners.msg
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/CbCamCorners.msg (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/msg/CbCamCorners.msg 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,5 @@
+# Stores a set of checkerboard corners as sensed by a camera
+Header header
+CbCamCorner[] corners # Array of the sensed corners
+int16 num_x # The number of checkerboard corners in the x direction
+int16 num_y # The number of checkerboard corners in the y direction
\ No newline at end of file
Added: pkg/trunk/calibration/calibration_msgs/msg/CbStereoCorners.msg
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/CbStereoCorners.msg (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/msg/CbStereoCorners.msg 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,3 @@
+Header header
+CbCamCorners left
+CbCamCorners right
\ No newline at end of file
Added: pkg/trunk/calibration/calibration_msgs/msg/PixelPoint.msg
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/PixelPoint.msg (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/msg/PixelPoint.msg 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,3 @@
+# Stores the xy location of a point in an image in pixel coordinates
+float64 x
+float64 y
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+rospack(pr2_calibration_actions)
+
+genmsg()
+
+rospack_add_library(${PROJECT_NAME} src/stereo_cb_action.cpp)
+rospack_add_boost_directories()
+rospack_link_boost(${PROJECT_NAME} thread)
+
+
+rospack_add_executable(run_action_stereo_cb src/run_action_stereo_cb.cpp)
+target_link_libraries(run_action_stereo_cb ${PROJECT_NAME})
+
+rospack_add_executable(test_stereo_cb src/test_stereo_cb.cpp)
+target_link_libraries(test_stereo_cb ${PROJECT_NAME})
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
\ No newline at end of file
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/Makefile
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/Makefile (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/Makefile 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/stereo_cb_action.h
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/stereo_cb_action.h (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/stereo_cb_action.h 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,86 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, 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 PR2_CALIBRATION_ACTIONS_STEREO_CB_ACTION_H_
+#define PR2_CALIBRATION_ACTIONS_STEREO_CB_ACTION_H_
+
+#include <string>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition.hpp>
+
+#include "ros/ros.h"
+#include "robot_actions/action.h"
+
+#include "pr2_calibration_actions/CbCamCmd.h"
+#include "calibration_msgs/CbStereoCorners.h"
+
+namespace pr2_calibration_actions
+{
+
+/**
+ *
+ */
+class StereoCbAction : public robot_actions::Action<CbCamCmd, calibration_msgs::CbStereoCorners>
+{
+public:
+ StereoCbAction();
+
+ virtual robot_actions::ResultStatus execute(const CbCamCmd& goal, calibration_msgs::CbStereoCorners& feedback) ;
+
+
+private:
+ void handlePreempt() ;
+ void cornersCallback(const calibration_msgs::CbStereoCornersConstPtr& msg) ;
+ void resetStereoAvg(const CbCamCmd& goal) ;
+ bool pixelTooFar(const calibration_msgs::PixelPoint& a, const calibration_msgs::PixelPoint& b, double tol) ;
+
+ ros::NodeHandle n_ ;
+ ros::Subscriber sub_ ;
+
+ std::string controller_;
+
+ boost::mutex data_mutex_ ;
+ boost::condition corners_available_ ;
+ calibration_msgs::CbStereoCorners avg_ ;
+ CbCamCmd goal_ ;
+ unsigned int num_samples_ ;
+ bool accumulating_ ;
+ bool need_to_abort_ ;
+
+} ;
+
+}
+
+
+#endif
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/manifest.xml
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/manifest.xml (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/manifest.xml 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,24 @@
+<package>
+ <description brief="pr2_calibration_actions">
+ Actions specific to the PR2 for collecting calibration information
+ </description>
+ <author>Vijay Pradeep / vpr...@wi...</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/calibration_msgs</url>
+
+ <depend package="std_msgs" />
+ <depend package="robot_msgs" />
+ <depend package="sensor_msgs" />
+ <depend package="calibration_msgs" />
+ <depend package="pr2_msgs" />
+
+ <depend package="robot_actions" />
+
+ <export>
+ <cpp cflags="-I${prefix}/msg/cpp" />
+ </export>
+
+</package>
+
+
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/CbCamCmd.msg
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/CbCamCmd.msg (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/CbCamCmd.msg 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,3 @@
+uint16 num_x # The number of checkerboard corners in the x direction
+uint16 num_y # The number of checkerboard corners in the y direction
+float64 pixel_tol # The max difference in sensed pixel location of a corner across a set of camera frames
\ No newline at end of file
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/StereoCbActionState.msg
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/StereoCbActionState.msg (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/StereoCbActionState.msg 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,4 @@
+Header header
+CbCamCmd goal
+calibration_msgs/CbStereoCorners feedback
+robot_actions/ActionStatus status
\ No newline at end of file
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/run_action_stereo_cb.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/run_action_stereo_cb.cpp (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/run_action_stereo_cb.cpp 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,63 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, 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 "robot_actions/action_runner.h"
+
+#include "pr2_calibration_actions/stereo_cb_action.h"
+#include "pr2_calibration_actions/StereoCbActionState.h"
+#include "pr2_calibration_actions/CbCamCmd.h"
+#include "calibration_msgs/CbStereoCorners.h"
+
+
+using namespace pr2_calibration_actions ;
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "stereo_cb_runner") ;
+
+ ros::NodeHandle n ;
+
+ StereoCbAction action ;
+ robot_actions::ActionRunner runner(10.0) ;
+
+ runner.connect<CbCamCmd, StereoCbActionState, calibration_msgs::CbStereoCorners>(action) ;
+
+ ROS_DEBUG("Calling run") ;
+ runner.run() ;
+ ROS_DEBUG("Done Calling run") ;
+
+ ros::spin() ;
+
+ return 0 ;
+}
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/stereo_cb_action.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/stereo_cb_action.cpp (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/stereo_cb_action.cpp 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,178 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, 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 "pr2_calibration_actions/stereo_cb_action.h"
+#include "calibration_msgs/CbCamCorners.h"
+
+using namespace pr2_calibration_actions ;
+using namespace calibration_msgs ;
+
+StereoCbAction::StereoCbAction() : robot_actions::Action<CbCamCmd, CbStereoCorners>("stereo_cb_action")
+{
+ sub_ = n_.subscribe("stereo_corners", 1, &StereoCbAction::cornersCallback, this) ;
+ accumulating_ = false ;
+}
+
+robot_actions::ResultStatus StereoCbAction::execute(const CbCamCmd& goal, CbStereoCorners& feedback)
+{
+ boost::mutex::scoped_lock lock(data_mutex_) ;
+
+ resetStereoAvg(goal) ;
+ accumulating_ = true ;
+ need_to_abort_ = false ;
+ goal_ = goal ;
+
+ boost::posix_time::milliseconds condition_timeout(1000.0f) ; // 1 sec signal timeout
+ while(true)
+ {
+ corners_available_.timed_wait(data_mutex_, condition_timeout) ;
+
+ if (need_to_abort_)
+ {
+ ROS_DEBUG("ABORTING") ;
+ feedback = avg_ ;
+ return robot_actions::ABORTED ;
+ }
+
+ if (isPreemptRequested())
+ {
+ ROS_DEBUG("PREEMPTED") ;
+ feedback = avg_ ;
+ return robot_actions::PREEMPTED ;
+ }
+ }
+}
+
+void StereoCbAction::handlePreempt()
+{
+ ROS_DEBUG("Handle Preempt") ;
+ corners_available_.notify_all() ;
+}
+
+void StereoCbAction::cornersCallback(const calibration_msgs::CbStereoCornersConstPtr& msg)
+{
+ if (msg->left.corners.size() == 0)
+ {
+ ROS_INFO("Didn't see checkerboard in left cam") ;
+ return ;
+ }
+
+ if (msg->right.corners.size() == 0)
+ {
+ ROS_INFO("Didn't see checkerboard in right cam") ;
+ return ;
+ }
+
+ boost::mutex::scoped_lock lock(data_mutex_) ;
+
+ if (!accumulating_)
+ return ;
+
+ const unsigned int N = msg->left.corners.size() ;
+ if (msg->right.corners.size() != N)
+ {
+ ROS_ERROR("Left and right images don't have same # of corners") ;
+ need_to_abort_ = true ;
+ return ;
+ }
+
+ if (avg_.left.corners.size() != N)
+ {
+ ROS_ERROR("Received image does not have same # of corners as avg ") ;
+ need_to_abort_ = true ;
+ return ;
+ }
+
+ // Check pixel distances. Trigger an abort if the pixel dist is too far
+ for (unsigned int i=0; i<N; i++)
+ {
+ need_to_abort_ = need_to_abort_ || pixelTooFar(avg_.left.corners[i].point, msg->left.corners[i].point, goal_.pixel_tol) ;
+ need_to_abort_ = need_to_abort_ || pixelTooFar(avg_.right.corners[i].point, msg->right.corners[i].point, goal_.pixel_tol) ;
+ }
+
+ for (unsigned int i=0; i<N; i++)
+ {
+
+ if (avg_.left.corners[i].x_index != msg->left.corners[i].x_index ||
+ avg_.left.corners[i].y_index != msg->left.corners[i].y_index ||
+ avg_.right.corners[i].x_index != msg->right.corners[i].x_index ||
+ avg_.right.corners[i].y_index != msg->right.corners[i].y_index)
+ {
+ ROS_ERROR("Corner indicies don't line up. This action doesn't support this [tricky] case yet") ;
+ need_to_abort_ = true ;
+ }
+
+ avg_.left.corners[i].point.x = (avg_.left.corners[i].point.x*num_samples_ + msg->left.corners[i].point.x)/ (num_samples_+1) ;
+ avg_.left.corners[i].point.y = (avg_.left.corners[i].point.y*num_samples_ + msg->left.corners[i].point.y)/ (num_samples_+1) ;
+ avg_.right.corners[i].point.x = (avg_.right.corners[i].point.x*num_samples_ + msg->right.corners[i].point.x)/(num_samples_+1) ;
+ avg_.right.corners[i].point.y = (avg_.right.corners[i].point.y*num_samples_ + msg->right.corners[i].point.y)/(num_samples_+1) ;
+ }
+ num_samples_++ ;
+
+ corners_available_.notify_all() ;
+}
+
+void resetCbCamCorners(CbCamCorners& cam, const CbCamCmd& goal)
+{
+ cam.corners.resize(goal.num_x * goal.num_y) ;
+
+ for (unsigned int j=0; j<goal.num_y; j++)
+ {
+ for (unsigned int i=0; i<goal.num_x; i++)
+ {
+ cam.corners[j*goal.num_x + i].x_index = i ;
+ cam.corners[j*goal.num_x + i].y_index = j ;
+ cam.corners[j*goal.num_x + i].point.x = 0.0 ;
+ cam.corners[j*goal.num_x + i].point.y = 0.0 ;
+ }
+ }
+
+ cam.num_x = goal.num_x ;
+ cam.num_y = goal.num_y ;
+}
+
+void StereoCbAction::resetStereoAvg(const CbCamCmd& goal)
+{
+ resetCbCamCorners(avg_.left, goal) ;
+ resetCbCamCorners(avg_.right, goal) ;
+}
+
+
+bool StereoCbAction::pixelTooFar(const PixelPoint& a, const PixelPoint& b, double tol)
+{
+ double dx = a.x - b.x ;
+ double dy = a.y - b.y ;
+
+ return dx > tol || dx < -tol || dy > tol || dy < -tol ;
+}
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/test_stereo_cb.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/test_stereo_cb.cpp (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/test_stereo_cb.cpp 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,95 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, 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 "robot_actions/action_client.h"
+
+#include "pr2_calibration_actions/StereoCbActionState.h"
+#include "pr2_calibration_actions/CbCamCmd.h"
+#include "calibration_msgs/CbStereoCorners.h"
+
+
+using namespace pr2_calibration_actions ;
+
+void spinThread()
+{
+ ros::spin();
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "test_single_scan") ;
+
+ ros::NodeHandle n ;
+
+ boost::thread spinthread = boost::thread(boost::bind(&spinThread)) ;
+
+ robot_actions::ActionClient<CbCamCmd, StereoCbActionState, calibration_msgs::CbStereoCorners> client("stereo_cb_action") ;
+ ros::Duration duration ;
+
+ duration.fromSec(3.0) ;
+ ROS_DEBUG("Sleeping") ;
+ duration.sleep() ;
+ ROS_DEBUG("Done Sleeping") ;
+
+ CbCamCmd goal ;
+ goal.num_x = 6 ;
+ goal.num_y = 8 ;
+ goal.pixel_tol = 2 ;
+
+ calibration_msgs::CbStereoCorners feedback ;
+
+ robot_actions::ResultStatus result = client.execute(goal, feedback, ros::Duration().fromSec(30.0)) ;
+
+ ROS_DEBUG("ResultStatus=%u", result) ;
+
+ switch(result)
+ {
+ case robot_actions::SUCCESS :
+ ROS_DEBUG("SUCCESS!") ;
+ break ;
+ case robot_actions::ABORTED :
+ ROS_DEBUG("aborted") ;
+ break ;
+ case robot_actions::PREEMPTED :
+ ROS_DEBUG("preempted") ;
+ break ;
+ default :
+ ROS_DEBUG("other") ;
+ break ;
+ }
+
+ ROS_DEBUG("CB Size: [%u, %u]", feedback.left.num_x, feedback.right.num_y) ;
+
+ return 0 ;
+}
Modified: pkg/trunk/vision/stereo_checkerboard_detector/manifest.xml
===================================================================
--- pkg/trunk/vision/stereo_checkerboard_detector/manifest.xml 2009-07-09 00:53:46 UTC (rev 18527)
+++ pkg/trunk/vision/stereo_checkerboard_detector/manifest.xml 2009-07-09 00:54:16 UTC (rev 18528)
@@ -9,6 +9,7 @@
<depend package="roscpp"/>
<depend package="std_msgs"/>
<depend package="robot_msgs"/>
+ <depend package="calibration_msgs"/>
<depend package="opencv_latest"/>
<depend package="sensor_msgs"/>
<depend package="topic_synchronizer"/>
Modified: pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp
===================================================================
--- pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp 2009-07-09 00:53:46 UTC (rev 18527)
+++ pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp 2009-07-09 00:54:16 UTC (rev 18528)
@@ -43,9 +43,12 @@
#include "topic_synchronizer/topic_synchronizer.h"
#include "visualization_msgs/Marker.h"
+#include "calibration_msgs/CbStereoCorners.h"
+#include "calibration_msgs/CbCamCorners.h"
using namespace stereo_checkerboard_detector ;
using namespace std ;
+using namespace calibration_msgs ;
class StereoCheckerboardNode
{
@@ -56,13 +59,16 @@
ros::Duration().fromSec(0.05),
&StereoCheckerboardNode::msgTimeout )
{
- cb_helper_.setSize(6,8, .107) ;
+ num_x_ = 6 ;
+ num_y_ = 8 ;
+ cb_helper_.setSize(num_x_, num_y_, .107) ;
sync_.subscribe("stereo/left/image_rect", left_image_msg_, 1) ;
sync_.subscribe("stereo/left/cam_info", left_info_msg_, 1) ;
sync_.subscribe("stereo/right/image_rect",right_image_msg_, 1) ;
sync_.subscribe("stereo/right/cam_info", right_info_msg_, 1) ;
+ node_->advertise<calibration_msgs::CbStereoCorners>("cb_stereo_corners", 1) ;
node_->advertise<robot_msgs::PointCloud>("cb_corners", 1) ;
node_->advertise<robot_msgs::PointCloud>("cb_corners_left", 1) ;
node_->advertise<robot_msgs::PointCloud>("cb_corners_right", 1) ;
@@ -90,6 +96,30 @@
}
}
+ void pointVecToCbCamCorners(const vector<robot_msgs::Point>& vec, unsigned int num_x, unsigned int num_y,
+ calibration_msgs::CbCamCorners& corners)
+ {
+ corners.num_x = num_x ;
+ corners.num_y = num_y ;
+ if (vec.size() != num_x * num_y)
+ {
+ ROS_ERROR("vec.size()=%u, num_x=%u, num_y=%u", vec.size(), num_x, num_y) ;
+ return ;
+ }
+
+ for (unsigned int j=0; j<num_y; j++)
+ {
+ for (unsigned int i=0; i<num_x; i++)
+ {
+ const unsigned int ind = j*num_x + num_x ;
+ corners.corners[ind].x_index = i ;
+ corners.corners[ind].y_index = j ;
+ corners.corners[ind].point.x = vec[ind].x ;
+ corners.corners[ind].point.y = vec[ind].y ;
+ }
+ }
+ }
+
void msgCallback(ros::Time t)
{
bool found = cb_helper_.findCheckerboard(left_image_msg_, right_image_msg_, left_info_msg_, right_info_msg_) ;
@@ -97,6 +127,13 @@
robot_msgs::PointCloud cloud ;
cloud.header.frame_id = left_image_msg_.header.frame_id ;
cloud.header.stamp = left_image_msg_.header.stamp ;
+
+ CbStereoCorners stereo_corners ;
+ stereo_corners.header.frame_id = left_image_msg_.header.frame_id ;
+ stereo_corners.header.stamp = left_image_msg_.header.stamp ;
+ stereo_corners.left.header = stereo_corners.header ;
+ stereo_corners.right.header = stereo_corners.header ;
+
if (found)
{
//unsigned int N = xyz.size() ;
@@ -109,15 +146,21 @@
node_->publish("cb_corners", cloud) ;
// Publish the left corners
- cb_helper_.getCornersLeft(point_vec) ;
- pointVecToCloud(point_vec, cloud) ;
+ vector<robot_msgs::Point> left_point_vec ;
+ cb_helper_.getCornersLeft(left_point_vec) ;
+ pointVecToCloud(left_point_vec, cloud) ;
+ pointVecToCbCamCorners(left_point_vec, num_x_, num_y_, stereo_corners.left) ;
node_->publish("cb_corners_left", cloud) ;
// Publish the right corners
- cb_helper_.getCornersRight(point_vec) ;
- pointVecToCloud(point_vec, cloud) ;
+ vector<robot_msgs::Point> right_point_vec ;
+ cb_helper_.getCornersRight(right_point_vec) ;
+ pointVecToCloud(right_point_vec, cloud) ;
+ pointVecToCbCamCorners(right_point_vec, num_x_, num_y_, stereo_corners.right) ;
node_->publish("cb_corners_right", cloud) ;
+ node_->publish("cb_stereo_corners", stereo_corners) ;
+
// Publish a mini-axes defining the checkerboard
tf::Pose pose ;
cb_helper_.getPose(pose) ;
@@ -136,13 +179,20 @@
cloud.pts.clear() ;
node_->publish("cb_corners_left", cloud) ;
node_->publish("cb_corners_right", cloud) ;
+
+ stereo_corners.left.num_x = num_x_ ;
+ stereo_corners.left.num_y = num_y_ ;
+ stereo_corners.left.corners.clear() ;
+ stereo_corners.right.num_x = num_x_ ;
+ stereo_corners.right.num_y = num_y_ ;
+ stereo_corners.left.corners.clear() ;
+ node_->publish("cb_stereo_corners", stereo_corners) ;
printf("Not found\n") ;
}
node_->publish("left_cb_debug", cb_helper_.getLeftDebug()) ;
node_->publish("right_cb_debug", cb_helper_.getRightDebug()) ;
-
}
void msgTimeout(ros::Time t)
@@ -217,6 +267,9 @@
StereoCheckerboardHelper cb_helper_ ;
TopicSynchronizer<StereoCheckerboardNode> sync_ ;
+ unsigned int num_x_ ;
+ unsigned int num_y_ ;
+
sensor_msgs::Image left_image_msg_ ;
sensor_msgs::CamInfo left_info_msg_ ;
sensor_msgs::Image right_image_msg_ ;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-09 02:06:25
|
Revision: 18537
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18537&view=rev
Author: isucan
Date: 2009-07-09 02:06:18 +0000 (Thu, 09 Jul 2009)
Log Message:
-----------
updated method for maintaining robot pose
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/mainpage.dox
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-07-09 02:06:18 UTC (rev 18537)
@@ -462,7 +462,7 @@
for (unsigned int i = 0 ; i < joints.size() ; ++i)
{
start_state[i].header.frame_id = planningMonitor_->getFrameId();
- start_state[i].header.stamp = planningMonitor_->lastStateUpdate();
+ start_state[i].header.stamp = planningMonitor_->lastMechanismStateUpdate();
start_state[i].joint_name = joints[i]->name;
st.copyParamsJoint(start_state[i].value, joints[i]->name);
}
@@ -497,7 +497,7 @@
motion_planning_msgs::JointConstraint jc;
jc.joint_name = arm_joint_names_[i];
jc.header.frame_id = req.goal_constraints.pose_constraint[0].pose.header.frame_id;
- jc.header.stamp = planningMonitor_->lastStateUpdate();
+ jc.header.stamp = planningMonitor_->lastMechanismStateUpdate();
unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
for (unsigned int j = 0 ; j < u ; ++j)
{
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-09 02:06:18 UTC (rev 18537)
@@ -429,7 +429,7 @@
return;
visualization_msgs::Marker mk;
- mk.header.stamp = psetup->model->planningMonitor->lastStateUpdate();
+ mk.header.stamp = psetup->model->planningMonitor->lastMechanismStateUpdate();
mk.header.frame_id = psetup->model->planningMonitor->getFrameId();
mk.ns = nh_.getName();
mk.id = 1;
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-09 02:06:18 UTC (rev 18537)
@@ -56,7 +56,7 @@
planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, frame);
}
else
- planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_);
+ planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, "torso_lift_link");
planKinematicPathService_ = nodeHandle_.advertiseService("plan_kinematic_path", &OMPLPlanning::planToGoal, this);
}
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-09 02:06:18 UTC (rev 18537)
@@ -93,7 +93,7 @@
for (unsigned int i = 0 ; i < req.goal_constraints.joint_constraint.size(); ++i)
{
req.goal_constraints.joint_constraint[i].header.stamp = ros::Time::now();
- req.goal_constraints.joint_constraint[i].header.frame_id = "base_link";
+ req.goal_constraints.joint_constraint[i].header.frame_id = "/base_link";
req.goal_constraints.joint_constraint[i].joint_name = names[i];
req.goal_constraints.joint_constraint[i].value.resize(1);
req.goal_constraints.joint_constraint[i].toleranceAbove.resize(1);
@@ -146,7 +146,7 @@
req.goal_constraints.joint_constraint.resize(1);
req.goal_constraints.joint_constraint[0].header.stamp = ros::Time::now();
- req.goal_constraints.joint_constraint[0].header.frame_id = "base_link";
+ req.goal_constraints.joint_constraint[0].header.frame_id = "/base_link";
req.goal_constraints.joint_constraint[0].joint_name = "base_joint";
req.goal_constraints.joint_constraint[0].value.resize(3);
req.goal_constraints.joint_constraint[0].toleranceAbove.resize(3);
@@ -166,7 +166,7 @@
// allow 1 second computation time
- req.allowed_time = 20.0;
+ req.allowed_time = 0.5;
// define the service messages
motion_planning_srvs::MotionPlan::Response res;
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-07-09 02:06:18 UTC (rev 18537)
@@ -115,24 +115,35 @@
return robot_frame_;
}
- /** \brief Return true if a full mechanism state has been received */
+ /** \brief Return true if a full mechanism state has been received (including pose, if pose inclusion is enabled) */
bool haveState(void) const
{
return haveMechanismState_ && (!includePose_ || (includePose_ && havePose_));
}
-
+
/** \brief Return the time of the last state update */
- const ros::Time& lastStateUpdate(void) const
+ const ros::Time& lastMechanismStateUpdate(void) const
{
- return lastStateUpdate_;
+ return lastMechanismStateUpdate_;
}
- /** \brief Wait until a full mechanism state is received */
+ /** \brief Return the time of the last pose update */
+ const ros::Time& lastPoseUpdate(void) const
+ {
+ return lastPoseUpdate_;
+ }
+
+ /** \brief Wait until a full mechanism state is received (including pose, if pose inclusion is enabled) */
void waitForState(void) const;
- /** \brief Return true if a full mechanism state has been received in the last sec seconds. If sec < 10us, this function always returns true. */
- bool isStateUpdated(double sec) const;
+ /** \brief Return true if a mechanism state has been received in the last sec seconds. If sec < 10us, this function always returns true. */
+ bool isMechanismStateUpdated(double sec) const;
+ /** \brief Return true if a pose has been received in the last
+ sec seconds. If sec < 10us, this function always returns
+ true. */
+ bool isPoseUpdated(double sec) const;
+
/** \brief Return true if the pose is included in the state */
bool isPoseIncluded(void) const
{
@@ -170,7 +181,8 @@
bool havePose_;
bool haveMechanismState_;
- ros::Time lastStateUpdate_;
+ ros::Time lastMechanismStateUpdate_;
+ ros::Time lastPoseUpdate_;
};
}
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h 2009-07-09 02:06:18 UTC (rev 18537)
@@ -114,7 +114,8 @@
double intervalCollisionMap_;
double intervalState_;
-
+ double intervalPose_;
+
};
Modified: pkg/trunk/motion_planning/planning_environment/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/planning_environment/mainpage.dox 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/mainpage.dox 2009-07-09 02:06:18 UTC (rev 18537)
@@ -191,6 +191,8 @@
- @b "~refresh_interval_kinematic_state"/double : if more than this interval passes, the maintained kinematic state is considered invalid
+ - @b "~refresh_interval_pose"/double : if more than this interval passes, the maintained pose is considered invalid
+
- @b "~bounding_planes"/string : a sequence of plane equations specified as "a1 b1 c1 d1 a2 b2 c2 d2 ..." where each plane is defined by the equation ax+by+cz+d=0
- @b "~box_scale"/double : boxes from the collision map are approximated with spheres using the extents of the boxes. The maximum extent of the box is multiplied by the constant specified by this parameter to obtain the radius of the sphere approximating the box
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-07-09 02:06:18 UTC (rev 18537)
@@ -105,6 +105,9 @@
bool planning_environment::CollisionSpaceMonitor::isMapUpdated(double sec) const
{
+ if (!haveMap_)
+ return false;
+
// less than 10us is considered 0
if (sec > 1e-5 && lastMapUpdate_ < ros::Time::now() - ros::Duration(sec))
return false;
Modified: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-07-09 02:06:18 UTC (rev 18537)
@@ -114,7 +114,7 @@
first_time = false;
- lastStateUpdate_ = mechanismState->header.stamp;
+ lastMechanismStateUpdate_ = mechanismState->header.stamp;
if (!haveMechanismState_)
haveMechanismState_ = robotState_->seenAll();
@@ -169,6 +169,7 @@
change = change || this_changed;
}
+ lastPoseUpdate_ = tm;
havePose_ = true;
}
else
@@ -188,24 +189,39 @@
while (nh_.ok() && !haveState())
{
if (s == 0)
- ROS_INFO("Waiting for mechanism state ...");
+ ROS_INFO("Waiting for robot state ...");
s = (s + 1) % 40;
ros::spinOnce();
ros::Duration().fromSec(0.05).sleep();
}
if (haveState())
- ROS_INFO("Mechanism state received!");
+ ROS_INFO("Robot state received!");
}
-bool planning_environment::KinematicModelStateMonitor::isStateUpdated(double sec) const
-{
+bool planning_environment::KinematicModelStateMonitor::isMechanismStateUpdated(double sec) const
+{
+ if (!haveMechanismState_)
+ return false;
+
// less than 10us is considered 0
- if (sec > 1e-5 && lastStateUpdate_ < ros::Time::now() - ros::Duration(sec))
+ if (sec > 1e-5 && lastMechanismStateUpdate_ < ros::Time::now() - ros::Duration(sec))
return false;
else
return true;
}
+bool planning_environment::KinematicModelStateMonitor::isPoseUpdated(double sec) const
+{
+ if (!havePose_)
+ return false;
+
+ // less than 10us is considered 0
+ if (sec > 1e-5 && lastPoseUpdate_ < ros::Time::now() - ros::Duration(sec))
+ return false;
+ else
+ return true;
+}
+
void planning_environment::KinematicModelStateMonitor::printRobotState(void) const
{
std::stringstream ss;
Modified: pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp 2009-07-09 02:06:18 UTC (rev 18537)
@@ -42,6 +42,7 @@
{
nh_.param<double>("~refresh_interval_collision_map", intervalCollisionMap_, 0.0);
nh_.param<double>("~refresh_interval_kinematic_state", intervalState_, 0.0);
+ nh_.param<double>("~refresh_interval_pose", intervalPose_, 0.0);
}
bool planning_environment::PlanningMonitor::isEnvironmentSafe(void) const
@@ -52,11 +53,18 @@
return false;
}
- if (!isStateUpdated(intervalState_))
+ if (!isMechanismStateUpdated(intervalState_))
{
ROS_WARN("Planning is not safe: robot state not updated in the last %f seconds", intervalState_);
return false;
- }
+ }
+
+ if (includePose_)
+ if (!isPoseUpdated(intervalPose_))
+ {
+ ROS_WARN("Planning is not safe: robot pose not updated in the last %f seconds", intervalPose_);
+ return false;
+ }
return true;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-09 02:25:33
|
Revision: 18540
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18540&view=rev
Author: isucan
Date: 2009-07-09 02:25:32 +0000 (Thu, 09 Jul 2009)
Log Message:
-----------
TF is now a constructor argument
Modified Paths:
--------------
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.cpp
pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/util/self_watch/self_watch.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-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-07-09 02:25:32 UTC (rev 18540)
@@ -93,7 +93,8 @@
planning_environment::CollisionModels *collisionModels_;
planning_environment::PlanningMonitor *planningMonitor_;
-
+ tf::TransformListener tf_;
+
bool getControlJointNames(std::vector<std::string> &joint_names);
bool fillStartState(std::vector<motion_planning_msgs::KinematicJoint> &start);
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -188,8 +188,8 @@
return 0;
boost::thread th(&spinThread);
-
- planning_environment::KinematicModelStateMonitor km(&rm);
+ tf::TransformListener tf;
+ planning_environment::KinematicModelStateMonitor km(&rm, &tf);
km.waitForState();
std::cout << std::endl << std::endl << "Using joints:" << std::endl;
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -65,7 +65,7 @@
// monitor robot
collisionModels_ = new planning_environment::CollisionModels("robot_description");
- planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_);
+ planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, &tf_);
if (collisionModels_->getKinematicModel()->getGroupID(arm_) < 0)
{
Modified: pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -59,7 +59,7 @@
id_ = 0;
visualizationMarkerPublisher_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
collisionModels_ = new planning_environment::CollisionModels("robot_description");
- collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_);
+ collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_, &tf_);
if (collisionModels_->loadedModels())
{
collisionSpaceMonitor_->setOnAfterMapUpdateCallback(boost::bind(&DisplayPlannerCollisionModel::afterWorldUpdate, this, _1));
@@ -144,6 +144,7 @@
int id_;
ros::Publisher visualizationMarkerPublisher_;
ros::NodeHandle nh_;
+ tf::TransformListener tf_;
planning_environment::CollisionModels *collisionModels_;
planning_environment::CollisionSpaceMonitor *collisionSpaceMonitor_;
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -53,10 +53,10 @@
if (nodeHandle_.hasParam("~planning_frame_id"))
{
std::string frame; nodeHandle_.param("~planning_frame_id", frame, std::string(""));
- planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, frame);
+ planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, &tf_, frame);
}
else
- planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, "torso_lift_link");
+ planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, &tf_);
planKinematicPathService_ = nodeHandle_.advertiseService("plan_kinematic_path", &OMPLPlanning::planToGoal, this);
}
@@ -211,6 +211,7 @@
ros::NodeHandle nodeHandle_;
planning_environment::CollisionModels *collisionModels_;
planning_environment::PlanningMonitor *planningMonitor_;
+ tf::TransformListener tf_;
ros::ServiceServer planKinematicPathService_;
Modified: pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -57,7 +57,7 @@
{
stateValidityPublisher_ = nh_.advertise<std_msgs::Byte>("state_validity", 1);
collisionModels_ = new planning_environment::CollisionModels("robot_description");
- collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_);
+ collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_, &tf_);
if (collisionModels_->loadedModels())
{
collisionSpaceMonitor_->setOnAfterMapUpdateCallback(boost::bind(&StateValidityMonitor::afterWorldUpdate, this, _1));
@@ -103,6 +103,7 @@
int last_;
ros::NodeHandle nh_;
ros::Publisher stateValidityPublisher_;
+ tf::TransformListener tf_;
planning_environment::CollisionModels *collisionModels_;
planning_environment::CollisionSpaceMonitor *collisionSpaceMonitor_;
};
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -62,7 +62,7 @@
plan_id_ = -1;
robot_stopped_ = true;
rm_ = new planning_environment::RobotModels("robot_description");
- kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_);
+ kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_, &tf_);
// we use the topic for sending commands to the controller, so we need to advertise it
jointCommandPublisher_ = nh_.advertise<manipulation_msgs::JointTraj>("right_arm/trajectory_controller/trajectory_command", 1);
@@ -229,6 +229,7 @@
ros::Publisher displayPathPublisher_;
planning_environment::RobotModels *rm_;
planning_environment::KinematicModelStateMonitor *kmsm_;
+ tf::TransformListener tf_;
};
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -52,7 +52,7 @@
TestExecutionPath(void)
{
rm_ = new planning_environment::RobotModels("robot_description");
- kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_);
+ kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_, &tf_);
jointCommandPublisher_ = nh_.advertise<manipulation_msgs::JointTraj>("right_arm/trajectory_controller/trajectory_command", 1);
sleep_duration_ = 4;
use_topic_ = false;
@@ -183,6 +183,8 @@
ros::Publisher jointCommandPublisher_;
planning_environment::RobotModels *rm_;
planning_environment::KinematicModelStateMonitor *kmsm_;
+ tf::TransformListener tf_;
+
};
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-07-09 02:25:32 UTC (rev 18540)
@@ -56,13 +56,13 @@
{
public:
- CollisionSpaceMonitor(CollisionModels *cm, std::string frame_id) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), frame_id)
+ CollisionSpaceMonitor(CollisionModels *cm, tf::TransformListener *tf, std::string frame_id) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), tf, frame_id)
{
cm_ = cm;
setupCSM();
}
- CollisionSpaceMonitor(CollisionModels *cm) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm))
+ CollisionSpaceMonitor(CollisionModels *cm, tf::TransformListener *tf) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), tf)
{
cm_ = cm;
setupCSM();
@@ -90,12 +90,6 @@
return cm_;
}
- /** \brief Return the transform listener */
- tf::TransformListener *getTransformListener(void) const
- {
- return tf_;
- }
-
/** \brief Return the scaling employed when creating spheres
from boxes in a collision map. The radius of a sphere is
this scaling multiplied by the largest extent of the box */
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-07-09 02:25:32 UTC (rev 18540)
@@ -56,16 +56,18 @@
{
public:
- KinematicModelStateMonitor(RobotModels *rm)
+ KinematicModelStateMonitor(RobotModels *rm, tf::TransformListener *tf)
{
rm_ = rm;
+ tf_ = tf;
includePose_ = false;
setupRSM();
}
- KinematicModelStateMonitor(RobotModels *rm, const std::string &frame_id)
+ KinematicModelStateMonitor(RobotModels *rm, tf::TransformListener *tf, const std::string &frame_id)
{
rm_ = rm;
+ tf_ = tf;
includePose_ = true;
frame_id_ = frame_id;
setupRSM();
@@ -75,8 +77,6 @@
{
if (robotState_)
delete robotState_;
- if (tf_)
- delete tf_;
}
/** \brief Define a callback for when the state is changed */
@@ -103,6 +103,12 @@
return robotState_;
}
+ /** \brief Return the transform listener */
+ tf::TransformListener *getTransformListener(void) const
+ {
+ return tf_;
+ }
+
/** \brief Return the frame id of the state */
const std::string& getFrameId(void) const
{
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h 2009-07-09 02:25:32 UTC (rev 18540)
@@ -54,12 +54,12 @@
{
public:
- PlanningMonitor(CollisionModels *cm, std::string frame_id) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), frame_id)
+ PlanningMonitor(CollisionModels *cm, tf::TransformListener *tf, std::string frame_id) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), tf, frame_id)
{
loadParams();
}
- PlanningMonitor(CollisionModels *cm) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm))
+ PlanningMonitor(CollisionModels *cm, tf::TransformListener *tf) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), tf)
{
loadParams();
}
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -60,9 +60,6 @@
haveMap_ = false;
- if (!tf_)
- tf_ = new tf::TransformListener();
-
collisionSpace_ = cm_->getODECollisionModel().get();
// the factor by which a boxes maximum extent is multiplied to get the radius of the sphere to be placed in the collision space
Modified: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -43,7 +43,6 @@
kmodel_ = NULL;
robotState_ = NULL;
onStateUpdate_ = NULL;
- tf_ = NULL;
tfWait_ = ros::Duration(0.1);
havePose_ = haveMechanismState_ = false;
if (rm_->loadedModels())
@@ -68,10 +67,7 @@
ROS_DEBUG("Robot frame is '%s'", robot_frame_.c_str());
if (includePose_)
- {
- tf_ = new tf::TransformListener();
ROS_DEBUG("Maintaining robot pose in frame '%s'", frame_id_.c_str());
- }
else
frame_id_ = robot_frame_;
}
Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -73,7 +73,7 @@
// create a state that can be used to monitor the
// changes in the joints of the kinematic model
- m_stateMonitor = boost::shared_ptr<planning_environment::KinematicModelStateMonitor>(new planning_environment::KinematicModelStateMonitor(m_envModels.get()));
+ m_stateMonitor = boost::shared_ptr<planning_environment::KinematicModelStateMonitor>(new planning_environment::KinematicModelStateMonitor(m_envModels.get(), &m_tf));
m_robotState = m_stateMonitor->getRobotState();
m_stateMonitor->setOnStateUpdateCallback(boost::bind(&SelfWatch::stateUpdate, this));
@@ -143,7 +143,8 @@
}
ros::NodeHandle m_nodeHandle;
-
+ tf::TransformListener m_tf;
+
// we don't want to detect a collision after it happened, but this
// is what collision checkers do, so we scale the robot up by a
// small factor; when a collision is found between the inflated
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wa...@us...> - 2009-07-09 05:33:25
|
Revision: 18548
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18548&view=rev
Author: wattsk
Date: 2009-07-09 05:33:24 +0000 (Thu, 09 Jul 2009)
Log Message:
-----------
Defs, calibration, machine and launch files for head cart HCC
Added Paths:
-----------
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.machine
pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/hcc_calibration_controller.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/hcc.xacro.xml
Added: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.launch (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.launch 2009-07-09 05:33:24 UTC (rev 18548)
@@ -0,0 +1,30 @@
+<launch>
+ <!-- Machines -->
+ <include file="$(find pr2_head_cart)/hcc.machine" />
+
+ <!-- Description and etherCAT -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hcc.xacro.xml'" />
+
+ <node machine="realtime_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i eth0 -x robotdesc/pr2" />
+
+ <!-- Calibration -->
+ <node pkg="mechanism_bringup" type="calibrate.py"
+ args="$(find pr2_default_controllers)/calibration_controllers/hcc_calibration_controller.xml" output="screen" />
+
+ <!-- Robot state publisher -->
+ <node machine="two" pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
+ <param name="publish_frequency" type="double" value="50.0" />
+ <param name="tf_prefix" type="string" value="" />
+ </node>
+
+ <include file="$(find pr2_head_cart)/cart_sensors.launch" />
+ <include file="$(find pr2_head_cart)/cart_monitors.launch" />
+
+ <!-- NTP monitoring script Warns to console if sync error -->
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="hcc2" machine="realtime" />
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="fw1 7000" machine="two" />
+
+ <!-- Runtime Diagnostics Logging -->
+ <node pkg="rosrecord" type="rosrecord" args="-f /hwlog/hcc_runtime_automatic /diagnostics" />
+
+</launch>
\ No newline at end of file
Added: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.machine
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.machine (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.machine 2009-07-09 05:33:24 UTC (rev 18548)
@@ -0,0 +1,15 @@
+<launch>
+
+ <machine name="realtime" address="hcc1" ros-root="$(env ROS_ROOT)"
+ ros-package-path="$(env ROS_PACKAGE_PATH)" />
+
+ <machine name="realtime_root" user="root" address="hcc1"
+ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="never"/>
+
+ <machine name="two" address="hcc2" ros-root="$(env ROS_ROOT)"
+ros-package-path="$(env ROS_PACKAGE_PATH)" default="true" />
+
+ <machine name="two_root" address="hcc2" ros-root="$(env ROS_ROOT)"
+ros-package-path="$(env ROS_PACKAGE_PATH)" user="root" default="never" />
+
+</launch>
\ No newline at end of file
Added: pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/hcc_calibration_controller.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/hcc_calibration_controller.xml (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/hcc_calibration_controller.xml 2009-07-09 05:33:24 UTC (rev 18548)
@@ -0,0 +1,5 @@
+<controllers>
+ <include filename="$(find pr2_defs)/defs/head_defs.xml" />
+ <head_calibrator />
+ <tilting_laser_calibrator name="laser_tilt" />
+</controllers>
Added: pkg/trunk/stacks/pr2/pr2_defs/robots/hcc.xacro.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/robots/hcc.xacro.xml (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_defs/robots/hcc.xacro.xml 2009-07-09 05:33:24 UTC (rev 18548)
@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<robot name="hcb">
+
+ <!-- Include file with calibration parameters -->
+ <include filename="$(find pr2_defs)/calibration/default_cal.xml" />
+
+ <!-- declare the path where all models/textures/materials are stored -->
+ <resource location="$(find pr2_defs)/meshes"/>
+
+ <include filename="$(find pr2_defs)/defs/head_defs.xml" />
+ <include filename="$(find pr2_defs)/defs/prosilica_defs.xml" />
+ <include filename="$(find pr2_defs)/defs/gazebo_defs.xml" />
+
+ <!-- Make base joint for visualization -->
+ <joint name="base_joint" type="fixed" />
+ <link name="base_link">
+ <parent name="world" />
+ <origin xyz=" 0 0 0.53 " rpy=" 0 0 0" />
+ <joint name="base_joint" />
+ <inertial>
+ <mass value="100" />
+ <com xyz=" 0 0 0 " />
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+ </inertial>
+ <visual>
+ <!-- 1.06m above ground -->
+ <origin xyz="0 0 0" rpy="0 0 0 " />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/White</elem>
+ </map>
+ <geometry name="unit_box">
+ <mesh scale=".6 0.45 1.06" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0.0 0.0 0.0 " />
+ <geometry name="box_link_collision_geom">
+ <box size="0.6 0.45 1.06" />
+ </geometry>
+ </collision>
+ </link>
+
+ <!-- These dimensions need to be checked -->
+ <pr2_head name="head" parent="base_link">
+ <!-- 213mm above tilt plate. Pan/tilt intersect is origin. -->
+ <origin xyz="${cal_head_x + 0.13} ${cal_head_y} ${.53 + 0.213 + cal_head_z}"
+ rpy="${cal_head_roll} ${cal_head_pitch} ${cal_head_yaw}" />
+ </pr2_head>
+
+ <pr2_tilting_laser_alpha2 name="laser_tilt" parent="base_link">
+ <!-- Was 0.11m forward of head pan, now 115 mm on HCC -->
+ <!-- 66.5mm above head tilt plate -->
+ <origin xyz="${0.115 + 0.13} 0 ${0.0665 + .53}" rpy="0 0 0" />
+ </pr2_tilting_laser_alpha2>
+
+ <!-- Prosilica Camera -->
+ <prosilica_cam name="high_def" parent="head_plate_frame">
+ <!-- Center of camera is 109mm right of center screw, 35mm up -->
+ <!-- Camera is slightly recessed from front, where is camera origin? Lens? -->
+ <origin xyz="0 -0.109 0.035" rpy="0 0 0" />
+ </prosilica_cam>
+
+ <!-- Stereocam -->
+ <double_stereo_camera name="double_stereo" parent="head_plate_frame">
+ <origin xyz="0.0 0.0 0.003" rpy="0 0 0" />
+ </double_stereo_camera>
+
+ <!-- Define groups of links; a link may be part of multiple groups -->
+ <include filename="$(find pr2_defs)/robots/pr2_groups.xml" />
+
+</robot>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mee...@us...> - 2009-07-09 21:09:55
|
Revision: 18588
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18588&view=rev
Author: meeussen
Date: 2009-07-09 21:09:50 +0000 (Thu, 09 Jul 2009)
Log Message:
-----------
joint and link names are now stored inside kdl tree
Modified Paths:
--------------
pkg/trunk/calibration/kinematic_calibration/src/chain_modifier.cpp
pkg/trunk/calibration/kinematic_calibration/src/unittest_io.cpp
pkg/trunk/calibration/laser_cb_processing/src/pr2_tilt_laser_projector.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/src/discrete_space_information/robarm3d/environment_robarm3d.cpp
Modified: pkg/trunk/calibration/kinematic_calibration/src/chain_modifier.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/chain_modifier.cpp 2009-07-09 21:09:06 UTC (rev 18587)
+++ pkg/trunk/calibration/kinematic_calibration/src/chain_modifier.cpp 2009-07-09 21:09:50 UTC (rev 18588)
@@ -74,7 +74,7 @@
KDL::Frame f_tip = segment.getFrameToTip() ;
f_tip.p = f_tip.p + trans_ ;
f_tip.M = f_tip.M * rot_ ;
- segment = KDL::Segment(segment.getJoint(), f_tip, segment.getInertia()) ;
+ segment = KDL::Segment(segment.getName(), segment.getJoint(), f_tip, segment.getInertia()) ;
}
int ChainModifier::specifyAllParams(const NEWMAT::Matrix& all_params)
Modified: pkg/trunk/calibration/kinematic_calibration/src/unittest_io.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/unittest_io.cpp 2009-07-09 21:09:06 UTC (rev 18587)
+++ pkg/trunk/calibration/kinematic_calibration/src/unittest_io.cpp 2009-07-09 21:09:50 UTC (rev 18588)
@@ -74,12 +74,12 @@
break ;
// Now add the data to the chain
- Joint J(Joint::RotZ) ;
+ Joint J("Joint 1",Joint::RotZ) ;
Vector rot_axis(model[3], model[4], model[5]) ; // KDL doesn't need vector to be normalized. It even behaves nicely with vector=[0 0 0]
double rot_ang = rot_axis.Norm() ;
Rotation R(Rotation::Rot(rot_axis, rot_ang)) ;
Vector trans(model[0], model[1], model[2]) ;
- chain.addSegment(Segment(J, Frame(R, trans))) ;
+ chain.addSegment(Segment("Segment 1", J, Frame(R, trans))) ;
}
return chain ;
Modified: pkg/trunk/calibration/laser_cb_processing/src/pr2_tilt_laser_projector.cpp
===================================================================
--- pkg/trunk/calibration/laser_cb_processing/src/pr2_tilt_laser_projector.cpp 2009-07-09 21:09:06 UTC (rev 18587)
+++ pkg/trunk/calibration/laser_cb_processing/src/pr2_tilt_laser_projector.cpp 2009-07-09 21:09:50 UTC (rev 18588)
@@ -111,7 +111,7 @@
mech_chain.toKDL(chain_);
// Add the 'range' joint
- chain_.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::TransX))) ;
+ chain_.addSegment(KDL::Segment("RangeSegment", KDL::Joint("RangeJoint", KDL::Joint::TransX))) ;
if (chain_.getNrOfJoints() != 3)
ROS_ERROR("Num joints doesn't seem right") ;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-07-09 21:09:06 UTC (rev 18587)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-07-09 21:09:50 UTC (rev 18588)
@@ -339,7 +339,7 @@
KDL::Frame tool_frame;
tf::TransformTFToKDL(tool_offset, tool_frame);
- new_kdl_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None), tool_frame));
+ new_kdl_chain.addSegment(KDL::Segment("ToolOffset", KDL::Joint("ToolOffset", KDL::Joint::None), tool_frame));
kdl_chain_ = new_kdl_chain;
Modified: pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/src/discrete_space_information/robarm3d/environment_robarm3d.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/src/discrete_space_information/robarm3d/environment_robarm3d.cpp 2009-07-09 21:09:06 UTC (rev 18587)
+++ pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/src/discrete_space_information/robarm3d/environment_robarm3d.cpp 2009-07-09 21:09:50 UTC (rev 18588)
@@ -3921,15 +3921,13 @@
bool EnvironmentROBARM3D::InitKDLChain(const char *fKDL)
{
KDL::Tree my_tree;
- std::map<string, string> segment_joint_mapping;
- if (!treeFromString(fKDL, my_tree, segment_joint_mapping))
+ if (!treeFromString(fKDL, my_tree))
{
printf("Failed to parse tree from manipulator description file.\n");
return false;;
}
- std::vector<std::string> links;
- if (!my_tree.getChain("torso_lift_link", "r_wrist_roll_link", EnvROBARMCfg.arm_chain, links))
+ if (!my_tree.getChain("torso_lift_link", "r_wrist_roll_link", EnvROBARMCfg.arm_chain))
{
printf("Error: could not fetch the KDL chain for the desired manipulator. Exiting.\n");
return false;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-07-10 17:23:15
|
Revision: 18622
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18622&view=rev
Author: hsujohnhsu
Date: 2009-07-10 17:23:09 +0000 (Fri, 10 Jul 2009)
Log Message:
-----------
fix mesh file path names in urdf.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
pkg/trunk/stacks/pr2/pr2_defs/defs/arm_defs.xml
pkg/trunk/stacks/pr2/pr2_defs/defs/arm_grav_defs.xml
pkg/trunk/stacks/pr2/pr2_defs/defs/base_defs.xml
pkg/trunk/stacks/pr2/pr2_defs/defs/body_defs.xml
pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml
pkg/trunk/stacks/pr2/pr2_defs/defs/head_defs.xml
pkg/trunk/stacks/pr2/pr2_defs/defs/ptz_defs.xml
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp 2009-07-10 17:15:57 UTC (rev 18621)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp 2009-07-10 17:23:09 UTC (rev 18622)
@@ -250,7 +250,7 @@
int pos1 = mesh->filename.find(tmp_extension,0);
mesh->filename.replace(pos1,mesh->filename.size()-pos1+1,std::string(""));
// add mesh filename
- addKeyValue(geom, "mesh", "models/pr2/" + mesh->filename + ".mesh");
+ addKeyValue(geom, "mesh", mesh->filename + ".mesh");
}
else
@@ -294,7 +294,7 @@
if (mesh->filename.empty())
addKeyValue(visual, "mesh", "unit_" + type);
else
- addKeyValue(visual, "mesh", "models/pr2/" + mesh->filename + ".mesh");
+ addKeyValue(visual, "mesh", mesh->filename + ".mesh");
}
else
Modified: pkg/trunk/stacks/pr2/pr2_defs/defs/arm_defs.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/defs/arm_defs.xml 2009-07-10 17:15:57 UTC (rev 18621)
+++ pkg/trunk/stacks/pr2/pr2_defs/defs/arm_defs.xml 2009-07-10 17:23:09 UTC (rev 18622)
@@ -67,13 +67,13 @@
<elem key="material" value="PR2/Blue" />
</map>
<geometry name="${side}_shoulder_pan_visual">
- <mesh filename="shoulder_yaw" />
+ <mesh filename="pr2/shoulder_yaw" />
</geometry>
</visual>
<collision>
<origin xyz="0.0 0 0.0" rpy="0 0 0" />
<geometry name="${side}_shoulder_pan_collision">
- <mesh filename="convex/shoulder_yaw_convex.stlb" />
+ <mesh filename="pr2/convex/shoulder_yaw_convex.stlb" />
</geometry>
</collision>
<map name="${side}_shoulder_pan_sensor" flag="gazebo">
@@ -137,13 +137,13 @@
<elem key="material" value="PR2/Grey" />
</map>
<geometry name="${side}_shoulder_lift_visual">
- <mesh filename="shoulder_lift" />
+ <mesh filename="pr2/shoulder_lift" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_shoulder_lift_collision">
- <mesh filename="convex/shoulder_lift_convex.stlb" />
+ <mesh filename="pr2/convex/shoulder_lift_convex.stlb" />
</geometry>
</collision>
<map name="${side}_shoulder_lift_sensor" flag="gazebo">
@@ -208,7 +208,7 @@
<elem key="material" value="PR2/RollLinks" />
</map>
<geometry name="${side}_upper_arm_roll_visual">
- <mesh filename="upper_arm_roll" />
+ <mesh filename="pr2/upper_arm_roll" />
</geometry>
</visual>
<collision> <!-- TODO: collision tag should be optional -->
@@ -262,14 +262,14 @@
<elem key="material" value="PR2/Green" />
</map>
<geometry name="${side}_upper_arm_visual">
- <mesh filename="upper_arm" />
+ <mesh filename="pr2/upper_arm" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_upper_arm_collision">
- <mesh filename="convex/upper_arm_convex.stlb" />
+ <mesh filename="pr2/convex/upper_arm_convex.stlb" />
</geometry>
</collision>
@@ -329,13 +329,13 @@
<elem key="material" value="PR2/Grey" />
</map>
<geometry name="${side}_elbow_flex_visual">
- <mesh filename="elbow_flex" />
+ <mesh filename="pr2/elbow_flex" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_elbow_flex_collision">
- <mesh filename="convex/elbow_flex_convex.stlb" />
+ <mesh filename="pr2/convex/elbow_flex_convex.stlb" />
</geometry>
</collision>
<map name="${side}_elbow_flex_sensor" flag="gazebo">
@@ -439,7 +439,7 @@
<elem key="material" value="PR2/RollLinks" />
</map>
<geometry name="${side}_forearm_roll_visual">
- <mesh filename="forearm_roll" />
+ <mesh filename="pr2/forearm_roll" />
</geometry>
</visual>
<collision> <!-- TODO: collision tag should be optional -->
@@ -484,13 +484,13 @@
<elem key="material" value="PR2/Blue" />
</map>
<geometry name="${side}_forearm_visual">
- <mesh filename="forearm" />
+ <mesh filename="pr2/forearm" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_forearm_collision">
- <mesh filename="convex/forearm_convex.stlb" />
+ <mesh filename="pr2/convex/forearm_convex.stlb" />
</geometry>
</collision>
<map name="${side}_forearm_sensor" flag="gazebo">
@@ -547,13 +547,13 @@
<elem key="material" value="PR2/Grey" />
</map>
<geometry name="${side}_wrist_flex_visual">
- <mesh filename="wrist_flex" />
+ <mesh filename="pr2/wrist_flex" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_wrist_flex_collision">
- <mesh filename="convex/wrist_flex_convex.stlb" />
+ <mesh filename="pr2/convex/wrist_flex_convex.stlb" />
</geometry>
</collision>
<map name="${side}_wrist_flex_sensor" flag="gazebo">
@@ -607,7 +607,7 @@
<elem key="material" value="PR2/RollLinks" />
</map>
<geometry name="${side}_wrist_roll_visual">
- <mesh filename="wrist_roll" />
+ <mesh filename="pr2/wrist_roll" />
</geometry>
</visual>
<collision>
Modified: pkg/trunk/stacks/pr2/pr2_defs/defs/arm_grav_defs.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/defs/arm_grav_defs.xml 2009-07-10 17:15:57 UTC (rev 18621)
+++ pkg/trunk/stacks/pr2/pr2_defs/defs/arm_grav_defs.xml 2009-07-10 17:23:09 UTC (rev 18622)
@@ -49,7 +49,7 @@
<elem key="material">PR2/Blue</elem>
</map>
<geometry name="${side}_shoulder_pan_visual">
- <mesh filename="shoulder_yaw" />
+ <mesh filename="pr2/shoulder_yaw" />
</geometry>
</visual>
<collision>
@@ -113,7 +113,7 @@
<elem key="material">PR2/Grey</elem>
</map>
<geometry name="${side}_shoulder_lift_visual">
- <mesh filename="shoulder_lift" />
+ <mesh filename="pr2/shoulder_lift" />
</geometry>
</visual>
<collision>
@@ -180,7 +180,7 @@
<elem key="material">PR2/Green</elem>
</map>
<geometry name="${side}_upper_arm_roll_visual">
- <mesh filename="upper_arm" />
+ <mesh filename="pr2/upper_arm" />
</geometry>
</visual>
@@ -248,7 +248,7 @@
<elem key="material">PR2/Grey</elem>
</map>
<geometry name="${side}_elbow_flex_visual">
- <mesh filename="elbow_flex" />
+ <mesh filename="pr2/elbow_flex" />
</geometry>
</visual>
<collision>
@@ -361,7 +361,7 @@
<elem key="material">PR2/Blue</elem>
</map>
<geometry name="${side}_forearm_roll_visual">
- <mesh filename="forearm" />
+ <mesh filename="pr2/forearm" />
</geometry>
</visual>
<collision>
@@ -426,7 +426,7 @@
<elem key="material">PR2/Grey</elem>
</map>
<geometry name="${side}_wrist_flex_visual">
- <mesh filename="wrist_flex" />
+ <mesh filename="pr2/wrist_flex" />
</geometry>
</visual>
<collision>
@@ -482,7 +482,7 @@
<elem key="material">PR2/Red</elem>
</map>
<geometry name="${side}_wrist_roll_visual">
- <mesh filename="wrist_roll" />
+ <mesh filename="pr2/wrist_roll" />
</geometry>
</visual>
<collision>
Modified: pkg/trunk/stacks/pr2/pr2_defs/defs/base_defs.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/defs/base_defs.xml 2009-07-10 17:15:57 UTC (rev 18621)
+++ pkg/trunk/stacks/pr2/pr2_defs/defs/base_defs.xml 2009-07-10 17:23:09 UTC (rev 18622)
@@ -36,7 +36,7 @@
<elem key="material" value="PR2/${parent}_${suffix}_wheel_link" />
</map>
<geometry name="${parent}_${suffix}_wheel_visual">
- <mesh filename="wheel" />
+ <mesh filename="pr2/wheel" />
</geometry>
</visual>
@@ -102,7 +102,7 @@
<elem key="material" value="PR2/Green" />
</map>
<geometry name="${suffix}_caster_rotation_visual">
- <mesh filename="caster" scale="1 1 1"/>
+ <mesh filename="pr2/caster" scale="1 1 1"/>
</geometry>
</visual>
@@ -153,7 +153,7 @@
<elem key="material" value="PR2/${parent}_${suffix}_wheel_link" />
</map>
<geometry name="${parent}_${suffix}_wheel_visual">
- <mesh filename="wheel" />
+ <mesh filename="pr2/wheel" />
</geometry>
</visual>
@@ -219,7 +219,7 @@
<elem key="material" value="PR2/Green" />
</map>
<geometry name="${suffix}_caster_rotation_visual">
- <mesh filename="caster" scale="1 1 1"/>
+ <mesh filename="pr2/caster" scale="1 1 1"/>
</geometry>
</visual>
@@ -265,7 +265,7 @@
<elem key="material" value="PR2/Green" />
</map>
<geometry name="${name}_visual">
- <mesh filename="base" />
+ <mesh filename="pr2/base" />
</geometry>
</visual>
@@ -417,7 +417,7 @@
<elem key="material" value="PR2/Green" />
</map>
<geometry name="${name}_visual">
- <mesh filename="base" />
+ <mesh filename="pr2/base" />
</geometry>
</visual>
Modified: pkg/trunk/stacks/pr2/pr2_defs/defs/body_defs.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/defs/body_defs.xml 2009-07-10 17:15:57 UTC (rev 18621)
+++ pkg/trunk/stacks/pr2/pr2_defs/defs/body_defs.xml 2009-07-10 17:23:09 UTC (rev 18622)
@@ -41,7 +41,7 @@
<elem key="material" value="PR2/Grey2" />
</map>
<geometry name="${name}_visual">
- <mesh filename="torso" />
+ <mesh filename="pr2/torso" />
</geometry>
</visual>
Modified: pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml 2009-07-10 17:15:57 UTC (rev 18621)
+++ pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml 2009-07-10 17:23:09 UTC (rev 18622)
@@ -79,13 +79,13 @@
<elem key="material" value="PR2/Grey" />
</map>
<geometry name="${prefix}_l_finger_visual">
- <mesh filename="upper_finger_l" />
+ <mesh filename="pr2/upper_finger_l" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${prefix}_l_finger_collision">
- <mesh filename="convex/upper_finger_l_convex.stlb" />
+ <mesh filename="pr2/convex/upper_finger_l_convex.stlb" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
@@ -163,13 +163,13 @@
<elem key="material" value="PR2/Grey" />
</map>
<geometry name="${prefix}_r_finger_visual">
- <mesh filename="upper_finger_r" />
+ <mesh filename="pr2/upper_finger_r" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${prefix}_r_finger_collision">
- <mesh filename="convex/upper_finger_r_convex.stlb" />
+ <mesh filename="pr2/convex/upper_finger_r_convex.stlb" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
@@ -244,13 +244,13 @@
<elem key="material" value="PR2/Green" />
</map>
<geometry name="${prefix}_l_finger_tip_visual">
- <mesh filename="finger_tip_l" />
+ <mesh filename="pr2/finger_tip_l" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${prefix}_l_finger_tip_collision">
- <mesh filename="convex/finger_tip_l_convex.stlb" />
+ <mesh filename="pr2/convex/finger_tip_l_convex.stlb" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
@@ -326,13 +326,13 @@
<elem key="material" value="PR2/Green" />
</map>
<geometry name="${prefix}_r_finger_tip_visual">
- <mesh filename="finger_tip_r" />
+ <mesh filename="pr2/finger_tip_r" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${prefix}_r_finger_tip_collision">
- <mesh filename="convex/finger_tip_r_convex.stlb" />
+ <mesh filename="pr2/convex/finger_tip_r_convex.stlb" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
@@ -476,13 +476,13 @@
<elem key="material" value="PR2/Red" />
</map>
<geometry name="${side}_gripper_palm_visual">
- <mesh filename="gripper_palm" />
+ <mesh filename="pr2/gripper_palm" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_gripper_palm_collision">
- <mesh filename="convex/gripper_palm_convex.stlb" />
+ <mesh filename="pr2/convex/gripper_palm_convex.stlb" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
@@ -701,13 +701,13 @@
<elem key="material" value="PR2/Red" />
</map>
<geometry name="${side}_gripper_palm_visual">
- <mesh filename="gripper_palm" />
+ <mesh filename="pr2/gripper_palm" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_gripper_palm_collision">
- <mesh filename="convex/gripper_palm_convex.stlb" />
+ <mesh filename="pr2/convex/gripper_palm_convex.stlb" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
Modified: pkg/trunk/stacks/pr2/pr2_defs/defs/head_defs.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/defs/head_defs.xml 2009-07-10 17:15:57 UTC (rev 18621)
+++ pkg/trunk/stacks/pr2/pr2_defs/defs/head_defs.xml 2009-07-10 17:23:09 UTC (rev 18622)
@@ -36,7 +36,7 @@
<elem key="material" value="PR2/Red" />
</map>
<geometry name="${name}_mount_visual">
- <mesh filename="hok_tilt" />
+ <mesh filename="pr2/hok_tilt" />
</geometry>
</visual>
@@ -417,7 +417,7 @@
<elem key="material" value="PR2/Blue" />
</map>
<geometry name="${name}_visual">
- <mesh filename="head_pan" />
+ <mesh filename="pr2/head_pan" />
</geometry>
</visual>
@@ -467,7 +467,7 @@
<elem key="material" value="PR2/Green" />
</map>
<geometry name="${name}_visual">
- <mesh filename="head_tilt" />
+ <mesh filename="pr2/head_tilt" />
</geometry>
</visual>
@@ -559,7 +559,7 @@
<elem key="material" value="PR2/Red" />
</map>
<geometry name="${name}_mount_visual">
- <mesh filename="hok_tilt" />
+ <mesh filename="pr2/hok_tilt" />
</geometry>
</visual>
@@ -677,7 +677,7 @@
<elem key="material" value="PR2/Blue" />
</map>
<geometry name="${name}_visual">
- <mesh filename="head_pan" />
+ <mesh filename="pr2/head_pan" />
</geometry>
</visual>
@@ -726,7 +726,7 @@
<elem key="material" value="PR2/Green" />
</map>
<geometry name="${name}_visual">
- <mesh filename="head_tilt" />
+ <mesh filename="pr2/head_tilt" />
</geometry>
</visual>
@@ -774,7 +774,7 @@
<elem key="material" value="PR2/Green" />
</map>
<geometry name="${name}_visual">
- <mesh filename="head_tilt" />
+ <mesh filename="pr2/head_tilt" />
</geometry>
</visual>
@@ -821,7 +821,7 @@
<elem key="material" value="PR2/Green" />
</map>
<geometry name="${name}_visual">
- <mesh filename="head_tilt" />
+ <mesh filename="pr2/head_tilt" />
</geometry>
</visual>
Modified: pkg/trunk/stacks/pr2/pr2_defs/defs/ptz_defs.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/defs/ptz_defs.xml 2009-07-10 17:15:57 UTC (rev 18621)
+++ pkg/trunk/stacks/pr2/pr2_defs/defs/ptz_defs.xml 2009-07-10 17:23:09 UTC (rev 18622)
@@ -33,7 +33,7 @@
<elem key="material" value="PR2/Blue" />
</map>
<geometry name="${side}_ptz_pan_visual">
- <mesh filename="ptz_base_${side}" />
+ <mesh filename="pr2/ptz_base_${side}" />
</geometry>
</visual>
@@ -71,7 +71,7 @@
<elem key="material" value="PR2/Red" />
</map>
<geometry name="${side}_ptz_tilt_visual">
- <mesh filename="ptz_top_${side}" />
+ <mesh filename="pr2/ptz_top_${side}" />
</geometry>
</visual>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-10 18:17:56
|
Revision: 18625
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18625&view=rev
Author: isucan
Date: 2009-07-10 18:17:51 +0000 (Fri, 10 Jul 2009)
Log Message:
-----------
point <-- pointstamped in workspace bounds definition for planning
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg
pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-07-10 18:11:52 UTC (rev 18624)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-07-10 18:17:51 UTC (rev 18625)
@@ -112,11 +112,7 @@
req.params.model_id = arm_; // the model to plan for (should be defined in planning.yaml)
req.params.distance_metric = "L2Square"; // the metric to be used in the robot's state space
-
- // this volume is only needed if planar or floating joints move in the space
- req.params.volumeMin.x = req.params.volumeMin.y = req.params.volumeMin.z = 0.0;
- req.params.volumeMax.x = req.params.volumeMax.y = req.params.volumeMax.z = 0.0;
-
+
// forward the goal & path constraints
req.goal_constraints = goal.goal_constraints;
req.path_constraints = goal.path_constraints;
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg 2009-07-10 18:11:52 UTC (rev 18624)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg 2009-07-10 18:17:51 UTC (rev 18625)
@@ -17,8 +17,8 @@
# Lower coordinate for a box defining the volume in the workspace the
# motion planner is active in. If planning in 2D, only first 2 values
# (x, y) of the points are used.
-robot_msgs/Point volumeMin
+robot_msgs/PointStamped volumeMin
# Higher coordinate for the box described above
-robot_msgs/Point volumeMax
+robot_msgs/PointStamped volumeMax
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-10 18:11:52 UTC (rev 18624)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-10 18:17:51 UTC (rev 18625)
@@ -134,21 +134,43 @@
static_cast<StateValidityPredicate*>(psetup->si->getStateValidityChecker())->setConstraints(req.path_constraints);
/* set the workspace volume for planning */
- // only area or volume should go through
- if (SpaceInformationKinematicModel *s = dynamic_cast<SpaceInformationKinematicModel*>(psetup->si))
+
+ if (psetup->model->planningMonitor->getTransformListener()->frameExists(req.params.volumeMin.header.frame_id) &&
+ psetup->model->planningMonitor->getTransformListener()->frameExists(req.params.volumeMax.header.frame_id))
{
- s->setPlanningArea(req.params.volumeMin.x, req.params.volumeMin.y,
- req.params.volumeMax.x, req.params.volumeMax.y);
- s->setPlanningVolume(req.params.volumeMin.x, req.params.volumeMin.y, req.params.volumeMin.z,
- req.params.volumeMax.x, req.params.volumeMax.y, req.params.volumeMax.z);
+ bool err = false;
+ try
+ {
+ psetup->model->planningMonitor->getTransformListener()->transformPoint(psetup->model->planningMonitor->getFrameId(), req.params.volumeMin, req.params.volumeMin);
+ psetup->model->planningMonitor->getTransformListener()->transformPoint(psetup->model->planningMonitor->getFrameId(), req.params.volumeMax, req.params.volumeMax);
+ }
+ catch(...)
+ {
+ err = true;
+ }
+ if (err)
+ ROS_ERROR("Unable to transform workspace bounds to planning frame");
+ else
+ {
+ // only area or volume should go through
+ if (SpaceInformationKinematicModel *s = dynamic_cast<SpaceInformationKinematicModel*>(psetup->si))
+ {
+ s->setPlanningArea(req.params.volumeMin.point.x, req.params.volumeMin.point.y,
+ req.params.volumeMax.point.x, req.params.volumeMax.point.y);
+ s->setPlanningVolume(req.params.volumeMin.point.x, req.params.volumeMin.point.y, req.params.volumeMin.point.z,
+ req.params.volumeMax.point.x, req.params.volumeMax.point.y, req.params.volumeMax.point.z);
+ }
+ if (SpaceInformationDynamicModel *s = dynamic_cast<SpaceInformationDynamicModel*>(psetup->si))
+ {
+ s->setPlanningArea(req.params.volumeMin.point.x, req.params.volumeMin.point.y,
+ req.params.volumeMax.point.x, req.params.volumeMax.point.y);
+ s->setPlanningVolume(req.params.volumeMin.point.x, req.params.volumeMin.point.y, req.params.volumeMin.point.z,
+ req.params.volumeMax.point.x, req.params.volumeMax.point.y, req.params.volumeMax.point.z);
+ }
+ }
}
- if (SpaceInformationDynamicModel *s = dynamic_cast<SpaceInformationDynamicModel*>(psetup->si))
- {
- s->setPlanningArea(req.params.volumeMin.x, req.params.volumeMin.y,
- req.params.volumeMax.x, req.params.volumeMax.y);
- s->setPlanningVolume(req.params.volumeMin.x, req.params.volumeMin.y, req.params.volumeMin.z,
- req.params.volumeMax.x, req.params.volumeMax.y, req.params.volumeMax.z);
- }
+ else
+ ROS_DEBUG("No workspace bounding volume was set");
psetup->si->setStateDistanceEvaluator(psetup->sde[req.params.distance_metric]);
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-10 18:11:52 UTC (rev 18624)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-10 18:17:51 UTC (rev 18625)
@@ -133,13 +133,17 @@
req.params.distance_metric = "L2Square";
req.params.planner_id = "dynamic::KPIECE";
- req.params.volumeMin.x = -3;
- req.params.volumeMin.y = -3;
- req.params.volumeMin.z = 0;
+ req.params.volumeMin.header.frame_id = "/base_link";
+ req.params.volumeMin.header.stamp = ros::Time::now();
+ req.params.volumeMax.header = req.params.volumeMin.header;
+
+ req.params.volumeMin.point.x = -3;
+ req.params.volumeMin.point.y = -3;
+ req.params.volumeMin.point.z = 0;
- req.params.volumeMax.x = 3;
- req.params.volumeMax.y = 3;
- req.params.volumeMax.z = 0;
+ req.params.volumeMax.point.x = 3;
+ req.params.volumeMax.point.y = 3;
+ req.params.volumeMax.point.z = 0;
req.times = 1;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-07-11 06:32:14
|
Revision: 18660
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18660&view=rev
Author: rdiankov
Date: 2009-07-11 06:32:06 +0000 (Sat, 11 Jul 2009)
Log Message:
-----------
fixed ormanipulation openrave demo
Modified Paths:
--------------
pkg/trunk/openrave_planning/openrave/Makefile
pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m
pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
pkg/trunk/openrave_planning/ormanipulation/octave/grasp_pr2_ricebox.mat
pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml
Modified: pkg/trunk/openrave_planning/openrave/Makefile
===================================================================
--- pkg/trunk/openrave_planning/openrave/Makefile 2009-07-11 05:17:03 UTC (rev 18659)
+++ pkg/trunk/openrave_planning/openrave/Makefile 2009-07-11 06:32:06 UTC (rev 18660)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 825
+SVN_REVISION = -r 829
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
#SVN_PATCH = fini_patch.patch
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m 2009-07-11 05:17:03 UTC (rev 18659)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/MakePR2GraspTables.m 2009-07-11 06:32:06 UTC (rev 18660)
@@ -65,10 +65,10 @@
orBodySetTransform(Target.id, [0 0 0], [1 0 0 0]); % identity
standoffs = [0.01 0.03];
-rolls = [0 pi/4 pi/2]; % hand is symmetric
+rolls = [0 pi/2]; % hand is symmetric
use_noise = 0;
stepsize = 0.02;
-add_spherenorms = 1;
+add_spherenorms = 0;
% start simulating grasps
[GraspTable, GraspStats] = MakeGraspTable(robot,Target,preshapes, standoffs, rolls,use_noise,stepsize,add_spherenorms);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-07-11 05:17:03 UTC (rev 18659)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/SetupTableScene.m 2009-07-11 06:32:06 UTC (rev 18660)
@@ -80,7 +80,7 @@
%% set the default joint values
orBodySetJointValues (robot.id,[1.582704 1.081165 0.000000 -2.299995 -0.000000 0.000000 0.000000],robot.manips{1}.armjoints);
orBodySetJointValues (robot.id,[-0.979114 -0.400000 0.000000 -1.535151 0.000000 0.000000 0.000000],robot.manips{2}.armjoints);
-orBodySetJointValues (robot.id,0.05,find(cellfun(@(x) strcmp('torso_lift_joint',x), robot.jointnames),1,'first'));
+orBodySetJointValues (robot.id,0.1,find(cellfun(@(x) strcmp('torso_lift_joint',x), robot.jointnames),1,'first')-1);
robothome = orBodyGetJointValues(robot.id);
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/grasp_pr2_ricebox.mat
===================================================================
(Binary files differ)
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-07-11 05:17:03 UTC (rev 18659)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-07-11 06:32:06 UTC (rev 18660)
@@ -21,4 +21,4 @@
orEnvSetOptions('wdims 800 600');
orEnvSetOptions('simulation timestep 0.001');
orEnvSetOptions('collision ode');
-orEnvSetOptions('debug debug');
+#orEnvSetOptions('debug debug');
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml 2009-07-11 05:17:03 UTC (rev 18659)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml 2009-07-11 06:32:06 UTC (rev 18660)
@@ -18,7 +18,7 @@
<base>torso_lift_link</base>
<effector>r_gripper_palm_link</effector>
<armjoints>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</armjoints>
- <joints>r_gripper_palm_joint r_gripper_l_finger_joint r_gripper_tool_joint</joints>
+ <joints>r_gripper_palm_joint r_gripper_float_joint r_gripper_l_finger_joint r_gripper_tool_joint</joints>
<closed>0 0</closed>
<opened>0 0.8</opened>
<iksolver>PR2Rightikfast</iksolver>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-12 21:39:43
|
Revision: 18679
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18679&view=rev
Author: isucan
Date: 2009-07-12 21:39:40 +0000 (Sun, 12 Jul 2009)
Log Message:
-----------
namespace model for bodies in collision space
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
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/src/collision_test_speed.cpp
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-07-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-07-12 21:39:40 UTC (rev 18679)
@@ -41,7 +41,7 @@
void planning_environment::CollisionModels::setupModel(boost::shared_ptr<collision_space::EnvironmentModel> &model, const std::vector<std::string> &links)
{
model->lock();
- model->addRobotModel(kmodel_, links, scale_, padd_);
+ model->setRobotModel(kmodel_, links, scale_, padd_);
// form all pairs of links that can collide and add them as self-collision groups
for (unsigned int i = 0 ; i < self_collision_check_groups_.size() ; ++i)
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-07-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-07-12 21:39:40 UTC (rev 18679)
@@ -82,7 +82,7 @@
else
for (unsigned int i = 0 ; i < planeValues.size() / 4 ; ++i)
{
- collisionSpace_->addStaticPlane(planeValues[i * 4], planeValues[i * 4 + 1], planeValues[i * 4 + 2], planeValues[i * 4 + 3]);
+ collisionSpace_->addPlane("bounds", planeValues[i * 4], planeValues[i * 4 + 1], planeValues[i * 4 + 2], planeValues[i * 4 + 3]);
ROS_INFO("Added static plane %fx + %fy + %fz + %f = 0", planeValues[i * 4], planeValues[i * 4 + 1], planeValues[i * 4 + 2], planeValues[i * 4 + 3]);
}
@@ -209,9 +209,9 @@
collisionSpace_->lock();
if (clear)
- collisionSpace_->clearObstacles();
+ collisionSpace_->clearObstacles("points");
if (n > 0)
- collisionSpace_->addPointCloud(n, data);
+ collisionSpace_->addPointCloudSpheres("points", n, data);
collisionSpace_->unlock();
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-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-07-12 21:39:40 UTC (rev 18679)
@@ -49,14 +49,11 @@
/** \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 certain set of fixed
- (addStatic*) obstacles that never change, a set of obstacles that
- can change (removed by clearObstacles()) and a kinematic
- robot model. The class provides functionality for checking whether a
- given robot is in collision.
+ /** \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
+ obstacles and a robot model. The obstacles are placed in different
+ namespaces so they can be added and removed selectively.
*/
class EnvironmentModel
{
@@ -109,7 +106,7 @@
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual void addRobotModel(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<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) = 0;
@@ -137,21 +134,20 @@
/**********************************************************************/
- /* Collision Bodies Definition (Dynamic) */
+ /* Collision Bodies */
/**********************************************************************/
/** \brief Remove all obstacles from collision model */
virtual void clearObstacles(void) = 0;
+ /** \brief Remove obstacles from a specific namespace in the collision model */
+ virtual void clearObstacles(const std::string &ns) = 0;
+
/** \brief Add a point cloud to the collision space */
- virtual void addPointCloud(unsigned int n, const double* points) = 0;
+ virtual void addPointCloudSpheres(const std::string &ns, unsigned int n, const double* points) = 0;
- /**********************************************************************/
- /* Collision Bodies Definition (Static) */
- /**********************************************************************/
-
/** \brief Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
- virtual void addStaticPlane(double a, double b, double c, double d) = 0;
+ virtual void addPlane(const std::string &ns, double a, double b, double c, double d) = 0;
/**********************************************************************/
/* Miscellaneous Routines */
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-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-07-12 21:39:40 UTC (rev 18679)
@@ -40,6 +40,7 @@
#include "collision_space/environment.h"
#include "btBulletCollisionCommon.h"
+#include <map>
namespace collision_space
{
@@ -76,11 +77,14 @@
/** \brief Remove all obstacles from collision model */
virtual void clearObstacles(void);
+ /** \brief Remove obstacles from a specific namespace in the collision model */
+ virtual void clearObstacles(const std::string &ns);
+
/** \brief Add a point cloud to the collision space */
- virtual void addPointCloud(unsigned int n, const double *points);
+ virtual void addPointCloudSpheres(const std::string &ns, unsigned int n, const double *points);
/** \brief Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
- virtual void addStaticPlane(double a, double b, double c, double 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
@@ -88,7 +92,7 @@
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual void addRobotModel(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<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);
@@ -224,13 +228,14 @@
btCollisionObject* createCollisionBody(shapes::Shape *shape, double scale, double padding);
void freeMemory(void);
- SelfCollisionFilterCallback m_selfCollisionFilterCallback;
- GenericCollisionFilterCallback m_genericCollisionFilterCallback;
+ SelfCollisionFilterCallback m_selfCollisionFilterCallback;
+ GenericCollisionFilterCallback m_genericCollisionFilterCallback;
- ModelInfo m_modelGeom;
- std::vector<btCollisionObject*> m_dynamicObstacles;
- btCollisionWorld *m_world;
- btDefaultCollisionConfiguration *m_config;
+ ModelInfo m_modelGeom;
+ std::map<std::string, std::vector<btCollisionObject*> >
+ m_obstacles;
+ btCollisionWorld *m_world;
+ btDefaultCollisionConfiguration *m_config;
};
}
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-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-07-12 21:39:40 UTC (rev 18679)
@@ -39,6 +39,7 @@
#include "collision_space/environment.h"
#include <ode/ode.h>
+#include <map>
namespace collision_space
{
@@ -54,8 +55,6 @@
EnvironmentModelODE(void) : EnvironmentModel()
{
dInitODE2(0);
- m_space = dHashSpaceCreate(0);
- m_spaceBasicGeoms = dHashSpaceCreate(0);
}
virtual ~EnvironmentModelODE(void)
@@ -63,19 +62,7 @@
freeMemory();
dCloseODE();
}
-
- /** \brief The space ID for the objects that can be changed in the
- map. clearObstacles will invalidate this ID. Collision
- checking on this space is optimized for many small
- objects. */
- dSpaceID getODESpace(void) const;
- /** \brief Return the space ID for the space in which static objects are added */
- dSpaceID getODEBasicGeomSpace(void) const;
-
- /** \brief Return the space ID for the space in which the robot model is instanciated */
- dSpaceID getModelODESpace(void) const;
-
/** \brief Get the list of contacts (collisions) */
virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1);
@@ -88,19 +75,22 @@
/** \brief Remove all obstacles from collision model */
virtual void clearObstacles(void);
+ /** \brief Remove obstacles from a specific namespace in the collision model */
+ virtual void clearObstacles(const std::string &ns);
+
/** \brief Add a point cloud to the collision space */
- virtual void addPointCloud(unsigned int n, const double *points);
+ virtual void addPointCloudSpheres(const std::string &ns, unsigned int n, const double *points);
/** \brief Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
- virtual void addStaticPlane(double a, double b, double c, double 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 addRobotModel(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<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);
@@ -113,12 +103,6 @@
protected:
- /** \brief Internal function for collision detection */
- void testCollision(void *data);
- void testSelfCollision(void *data);
- void testStaticBodyCollision(void *data);
- void testDynamicBodyCollision(void *data);
-
class ODECollide2
{
public:
@@ -140,6 +124,7 @@
void clear(void);
void setup(void);
void collide(dGeomID geom, void *data, dNearCallback *nearCallback);
+ bool empty(void);
private:
@@ -226,33 +211,108 @@
unsigned int index;
};
+ /** \brief Structure for maintaining ODE temporary data */
+ struct ODEStorage
+ {
+ ~ODEStorage(void)
+ {
+ clear();
+ }
+
+ void clear(void)
+ {
+ for (unsigned int i = 0 ; i < meshIndices.size() ; ++i)
+ delete[] meshIndices[i];
+ meshIndices.clear();
+ for (unsigned int i = 0 ; i < meshData.size() ; ++i)
+ dGeomTriMeshDataDestroy(meshData[i]);
+ meshData.clear();
+ }
+
+ /* Pointers for ODE indices; we need this around in ODE's assumed datatype */
+ std::vector<dTriIndex*> meshIndices;
+ std::vector<dTriMeshDataID> meshData;
+ };
+
struct ModelInfo
{
- std::vector< kGeom* > linkGeom;
- double scale;
- double padding;
- dSpaceID space;
+ std::vector< kGeom* > linkGeom;
+ double scale;
+ double padding;
+ dSpaceID space;
+ ODEStorage storage;
};
- dGeomID createODEGeom(dSpaceID space, shapes::Shape *shape, double scale, double padding);
- void updateGeom(dGeomID geom, btTransform &pose) const;
- void freeMemory(void);
+ struct CollisionNamespace
+ {
+ CollisionNamespace(const std::string &nm) : name(nm)
+ {
+ space = dHashSpaceCreate(0);
+ }
+
+ virtual ~CollisionNamespace(void)
+ {
+ if (space)
+ dSpaceDestroy(space);
+ }
+
+ void clear(void)
+ {
+ if (space)
+ dSpaceDestroy(space);
+ space = dHashSpaceCreate(0);
+ geoms.clear();
+ collide2.clear();
+ storage.clear();
+ }
+
+ std::string name;
+ dSpaceID space;
+ std::vector<dGeomID> geoms;
+ ODECollide2 collide2;
+ ODEStorage storage;
+ };
- ModelInfo m_modelGeom;
- dSpaceID m_space;
- dSpaceID m_spaceBasicGeoms;
+ struct CollisionData
+ {
+ CollisionData(void)
+ {
+ done = false;
+ collides = false;
+ max_contacts = 0;
+ contacts = NULL;
+ selfCollisionTest = NULL;
+ link1 = link2 = NULL;
+ }
+
+ bool done;
+
+ bool collides;
+ unsigned int max_contacts;
+ std::vector<EnvironmentModelODE::Contact> *contacts;
+ std::vector< std::vector<bool> > *selfCollisionTest;
+
+ planning_models::KinematicModel::Link *link1;
+ planning_models::KinematicModel::Link *link2;
+ };
- /* This is where geoms from the world (that can be cleared and recreated) are added; the space for this is m_space */
- ODECollide2 m_collide2;
+
+ /** \brief Internal function for collision detection */
+ void testCollision(CollisionData *data);
- /* This is where static geoms from the world (that are not cleared) are added; the space for this is m_spaceBasicGeoms */
- std::vector<dGeomID> m_basicGeoms;
+ /** \brief Internal function for collision detection */
+ void testSelfCollision(CollisionData *data);
+
+ /** \brief Internal function for collision detection */
+ void testBodyCollision(CollisionNamespace *cn, CollisionData *data);
+
+ dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, shapes::Shape *shape, double scale, double padding);
+ void updateGeom(dGeomID geom, btTransform &pose) const;
+ void freeMemory(void);
- private:
+ ModelInfo m_modelGeom;
+ std::map<std::string, CollisionNamespace*> m_collNs;
- /* Pointers for ODE indices; we need this around in ODE's assumed datatype */
- std::vector<dTriIndex*> m_meshIndices;
- std::vector<dTriMeshDataID> m_meshData;
};
}
Modified: pkg/trunk/world_models/collision_space/src/environment.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environment.cpp 2009-07-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/world_models/collision_space/src/environment.cpp 2009-07-12 21:39:40 UTC (rev 18679)
@@ -46,7 +46,7 @@
m_verbose = verbose;
}
-void collision_space::EnvironmentModel::addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+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;
Modified: pkg/trunk/world_models/collision_space/src/environmentBullet.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-07-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/world_models/collision_space/src/environmentBullet.cpp 2009-07-12 21:39:40 UTC (rev 18679)
@@ -59,9 +59,9 @@
delete m_config; */
}
-void collision_space::EnvironmentModelBullet::addRobotModel(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<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
- collision_space::EnvironmentModel::addRobotModel(model, links, scale, padding);
+ collision_space::EnvironmentModel::setRobotModel(model, links, scale, padding);
m_modelGeom.scale = scale;
m_modelGeom.padding = padding;
@@ -266,7 +266,7 @@
return false;
}
-void collision_space::EnvironmentModelBullet::addPointCloud(unsigned int n, const double *points)
+void collision_space::EnvironmentModelBullet::addPointCloudSpheres(const std::string &ns, unsigned int n, const double *points)
{
btTransform t;
t.setIdentity();
@@ -280,30 +280,41 @@
t.setOrigin(btVector3(points[i4], points[i4 + 1], points[i4 + 2]));
object->setWorldTransform(t);
m_world->addCollisionObject(object);
- m_dynamicObstacles.push_back(object);
+ m_obstacles[ns].push_back(object);
}
}
-void collision_space::EnvironmentModelBullet::addStaticPlane(double a, double b, double c, double d)
+void collision_space::EnvironmentModelBullet::addPlane(const std::string &ns, double a, double b, double c, double d)
{
btCollisionObject *object = new btCollisionObject();
btCollisionShape *shape = new btStaticPlaneShape(btVector3(a, b, c), d);
object->setCollisionShape(shape);
m_world->addCollisionObject(object);
+ m_obstacles[ns].push_back(object);
}
-void collision_space::EnvironmentModelBullet::clearObstacles(void)
+void collision_space::EnvironmentModelBullet::clearObstacles(const std::string &ns)
{
- for (unsigned int i = 0 ; i < m_dynamicObstacles.size() ; ++i)
+ if (m_obstacles.find(ns) != m_obstacles.end())
{
- btCollisionObject* obj = m_dynamicObstacles[i];
- m_world->removeCollisionObject(obj);
- // delete obj->getCollisionShape();
- // delete obj;
+ unsigned int n = m_obstacles[ns].size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ btCollisionObject* obj = m_obstacles[ns][i];
+ m_world->removeCollisionObject(obj);
+ // delete obj->getCollisionShape();
+ // delete obj;
+ }
+ m_obstacles.erase(ns);
}
- m_dynamicObstacles.clear();
}
+void collision_space::EnvironmentModelBullet::clearObstacles(void)
+{
+ for (std::map<std::string, std::vector<btCollisionObject*> >::iterator it = m_obstacles.begin() ; it != m_obstacles.end() ; ++it)
+ clearObstacles(it->first);
+}
+
int collision_space::EnvironmentModelBullet::setCollisionCheck(const std::string &link, bool state)
{
int result = -1;
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-12 04:39:55 UTC (rev 18678)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-12 21:39:40 UTC (rev 18679)
@@ -46,21 +46,13 @@
delete m_modelGeom.linkGeom[j];
if (m_modelGeom.space)
dSpaceDestroy(m_modelGeom.space);
- if (m_space)
- dSpaceDestroy(m_space);
- if (m_spaceBasicGeoms)
- dSpaceDestroy(m_spaceBasicGeoms);
- for (unsigned int i = 0 ; i < m_meshIndices.size() ; ++i)
- delete[] m_meshIndices[i];
- m_meshIndices.clear();
- for (unsigned int i = 0 ; i < m_meshData.size() ; ++i)
- dGeomTriMeshDataDestroy(m_meshData[i]);
- m_meshData.clear();
+ for (std::map<std::string, CollisionNamespace*>::iterator it = m_collNs.begin() ; it != m_collNs.end() ; ++it)
+ delete it->second;
}
-void collision_space::EnvironmentModelODE::addRobotModel(const boost::shared_ptr<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::addRobotModel(model, links, scale, padding);
+ collision_space::EnvironmentModel::setRobotModel(model, links, scale, padding);
m_modelGeom.space = dSweepAndPruneSpaceCreate(0, dSAP_AXES_XZY);
m_modelGeom.scale = scale;
@@ -79,13 +71,13 @@
kg->link = link;
kg->enabled = true;
kg->index = i;
- dGeomID g = createODEGeom(m_modelGeom.space, link->shape, scale, padding);
+ dGeomID g = createODEGeom(m_modelGeom.space, m_modelGeom.storage, link->shape, scale, padding);
assert(g);
dGeomSetData(g, reinterpret_cast<void*>(kg));
kg->geom.push_back(g);
for (unsigned int k = 0 ; k < kg->link->attachedBodies.size() ; ++k)
{
- dGeomID ga = createODEGeom(m_modelGeom.space, kg->link->attachedBodies[k]->shape, scale, padding);
+ dGeomID ga = createODEGeom(m_modelGeom.space, m_modelGeom.storage, kg->link->attachedBodies[k]->shape, scale, padding);
assert(ga);
dGeomSetData(ga, reinterpret_cast<void*>(kg));
kg->geom.push_back(ga);
@@ -94,7 +86,7 @@
}
}
-dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, shapes::Shape *shape, double scale, double padding)
+dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, ODEStorage &storage, shapes::Shape *shape, double scale, double padding)
{
dGeomID g = NULL;
switch (shape->type)
@@ -126,8 +118,8 @@
indices[i] = mesh->triangles[i];
dGeomTriMeshDataBuildDouble(data, mesh->vertices, sizeof(double) * 3, mesh->vertexCount, indices, icount, sizeof(dTriIndex) * 3);
g = dCreateTriMesh(space, data, NULL, NULL, NULL);
- m_meshIndices.push_back(indices);
- m_meshData.push_back(data);
+ storage.meshIndices.push_back(indices);
+ storage.meshData.push_back(data);
}
default:
@@ -162,7 +154,7 @@
const unsigned int nab = kg->link->attachedBodies.size();
for (unsigned int k = 0 ; k < nab ; ++k)
{
- dGeomID ga = createODEGeom(m_modelGeom.space, kg->link->attachedBodies[k]->shape, m_modelGeom.scale, m_modelGeom.padding);
+ dGeomID ga = createODEGeom(m_modelGeom.space, m_modelGeom.storage, kg->link->attachedBodies[k]->shape, m_modelGeom.scale, m_modelGeom.padding);
assert(ga);
dGeomSetData(ga, reinterpret_cast<void*>(kg));
kg->geom.push_back(ga);
@@ -183,6 +175,11 @@
}
}
+bool collision_space::EnvironmentModelODE::ODECollide2::empty(void)
+{
+ return m_geomsX.empty();
+}
+
void collision_space::EnvironmentModelODE::ODECollide2::registerSpace(dSpaceID space)
{
int n = dSpaceGetNumGeoms(space);
@@ -299,50 +296,13 @@
}
}
-dSpaceID collision_space::EnvironmentModelODE::getODESpace(void) const
-{
- return m_space;
-}
-
-dSpaceID collision_space::EnvironmentModelODE::getODEBasicGeomSpace(void) const
-{
- return m_spaceBasicGeoms;
-}
-
-dSpaceID collision_space::EnvironmentModelODE::getModelODESpace(void) const
-{
- return m_modelGeom.space;
-}
-
namespace collision_space
{
- struct CollisionData
- {
- CollisionData(void)
- {
- done = false;
- collides = false;
- max_contacts = 0;
- contacts = NULL;
- selfCollisionTest = NULL;
- link1 = link2 = NULL;
- }
-
- bool done;
-
- bool collides;
- unsigned int max_contacts;
- std::vector<EnvironmentModelODE::Contact> *contacts;
- std::vector< std::vector<bool> > *selfCollisionTest;
-
- planning_models::KinematicModel::Link *link1;
- planning_models::KinematicModel::Link *link2;
- };
-
+
void nearCallbackFn(void *data, dGeomID o1, dGeomID o2)
{
- CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
+ EnvironmentModelODE::CollisionData *cdata = reinterpret_cast<EnvironmentModelODE::CollisionData*>(data);
if (cdata->done)
return;
@@ -422,7 +382,7 @@
cdata.contacts = &contacts;
cdata.max_contacts = max_count;
contacts.clear();
- testCollision(reinterpret_cast<void*>(&cdata));
+ testCollision(&cdata);
return cdata.collides;
}
@@ -430,7 +390,7 @@
{
CollisionData cdata;
cdata.selfCollisionTest = &m_selfCollisionTest;
- testCollision(reinterpret_cast<void*>(&cdata));
+ testCollision(&cdata);
return cdata.collides;
}
@@ -438,119 +398,136 @@
{
CollisionData cdata;
cdata.selfCollisionTest = &m_selfCollisionTest;
- testSelfCollision(reinterpret_cast<void*>(&cdata));
+ testSelfCollision(&cdata);
return cdata.collides;
}
-void collision_space::EnvironmentModelODE::testSelfCollision(void *data)
+void collision_space::EnvironmentModelODE::testSelfCollision(CollisionData *cdata)
{
- dSpaceCollide(m_modelGeom.space, data, nearCallbackFn);
+ dSpaceCollide(m_modelGeom.space, cdata, nearCallbackFn);
}
-void collision_space::EnvironmentModelODE::testStaticBodyCollision(void *data)
-{
- CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
+void collision_space::EnvironmentModelODE::testBodyCollision(CollisionNamespace *cn, CollisionData *cdata)
+{
cdata->link2 = NULL;
- for (int i = m_modelGeom.linkGeom.size() - 1 ; i >= 0 && !cdata->done ; --i)
+
+ if (cn->collide2.empty())
{
- /* skip disabled bodies */
- if (!m_modelGeom.linkGeom[i]->enabled)
- continue;
- const unsigned int ng = m_modelGeom.linkGeom[i]->geom.size();
- cdata->link1 = m_modelGeom.linkGeom[i]->link;
-
- for (unsigned int ig = 0 ; ig < ng && !cdata->done ; ++ig)
+ // if there is no collide2 structure, then there is a list of geoms
+ for (int i = m_modelGeom.linkGeom.size() - 1 ; i >= 0 && !cdata->done ; --i)
{
- dGeomID g1 = m_modelGeom.linkGeom[i]->geom[ig];
- dReal aabb1[6];
- dGeomGetAABB(g1, aabb1);
- for (int j = m_basicGeoms.size() - 1 ; j >= 0 ; --j)
- {
- dGeomID g2 = m_basicGeoms[j];
- dReal aabb2[6];
- dGeomGetAABB(g2, aabb2);
-
- if (!(aabb1[2] > aabb2[3] ||
- aabb1[3] < aabb2[2] ||
- aabb1[4] > aabb2[5] ||
- aabb1[5] < aabb2[4]))
- dSpaceCollide2(g1, g2, data, nearCallbackFn);
-
- if (cdata->collides && m_verbose)
- m_msg.inform("Collision between static body and link '%s'\n",
- m_modelGeom.linkGeom[i]->link->name.c_str());
- }
- }
- }
-}
-
-void collision_space::EnvironmentModelODE::testDynamicBodyCollision(void *data)
-{
- CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
- cdata->link2 = NULL;
- m_collide2.setup();
- for (int i = m_modelGeom.linkGeom.size() - 1 ; i >= 0 && !cdata->done ; --i)
- if (m_modelGeom.linkGeom[i]->enabled)
- {
+ /* skip disabled bodies */
+ if (!m_modelGeom.linkGeom[i]->enabled)
+ continue;
const unsigned int ng = m_modelGeom.linkGeom[i]->geom.size();
cdata->link1 = m_modelGeom.linkGeom[i]->link;
+
for (unsigned int ig = 0 ; ig < ng && !cdata->done ; ++ig)
{
- m_collide2.collide(m_modelGeom.linkGeom[i]->geom[ig], data, nearCallbackFn);
- if (cdata->collides && m_verbose)
- m_msg.inform("Collision between dynamic body and link '%s'\n",
- m_modelGeom.linkGeom[i]->link->name.c_str());
+ dGeomID g1 = m_modelGeom.linkGeom[i]->geom[ig];
+ dReal aabb1[6];
+ dGeomGetAABB(g1, aabb1);
+ for (int j = cn->geoms.size() - 1 ; j >= 0 ; --j)
+ {
+ dGeomID g2 = cn->geoms[j];
+ dReal aabb2[6];
+ dGeomGetAABB(g2, aabb2);
+
+ if (!(aabb1[2] > aabb2[3] ||
+ aabb1[3] < aabb2[2] ||
+ aabb1[4] > aabb2[5] ||
+ aabb1[5] < aabb2[4]))
+ dSpaceCollide2(g1, g2, cdata, nearCallbackFn);
+
+ if (cdata->collides && m_verbose)
+ m_msg.inform("Collision between body in namespace '%s' and link '%s'\n",
+ cn->name.c_str(), m_modelGeom.linkGeom[i]->link->name.c_str());
+ }
}
}
+ }
+ else
+ {
+ cn->collide2.setup();
+ for (int i = m_modelGeom.linkGeom.size() - 1 ; i >= 0 && !cdata->done ; --i)
+ if (m_modelGeom.linkGeom[i]->enabled)
+ {
+ const unsigned int ng = m_modelGeom.linkGeom[i]->geom.size();
+ cdata->link1 = m_modelGeom.linkGeom[i]->link;
+ for (unsigned int ig = 0 ; ig < ng && !cdata->don...
[truncated message content] |
|
From: <is...@us...> - 2009-07-13 21:31:28
|
Revision: 18693
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18693&view=rev
Author: isucan
Date: 2009-07-13 21:31:24 +0000 (Mon, 13 Jul 2009)
Log Message:
-----------
better organization of planning_environment, fixed the way we attach objects to the robot and began work on representing objects by their model in the collision space
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/ModelBase.h
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/GoalDefinitions.h
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/StateValidator.h
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/PlannerSetup.h
pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp
pkg/trunk/motion_planning/planning_research/sbpl_pm_wrapper/include/sbpl_pm_wrapper/pm_wrapper.h
pkg/trunk/sandbox/chomp_motion_planner/include/chomp_motion_planner/chomp_robot_model.h
pkg/trunk/stacks/geometry/bullet/Makefile.bullet
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/planning_display.cpp
pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h
pkg/trunk/util/geometric_shapes/src/load_mesh.cpp
pkg/trunk/util/robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/
pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/collision_models.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/robot_models.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/
pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/construct_object.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/kinematic_state_constraint_evaluator.h
pkg/trunk/motion_planning/planning_environment/src/models/
pkg/trunk/motion_planning/planning_environment/src/models/collision_models.cpp
pkg/trunk/motion_planning/planning_environment/src/models/robot_models.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/
pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/view_state_validity.cpp
pkg/trunk/motion_planning/planning_environment/src/util/
pkg/trunk/motion_planning/planning_environment/src/util/construct_object.cpp
pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp
pkg/trunk/stacks/common_msgs/mapping_msgs/msg/AttachedObject.msg
pkg/trunk/stacks/common_msgs/mapping_msgs/msg/Object.msg
Removed Paths:
-------------
pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/msg/AttachedObject.msg
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_state_constraint_evaluator.cpp
pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
pkg/trunk/motion_planning/planning_environment/src/view_state_validity.cpp
Deleted: pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/msg/AttachedObject.msg
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/msg/AttachedObject.msg 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_msgs/msg/AttachedObject.msg 2009-07-13 21:31:24 UTC (rev 18693)
@@ -1,8 +0,0 @@
-# This mesage defines what objects are attached to a link. The objects
-# are assumed to be defined in the reference frame of the link they
-# are attached to
-
-Header header
-string robot_name
-string link_name
-ObjectOnTable[] objects
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-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -47,7 +47,7 @@
#include <motion_planning_srvs/MotionPlan.h>
#include <ros/ros.h>
-#include <planning_environment/planning_monitor.h>
+#include <planning_environment/monitors/planning_monitor.h>
namespace move_arm
{
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-13 21:31:24 UTC (rev 18693)
@@ -39,7 +39,7 @@
#include <ros/ros.h>
#include <robot_actions/action_client.h>
-#include <planning_environment/kinematic_model_state_monitor.h>
+#include <planning_environment/monitors/kinematic_model_state_monitor.h>
#include <pr2_robot_actions/MoveArmGoal.h>
#include <pr2_robot_actions/MoveArmState.h>
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/ModelBase.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/ModelBase.h 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/ModelBase.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -37,7 +37,7 @@
#ifndef OMPL_PLANNING_MODEL_BASE_
#define OMPL_PLANNING_MODEL_BASE_
-#include <planning_environment/planning_monitor.h>
+#include <planning_environment/monitors/planning_monitor.h>
#include <string>
namespace ompl_planning
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/GoalDefinitions.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/GoalDefinitions.h 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/GoalDefinitions.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -38,7 +38,7 @@
#define OMPL_PLANNING_EXTENSIONS_GOAL_DEFINITIONS_
#include "ompl_planning/ModelBase.h"
-#include <planning_environment/kinematic_state_constraint_evaluator.h>
+#include <planning_environment/util/kinematic_state_constraint_evaluator.h>
#include <ompl/extension/kinematic/SpaceInformationKinematic.h>
namespace ompl_planning
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/StateValidator.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/StateValidator.h 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/StateValidator.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -39,7 +39,7 @@
#include <ompl/base/StateValidityChecker.h>
#include <collision_space/environment.h>
-#include <planning_environment/kinematic_state_constraint_evaluator.h>
+#include <planning_environment/util/kinematic_state_constraint_evaluator.h>
#include "ompl_planning/ModelBase.h"
#include "ompl_planning/extensions/SpaceInformation.h"
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/PlannerSetup.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/PlannerSetup.h 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/planners/PlannerSetup.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -49,7 +49,7 @@
#include <ompl/base/Planner.h>
#include <ompl/extension/kinematic/PathSmootherKinematic.h>
-#include <planning_environment/robot_models.h>
+#include <planning_environment/models/robot_models.h>
#include <ros/console.h>
#include <string>
Modified: pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-07-13 21:31:24 UTC (rev 18693)
@@ -41,7 +41,7 @@
**/
-#include <planning_environment/collision_space_monitor.h>
+#include <planning_environment/monitors/collision_space_monitor.h>
#include <visualization_msgs/Marker.h>
#include <std_msgs/Byte.h>
Modified: pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-07-13 21:31:24 UTC (rev 18693)
@@ -5,19 +5,23 @@
set(ROS_BUILD_TYPE Debug)
-rospack_add_library(planning_environment src/robot_models.cpp
- src/collision_models.cpp
- src/kinematic_model_state_monitor.cpp
- src/collision_space_monitor.cpp
- src/kinematic_state_constraint_evaluator.cpp
- src/planning_monitor.cpp)
+rospack_add_library(planning_environment src/models/robot_models.cpp
+ src/models/collision_models.cpp
+ src/monitors/kinematic_model_state_monitor.cpp
+ src/monitors/collision_space_monitor.cpp
+ src/monitors/planning_monitor.cpp
+ src/util/kinematic_state_constraint_evaluator.cpp
+ src/util/construct_object.cpp)
rospack_add_openmp_flags(planning_environment)
rospack_link_boost(planning_environment thread)
# Utility apps
-rospack_add_executable(view_state_validity src/view_state_validity.cpp)
+rospack_add_executable(view_state_validity src/tools/view_state_validity.cpp)
target_link_libraries(view_state_validity planning_environment)
+rospack_add_executable(clear_known_objects src/tools/clear_known_objects.cpp)
+target_link_libraries(clear_known_objects planning_environment)
+
# Tests
# Create a model of the PR2
Deleted: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -1,103 +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.
-*********************************************************************/
-
-/** \author Ioan Sucan */
-
-#ifndef PLANNING_ENVIRONMENT_COLLISION_MODELS_
-#define PLANNING_ENVIRONMENT_COLLISION_MODELS_
-
-#include "planning_environment/robot_models.h"
-#include <collision_space/environment.h>
-
-namespace planning_environment
-{
-
- /** \brief A class capable of loading a robot model from the parameter server */
-
- class CollisionModels : public RobotModels
- {
- public:
-
- CollisionModels(const std::string &description, double scale = 1.0, double padd = 0.0) : RobotModels(description), scale_(scale), padd_(padd)
- {
- loadCollision(collision_check_links_);
- }
-
- CollisionModels(const std::string &description, const std::vector<std::string> &links, double scale = 1.0, double padd = 0.0) : RobotModels(description), scale_(scale), padd_(padd)
- {
- loadCollision(links);
- }
-
- virtual ~CollisionModels(void)
- {
- }
-
- /** \brief Reload the robot description and recreate the model */
- virtual void reload(void)
- {
- RobotModels::reload();
- ode_collision_model_.reset();
- bullet_collision_model_.reset();
- loadCollision(collision_check_links_);
- }
-
- /** \brief Return the instance of the constructed ODE collision model */
- const boost::shared_ptr<collision_space::EnvironmentModel> &getODECollisionModel(void) const
- {
- return ode_collision_model_;
- }
-
- /** \brief Return the instance of the constructed Bullet collision model */
- const boost::shared_ptr<collision_space::EnvironmentModel> &getBulletCollisionModel(void) const
- {
- return bullet_collision_model_;
- }
-
- protected:
-
- void loadCollision(const std::vector<std::string> &links);
- void setupModel(boost::shared_ptr<collision_space::EnvironmentModel> &model, const std::vector<std::string> &links);
-
- boost::shared_ptr<collision_space::EnvironmentModel> ode_collision_model_;
- boost::shared_ptr<collision_space::EnvironmentModel> bullet_collision_model_;
-
- double scale_;
- double padd_;
- };
-
-
-}
-
-#endif
-
Deleted: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -1,167 +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.
-*********************************************************************/
-
-/** \author Ioan Sucan */
-
-#ifndef PLANNING_ENVIRONMENT_COLLISION_SPACE_MONITOR_
-#define PLANNING_ENVIRONMENT_COLLISION_SPACE_MONITOR_
-
-#include "planning_environment/collision_models.h"
-#include "planning_environment/kinematic_model_state_monitor.h"
-
-#include <tf/message_notifier.h>
-#include <mapping_msgs/CollisionMap.h>
-#include <tabletop_msgs/AttachedObject.h>
-
-#include <boost/thread/mutex.hpp>
-
-namespace planning_environment
-{
-
- /** \brief @b CollisionSpaceMonitor is a class which in addition to being aware of a robot model,
- is also aware of a collision space.
- */
- class CollisionSpaceMonitor : public KinematicModelStateMonitor
- {
- public:
-
- CollisionSpaceMonitor(CollisionModels *cm, tf::TransformListener *tf, std::string frame_id) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), tf, frame_id)
- {
- cm_ = cm;
- setupCSM();
- }
-
- CollisionSpaceMonitor(CollisionModels *cm, tf::TransformListener *tf) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), tf)
- {
- cm_ = cm;
- setupCSM();
- }
-
- virtual ~CollisionSpaceMonitor(void)
- {
- if (collisionMapNotifier_)
- delete collisionMapNotifier_;
- if (collisionMapUpdateNotifier_)
- delete collisionMapUpdateNotifier_;
- if (attachedBodyNotifier_)
- delete attachedBodyNotifier_;
- }
-
- /** \brief Return the instance of the environment model maintained */
- collision_space::EnvironmentModel* getEnvironmentModel(void) const
- {
- return collisionSpace_;
- }
-
- /** \brief Return the instance of collision models that is being used */
- CollisionModels* getCollisionModels(void) const
- {
- return cm_;
- }
-
- /** \brief Return the scaling employed when creating spheres
- from boxes in a collision map. The radius of a sphere is
- this scaling multiplied by the largest extent of the box */
- double getBoxScale(void) const
- {
- return boxScale_;
- }
-
- /** \brief Define a callback for before updating a map */
- void setOnBeforeMapUpdateCallback(const boost::function<void(const mapping_msgs::CollisionMapConstPtr)> &callback)
- {
- onBeforeMapUpdate_ = callback;
- }
-
- /** \brief Define a callback for after updating a map */
- void setOnAfterMapUpdateCallback(const boost::function<void(const mapping_msgs::CollisionMapConstPtr)> &callback)
- {
- onAfterMapUpdate_ = callback;
- }
-
- /** \brief Define a callback for after updating a map */
- void setOnAfterAttachBodyCallback(const boost::function<void(planning_models::KinematicModel::Link*)> &callback)
- {
- onAfterAttachBody_ = callback;
- }
-
- /** \brief Return true if map has been received */
- bool haveMap(void) const
- {
- return haveMap_;
- }
-
- /** \brief Return true if a map update has been received in the last sec seconds. If sec < 10us, this function always returns true. */
- bool isMapUpdated(double sec) const;
-
-
- /** \brief Wait until a map is received */
- void waitForMap(void) const;
-
- /** \brief Return the last update time for the map */
- const ros::Time& lastMapUpdate(void) const
- {
- return lastMapUpdate_;
- }
-
- protected:
-
- void setupCSM(void);
- void updateCollisionSpace(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear);
- void collisionMapCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
- void collisionMapUpdateCallback(const mapping_msgs::CollisionMapConstPtr &collisionMap);
- void attachObjectCallback(const tabletop_msgs::AttachedObjectConstPtr &attachedObject);
-
- CollisionModels *cm_;
- collision_space::EnvironmentModel *collisionSpace_;
- double boxScale_;
- boost::mutex mapUpdateLock_;
-
- bool haveMap_;
- ros::Time lastMapUpdate_;
- tf::MessageNotifier<mapping_msgs::CollisionMap> *collisionMapNotifier_;
- tf::MessageNotifier<mapping_msgs::CollisionMap> *collisionMapUpdateNotifier_;
- tf::MessageNotifier<tabletop_msgs::AttachedObject> *attachedBodyNotifier_;
-
- boost::function<void(const mapping_msgs::CollisionMapConstPtr)> onBeforeMapUpdate_;
- boost::function<void(const mapping_msgs::CollisionMapConstPtr)> onAfterMapUpdate_;
- boost::function<void(planning_models::KinematicModel::Link*)> onAfterAttachBody_;
-
- };
-
-
-}
-
-#endif
-
Deleted: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-07-13 21:30:06 UTC (rev 18692)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-07-13 21:31:24 UTC (rev 18693)
@@ -1,196 +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.
-*********************************************************************/
-
-/** \author Ioan Sucan */
-
-#ifndef PLANNING_ENVIRONMENT_KINEMATIC_MODEL_STATE_MONITOR_
-#define PLANNING_ENVIRONMENT_KINEMATIC_MODEL_STATE_MONITOR_
-
-#include "planning_environment/robot_models.h"
-#include <tf/transform_datatypes.h>
-#include <tf/transform_listener.h>
-#include <mechanism_msgs/MechanismState.h>
-#include <boost/bind.hpp>
-#include <vector>
-#include <string>
-
-namespace planning_environment
-{
-
- /** \brief @b KinematicModelStateMonitor is a class that monitors the robot state for the kinematic model defined in @b RobotModels
- If the pose is not included, the robot state is the frame of the link it attaches to the world. If the pose is included,
- the frame of the robot is the one in which the pose is published.
- */
- class KinematicModelStateMonitor
- {
- public:
-
- KinematicModelStateMonitor(RobotModels *rm, tf::TransformListener *tf)
- {
- rm_ = rm;
- tf_ = tf;
- includePose_ = false;
- setupRSM();
- }
-
- KinematicModelStateMonitor(RobotModels *rm, tf::TransformListener *tf, const std::string &frame_id)
- {
- rm_ = rm;
- tf_ = tf;
- includePose_ = true;
- frame_id_ = frame_id;
- setupRSM();
- }
-
- virtual ~KinematicModelStateMonitor(void)
- {
- if (robotState_)
- delete robotState_;
- }
-
- /** \brief Define a callback for when the state is changed */
- void setOnStateUpdateCallback(const boost::function<void(void)> &callback)
- {
- onStateUpdate_ = callback;
- }
-
- /** \brief Get the kinematic model that is being monitored */
- planning_models::KinematicModel* getKinematicModel(void) const
- {
- return kmodel_;
- }
-
- /** \brief Get the instance of @b RobotModels that is being used */
- RobotModels* getRobotModels(void) const
- {
- return rm_;
- }
-
- /** \brief Return a pointer to the maintained robot state */
- const planning_models::StateParams* getRobotState(void) const
- {
- return robotState_;
- }
-
- /** \brief Return the transform listener */
- tf::TransformListener *getTransformListener(void) const
- {
- return tf_;
- }
-
- /** \brief Return the frame id of the state */
- const std::string& getFrameId(void) const
- {
- return frame_id_;
- }
-
- /** \brief Return the robot frame id (robot without pose) */
- const std::string& getRobotFrameId(void) const
- {
- return robot_frame_;
- }
-
- /** \brief Return true if a full mechanism state has been received (including pose, if pose inclusion is enabled) */
- bool haveState(void) const
- {
- return haveMechanismState_ && (!includePose_ || (includePose_ && havePose_));
- }
-
- /** \brief Return the time of the last state update */
- const ros::Time& lastMechanismStateUpdate(void) const
- {
- return lastMechanismStateUpdate_;
- }
-
- /** \brief Return the time of the last pose update */
- const ros::Time& lastPoseUpdate(void) const
- {
- return lastPoseUpdate_;
- }
-
- /** \brief Wait until a full mechanism state is received (including pose, if pose inclusion is enabled) */
- void waitForState(void) const;
-
- /** \brief Return true if a mechanism state has been received in the last sec seconds. If sec < 10us, this function always returns true. */
- bool isMechanismStateUpdated(double sec) const;
-
- /** \brief Return true if a pose has been received in the last
- sec seconds. If sec < 10us, this function always returns
- true. */
- bool isPoseUpdated(double sec) const;
-
- /** \brief Return true if the pose is included in the state */
- bool isPoseIncluded(void) const
- {
- return includePose_;
- }
-
- /** \brief Output the current state as ROS INFO */
- void printRobotState(void) const;
-
- protected:
-
- void setupRSM(void);
- void mechanismStateCallback(const mechanism_msgs::MechanismStateConstPtr &mechanismState);
-
-
- RobotModels *rm_;
- bool includePose_;
- planning_models::KinematicModel *kmodel_;
- std::string planarJoint_;
- std::string floatingJoint_;
-
- ros::NodeHandle nh_;
- ros::Subscriber mechanismStateSubscriber_;
- tf::TransformListener *tf_;
-
- /** \brief How long to wait for a TF if it is not yet available, before failing */
- ros::Duration tfWait_;
-
- planning_models::StateParams *robotState_;
- tf::Pose pose_;
- std::string robot_frame_;
- std::string frame_id_;
-
- boost::function<void(void)> onStateUpdate_;
-
- bool havePose_;
- bool haveMechanismState_;
- ros::Time ...
[truncated message content] |
|
From: <mee...@us...> - 2009-07-14 22:24:14
|
Revision: 18799
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18799&view=rev
Author: meeussen
Date: 2009-07-14 22:24:04 +0000 (Tue, 14 Jul 2009)
Log Message:
-----------
get the link name from the kdl segment
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/chain.h
pkg/trunk/stacks/mechanism/mechanism_model/src/chain.cpp
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-07-14 22:23:11 UTC (rev 18798)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-07-14 22:24:04 UTC (rev 18799)
@@ -89,6 +89,7 @@
KDL::Frame desired_frame_;
mechanism::Chain chain_;
+ KDL::Chain kdl_chain_;
double dist_to_line_;
double f_r_;
@@ -101,7 +102,6 @@
mechanism::RobotState *robot_;
std::string controller_name_;
- KDL::Chain kdl_chain_;
boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
boost::scoped_ptr<KDL::ChainFkSolverPos> jnt_to_pose_solver_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-07-14 22:23:11 UTC (rev 18798)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-07-14 22:24:04 UTC (rev 18799)
@@ -467,13 +467,13 @@
ros::Node *node = ros::Node::instance();
- task_frame_name_ = c_.chain_.getLinkName(0);
+ task_frame_name_ = c_.kdl_chain_.getSegment(0).getName();
//node->subscribe(name_ + "/command", command_msg_, &CartesianHybridControllerNode::command, this, 5);
command_notifier_.reset(new tf::MessageNotifier<manipulation_msgs::TaskFrameFormalism>(
&TF, node,
boost::bind(&CartesianHybridControllerNode::command, this, _1),
- name_ + "/command", c_.chain_.getLinkName(0), 100));
+ name_ + "/command", c_.kdl_chain_.getSegment(0).getName(), 100));
pub_state_.reset(new realtime_tools::RealtimePublisher<robot_mechanism_controllers::CartesianHybridState>(name_ + "/state", 1));
pub_tf_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>("/tf_message", 5));
@@ -542,7 +542,7 @@
{
//pub_tf_->msg_.transforms[0].header.stamp.fromSec();
pub_tf_->msg_.transforms[0].header.frame_id = name_ + "/tool_frame";
- pub_tf_->msg_.transforms[0].parent_id = c_.chain_.getLinkName();
+ pub_tf_->msg_.transforms[0].parent_id = c_.kdl_chain_.getSegment(c_.kdl_chain_.getNrOfSegments()-1).getName();
tf::Transform t;
tf::TransformKDLToTF(c_.tool_frame_offset_, t);
tf::transformTFToMsg(t, pub_tf_->msg_.transforms[0].transform);
@@ -558,7 +558,7 @@
tf::Stamped<tf::Transform> task_frame;
try {
- TF.lookupTransform(c_.chain_.getLinkName(0), tff_msg->header.frame_id, tff_msg->header.stamp,
+ TF.lookupTransform(c_.kdl_chain_.getSegment(0).getName(), tff_msg->header.frame_id, tff_msg->header.stamp,
task_frame);
}
catch (tf::TransformException &ex)
@@ -597,17 +597,17 @@
robot_srvs::SetPoseStamped::Request &req,
robot_srvs::SetPoseStamped::Response &resp)
{
- if (!TF.canTransform(c_.chain_.getLinkName(-1), req.p.header.frame_id,
+ if (!TF.canTransform(c_.kdl_chain_.getSegment(c_.kdl_chain_.getNrOfSegments()-1).getName(), req.p.header.frame_id,
req.p.header.stamp, ros::Duration(3.0)))
{
- ROS_ERROR("Cannot transform %s -> %s at %lf", c_.chain_.getLinkName(-1).c_str(),
+ ROS_ERROR("Cannot transform %s -> %s at %lf", c_.kdl_chain_.getSegment(c_.kdl_chain_.getNrOfSegments()-1).getName().c_str(),
req.p.header.frame_id.c_str(), req.p.header.stamp.toSec());
return false;
}
robot_msgs::PoseStamped tool_in_tip_msg;
tf::Transform tool_in_tip;
- TF.transformPose(c_.chain_.getLinkName(-1), req.p, tool_in_tip_msg);
+ TF.transformPose(c_.kdl_chain_.getSegment(c_.kdl_chain_.getNrOfSegments()-1).getName(), req.p, tool_in_tip_msg);
tf::poseMsgToTF(tool_in_tip_msg.pose, tool_in_tip);
tool_in_tip.setOrigin(tf::Vector3(0,0,0));
tf::TransformTFToKDL(tool_in_tip, c_.tool_frame_offset_);
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-07-14 22:23:11 UTC (rev 18798)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-07-14 22:24:04 UTC (rev 18799)
@@ -481,7 +481,8 @@
robot_msgs::PoseStamped tool_offset_msg;
try
{
- TF.transformPose(controller_.chain_.getLinkName(), req.p, tool_offset_msg);
+ //TF.transformPose(controller_.chain_.getLinkName(), req.p, tool_offset_msg);
+ TF.transformPose(controller_.kdl_chain_.getSegment(controller_.kdl_chain_.getNrOfSegments()-1).getName(), req.p, tool_offset_msg);
}
catch(tf::TransformException& ex)
{
Modified: pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/chain.h
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/chain.h 2009-07-14 22:23:11 UTC (rev 18798)
+++ pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/chain.h 2009-07-14 22:24:04 UTC (rev 18799)
@@ -66,7 +66,6 @@
// get KDL chain
void toKDL(KDL::Chain &chain);
- std::string getLinkName(int index = -1);
Joint* getJoint(unsigned int actuated_joint_i);
private:
@@ -75,7 +74,6 @@
std::vector<int> joint_indices_; // ONLY joints that can be actuated (not fixed joints)
std::vector<int> all_joint_indices_; // Includes fixed joints
- std::vector<std::string> link_names_;
};
}
Modified: pkg/trunk/stacks/mechanism/mechanism_model/src/chain.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_model/src/chain.cpp 2009-07-14 22:23:11 UTC (rev 18798)
+++ pkg/trunk/stacks/mechanism/mechanism_model/src/chain.cpp 2009-07-14 22:24:04 UTC (rev 18799)
@@ -149,14 +149,5 @@
}
-std::string Chain::getLinkName(int index)
-{
- if (index == -1)
- index = link_names_.size() - 1;
- return link_names_[index];
}
-
-
-
-}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-07-15 18:22:40
|
Revision: 18865
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18865&view=rev
Author: isucan
Date: 2009-07-15 18:22:37 +0000 (Wed, 15 Jul 2009)
Log Message:
-----------
removing points on known object models seems to work
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/stereo-perception.launch
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
pkg/trunk/mapping/collision_map/collision_map_occ.launch
pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp
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-15 18:21:24 UTC (rev 18864)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/laser-perception.launch 2009-07-15 18:22:37 UTC (rev 18865)
@@ -1,24 +1,40 @@
<launch>
+ <!-- send additional description parameters -->
<include file="$(find pr2_defs)/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="control_laser.py" args="laser_tilt_controller sine 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="robotdesc/pr2" />
+
+<!-- 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" />
+ </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="robotdesc/pr2" />
- <remap from="cloud_in" to="tilt_scan_cloud" />
+ <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" />
</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" />
@@ -35,6 +51,7 @@
<remap from="full_cloud" to="full_cloud_annotated" />
</node>
+ <!-- start collision map -->
<include file="$(find collision_map)/collision_map_occ.launch" />
</launch>
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/stereo-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/stereo-perception.launch 2009-07-15 18:21:24 UTC (rev 18864)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/launch/stereo-perception.launch 2009-07-15 18:22:37 UTC (rev 18865)
@@ -3,9 +3,21 @@
<include file="$(find pr2_defs)/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="robotdesc/pr2" />
+
+<!-- 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" />
+ </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="robotdesc/pr2" />
- <remap from="cloud_in" to="/narrow_stereo/cloud" />
+ <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>
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-07-15 18:21:24 UTC (rev 18864)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/src/simple.cpp 2009-07-15 18:22:37 UTC (rev 18865)
@@ -90,14 +90,20 @@
o1.header.frame_id = "/base_link";
o1.header.stamp = ros::Time::now();
o1.id = "Part1";
- o1.object.type = mapping_msgs::Object::SPHERE;
- o1.object.dimensions.resize(1);
- o1.object.dimensions[0] = 0.32;
- // o1.object.dimensions[1] = 0.32;
- // o1.object.dimensions[2] = 0.32;
- o1.pose.position.x = 0.7;
- o1.pose.position.z = 0.6;
+ o1.object.type = mapping_msgs::Object::BOX;
+ o1.object.dimensions.resize(3);
+ o1.object.dimensions[0] = 0.3;
+ o1.object.dimensions[1] = 0.3;
+ o1.object.dimensions[2] = 0.3;
+ o1.pose.position.x = 1.4;
+ o1.pose.position.y = 0.0;
+ o1.pose.position.z = 0.0;
+ o1.pose.orientation.x = 0.0;
+ o1.pose.orientation.y = 0.0;
+ o1.pose.orientation.z = 0.0;
+ o1.pose.orientation.w = 1.0;
+
while(1){
pub.publish(o1); sleep(1);
}
Modified: pkg/trunk/mapping/collision_map/collision_map_occ.launch
===================================================================
--- pkg/trunk/mapping/collision_map/collision_map_occ.launch 2009-07-15 18:21:24 UTC (rev 18864)
+++ pkg/trunk/mapping/collision_map/collision_map_occ.launch 2009-07-15 18:22:37 UTC (rev 18865)
@@ -11,6 +11,7 @@
<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 -->
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-15 18:21:24 UTC (rev 18864)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp 2009-07-15 18:22:37 UTC (rev 18865)
@@ -66,6 +66,8 @@
nh_.param<double>("~object_scale", scale_, 1.0);
nh_.param<double>("~object_padd", padd_, 0.02);
+ ROS_INFO("Clearing points on known objects using '%s' as fixed frame, %f padding and %f scaling", fixed_frame_.c_str(), padd_, scale_);
+
cloudPublisher_ = nh_.advertise<robot_msgs::PointCloud>("cloud_out", 1);
kmsm_->setOnAfterAttachBodyCallback(boost::bind(&ClearKnownObjects::attachObjectEvent, this, _1));
cloudNotifier_ = new tf::MessageNotifier<robot_msgs::PointCloud>(tf_, boost::bind(&ClearKnownObjects::cloudCallback, this, _1), "cloud_in", fixed_frame_, 1);
@@ -125,10 +127,8 @@
double rsquare;
};
- void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
+ void computeMask(const robot_msgs::PointCloud &cloud, std::vector<int> &mask)
{
- updateObjects_.lock();
-
// check if we have attached bodies
if (attachedObjects_.size() > 0)
{
@@ -174,18 +174,23 @@
kmsm_->getKinematicModel()->unlock();
}
- // transform pointcloud into fixed frame
- robot_msgs::PointCloud cloudTransf;
- tf_.transformPointCloud(fixed_frame_, *cloud, cloudTransf);
+ // transform pointcloud into fixed frame, if needed
+ robot_msgs::PointCloud temp;
+ const robot_msgs::PointCloud *cloudTransf = &cloud;
+ if (fixed_frame_ != cloud.header.frame_id)
+ {
+ tf_.transformPointCloud(fixed_frame_, cloud, temp);
+ cloudTransf = &temp;
+ }
// compute mask for cloud
- int n = cloud->pts.size();
- std::vector<int> mask(n);
+ int n = cloud.pts.size();
+ mask.resize(n);
#pragma omp parallel for
for (int i = 0 ; i < n ; ++i)
{
- btVector3 pt = btVector3(cloudTransf.pts[i].x, cloudTransf.pts[i].y, cloudTransf.pts[i].z);
+ btVector3 pt = btVector3(cloudTransf->pts[i].x, cloudTransf->pts[i].y, cloudTransf->pts[i].z);
int out = 1;
for (unsigned int j = 0 ; out && j < attachedObjects_.size() ; ++j)
@@ -200,38 +205,61 @@
mask[i] = out;
}
+ }
+
+ void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
+ {
+ std::vector<int> mask;
+ bool filter = false;
- updateObjects_.unlock();
+ updateObjects_.lock();
- // publish new cloud
- const unsigned int np = cloud->pts.size();
- robot_msgs::PointCloud data_out;
-
- // fill in output data with points that are NOT in the known objects
- data_out.header = cloud->header;
-
- data_out.pts.resize(0);
- data_out.pts.reserve(np);
-
- data_out.chan.resize(cloud->chan.size());
- for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
+ if (attachedObjects_.size() > 0 || objectsInMap_.size() > 0)
{
- ROS_ASSERT(cloud->chan[i].vals.size() == cloud->pts.size());
- data_out.chan[i].name = cloud->chan[i].name;
- data_out.chan[i].vals.reserve(cloud->chan[i].vals.size());
+ computeMask(*cloud, mask);
+ filter = true;
}
- for (unsigned int i = 0 ; i < np ; ++i)
- if (mask[i])
+ updateObjects_.unlock();
+
+ if (filter)
+ {
+ // publish new cloud
+ const unsigned int np = cloud->pts.size();
+ robot_msgs::PointCloud data_out;
+
+ // fill in output data with points that are NOT in the known objects
+ data_out.header = cloud->header;
+
+ data_out.pts.resize(0);
+ data_out.pts.reserve(np);
+
+ data_out.chan.resize(cloud->chan.size());
+ for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
{
- data_out.pts.push_back(cloud->pts[i]);
- for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
- data_out.chan[j].vals.push_back(cloud->chan[j].vals[i]);
+ ROS_ASSERT(cloud->chan[i].vals.size() == cloud->pts.size());
+ data_out.chan[i].name = cloud->chan[i].name;
+ data_out.chan[i].vals.reserve(cloud->chan[i].vals.size());
}
-
- cloudPublisher_.publish(data_out);
+
+ for (unsigned int i = 0 ; i < np ; ++i)
+ if (mask[i])
+ {
+ data_out.pts.push_back(cloud->pts[i]);
+ for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
+ data_out.chan[j].vals.push_back(cloud->chan[j].vals[i]);
+ }
+
+ ROS_DEBUG("Published filtered cloud (%d points out of %d)", data_out.pts.size(), cloud->pts.size());
+ cloudPublisher_.publish(data_out);
+ }
+ else
+ {
+ cloudPublisher_.publish(*cloud);
+ ROS_DEBUG("Republished unchanged cloud");
+ }
}
-
+
void objectInMapCallback(const mapping_msgs::ObjectInMapConstPtr &objectInMap)
{
updateObjects_.lock();
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-15 18:21:24 UTC (rev 18864)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp 2009-07-15 18:22:37 UTC (rev 18865)
@@ -138,7 +138,7 @@
{
case mapping_msgs::Object::SPHERE:
mk.type = visualization_msgs::Marker::SPHERE;
- mk.scale.x = mk.scale.y = mk.scale.z = obj.dimensions[0];
+ mk.scale.x = mk.scale.y = mk.scale.z = obj.dimensions[0] * 2.0;
break;
case mapping_msgs::Object::BOX:
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-07-15 21:59:44
|
Revision: 18892
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18892&view=rev
Author: hsujohnhsu
Date: 2009-07-15 21:59:38 +0000 (Wed, 15 Jul 2009)
Log Message:
-----------
*added extension to urdf mesh filename in robot visual tag; <mesh filename="*.stlb"/>
*added <map> for gazebo collision mesh
*update urdf2gazebo parser to deal with the changes
*update rviz urdf .mesh extraction to deal with the changes
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/urdf2gazebo.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
pkg/trunk/stacks/pr2/pr2_defs/defs/arm_defs.xml
pkg/trunk/stacks/pr2/pr2_defs/defs/base_defs.xml
pkg/trunk/stacks/pr2/pr2_defs/defs/body_defs.xml
pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml
pkg/trunk/stacks/pr2/pr2_defs/defs/head_defs.xml
pkg/trunk/stacks/pr2/pr2_defs/defs/ptz_defs.xml
pkg/trunk/stacks/visualization/rviz/src/rviz/robot/robot_link.cpp
Removed Paths:
-------------
pkg/trunk/stacks/pr2/pr2_defs/defs/arm_grav_defs.xml
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/urdf2gazebo.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/urdf2gazebo.h 2009-07-15 21:42:11 UTC (rev 18891)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/urdf2gazebo.h 2009-07-15 21:59:38 UTC (rev 18892)
@@ -68,6 +68,8 @@
void copyGazeboMap(const robot_desc::URDF::Map& data, TiXmlElement *elem, const std::vector<std::string> *tags);
+ void copyOgreMap(const robot_desc::URDF::Map& data, TiXmlElement *elem, const std::vector<std::string> *tags);
+
std::string getGeometrySize(robot_desc::URDF::Link::Geometry* geometry, int *sizeCount, double *sizeVals);
void convertLink(TiXmlElement *root, robot_desc::URDF::Link *link, const btTransform &transform, bool enforce_limits);
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp 2009-07-15 21:42:11 UTC (rev 18891)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp 2009-07-15 21:59:38 UTC (rev 18892)
@@ -118,6 +118,33 @@
}
}
+void URDF2Gazebo::copyOgreMap(const robot_desc::URDF::Map& data, TiXmlElement *elem, const std::vector<std::string> *tags = NULL)
+{
+ std::vector<std::string> ogre_names;
+ data.getMapTagNames("ogre", ogre_names);
+ for (unsigned int k = 0 ; k < ogre_names.size() ; ++k)
+ {
+ std::map<std::string, std::string> m = data.getMapTagValues("ogre", ogre_names[k]);
+ std::vector<std::string> accepted_tags;
+ if (tags)
+ accepted_tags = *tags;
+ else
+ for (std::map<std::string, std::string>::iterator it = m.begin() ; it != m.end() ; it++)
+ accepted_tags.push_back(it->first);
+
+ for (unsigned int i = 0 ; i < accepted_tags.size() ; ++i)
+ if (m.find(accepted_tags[i]) != m.end())
+ addKeyValue(elem, accepted_tags[i], m[accepted_tags[i]]);
+
+ std::map<std::string, const TiXmlElement*> x = data.getMapTagXML("ogre", ogre_names[k]);
+ for (std::map<std::string, const TiXmlElement*>::iterator it = x.begin() ; it != x.end() ; it++)
+ {
+ for (const TiXmlNode *child = it->second->FirstChild() ; child ; child = child->NextSibling())
+ elem->LinkEndChild(child->Clone());
+ }
+ }
+}
+
std::string URDF2Gazebo::getGeometrySize(robot_desc::URDF::Link::Geometry* geometry, int *sizeCount, double *sizeVals)
{
std::string type;
@@ -294,7 +321,15 @@
if (mesh->filename.empty())
addKeyValue(visual, "mesh", "unit_" + type);
else
- addKeyValue(visual, "mesh", mesh->filename + ".mesh");
+ {
+ // skipping this block as we test the copyOgreMap function
+ // // strip extension from filename
+ // std::string tmp_extension(".stl");
+ // int pos1 = mesh->filename.find(tmp_extension,0);
+ // mesh->filename.replace(pos1,mesh->filename.size()-pos1+1,std::string(""));
+ // // add mesh filename
+ // addKeyValue(visual, "mesh", mesh->filename + ".mesh");
+ }
}
else
@@ -307,6 +342,9 @@
}
copyGazeboMap(link->visual->data, visual);
+
+ // ogre mesh map for all trimeshes visualized
+ copyOgreMap(link->visual->data, visual);
}
/* end create visual node */
Modified: pkg/trunk/stacks/pr2/pr2_defs/defs/arm_defs.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/defs/arm_defs.xml 2009-07-15 21:42:11 UTC (rev 18891)
+++ pkg/trunk/stacks/pr2/pr2_defs/defs/arm_defs.xml 2009-07-15 21:59:38 UTC (rev 18892)
@@ -67,14 +67,20 @@
<elem key="material" value="PR2/Blue" />
</map>
<geometry name="${side}_shoulder_pan_visual">
- <mesh filename="shoulder_yaw" />
+ <mesh filename="shoulder_yaw.stlb" />
</geometry>
+ <map name="${side}_shoulder_pan_visual_mesh" flag="ogre">
+ <elem key="mesh" value="shoulder_yaw.mesh" />
+ </map>
</visual>
<collision>
<origin xyz="0.0 0 0.0" rpy="0 0 0" />
<geometry name="${side}_shoulder_pan_collision">
<mesh filename="convex/shoulder_yaw_convex.stlb" />
</geometry>
+ <map name="${side}_shoulder_pan_collision_mesh" flag="ogre">
+ <elem key="mesh" value="convex/shoulder_yaw_convex.mesh" />
+ </map>
</collision>
<map name="${side}_shoulder_pan_sensor" flag="gazebo">
<verbatim key="${side}_shoulder_pan_bumper_sensor">
@@ -137,14 +143,20 @@
<elem key="material" value="PR2/Grey" />
</map>
<geometry name="${side}_shoulder_lift_visual">
- <mesh filename="shoulder_lift" />
+ <mesh filename="shoulder_lift.stlb" />
</geometry>
+ <map name="${side}_shoulder_lift_visual_mesh" flag="ogre">
+ <elem key="mesh" value="shoulder_lift.mesh" />
+ </map>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_shoulder_lift_collision">
<mesh filename="convex/shoulder_lift_convex.stlb" />
</geometry>
+ <map name="${side}_shoulder_lift_collision_mesh" flag="ogre">
+ <elem key="mesh" value="convex/shoulder_lift_convex.mesh" />
+ </map>
</collision>
<map name="${side}_shoulder_lift_sensor" flag="gazebo">
<verbatim key="${side}_shoulder_lift_bumper_sensor">
@@ -171,7 +183,7 @@
<mechanicalReduction>${61.89/cal_r_shoulder_lift_gearing}</mechanicalReduction>
</transmission>
- <!-- Upperarm roll -->
+ <!-- Upperarm roll: internal fixed attchment point for upper arm -->
<joint name="${side}_upper_arm_roll_joint" type="revolute">
<axis xyz="1 0 0" />
@@ -208,8 +220,11 @@
<elem key="material" value="PR2/RollLinks" />
</map>
<geometry name="${side}_upper_arm_roll_visual">
- <mesh filename="upper_arm_roll" />
+ <mesh filename="upper_arm_roll.stlb" />
</geometry>
+ <map name="${side}_upper_arm_roll_visual_mesh" flag="ogre">
+ <elem key="mesh" value="upper_arm_roll.mesh" />
+ </map>
</visual>
<collision> <!-- TODO: collision tag should be optional -->
<origin xyz="0.0 0 0" rpy="0 0 0" />
@@ -262,8 +277,11 @@
<elem key="material" value="PR2/Green" />
</map>
<geometry name="${side}_upper_arm_visual">
- <mesh filename="upper_arm" />
+ <mesh filename="upper_arm.stlb" />
</geometry>
+ <map name="${side}_upper_arm_visual_mesh" flag="ogre">
+ <elem key="mesh" value="upper_arm.mesh" />
+ </map>
</visual>
<collision>
@@ -271,6 +289,9 @@
<geometry name="${side}_upper_arm_collision">
<mesh filename="convex/upper_arm_convex.stlb" />
</geometry>
+ <map name="${side}_upper_arm_collision_mesh" flag="ogre">
+ <elem key="mesh" value="convex/upper_arm_convex.mesh" />
+ </map>
</collision>
<map name="${side}_upper_arm_sensor" flag="gazebo">
@@ -329,14 +350,20 @@
<elem key="material" value="PR2/Grey" />
</map>
<geometry name="${side}_elbow_flex_visual">
- <mesh filename="elbow_flex" />
+ <mesh filename="elbow_flex.stlb" />
</geometry>
+ <map name="${side}_elbow_flex_visual_mesh" flag="ogre">
+ <elem key="mesh" value="elbow_flex.mesh" />
+ </map>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_elbow_flex_collision">
<mesh filename="convex/elbow_flex_convex.stlb" />
</geometry>
+ <map name="${side}_elbow_flex_collision_mesh" flag="ogre">
+ <elem key="mesh" value="convex/elbow_flex_convex.mesh" />
+ </map>
</collision>
<map name="${side}_elbow_flex_sensor" flag="gazebo">
<verbatim key="${side}_elbow_flex_bumper_sensor">
@@ -439,8 +466,11 @@
<elem key="material" value="PR2/RollLinks" />
</map>
<geometry name="${side}_forearm_roll_visual">
- <mesh filename="forearm_roll" />
+ <mesh filename="forearm_roll.stlb" />
</geometry>
+ <map name="${side}_forearm_roll_visual_mesh" flag="ogre">
+ <elem key="mesh" value="forearm_roll.mesh" />
+ </map>
</visual>
<collision> <!-- TODO: collision tag should be optional -->
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -484,14 +514,20 @@
<elem key="material" value="PR2/Blue" />
</map>
<geometry name="${side}_forearm_visual">
- <mesh filename="forearm" />
+ <mesh filename="forearm.stlb" />
</geometry>
+ <map name="${side}_forearm_visual_mesh" flag="ogre">
+ <elem key="mesh" value="forearm.mesh" />
+ </map>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_forearm_collision">
<mesh filename="convex/forearm_convex.stlb" />
</geometry>
+ <map name="${side}_forearm_collision_mesh" flag="ogre">
+ <elem key="mesh" value="convex/forearm_convex.mesh" />
+ </map>
</collision>
<map name="${side}_forearm_sensor" flag="gazebo">
<verbatim key="${side}_forearm_bumper_sensor">
@@ -547,14 +583,20 @@
<elem key="material" value="PR2/Grey" />
</map>
<geometry name="${side}_wrist_flex_visual">
- <mesh filename="wrist_flex" />
+ <mesh filename="wrist_flex.stlb" />
</geometry>
+ <map name="${side}_wrist_flex_visual_mesh" flag="ogre">
+ <elem key="mesh" value="wrist_flex.mesh" />
+ </map>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="${side}_wrist_flex_collision">
<mesh filename="convex/wrist_flex_convex.stlb" />
</geometry>
+ <map name="${side}_wrist_flex_collision_mesh" flag="ogre">
+ <elem key="mesh" value="convex/wrist_flex_convex.mesh" />
+ </map>
</collision>
<map name="${side}_wrist_flex_sensor" flag="gazebo">
<verbatim key="${side}_wrist_flex_bumper_sensor">
@@ -607,8 +649,11 @@
<elem key="material" value="PR2/RollLinks" />
</map>
<geometry name="${side}_wrist_roll_visual">
- <mesh filename="wrist_roll" />
+ <mesh filename="wrist_roll.stlb" />
</geometry>
+ <map name="${side}_wrist_roll_visual_mesh" flag="ogre">
+ <elem key="mesh" value="wrist_roll.mesh" />
+ </map>
</visual>
<collision>
<origin xyz="0.0 0 0" rpy="0 0 0" />
Deleted: pkg/trunk/stacks/pr2/pr2_defs/defs/arm_grav_defs.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/defs/arm_grav_defs.xml 2009-07-15 21:42:11 UTC (rev 18891)
+++ pkg/trunk/stacks/pr2/pr2_defs/defs/arm_grav_defs.xml 2009-07-15 21:59:38 UTC (rev 18892)
@@ -1,630 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
- xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
- xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
-
- <property name="M_PI" value="3.1415926535897931" />
- <property name="VELOCITY_LIMIT_SCALE" value="0.6" />
-
- <property name="shoulder_lift_length" value="0.10" />
- <property name="shoulder_lift_radius" value="0.12" />
-
-
-
-
-
-
-
-
- <macro name="pr2_upper_arm" params="side parent reflect *origin">
-
- <!-- Shoulder pan -->
-
- <joint name="${side}_shoulder_pan_joint" type="revolute">
- <axis xyz="0 0 1" />
-
- <limit min="${reflect*M_PI/4-1.5}" max="${reflect*M_PI/4+1.5}"
- effort="30" velocity="${VELOCITY_LIMIT_SCALE*3.48}"
- k_position="100" k_velocity="10"
- safety_length_min="0.15" safety_length_max="0.15" />
- <calibration reference_position="${reflect*M_PI/4}" values="1.5 -1" />
-
- <joint_properties damping="10.0" />
- </joint>
-
- <link name="${side}_shoulder_pan_link">
- <parent name="${parent}_link" />
- <insert_block name="origin" />
- <joint name="${side}_shoulder_pan_joint" />
- <inertial>
- <mass value="16.29553" />
- <com xyz="-0.005215 -0.030552 -0.175743" />
- <inertia ixx="0.793291393007" ixy="0.003412032973" ixz="0.0096614481"
- iyy="0.818419457224" iyz="-0.033999499551"
- izz="0.13915007406" />
- </inertial>
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <map name="gazebo_material" flag="gazebo">
- <elem key="material">PR2/Blue</elem>
- </map>
- <geometry name="${side}_shoulder_pan_visual">
- <mesh filename="shoulder_yaw" />
- </geometry>
- </visual>
- <collision>
- <origin xyz="0.05 0 -0.2" rpy="0 0 0 " />
- <geometry name="${side}_shoulder_pan_collision">
- <box size="0.347 0.254 0.646" />
- </geometry>
- </collision>
- <map name="${side}_shoulder_pan_sensor" flag="gazebo">
- <verbatim key="${side}_shoulder_pan_bumper_sensor">
- <sensor:contact name="${side}_shoulder_pan_contact_sensor">
- <geom>${side}_shoulder_pan_collision</geom>
- <updateRate>100.0</updateRate>
- <controller:ros_bumper name="${side}_shoulder_pan_ros_bumper_controller" plugin="libros_bumper.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <bumperTopicName>${side}_shoulder_pan_bumper</bumperTopicName>
- <interface:bumper name="${side}_shoulder_pan_ros_bumper_iface" />
- </controller:ros_bumper>
- </sensor:contact>
- </verbatim>
- </map>
- <map name="${side}_shoulder_pan_gravity" flag="gazebo">
- <elem key="turnGravityOff">false</elem>
- </map>
- </link>
-
- <transmission type="SimpleTransmission" name="${side}_shoulder_pan_trans">
- <actuator name="${side}_shoulder_pan_motor" />
- <joint name="${side}_shoulder_pan_joint" />
- <mechanicalReduction>63.16</mechanicalReduction>
- </transmission>
-
- <!-- Shoulder lift -->
-
- <joint name="${side}_shoulder_lift_joint" type="revolute">
- <axis xyz="0 1 0" />
- <anchor xyz="0 0 0" />
-
- <limit min="-0.4" max="1.5" effort="30" velocity="${VELOCITY_LIMIT_SCALE*3.47}"
- k_position="100" k_velocity="10"
- safety_length_min="0.1" safety_length_max="0.1" />
- <calibration reference_position="0" values="1.5 -1 " />
-
- <joint_properties damping="100.0" />
- </joint>
-
- <link name="${side}_shoulder_lift_link">
- <parent name="${side}_shoulder_pan_link" />
- <origin xyz="0.1 0 0" rpy="0 0 0" />
- <joint name="${side}_shoulder_lift_joint" />
- <inertial>
- <mass value="2.41223" />
- <com xyz="-0.035895 0.014422 -0.028263" />
- <inertia ixx="0.016640333585" ixy="0.002696462583" ixz="0.001337742275"
- iyy="0.017232603914" iyz="-0.003605106514" izz="0.018723553425" />
- </inertial>
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <map name="gazebo_material" flag="gazebo">
- <elem key="material">PR2/Grey</elem>
- </map>
- <geometry name="${side}_shoulder_lift_visual">
- <mesh filename="shoulder_lift" />
- </geometry>
- </visual>
- <collision>
- <origin xyz="0 0 0 " rpy="${M_PI/2} 0 0" />
- <geometry name="${side}_shoulder_lift_collision">
- <cylinder radius="${shoulder_lift_radius}" length="${shoulder_lift_length}" />
- </geometry>
- </collision>
- <map name="${side}_shoulder_lift_sensor" flag="gazebo">
- <verbatim key="${side}_shoulder_lift_bumper_sensor">
- <sensor:contact name="${side}_shoulder_lift_contact_sensor">
- <geom>${side}_shoulder_lift_collision</geom>
- <updateRate>100.0</updateRate>
- <controller:ros_bumper name="${side}_shoulder_lift_ros_bumper_controller" plugin="libros_bumper.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <bumperTopicName>${side}_shoulder_lift_bumper</bumperTopicName>
- <interface:bumper name="${side}_shoulder_lift_ros_bumper_iface" />
- </controller:ros_bumper>
- </sensor:contact>
- </verbatim>
- </map>
- <map name="${side}_shoulder_lift_gravity" flag="gazebo">
- <elem key="turnGravityOff">false</elem>
- </map>
- </link>
-
- <transmission type="SimpleTransmission" name="${side}_shoulder_lift_trans">
- <actuator name="${side}_shoulder_lift_motor" />
- <joint name="${side}_shoulder_lift_joint" />
- <mechanicalReduction>57.36</mechanicalReduction>
- </transmission>
-
- <!-- Upperarm roll -->
-
- <joint name="${side}_upper_arm_roll_joint" type="revolute">
- <axis xyz="1 0 0" />
- <anchor xyz="0 0 0" />
-
- <limit min="-3.9" max="0.8" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.45}"
- k_position="100" k_velocity="2"
- safety_length_min="0.15" safety_length_max="0.15" />
- <calibration reference_position="${-M_PI/2}" values="1.5 -1 " />
-
- <joint_properties damping="1.0" />
- </joint>
-
- <link name="${side}_upper_arm_roll_link">
- <parent name="${side}_shoulder_lift_link" />
- <origin xyz="0 0 0" rpy="0 0 0" />
- <joint name="${side}_upper_arm_roll_joint" />
-
- <inertial>
- <mass value="4.9481" />
- <com xyz=" 0.21227 0.001205 -0.016293 " /> <!-- x becomes y, y becomes z, z becomes x, not sure about the signs -->
- <inertia ixx="0.01338328491" ixy="0.00011947689" ixz="0.00154493231"
- iyy="0.07306071531" iyz="0.00054774592"
- izz="0.07212451075" /> <!-- x becomes y, y becomes z, z becomes x, not sure about the signs -->
- </inertial>
-
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <map name="gazebo_material" flag="gazebo">
- <elem key="material">PR2/Green</elem>
- </map>
- <geometry name="${side}_upper_arm_roll_visual">
- <mesh filename="upper_arm" />
- </geometry>
- </visual>
-
- <collision>
- <origin xyz="0.3 0 0" rpy="0 0 0" />
- <geometry name="${side}_upper_arm_roll_collision">
- <box size="0.362 0.144 0.157" />
- </geometry>
- </collision>
- <map name="${side}_upper_arm_roll_sensor" flag="gazebo">
- <verbatim key="${side}_upper_arm_roll_bumper_sensor">
- <sensor:contact name="${side}_upper_arm_roll_contact_sensor">
- <geom>${side}_upper_arm_roll_collision</geom>
- <updateRate>100.0</updateRate>
- <controller:ros_bumper name="${side}_upper_arm_roll_ros_bumper_controller" plugin="libros_bumper.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <bumperTopicName>${side}_upper_arm_roll_bumper</bumperTopicName>
- <interface:bumper name="${side}_upper_arm_roll_ros_bumper_iface" />
- </controller:ros_bumper>
- </sensor:contact>
- </verbatim>
- </map>
-
- <map name="${side}_upper_arm_roll_gravity" flag="gazebo">
- <elem key="turnGravityOff">false</elem>
- </map>
- </link>
-
- <transmission type="SimpleTransmission" name="${side}_upper_arm_roll_trans">
- <actuator name="${side}_upper_arm_roll_motor" />
- <joint name="${side}_upper_arm_roll_joint" />
- <mechanicalReduction>32.65</mechanicalReduction>
- </transmission>
-
- <!-- Elbow flex -->
-
- <joint name="${side}_elbow_flex_joint" type="revolute">
- <axis xyz="0 -1 0" />
- <anchor xyz="0 0 0" />
-
- <limit min="-0.1" max="2.3" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.5}"
- k_position="100" k_velocity="3"
- safety_length_min="0.1" safety_length_max="0.4" />
- <calibration reference_position="1.1" values="1.5 -1" />
-
- <joint_properties damping="10.0" />
- </joint>
-
- <link name="${side}_elbow_flex_link">
- <parent name="${side}_upper_arm_roll_link" />
- <origin xyz="0.4 0 0" rpy="0 0 0" />
- <joint name="${side}_elbow_flex_joint" />
-
- <inertial>
- <mass value="1.689246" />
- <com xyz="0.013173 0.001228 -0.011638" /> <!-- switching y to x, z to y, x to z -->
- <inertia ixx="0.00323681521" ixy="-0.00000116216" ixz="0.00029204667"
- iyy="0.00410053774" iyz="-0.00007735928"
- izz="0.00327529855" /> <!-- switching y to x, z to y, x to z -->
- </inertial>
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <map name="gazebo_material" flag="gazebo">
- <elem key="material">PR2/Grey</elem>
- </map>
- <geometry name="${side}_elbow_flex_visual">
- <mesh filename="elbow_flex" />
- </geometry>
- </visual>
- <collision>
- <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
- <geometry name="${side}_elbow_flex_collision">
- <cylinder radius="0.1" length="0.08" />
- </geometry>
- </collision>
- <map name="${side}_elbow_flex_sensor" flag="gazebo">
- <verbatim key="${side}_elbow_flex_bumper_sensor">
- <sensor:contact name="${side}_elbow_flex_contact_sensor">
- <geom>${side}_elbow_flex_collision</geom>
- <updateRate>100.0</updateRate>
- <controller:ros_bumper name="${side}_elbow_flex_ros_bumper_controller" plugin="libros_bumper.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <bumperTopicName>${side}_elbow_flex_bumper</bumperTopicName>
- <interface:bumper name="${side}_elbow_flex_ros_bumper_iface" />
- </controller:ros_bumper>
- </sensor:contact>
- </verbatim>
- </map>
- <map name="${side}_elbow_flex_gravity" flag="gazebo">
- <elem key="turnGravityOff">false</elem>
- </map>
- </link>
-
- <transmission type="SimpleTransmission" name="${side}_elbow_flex_trans">
- <actuator name="${side}_elbow_flex_motor" />
- <joint name="${side}_elbow_flex_joint" />
- <mechanicalReduction>36.17</mechanicalReduction>
- </transmission>
-
- <map name="sensor" flag="gazebo">
- <verbatim key="p3d_${side}_upper_arm">
- <!--
- <controller:ros_p3d name="p3d_${side}_shoulder_pan_controller" plugin="libros_p3d.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <bodyName>${side}_shoulder_pan_link</bodyName>
- <topicName>${side}_shoulder_pan_pose_ground_truth</topicName>
- <gaussianNoise>0.0</gaussianNoise>
- <frameName>map</frameName>
- <interface:position name="p3d_${side}_shoulder_pan_position_iface" />
- </controller:ros_p3d>
- <controller:ros_p3d name="p3d_${side}_shoulder_lift_controller" plugin="libros_p3d.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <bodyName>${side}_shoulder_lift_link</bodyName>
- <topicName>${side}_shoulder_lift_pose_ground_truth</topicName>
- <gaussianNoise>0.0</gaussianNoise>
- <frameName>map</frameName>
- <interface:position name="p3d_${side}_shoulder_lift_position_iface" />
- </controller:ros_p3d>
- <controller:ros_p3d name="p3d_${side}_upper_arm_roll_controller" plugin="libros_p3d.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <bodyName>${side}_upper_arm_roll_link</bodyName>
- <topicName>${side}_upper_arm_roll_pose_ground_truth</topicName>
- <gaussianNoise>0.0</gaussianNoise>
- <frameName>map</frameName>
- <interface:position name="p3d_${side}_upper_arm_roll_position_iface" />
- </controller:ros_p3d>
- <controller:ros_p3d name="p3d_${side}_elbow_flex_controller" plugin="libros_p3d.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <bodyName>${side}_elbow_flex_link</bodyName>
- <topicName>${side}_elbow_flex_pose_ground_truth</topicName>
- <gaussianNoise>0.0</gaussianNoise>
- <frameName>map</frameName>
- <interface:position name="p3d_${side}_elbow_flex_position_iface" />
- </controller:ros_p3d>
- -->
- </verbatim>
- </map>
-
-
- </macro>
-
-
-
- <macro name="pr2_forearm" params="side parent reflect">
-
- <!-- Forearm roll -->
-
- <joint name="${side}_forearm_roll_joint" type="revolute">
- <axis xyz="-1 0 0" />
- <anchor xyz="0 0 0" />
-
- <limit effort="30" velocity="${VELOCITY_LIMIT_SCALE*6}" k_velocity="1" />
- <calibration reference_position="0" values="1.5 -1" />
-
- <joint_properties damping="1.0" />
- </joint>
-
- <link name="${side}_forearm_roll_link">
- <parent name="${parent}_link" />
- <origin xyz="0 0 0" rpy="0 0 0" />
- <joint name="${side}_forearm_roll_joint" />
- <inertial>
- <mass value="1.804155" />
- <com xyz="0.179474 -0.000058 0.013779" /> <!-- z to x, x to y, y to z -->
- <inertia ixx="0.00175508987" ixy="0.00002937939" ixz="-0.00042767904"
- iyy="0.01243055254" iyz="-0.00000367110"
- izz="0.01356754885" /> <!-- z to x, x to y, y to z -->
- </inertial>
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <map name="gazebo_material" flag="gazebo">
- <elem key="material">PR2/Blue</elem>
- </map>
- <geometry name="${side}_forearm_roll_visual">
- <mesh filename="forearm" />
- </geometry>
- </visual>
- <collision>
- <origin xyz="0.22 0 0" rpy="0 0 0" />
- <geometry name="${side}_forearm_roll_collision">
- <box size="0.27 0.12 0.08" />
- </geometry>
- </collision>
- <map name="${side}_forearm_roll_sensor" flag="gazebo">
- <verbatim key="${side}_forearm_roll_bumper_sensor">
- <sensor:contact name="${side}_forearm_roll_contact_sensor">
- <geom>${side}_forearm_roll_collision</geom>
- <updateRate>100.0</updateRate>
- <controller:ros_bumper name="${side}_forearm_roll_ros_bumper_controller" plugin="libros_bumper.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <bumperTopicName>${side}_forearm_roll_bumper</bumperTopicName>
- <interface:bumper name="${side}_forearm_roll_ros_bumper_iface" />
- </controller:ros_bumper>
- </sensor:contact>
- </verbatim>
- </map>
- <map name="${side}_forearm_roll_gravity" flag="gazebo">
- <elem key="turnGravityOff">false</elem>
- </map>
- </link>
-
- <transmission type="SimpleTransmission" name="${side}_forearm_roll_trans">
- <actuator name="${side}_forearm_roll_motor" />
- <joint name="${side}_forearm_roll_joint" />
- <mechanicalReduction>${576/25*55/14}</mechanicalReduction>
- </transmission>
-
- <!-- Wrist flex -->
-
- <joint name="${side}_wrist_flex_joint" type="revolute">
- <axis xyz="0 -1 0" />
- <anchor xyz="0 0 0" />
-
- <limit min="-0.1" max="2.2" effort="200" velocity="${VELOCITY_LIMIT_SCALE*5.13}"
- k_position="20" k_velocity="4"
- safety_length_min="0.2" safety_length_max="0.2" />
- <calibration reference_position="0.4" values="1.5 -1" />
-
- <joint_properties damping="1.0" />
- </joint>
-
- <link name="${side}_wrist_flex_link">
- <parent name="${side}_forearm_roll_link" />
- <origin xyz="0.32025 0 0" rpy="0 0 0" />
- <joint name="${side}_wrist_flex_joint" />
- <inertial>
- <mass value="0.80305" />
- <com xyz="0.012193 0.000017 -0.004929" /> <!-- my best guess z to z, y to x, x to y -->
- <inertia ixx="0.00077829551" ixy="0.00018491414" ixz="-0.00000036897"
- iyy="0.00108384500" iyz="0.00000017027"
- izz="0.00071460255" /> <!-- my best guess z to z, y to x, x to y -->
- </inertial>
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <map name="gazebo...
[truncated message content] |