Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[2930] pkg/trunk (Page 5)

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

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.




Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[2930] pkg/trunk (Page 5)

personalrobots-commit