Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2930] pkg/trunk
|
From: <is...@us...> - 2008-08-11 22:46:25
|
Revision: 2930
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2930&view=rev
Author: isucan
Date: 2008-08-11 22:46:32 +0000 (Mon, 11 Aug 2008)
Log Message:
-----------
base class for nodes that are aware of a robot model
Modified Paths:
--------------
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/planning_models/include/planning_models/node.h
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-11 22:30:19 UTC (rev 2929)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-11 22:46:32 UTC (rev 2930)
@@ -74,7 +74,7 @@
req.goal_state.vals[i] = m_basePos[i];
req.start_state.vals[i] = m_basePos[i];
}
- req.goal_state.vals[0] += 4.5;
+ req.goal_state.vals[0] += 2.5;
req.allowed_time = 5.0;
@@ -160,8 +160,8 @@
while (!plan.haveBasePos())
dur.sleep();
- // plan.runTestLeftArm();
- plan.runTestBase();
+ plan.runTestLeftArm();
+ // plan.runTestBase();
plan.shutdown();
Added: pkg/trunk/motion_planning/planning_models/include/planning_models/node.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/node.h (rev 0)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/node.h 2008-08-11 22:46:32 UTC (rev 2930)
@@ -0,0 +1,166 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <ros/node.h>
+#include <ros/time.h>
+#include <urdf/URDF.h>
+#include <planning_models/kinematic.h>
+#include <std_msgs/RobotBase2DOdom.h>
+#include <rosTF/rosTF.h>
+
+namespace planning_models
+{
+
+ class NodeWithRobotModel : public ros::node
+ {
+
+ public:
+
+ NodeWithRobotModel(const std::string &robot_model, const std::string &name, uint32_t options = 0) : ros::node(name, options),
+ m_tf(*this, true, 1 * 1000000000ULL, 1000000000ULL)
+ {
+ m_urdf = NULL;
+ m_kmodel = NULL;
+ m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
+ loadRobotDescription(robot_model);
+
+ subscribe("localizedpose", m_localizedPose, &NodeWithRobotModel::localizedPoseCallback);
+ }
+
+ virtual ~NodeWithRobotModel(void)
+ {
+ if (m_urdf)
+ delete m_urdf;
+ if (m_kmodel)
+ delete m_kmodel;
+ }
+
+ void setRobotDescriptionFromData(const char *data)
+ {
+ robot_desc::URDF *file = new robot_desc::URDF();
+ if (file->loadString(data))
+ setRobotDescription(file);
+ else
+ delete file;
+ }
+
+ void setRobotDescriptionFromFile(const char *filename)
+ {
+ robot_desc::URDF *file = new robot_desc::URDF();
+ if (file->loadFile(filename))
+ setRobotDescription(file);
+ else
+ delete file;
+ }
+
+ virtual void setRobotDescription(robot_desc::URDF *file)
+ {
+ if (m_urdf)
+ delete m_urdf;
+ if (m_kmodel)
+ delete m_kmodel;
+
+ m_urdf = file;
+ m_kmodel = new planning_models::KinematicModel();
+ m_kmodel->setVerbose(false);
+ m_kmodel->build(*file);
+ }
+
+ virtual void loadRobotDescription(const std::string &robot_model)
+ {
+ if (!robot_model.empty() && robot_model != "-")
+ {
+ std::string content;
+ if (get_param(robot_model, content))
+ setRobotDescriptionFromData(content.c_str());
+ else
+ fprintf(stderr, "Robot model '%s' not found!\n", robot_model.c_str());
+ }
+ }
+
+ protected:
+
+ void localizedPoseCallback(void)
+ {
+ bool success = true;
+ libTF::TFPose2D pose;
+ pose.x = m_localizedPose.pos.x;
+ pose.y = m_localizedPose.pos.y;
+ pose.yaw = m_localizedPose.pos.th;
+ pose.time = m_localizedPose.header.stamp.to_ull();
+ pose.frame = m_localizedPose.header.frame_id;
+
+ try
+ {
+ pose = m_tf.transformPose2D("FRAMEID_MAP", pose);
+ }
+ catch(libTF::TransformReference::LookupException& ex)
+ {
+ fprintf(stderr, "Discarding pose: Transform reference lookup exception\n");
+ success = false;
+ }
+ catch(libTF::TransformReference::ExtrapolateException& ex)
+ {
+ fprintf(stderr, "Discarding pose: Extrapolation exception: %s\n", ex.what());
+ success = false;
+ }
+ catch(...)
+ {
+ fprintf(stderr, "Discarding pose: Exception in pose computation\n");
+ success = false;
+ }
+
+ if (success)
+ {
+ m_basePos[0] = pose.x;
+ m_basePos[1] = pose.y;
+ m_basePos[2] = pose.yaw;
+
+ baseUpdate();
+ }
+ }
+
+ virtual void baseUpdate(void)
+ {
+ }
+
+ rosTFClient m_tf;
+ std_msgs::RobotBase2DOdom m_localizedPose;
+ robot_desc::URDF *m_urdf;
+ planning_models::KinematicModel *m_kmodel;
+ double m_basePos[3];
+
+ };
+
+}
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-11 22:30:19 UTC (rev 2929)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-11 22:46:32 UTC (rev 2930)
@@ -96,30 +96,26 @@
- @b "world_3d_map/verbosity_level" : @b [int] sets the verbosity level (default 1)
**/
-#include <ros/node.h>
-#include <ros/time.h>
+#include <planning_models/node.h>
+
#include <rosthread/member_thread.h>
#include <rosthread/mutex.h>
+
#include <std_msgs/PointCloudFloat32.h>
-#include <std_msgs/RobotBase2DOdom.h>
#include <std_msgs/LaserScan.h>
#include <rostools/Log.h>
-#include <rosTF/rosTF.h>
-#include <urdf/URDF.h>
-#include <planning_models/kinematic.h>
#include <collision_space/util.h>
#include <random_utils/random_utils.h>
#include <deque>
#include <cmath>
-class World3DMap : public ros::node
+class World3DMap : public planning_models::NodeWithRobotModel
{
public:
- World3DMap(const char *robot_model) : ros::node("world_3d_map"),
- m_tf(*this, true, 1 * 1000000000ULL, 1000000000ULL)
+ World3DMap(const std::string &robot_model) : planning_models::NodeWithRobotModel(robot_model, "world_3d_map")
{
advertise<std_msgs::PointCloudFloat32>("world_3d_map");
advertise<rostools::Log>("roserr");
@@ -129,15 +125,12 @@
param("retain_pointcloud_fraction", m_retainPointcloudFraction, 0.02);
param("verbosity_level", m_verbose, 1);
- loadRobotDescription(robot_model);
-
/* create a thread that does the processing of the input data.
* and one that handles the publishing of the data */
m_active = true;
m_working = false;
m_shouldPublish = false;
random_utils::init(&m_rng);
- m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
m_processMutex.lock();
m_publishMutex.lock();
@@ -145,7 +138,6 @@
m_publishingThread = ros::thread::member_thread::startMemberFunctionThread<World3DMap>(this, &World3DMap::publishDataThread);
subscribe("scan", m_inputScan, &World3DMap::pointCloudCallback);
- subscribe("localizedpose", m_localizedPose, &World3DMap::localizedPoseCallback);
}
~World3DMap(void)
@@ -161,13 +153,14 @@
for (unsigned int i = 0 ; i < m_selfSeeParts.size() ; ++i)
delete m_selfSeeParts[i].body;
-
- if (m_kmodel)
- delete m_kmodel;
- if (m_urdf)
- delete m_urdf;
}
+ virtual void setRobotDescription(robot_desc::URDF *file)
+ {
+ planning_models::NodeWithRobotModel::setRobotDescription(file);
+ addSelfSeeBodies();
+ }
+
private:
struct RobotPart
@@ -176,91 +169,14 @@
planning_models::KinematicModel::Link *link;
};
- void setRobotDescriptionFromData(const char *data)
+ void baseUpdate(void)
{
- robot_desc::URDF *file = new robot_desc::URDF();
- if (file->loadString(data))
- setRobotDescription(file);
- else
- delete file;
+ planning_models::NodeWithRobotModel::baseUpdate();
+ int group = m_kmodel->getGroupID(m_urdf->getRobotName() + "::base");
+ if (group >= 0)
+ m_kmodel->computeTransforms(m_basePos, group);
}
-
- void setRobotDescription(robot_desc::URDF *file)
- {
- m_urdf = file;
- m_kmodel = new planning_models::KinematicModel();
- m_kmodel->setVerbose(false);
- m_kmodel->build(*file);
-
- addSelfSeeBodies();
- }
-
- void loadRobotDescription(const char *robot_model)
- {
- if (!robot_model || strcmp(robot_model, "-") == 0)
- {
- if (m_verbose)
- printf("No robot model will be used\n");
- }
- else
- {
- std::string content;
- if (m_verbose)
- printf("Attempting to load model '%s'\n", robot_model);
- if (get_param(robot_model, content))
- {
- setRobotDescriptionFromData(content.c_str());
- if (m_verbose)
- printf("Success!\n");
- }
- else
- fprintf(stderr, "Robot model '%s' not found!\n", robot_model);
- }
- }
- void localizedPoseCallback(void)
- {
- bool success = true;
- libTF::TFPose2D pose;
- pose.x = m_localizedPose.pos.x;
- pose.y = m_localizedPose.pos.y;
- pose.yaw = m_localizedPose.pos.th;
- pose.time = m_localizedPose.header.stamp.to_ull();
- pose.frame = m_localizedPose.header.frame_id;
-
- try
- {
- pose = m_tf.transformPose2D("FRAMEID_MAP", pose);
- }
- catch(libTF::TransformReference::LookupException& ex)
- {
- fprintf(stderr, "Discarding pose: Transform reference lookup exception\n");
- success = false;
- }
- catch(libTF::TransformReference::ExtrapolateException& ex)
- {
- fprintf(stderr, "Discarding pose: Extrapolation exception: %s\n", ex.what());
- success = false;
- }
- catch(...)
- {
- fprintf(stderr, "Discarding pose: Exception in pose computation\n");
- success = false;
- }
-
- if (success)
- {
- m_basePos[0] = pose.x;
- m_basePos[1] = pose.y;
- m_basePos[2] = pose.yaw;
-
- int group = m_kmodel->getGroupID(m_urdf->getRobotName() + "::base");
- if (group >= 0)
- m_kmodel->computeTransforms(m_basePos, group);
- }
-
- }
-
void pointCloudCallback(void)
{
/* The idea is that if processing of previous input data is
@@ -530,17 +446,9 @@
return copy;
}
- rosTFClient m_tf;
-
- robot_desc::URDF *m_urdf;
- planning_models::KinematicModel *m_kmodel;
-
std::vector<RobotPart> m_selfSeeParts;
- double m_basePos[3];
-
std::deque<std_msgs::PointCloudFloat32*> m_currentWorld;// Pointers to saved clouds
-
double m_maxPublishFrequency;
double m_retainPointcloudDuration;
@@ -549,9 +457,7 @@
std_msgs::LaserScan m_inputScan; //Buffer for recieving cloud
std_msgs::PointCloudFloat32 m_toProcess; //Buffer (size 1) for incoming cloud
- std_msgs::RobotBase2DOdom m_localizedPose;
-
-
+
pthread_t *m_processingThread;
pthread_t *m_publishingThread;
ros::thread::mutex m_processMutex, m_publishMutex, m_worldDataMutex, m_flagMutex;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
