|
From: <jl...@us...> - 2008-11-11 07:09:54
|
Revision: 6514
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6514&view=rev
Author: jleibs
Date: 2008-11-11 07:09:49 +0000 (Tue, 11 Nov 2008)
Log Message:
-----------
Fixed timestamps in dcam_node and added example of subscribing to multiple nodes in synchronized fashion.
Modified Paths:
--------------
pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp
pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
Added Paths:
-----------
pkg/trunk/vision/stereo_view/
pkg/trunk/vision/stereo_view/CMakeLists.txt
pkg/trunk/vision/stereo_view/Makefile
pkg/trunk/vision/stereo_view/manifest.xml
pkg/trunk/vision/stereo_view/stereo_view.cpp
Modified: pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp 2008-11-11 06:09:22 UTC (rev 6513)
+++ pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp 2008-11-11 07:09:49 UTC (rev 6514)
@@ -251,6 +251,8 @@
stcam->stIm->imHeight, stcam->stIm->imWidth, 1,
"mono", "uint16",
stcam->stIm->imDisp );
+
+ img_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
publish("~disparity", img_);
stereo_info_.has_disparity = true;
@@ -258,7 +260,7 @@
stereo_info_.has_disparity = false;
}
- stereo_info_.header.stamp = ros::Time(stcam->stIm->imLeft->im_time * 1000);
+ stereo_info_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
stereo_info_.height = stcam->stIm->imHeight;
stereo_info_.width = stcam->stIm->imWidth;
@@ -288,16 +290,14 @@
void publishImages(std::string base_name, cam::ImageData* img_data)
{
-
- img_.header.stamp = ros::Time(img_data->im_time * 1000);
- cam_info_.header.stamp = ros::Time(img_data->im_time * 1000);
-
if (img_data->imRawType != COLOR_CODING_NONE)
{
fillImage(img_, "image_raw",
img_data->imHeight, img_data->imWidth, 1,
"mono", "byte",
img_data->imRaw );
+
+ img_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image_raw"), img_);
cam_info_.has_image = true;
} else {
@@ -310,6 +310,7 @@
img_data->imHeight, img_data->imWidth, 1,
"mono", "byte",
img_data->im );
+ img_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image"), img_);
cam_info_.has_image = true;
} else {
@@ -322,6 +323,8 @@
img_data->imHeight, img_data->imWidth, 4,
"rgba", "byte",
img_data->imColor );
+
+ img_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image_color"), img_);
cam_info_.has_image_color = true;
} else {
@@ -334,6 +337,7 @@
img_data->imHeight, img_data->imWidth, 1,
"mono", "byte",
img_data->imRect );
+ img_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image_rect"), img_);
cam_info_.has_image_rect = true;
} else {
@@ -346,12 +350,14 @@
img_data->imHeight, img_data->imWidth, 4,
"rgba", "byte",
img_data->imRectColor );
+ img_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
publish(base_name + std::string("image_rect_color"), img_);
cam_info_.has_image_rect_color = true;
} else {
cam_info_.has_image_rect_color = false;
}
+ cam_info_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
cam_info_.height = img_data->imHeight;
cam_info_.width = img_data->imWidth;
Modified: pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
===================================================================
--- pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-11 06:09:22 UTC (rev 6513)
+++ pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-11 07:09:49 UTC (rev 6514)
@@ -75,7 +75,7 @@
cvInitImageHeader(img_, cvSize(rosimg.byte_data.layout.dim[1].size, rosimg.byte_data.layout.dim[0].size), IPL_DEPTH_8U, 1);
cvSetData(img_, &(rosimg.byte_data.data[0]), rosimg.byte_data.layout.dim[1].stride);
} else if (rosimg.depth == "uint16") {
- cvInitImageHeader(img_, cvSize(rosimg.uint16_data.layout.dim[1].size, rosimg.uint16_data.layout.dim[0].size), IPL_DEPTH_8U, 1);
+ cvInitImageHeader(img_, cvSize(rosimg.uint16_data.layout.dim[1].size, rosimg.uint16_data.layout.dim[0].size), IPL_DEPTH_16U, 1);
cvSetData(img_, &(rosimg.uint16_data.data[0]), rosimg.uint16_data.layout.dim[1].stride*sizeof(uint16_t));
}
Added: pkg/trunk/vision/stereo_view/CMakeLists.txt
===================================================================
--- pkg/trunk/vision/stereo_view/CMakeLists.txt (rev 0)
+++ pkg/trunk/vision/stereo_view/CMakeLists.txt 2008-11-11 07:09:49 UTC (rev 6514)
@@ -0,0 +1,6 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+
+rospack(stereo_view)
+
+rospack_add_executable(stereo_view stereo_view.cpp)
Added: pkg/trunk/vision/stereo_view/Makefile
===================================================================
--- pkg/trunk/vision/stereo_view/Makefile (rev 0)
+++ pkg/trunk/vision/stereo_view/Makefile 2008-11-11 07:09:49 UTC (rev 6514)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/vision/stereo_view/manifest.xml
===================================================================
--- pkg/trunk/vision/stereo_view/manifest.xml (rev 0)
+++ pkg/trunk/vision/stereo_view/manifest.xml 2008-11-11 07:09:49 UTC (rev 6514)
@@ -0,0 +1,13 @@
+<package>
+ <description>
+ An opencv based viewer for dcam stereo images
+ </description>
+ <author>Jeremy Leibs</author>
+ <license>BSD</license>
+ <depend package="opencv_latest"/>
+ <depend package="std_msgs"/>
+ <depend package="std_srvs"/>
+ <depend package="image_msgs"/>
+ <depend package="roscpp"/>
+ <depend package="rosthread"/>
+</package>
Added: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp (rev 0)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-11 07:09:49 UTC (rev 6514)
@@ -0,0 +1,159 @@
+#include <vector>
+
+#include "opencv/cxcore.h"
+#include "opencv/cv.h"
+#include "opencv/highgui.h"
+
+#include "ros/node.h"
+#include "image_msgs/StereoInfo.h"
+#include "image_msgs/Image.h"
+#include "image_msgs/CvBridge.h"
+
+#include "rosthread/condition.h"
+
+using namespace std;
+
+class StereoView : public ros::node
+{
+public:
+
+ image_msgs::Image limage;
+ image_msgs::Image rimage;
+ image_msgs::Image dimage;
+
+ image_msgs::CvBridge lbridge;
+ image_msgs::CvBridge rbridge;
+ image_msgs::CvBridge dbridge;
+
+ std::string ltopic;
+ std::string rtopic;
+ std::string dtopic;
+
+ ros::Time image_time;
+
+ ros::thread::condition cond_all;
+
+ int count;
+ bool done;
+
+ StereoView() : ros::node("cv_view"), count(0), done(false)
+ {
+ ltopic = map_name("dcam") + std::string("/left/image_rect");
+ rtopic = map_name("dcam") + std::string("/right/image_rect");
+ dtopic = map_name("dcam") + std::string("/disparity");
+
+ cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
+ subscribe(ltopic, limage, &StereoView::image_cb, &limage, 1);
+
+ cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
+ subscribe(rtopic, rimage, &StereoView::image_cb, &rimage, 1);
+
+ cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
+ subscribe(dtopic, dimage, &StereoView::image_cb, &dimage, 1);
+ }
+
+ void image_cb(void* p)
+ {
+ image_msgs::Image* img = (image_msgs::Image*)(p);
+
+ cond_all.lock();
+
+ // If first to time, wait
+ if (count == 0)
+ {
+ wait_for_others(img);
+ return;
+ }
+
+ // If behind, skip
+ if (img->header.stamp < image_time)
+ {
+ cond_all.unlock();
+ return;
+ }
+
+ // If at time, increment and signal or wait
+ if (img->header.stamp == image_time)
+ {
+ count++;
+ if (count == 3)
+ {
+ cond_all.broadcast();
+ }
+ else
+ {
+ while (!done && img->header.stamp == image_time)
+ cond_all.wait();
+ }
+
+ cond_all.unlock();
+ return;
+ }
+
+ // If ahead, wake up others and then wait
+ if (img->header.stamp > image_time)
+ {
+ printf(" %s is already past with time: %d\n", img->label.c_str(), img->header.stamp.nsec);
+ cond_all.broadcast();
+ wait_for_others(img);
+ }
+ }
+
+ void wait_for_others(image_msgs::Image* img)
+ {
+ count = 1;
+ done = false;
+ image_time = img->header.stamp;
+ bool timed_out = false;
+
+ while (count < 3 && img->header.stamp == image_time && !timed_out)
+ if (!cond_all.timed_wait(1))
+ {
+ printf(" Timed out waiting for other images...\n");
+ timed_out = true;
+ }
+
+ if (img->header.stamp == image_time && !timed_out)
+ image_cb_all();
+ else
+ printf(" Got interrupted from time %d\n", img->header.stamp.nsec);
+
+ if (img->header.stamp == image_time)
+ {
+ done = true;
+ count = 0;
+ cond_all.broadcast();
+ }
+ cond_all.unlock();
+ }
+
+ void image_cb_all()
+ {
+ lbridge.fromImage(limage);
+ rbridge.fromImage(rimage);
+ dbridge.fromImage(dimage);
+
+ IplImage* disp = cvCreateImage(cvGetSize(dbridge.toIpl()), IPL_DEPTH_8U, 1);
+
+ // Disparity has to be scaled to be be nicely displayable
+ cvCvtScale(dbridge.toIpl(), disp, 1/4.0);
+
+ cvShowImage("left", lbridge.toIpl());
+ cvShowImage("right", rbridge.toIpl());
+ cvShowImage("disparity", disp);
+
+ cvWaitKey(5);
+
+ cvReleaseImage(&disp);
+ }
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ StereoView view;
+ view.spin();
+ ros::fini();
+ return 0;
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-11-11 07:51:51
|
Revision: 6515
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6515&view=rev
Author: jleibs
Date: 2008-11-11 07:51:46 +0000 (Tue, 11 Nov 2008)
Log Message:
-----------
Refactoring stereo_view to use the new topic_synchronizer
Modified Paths:
--------------
pkg/trunk/vision/stereo_view/manifest.xml
pkg/trunk/vision/stereo_view/stereo_view.cpp
Added Paths:
-----------
pkg/trunk/util/topic_synchronizer/
pkg/trunk/util/topic_synchronizer/manifest.xml
pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
Added: pkg/trunk/util/topic_synchronizer/manifest.xml
===================================================================
--- pkg/trunk/util/topic_synchronizer/manifest.xml (rev 0)
+++ pkg/trunk/util/topic_synchronizer/manifest.xml 2008-11-11 07:51:46 UTC (rev 6515)
@@ -0,0 +1,11 @@
+<package>
+ <description>A simple class to synchronize incoming topics by timestamp.</description>
+ <author>Jeremy Leibs</author>
+ <license>BSD</license>
+ <depend package="roscpp" />
+ <depend package="std_msgs" />
+ <depend package="rosthread" />
+ <export>
+ <cpp cflags="-I${prefix}"/>
+ </export>
+</package>
Added: pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
===================================================================
--- pkg/trunk/util/topic_synchronizer/topic_synchronizer.h (rev 0)
+++ pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2008-11-11 07:51:46 UTC (rev 6515)
@@ -0,0 +1,145 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+
+#ifndef TOPIC_SYNCHRONIZER_HH
+#define TOPIC_SYNCHRONIZER_HH
+
+#include "rosthread/condition.h"
+
+template <class N>
+class TopicSynchronizer
+{
+ private:
+
+ N* node_;
+ void (N::*callback_)();
+
+ ros::thread::condition cond_all_;
+
+ int expected_count_;
+
+ int count_;
+
+ bool done_;
+
+ ros::Time waiting_time_;
+
+ void msg_cb(void* p)
+ {
+ ros::Time* time = (ros::Time*)(p);
+
+ cond_all_.lock();
+
+ // If first to time, wait
+ if (count_ == 0)
+ {
+ wait_for_others(time);
+ return;
+ }
+
+ // If behind, skip
+ if (*time < waiting_time_)
+ {
+ cond_all_.unlock();
+ return;
+ }
+
+ // If at time, increment and signal or wait
+ if (*time == waiting_time_)
+ {
+ count_++;
+ if (count_ == expected_count_)
+ {
+ cond_all_.broadcast();
+ }
+ else
+ {
+ while (!done_ && *time == waiting_time_)
+ cond_all_.wait();
+ }
+
+ cond_all_.unlock();
+ return;
+ }
+
+ // If ahead, wake up others and then wait
+ if (*time > waiting_time_)
+ {
+ cond_all_.broadcast();
+ wait_for_others(time);
+ }
+ }
+
+ void wait_for_others(ros::Time* time)
+ {
+ count_ = 1;
+ done_ = false;
+
+ waiting_time_ = *time;
+ bool timed_out = false;
+
+ while (count_ < expected_count_ && *time == waiting_time_ && !timed_out)
+ if (!cond_all_.timed_wait(1))
+ {
+ printf(" Timed out waiting for other images...\n");
+ timed_out = true;
+ }
+
+ if (*time == waiting_time_ && !timed_out)
+ (*node_.*callback_)();
+
+ if (*time == waiting_time_)
+ {
+ done_ = true;
+ count_ = 0;
+ cond_all_.broadcast();
+ }
+ cond_all_.unlock();
+ }
+
+ public:
+
+ TopicSynchronizer(N* node, void (N::*callback)()) : node_(node), callback_(callback), expected_count_(0), count_(0), done_(false)
+ { }
+
+ template <class M>
+ void subscribe(std::string topic_name, M& msg, int queue_size)
+ {
+ node_->subscribe(topic_name, msg, &TopicSynchronizer<N>::msg_cb, this, &(msg.header.stamp), queue_size);
+ expected_count_++;
+ }
+};
+
+#endif
Modified: pkg/trunk/vision/stereo_view/manifest.xml
===================================================================
--- pkg/trunk/vision/stereo_view/manifest.xml 2008-11-11 07:09:49 UTC (rev 6514)
+++ pkg/trunk/vision/stereo_view/manifest.xml 2008-11-11 07:51:46 UTC (rev 6515)
@@ -9,5 +9,5 @@
<depend package="std_srvs"/>
<depend package="image_msgs"/>
<depend package="roscpp"/>
- <depend package="rosthread"/>
+ <depend package="topic_synchronizer"/>
</package>
Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-11 07:09:49 UTC (rev 6514)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-11 07:51:46 UTC (rev 6515)
@@ -1,3 +1,37 @@
+/*********************************************************************
+* 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 <vector>
#include "opencv/cxcore.h"
@@ -9,7 +43,7 @@
#include "image_msgs/Image.h"
#include "image_msgs/CvBridge.h"
-#include "rosthread/condition.h"
+#include "topic_synchronizer.h"
using namespace std;
@@ -29,104 +63,24 @@
std::string rtopic;
std::string dtopic;
- ros::Time image_time;
+ TopicSynchronizer<StereoView> sync;
- ros::thread::condition cond_all;
-
- int count;
- bool done;
-
- StereoView() : ros::node("cv_view"), count(0), done(false)
+ StereoView() : ros::node("cv_view"), sync(this, &StereoView::image_cb_all)
{
ltopic = map_name("dcam") + std::string("/left/image_rect");
rtopic = map_name("dcam") + std::string("/right/image_rect");
dtopic = map_name("dcam") + std::string("/disparity");
cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
- subscribe(ltopic, limage, &StereoView::image_cb, &limage, 1);
+ sync.subscribe(ltopic, limage, 1);
cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
- subscribe(rtopic, rimage, &StereoView::image_cb, &rimage, 1);
+ sync.subscribe(rtopic, rimage, 1);
cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
- subscribe(dtopic, dimage, &StereoView::image_cb, &dimage, 1);
+ sync.subscribe(dtopic, dimage, 1);
}
- void image_cb(void* p)
- {
- image_msgs::Image* img = (image_msgs::Image*)(p);
-
- cond_all.lock();
-
- // If first to time, wait
- if (count == 0)
- {
- wait_for_others(img);
- return;
- }
-
- // If behind, skip
- if (img->header.stamp < image_time)
- {
- cond_all.unlock();
- return;
- }
-
- // If at time, increment and signal or wait
- if (img->header.stamp == image_time)
- {
- count++;
- if (count == 3)
- {
- cond_all.broadcast();
- }
- else
- {
- while (!done && img->header.stamp == image_time)
- cond_all.wait();
- }
-
- cond_all.unlock();
- return;
- }
-
- // If ahead, wake up others and then wait
- if (img->header.stamp > image_time)
- {
- printf(" %s is already past with time: %d\n", img->label.c_str(), img->header.stamp.nsec);
- cond_all.broadcast();
- wait_for_others(img);
- }
- }
-
- void wait_for_others(image_msgs::Image* img)
- {
- count = 1;
- done = false;
- image_time = img->header.stamp;
- bool timed_out = false;
-
- while (count < 3 && img->header.stamp == image_time && !timed_out)
- if (!cond_all.timed_wait(1))
- {
- printf(" Timed out waiting for other images...\n");
- timed_out = true;
- }
-
- if (img->header.stamp == image_time && !timed_out)
- image_cb_all();
- else
- printf(" Got interrupted from time %d\n", img->header.stamp.nsec);
-
- if (img->header.stamp == image_time)
- {
- done = true;
- count = 0;
- cond_all.broadcast();
- }
- cond_all.unlock();
- }
-
void image_cb_all()
{
lbridge.fromImage(limage);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-11 08:23:53
|
Revision: 6519
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6519&view=rev
Author: hsujohnhsu
Date: 2008-11-11 08:23:50 +0000 (Tue, 11 Nov 2008)
Log Message:
-----------
* rosstage, fake_localization, P3D now uses PoseWithRatesStamped. No more TransformWithRateStamped.
* update gazebo_plugin tests to use new P3D message PoseWithRatesStamped.
* pr2_obs.launch geneates pr2_xml.model.
* remove IMUTopicName and use topicName in P3D xml specifications.
* add gaussian noise for robot_floorobj.world, P3D plugins.
* update pr2_robot_defs/head_defs.xml to use better inertia properties for tilting hokuyo.
Modified Paths:
--------------
pkg/trunk/demos/examples_gazebo/dual_link.xml
pkg/trunk/demos/examples_gazebo/multi_link.xml
pkg/trunk/demos/examples_gazebo/single_link.xml
pkg/trunk/demos/pr2_gazebo/pr2_obs.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.py
pkg/trunk/drivers/simulator/gazebo_plugin/test/testpendulum.py
pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.py
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
pkg/trunk/robot_descriptions/wg_robot_description/pr2/send_description.launch
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml
pkg/trunk/simulators/rosstage/rosstage.cc
Modified: pkg/trunk/demos/examples_gazebo/dual_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/dual_link.xml 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/demos/examples_gazebo/dual_link.xml 2008-11-11 08:23:50 UTC (rev 6519)
@@ -3,7 +3,8 @@
<!-- create model file for gazebo -->
<node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/dual_link_test/pr2_dual_link.xml $(find gazebo_robot_description)/world/pr2_xml_dual_link.model" />
<!-- send pr2_arm.xml to param server -->
- <include file="$(find wg_robot_description)/dual_link_test/send_description.xml" />
+ <!--include file="$(find wg_robot_description)/dual_link_test/send_description.xml" /-->
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/dual_link_test/pr2_dual_link.xml"" />
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_dual_link.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
Modified: pkg/trunk/demos/examples_gazebo/multi_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/multi_link.xml 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/demos/examples_gazebo/multi_link.xml 2008-11-11 08:23:50 UTC (rev 6519)
@@ -3,7 +3,8 @@
<!-- create model file for gazebo -->
<node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/multi_link_test/pr2_multi_link.xml $(find gazebo_robot_description)/world/pr2_xml_multi_link.model" />
<!-- send pr2_arm.xml to param server -->
- <include file="$(find wg_robot_description)/multi_link_test/send_description.xml" />
+ <!--include file="$(find wg_robot_description)/multi_link_test/send_description.xml" /-->
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/multi_link_test/pr2_multi_link.xml"" />
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_multi_link.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
Modified: pkg/trunk/demos/examples_gazebo/single_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/single_link.xml 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/demos/examples_gazebo/single_link.xml 2008-11-11 08:23:50 UTC (rev 6519)
@@ -3,7 +3,8 @@
<!-- create model file for gazebo -->
<node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/single_link_test/pr2_single_link.xml $(find gazebo_robot_description)/world/pr2_xml_single_link.model" />
<!-- send pr2_arm.xml to param server -->
- <include file="$(find wg_robot_description)/single_link_test/send_description.xml" />
+ <!--include file="$(find wg_robot_description)/single_link_test/send_description.xml" /-->
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/single_link_test/pr2_single_link.xml"" />
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_single_link.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
Modified: pkg/trunk/demos/pr2_gazebo/pr2_obs.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_obs.launch 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/demos/pr2_gazebo/pr2_obs.launch 2008-11-11 08:23:50 UTC (rev 6519)
@@ -1,5 +1,7 @@
<launch>
<group name="wg">
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml.model" />
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml_nolimit.model 1" />
<param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_obstacle.world" respawn="false">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch 2008-11-11 08:23:50 UTC (rev 6519)
@@ -3,11 +3,10 @@
<!-- if needed, group tag allows pushing components into namespace via ns="namespace" -->
<group name="wg">
<!-- create model file for gazebo -->
- <!--node pkg="xacro" type="xacro.py" args="$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml $(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml.expanded" /-->
<node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml.expanded $(find gazebo_robot_description)/world/pr2_xml_prototype1.model" />
<!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
(Mechanism Control, BaseControllerNode, etc...) -->
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml.expanded"" />
+ <include file="$(find wg_robot_description)/pr2_prototype1/send_description.xml" />
<!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_prototype1.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-11-11 08:23:50 UTC (rev 6519)
@@ -53,7 +53,6 @@
<updateRate>1000.0</updateRate>
<bodyName>body_name</bodyName>
<topicName>body_pose_ground_truth</topicName>
- <IMUTopicName>body_pose_IMU</IMUTopicName>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- option to initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
@@ -78,7 +77,6 @@
<updateRate>1000.0</updateRate>
<bodyName>body_name</bodyName>
<topicName>body_pose_ground_truth</topicName>
- <IMUTopicName>body_pose_IMU</IMUTopicName>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- option to initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
@@ -122,12 +120,10 @@
private: ros::node *rosnode;
/// \brief ros message
- private: std_msgs::TransformWithRateStamped transformMsg;
private: std_msgs::PoseWithRatesStamped poseMsg;
/// \brief topic name
private: std::string topicName;
- private: std::string IMUTopicName;
/// \brief frame transform name, should match link name
/// FIXME: extract link name directly?
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-11-11 08:23:50 UTC (rev 6519)
@@ -79,16 +79,14 @@
// this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(bodyName));
this->topicName = node->GetString("topicName", "ground_truth", 1);
- this->IMUTopicName = node->GetString("IMUTopicName", "", 1);
this->frameName = node->GetString("frameName", "", 1);
this->xyzOffsets = node->GetVector3("xyzOffsets", Vector3(0,0,0));
this->rpyOffsets = node->GetVector3("rpyOffsets", Vector3(0,0,0));
this->gaussianNoise = node->GetDouble("gaussianNoise",0.0,0); //read from xml file
std::cout << "==== topic name for P3D ======== " << this->topicName << std::endl;
- rosnode->advertise<std_msgs::TransformWithRateStamped>(this->topicName,10);
- if (this->IMUTopicName != "")
- rosnode->advertise<std_msgs::PoseWithRatesStamped>(this->IMUTopicName,10);
+ if (this->topicName != "")
+ rosnode->advertise<std_msgs::PoseWithRatesStamped>(this->topicName,10);
}
////////////////////////////////////////////////////////////////////////////////
@@ -138,46 +136,7 @@
this->lock.lock();
- /// @todo: remove transform message
- // copy data into a transform message
- this->transformMsg.header.frame_id = this->frameName;
- this->transformMsg.header.stamp.sec = (unsigned long)floor(cur_time);
- this->transformMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( cur_time - this->transformMsg.header.stamp.sec) );
-
- this->transformMsg.transform.translation.x = pos.x;
- this->transformMsg.transform.translation.y = pos.y;
- this->transformMsg.transform.translation.z = pos.z;
-
- this->transformMsg.transform.rotation.x = rot.x;
- this->transformMsg.transform.rotation.y = rot.y;
- this->transformMsg.transform.rotation.z = rot.z;
- this->transformMsg.transform.rotation.w = rot.u;
-
- this->transformMsg.rate.translation.x = vpos.x;
- this->transformMsg.rate.translation.y = vpos.y;
- this->transformMsg.rate.translation.z = vpos.z;
-
- // pass quaternion
- // this->transformMsg.rate.rotation.x = vrot.x;
- // this->transformMsg.rate.rotation.y = vrot.y;
- // this->transformMsg.rate.rotation.z = vrot.z;
- // this->transformMsg.rate.rotation.w = vrot.u;
-
- // pass euler anglular rates
- this->transformMsg.rate.rotation.x = veul.x;
- this->transformMsg.rate.rotation.y = veul.y;
- this->transformMsg.rate.rotation.z = veul.z;
- this->transformMsg.rate.rotation.w = 0;
-
- // publish to ros
- rosnode->publish(this->topicName,this->transformMsg);
-
-
-
-
-
-
- if (this->IMUTopicName != "")
+ if (this->topicName != "")
{
// copy data into pose message
this->poseMsg.header.frame_id = "map"; // @todo: should this be changeable?
@@ -212,7 +171,7 @@
this->poseMsg.acc.ang_acc.az = this->aeul.z + this->GaussianKernel(0,this->gaussianNoise) ;
// publish to ros
- rosnode->publish(this->IMUTopicName,this->poseMsg);
+ rosnode->publish(this->topicName,this->poseMsg);
}
this->lock.unlock();
@@ -225,9 +184,8 @@
// Finalize the controller
void P3D::FiniChild()
{
- rosnode->unadvertise(this->topicName);
- if (this->IMUTopicName != "")
- rosnode->unadvertise(this->IMUTopicName);
+ if (this->topicName != "")
+ rosnode->unadvertise(this->topicName);
}
//////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.py
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.py 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.py 2008-11-11 08:23:50 UTC (rev 6519)
@@ -69,26 +69,25 @@
def printBase(self, p3d):
- print "P3D pose translan: " + "x: " + str(p3d.transform.translation.x)
- print " " + "y: " + str(p3d.transform.translation.y)
- print " " + "z: " + str(p3d.transform.translation.z)
- print "P3D pose rotation: " + "x: " + str(p3d.transform.rotation.x)
- print " " + "y: " + str(p3d.transform.rotation.y)
- print " " + "z: " + str(p3d.transform.rotation.z)
- print " " + "w: " + str(p3d.transform.rotation.w)
- print "P3D rate translan: " + "x: " + str(p3d.rate.translation.x)
- print " " + "y: " + str(p3d.rate.translation.y)
- print " " + "z: " + str(p3d.rate.translation.z)
- print "P3D rate rotation: " + "x: " + str(p3d.rate.rotation.x)
- print " " + "y: " + str(p3d.rate.rotation.y)
- print " " + "z: " + str(p3d.rate.rotation.z)
- print " " + "w: " + str(p3d.rate.rotation.w)
+ print "P3D pose translan: " + "x: " + str(p3d.pos.position.x)
+ print " " + "y: " + str(p3d.pos.position.y)
+ print " " + "z: " + str(p3d.pos.position.z)
+ print "P3D pose rotation: " + "x: " + str(p3d.pos.orientation.x)
+ print " " + "y: " + str(p3d.pos.orientation.y)
+ print " " + "z: " + str(p3d.pos.orientation.z)
+ print " " + "w: " + str(p3d.pos.orientation.w)
+ print "P3D rate translan: " + "x: " + str(p3d.vel.vel.x)
+ print " " + "y: " + str(p3d.vel.vel.y)
+ print " " + "z: " + str(p3d.vel.vel.z)
+ print "P3D rate rotation: " + "x: " + str(p3d.vel.ang_vel.vx)
+ print " " + "y: " + str(p3d.vel.ang_vel.vy)
+ print " " + "z: " + str(p3d.vel.ang_vel.vz)
def p3dInput(self, p3d):
i = 0
print "base pose ground truth received"
self.printBase(p3d)
- error = abs(p3d.rate.rotation.z - TARGET_VW)
+ error = abs(p3d.vel.ang_vel.vz - TARGET_VW)
print " Error: " + str(error)
# has to reach target vw and maintain target vw for a duration of TARGET_DURATION seconds
if self.reached_target_vw:
@@ -107,7 +106,7 @@
def test_base(self):
print "LNK\n"
pub = rospy.Publisher("cmd_vel", BaseVel)
- rospy.Subscriber("base_pose_ground_truth", TransformWithRateStamped, self.p3dInput)
+ rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 60.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testpendulum.py
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testpendulum.py 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testpendulum.py 2008-11-11 08:23:50 UTC (rev 6519)
@@ -106,27 +106,26 @@
#def printPendulum(self, p3d):
- #print "P3D pose translan: " + "x: " + str(p3d.transform.translation.x)
- #print " " + "y: " + str(p3d.transform.translation.y)
- #print " " + "z: " + str(p3d.transform.translation.z)
- #print "P3D pose rotation: " + "x: " + str(p3d.transform.rotation.x)
- #print " " + "y: " + str(p3d.transform.rotation.y)
- #print " " + "z: " + str(p3d.transform.rotation.z)
- #print " " + "w: " + str(p3d.transform.rotation.w)
- #print "P3D rate translan: " + "x: " + str(p3d.rate.translation.x)
- #print " " + "y: " + str(p3d.rate.translation.y)
- #print " " + "z: " + str(p3d.rate.translation.z)
- #print "P3D rate rotation: " + "x: " + str(p3d.rate.rotation.x)
- #print " " + "y: " + str(p3d.rate.rotation.y)
- #print " " + "z: " + str(p3d.rate.rotation.z)
- #print " " + "w: " + str(p3d.rate.rotation.w)
+ #print "P3D pose translan: " + "x: " + str(p3d.pos.position.x)
+ #print " " + "y: " + str(p3d.pos.position.y)
+ #print " " + "z: " + str(p3d.pos.position.z)
+ #print "P3D pose rotation: " + "x: " + str(p3d.pos.orientation.x)
+ #print " " + "y: " + str(p3d.pos.orientation.y)
+ #print " " + "z: " + str(p3d.pos.orientation.z)
+ #print " " + "w: " + str(p3d.pos.orientation.w)
+ #print "P3D rate translan: " + "x: " + str(p3d.vel.vel.x)
+ #print " " + "y: " + str(p3d.vel.vel.y)
+ #print " " + "z: " + str(p3d.vel.vel.z)
+ #print "P3D rate rotation: " + "x: " + str(p3d.vel.ang_vel.vx)
+ #print " " + "y: " + str(p3d.vel.ang_vel.vy)
+ #print " " + "z: " + str(p3d.vel.ang_vel.vz)
def p3dInput1(self, p3d):
#print "link1 pose ground truth received"
#self.printPendulum(p3d)
- tmpx = p3d.transform.translation.x
- tmpz = p3d.transform.translation.z - 2.0
+ tmpx = p3d.pos.position.x
+ tmpz = p3d.pos.position.z - 2.0
#print "link1 origin (" + str(tmpx) + " , " + str(tmpz) + ")"
self.error1_total += math.sqrt(tmpx*tmpx+tmpz*tmpz)
self.error1_count += 1
@@ -136,19 +135,19 @@
def p3dInput2(self, p3d):
#print "link2 pose ground truth received"
#self.printPendulum(p3d)
- q = Q(p3d.transform.rotation.x , p3d.transform.rotation.y , p3d.transform.rotation.z , p3d.transform.rotation.w)
+ q = Q(p3d.pos.orientation.x , p3d.pos.orientation.y , p3d.pos.orientation.z , p3d.pos.orientation.w)
q.normalize()
v = q.getEuler()
- #FIXME: something wrong with the transform, need to fix it. abs masks the problem for now.
- #FIXME: something wrong with the transform, need to fix it. abs masks the problem for now.
- #FIXME: something wrong with the transform, need to fix it. abs masks the problem for now.
- #FIXME: something wrong with the transform, need to fix it. abs masks the problem for now.
- tmpx = abs(p3d.transform.translation.x) +0.0 - abs(math.cos(v.z)*math.cos(v.y))
- tmpz = abs(p3d.transform.translation.z) -2.0 + abs(math.sin(v.y))
+ #FIXME: something wrong with the pos, need to fix it. abs masks the problem for now.
+ #FIXME: something wrong with the pos, need to fix it. abs masks the problem for now.
+ #FIXME: something wrong with the pos, need to fix it. abs masks the problem for now.
+ #FIXME: something wrong with the pos, need to fix it. abs masks the problem for now.
+ tmpx = abs(p3d.pos.position.x) +0.0 - abs(math.cos(v.z)*math.cos(v.y))
+ tmpz = abs(p3d.pos.position.z) -2.0 + abs(math.sin(v.y))
#math.cos(v.x)*math.cos(v.z)-math.cos(v.x)*math.sin(v.y)*math.cos(v.z))
#print "link2 origin (" + str(tmpx) + " , " + str(tmpz) + ")"
- #print "link2 raw (" + str(p3d.transform.translation.x) + " , " + str(p3d.transform.translation.z) + ") total: " + str(self.error2_total)
+ #print "link2 raw (" + str(p3d.pos.position.x) + " , " + str(p3d.pos.position.z) + ") total: " + str(self.error2_total)
#print "link2 correc (" + str(math.cos(v.y)) + " , " + str(math.sin(v.y)) + ") angle: " + str(v.x) +","+str(v.y)+","+str(v.z)
self.error2_total += math.sqrt(tmpx*tmpx+tmpz*tmpz)
@@ -158,8 +157,8 @@
def test_pendulum(self):
print "LNK\n"
- rospy.Subscriber("link1_pose", TransformWithRateStamped, self.p3dInput1)
- rospy.Subscriber("link2_pose", TransformWithRateStamped, self.p3dInput2)
+ rospy.Subscriber("link1_pose", PoseWithRatesStamped, self.p3dInput1)
+ rospy.Subscriber("link2_pose", PoseWithRatesStamped, self.p3dInput2)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 20.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.py
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.py 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.py 2008-11-11 08:23:50 UTC (rev 6519)
@@ -62,26 +62,26 @@
self.hits = 0
self.runs = 0
- def positionInput(self, pose):
+ def positionInput(self, p3d):
self.runs = self.runs + 1
- print " got pose ", self.runs
+ print " got p3d ", self.runs
#if (pos.frame == 1):
- print "x ", pose.transform.translation.x
- print "y ", pose.transform.translation.y
- print "z ", pose.transform.translation.z
- dx = pose.transform.translation.x - TARGET_X
- dy = pose.transform.translation.y - TARGET_Y
- dz = pose.transform.translation.z - TARGET_Z
+ print "x ", p3d.pos.position.x
+ print "y ", p3d.pos.position.y
+ print "z ", p3d.pos.position.z
+ dx = p3d.pos.position.x - TARGET_X
+ dy = p3d.pos.position.y - TARGET_Y
+ dz = p3d.pos.position.z - TARGET_Z
d = math.sqrt((dx * dx) + (dy * dy)) #+ (dz * dz))
- print "P: " + str(pose.transform.translation.x) + " " + str(pose.transform.translation.y)
+ print "P: " + str(p3d.pos.position.x) + " " + str(p3d.pos.position.y)
#print "D: " + str(dx) + " " + str(dy) + " " + str(dz) + " " + str(d) + " < " + str(TARGET_RAD * TARGET_RAD)
if (d < TARGET_RAD):
- #print "HP: " + str(dx) + " " + str(dy) + " " + str(d) + " at " + str(pos.transform.translation.x) + " " + str(pos.transform.translation.y)
+ #print "HP: " + str(dx) + " " + str(dy) + " " + str(d) + " at " + str(p3d.pos.position.x) + " " + str(p3d.pos.position.y)
#print "DONE"
self.hits = self.hits + 1
print "Hit goal, " + str(self.hits)
if (self.runs < 100 and self.runs > 10):
- print "Obviously wrong transforms!"
+ print "Obviously wrong poses!"
self.success = False
self.fail = True
#os.system("killall gazebo")
@@ -95,7 +95,7 @@
def testslide(self):
print "LINK\n"
#rospy.Subscriber("Odom", RobotBase2DOdom, self.positionInput)
- rospy.Subscriber("base_pose_ground_truth", TransformWithRateStamped, self.positionInput)
+ rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.positionInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 50.0 #59 seconds
while not rospy.is_shutdown() and not self.success and not self.fail and time.time() < timeout_t:
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2008-11-11 08:23:50 UTC (rev 6519)
@@ -54,7 +54,7 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "base_pose_ground_truth"/std_msgs::TransformWithRateStamped : robot's odometric pose. Only the position information is used (velocity is ignored).
+- @b "base_pose_ground_truth"/std_msgs::PoseWithRatesStamped : robot's odometric pose. Only the position information is used (velocity is ignored).
- @b "initialpose"/Pose2DFloat32 : robot's odometric pose. Only the position information is used (velocity is ignored).
Publishes to (name / type):
@@ -73,7 +73,7 @@
#include <ros/time.h>
#include <std_msgs/RobotBase2DOdom.h>
-#include <std_msgs/TransformWithRateStamped.h>
+#include <std_msgs/PoseWithRatesStamped.h>
#include <std_msgs/Pose3DStamped.h>
#include <std_msgs/ParticleCloud2D.h>
#include <std_msgs/Pose2DFloat32.h>
@@ -117,7 +117,7 @@
ros::Time m_lastUpdate;
double m_maxPublishFrequency;
- std_msgs::TransformWithRateStamped m_basePosMsg;
+ std_msgs::PoseWithRatesStamped m_basePosMsg;
std_msgs::ParticleCloud2D m_particleCloud;
std_msgs::RobotBase2DOdom m_currentPos;
std_msgs::Pose2DFloat32 m_iniPos;
@@ -140,12 +140,12 @@
m_lastUpdate = ros::Time::now();
- tf::Transform txi(tf::Quaternion(m_basePosMsg.transform.rotation.x,
- m_basePosMsg.transform.rotation.y,
- m_basePosMsg.transform.rotation.z,
- m_basePosMsg.transform.rotation.w),
- tf::Point(m_basePosMsg.transform.translation.x,
- m_basePosMsg.transform.translation.y, 0.0));
+ tf::Transform txi(tf::Quaternion(m_basePosMsg.pos.orientation.x,
+ m_basePosMsg.pos.orientation.y,
+ m_basePosMsg.pos.orientation.z,
+ m_basePosMsg.pos.orientation.w),
+ tf::Point(m_basePosMsg.pos.position.x,
+ m_basePosMsg.pos.position.y, 0.0));
double x = txi.getOrigin().x() + m_iniPos.x;
double y = txi.getOrigin().y() + m_iniPos.y;
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-11 08:23:50 UTC (rev 6519)
@@ -333,7 +333,6 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_right</bodyName>
<topicName>gripper_roll_right_pose_ground_truth</topicName>
- <IMUTopicName>gripper_roll_right_pose_imu</IMUTopicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<interface:position name="p3d_right_wrist_position"/>
@@ -344,7 +343,6 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
<topicName>gripper_roll_left_pose_ground_truth</topicName>
- <IMUTopicName>gripper_roll_left_pose_imu</IMUTopicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<interface:position name="p3d_left_wrist_position"/>
@@ -355,7 +353,6 @@
<updateRate>1000.0</updateRate>
<bodyName>base</bodyName>
<topicName>base_pose_ground_truth</topicName>
- <IMUTopicName>base_pose_imu</IMUTopicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world 2008-11-11 08:23:50 UTC (rev 6519)
@@ -354,7 +354,7 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_right</bodyName>
<topicName>gripper_roll_right_pose_ground_truth</topicName>
- <IMUTopicName>gripper_roll_right_pose_imu</IMUTopicName>
+ <gaussianNoise>0.01</gaussianNoise>
<frameNam...
[truncated message content] |
|
From: <pof...@us...> - 2008-11-11 20:13:07
|
Revision: 6531
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6531&view=rev
Author: poftwaresatent
Date: 2008-11-11 20:13:02 +0000 (Tue, 11 Nov 2008)
Log Message:
-----------
Introduced planner-type aliases (you can say e.g. "arastar" to mean
"ARAPlanner") and optional dumping to std::ostream of available names
and aliases in case the name you specified was not recognized.
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp
pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2008-11-11 20:03:10 UTC (rev 6530)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2008-11-11 20:13:02 UTC (rev 6531)
@@ -229,7 +229,7 @@
string plannerType;
local_param("plannerType", plannerType, string("ARAPlanner"));
pMgr_ = new ompl::SBPLPlannerManager(env_->getDSI(), false, &mdpCfg_);
- if ( ! pMgr_->select(plannerType, false)) {
+ if ( ! pMgr_->select(plannerType, false, 0)) {
ROS_ERROR("in MoveBaseSBPL ctor: pMgr_->select(%s) failed", plannerType.c_str());
throw int(5);
}
Modified: pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp 2008-11-11 20:03:10 UTC (rev 6530)
+++ pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.cpp 2008-11-11 20:13:02 UTC (rev 6531)
@@ -47,36 +47,73 @@
#include <errno.h>
#include <cstring>
+using namespace std;
+
namespace {
- std::string mk_invalid_state_str(std::string const & method, int state) {
- std::ostringstream os;
+ string mk_invalid_state_str(string const & method, int state) {
+ ostringstream os;
os << "EnvironmentWrapper::invalid_state in method " << method << ": state = " << state;
return os.str();
}
+ static map<string, string> planner_alias;
+
}
namespace ompl {
+ std::string canonicalPlannerName(std::string const & name_or_alias)
+ {
+ if (planner_alias.empty()) {
+ planner_alias.insert(make_pair("ARAPlanner", "ARAPlanner"));
+ planner_alias.insert(make_pair("ara", "ARAPlanner"));
+ planner_alias.insert(make_pair("ARA", "ARAPlanner"));
+ planner_alias.insert(make_pair("arastar", "ARAPlanner"));
+ planner_alias.insert(make_pair("ARAStar", "ARAPlanner"));
+
+ planner_alias.insert(make_pair("ADPlanner", "ADPlanner"));
+ planner_alias.insert(make_pair("ad", "ADPlanner"));
+ planner_alias.insert(make_pair("AD", "ADPlanner"));
+ planner_alias.insert(make_pair("adstar", "ADPlanner"));
+ planner_alias.insert(make_pair("ADStar", "ADPlanner"));
+ }
+
+ map<string, string>::const_iterator is(planner_alias.find(name_or_alias));
+ if (planner_alias.end() == is)
+ return "";
+ return is->second;
+ }
+
+
SBPLPlanner * createSBPLPlanner(std::string const & name,
DiscreteSpaceInformation* environment,
bool bforwardsearch,
- MDPConfig* mdpCfg)
+ MDPConfig* mdpCfg,
+ std::ostream * opt_err_os)
{
- if ("ARAPlanner" == name)
+ string const canonical_name(canonicalPlannerName(name));
+ if ("ARAPlanner" == canonical_name)
return new ARAPlanner(environment, bforwardsearch);
- if ("ADPlanner" == name)
+ if ("ADPlanner" == canonical_name)
return new ADPlanner(environment, bforwardsearch);
// VIPlanner not instantiable... work in progress by Max
#ifdef UNDEFINED
- if ("VIPlanner" == name)
+ if ("VIPlanner" == canonical_name)
return new VIPlanner(environment, mdpCfg);
#endif // UNDEFINED
-
+
+ if (opt_err_os) {
+ *opt_err_os << "ompl::createSBPLPlanner(): no planner called \"name\"\n"
+ << " use \"ARAPlanner\" or \"ADPlanner\"\n"
+ << " or one of the registered aliases:\n";
+ for (map<string, string>::const_iterator is(planner_alias.begin()); is != planner_alias.end(); ++is)
+ *opt_err_os << " " << is->first << " --> " << is->second << "\n";
+ }
+
return 0;
}
@@ -109,11 +146,11 @@
bool SBPLPlannerManager::
- select(std::string const & name, bool recycle)
+ select(std::string const & name, bool recycle, std::ostream * opt_err_os)
{
if (recycle && (name == name_))
return true;
- SBPLPlanner * new_planner(createSBPLPlanner(name, environment_, bforwardsearch_, mdpCfg_));
+ SBPLPlanner * new_planner(createSBPLPlanner(name, environment_, bforwardsearch_, mdpCfg_, opt_err_os));
if ( ! new_planner)
return false;
delete planner_;
Modified: pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh
===================================================================
--- pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh 2008-11-11 20:03:10 UTC (rev 6530)
+++ pkg/trunk/motion_planning/sbpl_util/src/sbpl_util.hh 2008-11-11 20:13:02 UTC (rev 6531)
@@ -58,6 +58,13 @@
/**
+ Translate a name or alias into a string that createSBPLPlanner()
+ understands.
+ */
+ std::string canonicalPlannerName(std::string const & name_or_alias);
+
+
+ /**
Create a planner subclass based on its name.
\todo Use some sort of registry instead of hard-coded
@@ -73,7 +80,9 @@
/** Required by some planners (ARAPlanner, ADPlanner). */
bool bforwardsearch,
/** Required by some planners (VIPlanner). */
- MDPConfig* mdpCfg);
+ MDPConfig* mdpCfg,
+ /** optional stream to which error messages get written */
+ std::ostream * opt_err_os);
/**
@@ -171,7 +180,9 @@
\return true if createSBPLPlanner() succeeded OR the old instance was recycled.
*/
- bool select(std::string const & name, bool recycle);
+ bool select(std::string const & name, bool recycle,
+ /** optional stream to which error messages get written */
+ std::ostream * opt_err_os);
/** Dispatch to the currently select()-ed planner's
SBPLPlanner::replan(), measuring the time it actually takes to
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2008-11-11 23:16:56
|
Revision: 6552
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6552&view=rev
Author: jfaustwg
Date: 2008-11-11 23:16:51 +0000 (Tue, 11 Nov 2008)
Log Message:
-----------
Update trunk from user branch: tf::MessageNotifier
r11791@iad (orig r6401): jfaustwg | 2008-11-07 13:03:45 -0800
Creating personal branch
r11938@iad (orig r6524): jfaustwg | 2008-11-11 10:19:39 -0800
Adding tf::MessageNotifier for automatic buffering of messages until transformation data is available
r11962@iad (orig r6546): jfaustwg | 2008-11-11 14:51:19 -0800
* Remove all use of the message list from the message callback, instead pushing things into a queue and having the worker thread do a fast swap() operation on it to avoid lock contension.
* Fix tests to remove the race conditions it's possible to remove
Modified Paths:
--------------
pkg/trunk/util/tf/CMakeLists.txt
pkg/trunk/util/tf/manifest.xml
pkg/trunk/util/tf/test/testBroadcaster.cpp
Added Paths:
-----------
pkg/trunk/util/tf/include/tf/message_notifier.h
pkg/trunk/util/tf/src/message_notifier.cpp
pkg/trunk/util/tf/test/test_notifier.cpp
pkg/trunk/util/tf/test/test_notifier.xml
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:6549
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/util/tf/CMakeLists.txt
===================================================================
--- pkg/trunk/util/tf/CMakeLists.txt 2008-11-11 23:15:04 UTC (rev 6551)
+++ pkg/trunk/util/tf/CMakeLists.txt 2008-11-11 23:16:51 UTC (rev 6552)
@@ -4,8 +4,13 @@
genmsg()
-rospack_add_library(tf src/tf.cpp src/transform_listener.cpp src/cache.cpp)
+find_package(Boost 0 REQUIRED COMPONENTS thread)
+include_directories(${Boost_INCLUDE_DIRS})
+link_directories(${Boost_LIBRARY_DIRS})
+rospack_add_library(tf src/tf.cpp src/transform_listener.cpp src/cache.cpp src/message_notifier.cpp)
+target_link_libraries(tf ${Boost_LIBRARIES})
+
rospack_add_executable(simpletest simpletest.cpp)
target_link_libraries(simpletest tf)
#rospack_add_executable(testtf src/test/testtf.cc)
@@ -27,7 +32,6 @@
rospack_add_gtest(bullet_unittest test/bullet_unittest.cpp)
target_link_libraries(bullet_unittest tf)
-
rospack_add_executable(btTest test/quaternion.cpp)
rospack_add_executable(testListener test/testListener.cpp)
target_link_libraries(testListener tf)
@@ -36,3 +40,8 @@
target_link_libraries(testBroadcaster tf)
rospack_add_executable(transform_sender src/transform_sender.cpp)
+rospack_add_executable(test_notifier test/test_notifier.cpp)
+rospack_is_test_harness(test_notifier)
+rospack_add_gtest_build_flags(test_notifier)
+target_link_libraries(test_notifier tf ${Boost_LIBRARIES})
+rospack_add_rostest(test/test_notifier.xml)
Added: pkg/trunk/util/tf/include/tf/message_notifier.h
===================================================================
--- pkg/trunk/util/tf/include/tf/message_notifier.h (rev 0)
+++ pkg/trunk/util/tf/include/tf/message_notifier.h 2008-11-11 23:16:51 UTC (rev 6552)
@@ -0,0 +1,439 @@
+/*
+ * 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, Inc. 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 Josh Faust */
+
+#include <ros/node.h>
+#include <tf/tf.h>
+#include <tf/tfMessage.h>
+
+#include <string>
+#include <list>
+#include <vector>
+#include <boost/function.hpp>
+#include <boost/bind.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/thread.hpp>
+
+/// \todo remove backward compatability
+#include "rosTF/TransformArray.h"
+
+namespace tf
+{
+
+/**
+ * \brief For internal use only.
+ *
+ * In general, allocating memory inside a library's header using new/delete is a good way to cause difficult to find bugs.
+ * This provides a way for MessageNotifier to allocate from within the tf library, rather than within the calling library.
+ */
+void* notifierAllocate(uint32_t size);
+/**
+ * \brief For internal use only.
+ *
+ * In general, allocating memory inside a library's header using new/delete is a good way to cause difficult to find bugs.
+ * This provides a way for MessageNotifier to allocate from within the tf library, rather than within the calling library.
+ */
+void notifierDeallocate(void* p);
+
+class Transformer;
+
+/**
+ * \class MessageNotifier
+ * \brief Queues messages that include a Header until there is transform data available for the time the timestamp on the message.
+ *
+ * For use instead of extrapolation, MessageNotifier provides a way of waiting until it is possible to transform a message
+ * before getting a callback notifying that the message is available.
+ *
+ * \section callback THE CALLBACK
+ * The callback takes one argument, which is a boost shared_ptr to a message. MessageNotifier provides a MessagePtr typedef,
+ * so the signature of the callback is:
+\verbatim
+void funcName(const MessageNotifier<Message>::MessagePtr& message);
+\endverbatim
+ *
+ * A bare function can be passed directly as the callback. Methods must be passed using boost::bind. For example
+\verbatim
+boost::bind(&MyObject::funcName, this, _1);
+\endverbatim
+ *
+ * The message is \b not locked when your callback is invoked.
+ *
+ * \section threading THREADING
+ * MessageNotifier spins up a single thread to call your callback from, so that it's possible to do a lot of work in your callback
+ * without blocking the rest of the application.
+ *
+ * \section linking_boost LINKING BOOST
+ * MessageNotifier uses boost::thread. Since MessageNotifier is entirely implemented in a header (because it is templated),
+ * you must link against boost thread. An example of doing so in CMake:
+\verbatim
+find_package(Boost 0 REQUIRED COMPONENTS thread)
+include_directories(${Boost_INCLUDE_DIRS})
+link_directories(${Boost_LIBRARY_DIRS})
+target_link_libraries(tf ${Boost_LIBRARIES})
+\endverbatim
+ */
+template<class Message>
+class MessageNotifier
+{
+public:
+ typedef boost::shared_ptr<Message> MessagePtr;
+ typedef boost::function<void(const MessagePtr&)> Callback;
+
+ /**
+ * \brief Constructor
+ * \param tf The Transformer to use for checking if the transform data is available
+ * \param node The ros::node to subscribe on
+ * \param callback The function to call when a message is ready to be transformed
+ * \param topic The topic to listen on
+ * \param target_frame The frame we need to be able to transform to before a message is ready
+ * \param queue_size The number of messages to keep around waiting for transform data. This is passed directly to ros::node::subscribe.
+ * \note A queue size of 0 means infinite, which is dangerous
+ */
+ MessageNotifier(Transformer* tf, ros::node* node, Callback callback, const std::string& topic, const std::string& target_frame, uint32_t queue_size)
+ : tf_(tf)
+ , node_(node)
+ , callback_(callback)
+ , target_frame_(target_frame)
+ , queue_size_(queue_size)
+ , message_count_(0)
+ , destructing_(false)
+ {
+ setTopic(topic);
+
+ node_->subscribe("/tf_message", transforms_message_, &MessageNotifier::incomingTFMessage, this, 1);
+ node_->subscribe("/TransformArray", old_transforms_message_, &MessageNotifier::incomingOldTFMessage, this, 1);
+
+ thread_handle_ = new boost::thread(boost::bind(&MessageNotifier::workerThread, this));
+ }
+
+ /**
+ * \brief Destructor
+ */
+ ~MessageNotifier()
+ {
+ unsubscribeFromMessage();
+
+ node_->unsubscribe("/tf_message", &MessageNotifier::incomingTFMessage, this);
+ node_->unsubscribe("/TransformArray", &MessageNotifier::incomingOldTFMessage, this);
+
+ // Tell the worker thread that we're destructing
+ destructing_ = true;
+ new_data_.notify_all();
+
+ // Wait for the worker thread to exit
+ thread_handle_->join();
+
+ delete thread_handle_;
+ }
+
+ /**
+ * \brief Set the frame you need to be able to transform to before getting a message callback
+ */
+ void setTargetFrame(const std::string& target_frame)
+ {
+ boost::mutex::scoped_lock lock(queue_mutex_);
+
+ target_frame_ = target_frame;
+ new_data_.notify_all();
+ }
+
+ /**
+ * \brief Set the topic to listen on
+ */
+ void setTopic(const std::string& topic)
+ {
+ unsubscribeFromMessage();
+
+ topic_ = topic;
+
+ subscribeToMessage();
+ }
+
+ /**
+ * \brief Clear any messages currently in the queue
+ * \note Note that if callbacks are currently being invoked, this does *not*
+ */
+ void clear()
+ {
+ boost::mutex::scoped_lock notify_lock(notify_mutex_);
+ boost::mutex::scoped_lock list_lock(queue_mutex_);
+
+ messages_.clear();
+ }
+
+private:
+
+ typedef std::vector<MessagePtr> V_Message;
+ typedef std::list<MessagePtr> L_Message;
+
+ /**
+ * \brief Gather any messages ready to be transformed
+ * \param to_notify Filled with the messages ready to be transformed, in the order they were received
+ * \note Assumes the message list is already locked
+ */
+ void gatherReadyMessages(V_Message& to_notify)
+ {
+ to_notify.reserve(message_count_);
+
+ int i = 0;
+
+ typename L_Message::iterator it = messages_.begin();
+ typename L_Message::iterator end = messages_.end();
+ for (; it != end; ++i)
+ {
+ MessagePtr& message = *it;
+
+ /// \todo Once the canTransform function is implemented, don't do things this way
+ try
+ {
+ // Attempt to transform from the source frame to the target frame at the message's timestamp
+ Stamped<Vector3> v(Vector3(0.0, 0.0, 0.0), message->header.stamp, message->header.frame_id);
+ // If the transform fails an exception will be thrown, so the push_back statement will not be hit
+ tf_->transformVector(target_frame_, v, v);
+
+ // If we get here the transform succeeded, so push the message onto the notify list, and erase it from or message list
+ to_notify.push_back(message);
+
+ it = messages_.erase(it);
+ --message_count_;
+
+ //printf("Message %d ready, count now %d\n", i, message_count_);
+ }
+ catch(TransformException& e)
+ {
+ //printf("message %d: Transforms unavailable\n", i);
+ // If the transform failed, move on to the next message
+ ++it;
+ }
+ }
+ }
+
+ /**
+ * \brief Calls the notification callback on each message in the passed vector
+ * \param to_notify The list of messages to call the callback on
+ */
+ void notify(V_Message& to_notify)
+ {
+ boost::mutex::scoped_lock lock(notify_mutex_);
+
+ typename V_Message::iterator it = to_notify.begin();
+ typename V_Message::iterator end = to_notify.end();
+ for (; it != end; ++it)
+ {
+ callback_(*it);
+ }
+ }
+
+ /**
+ * \brief Adds messages into the message list, removing old messages if necessary
+ */
+ void processNewMessages(V_Message& messages)
+ {
+ typename V_Message::iterator it = messages.begin();
+ typename V_Message::iterator end = messages.end();
+ for (; it != end; ++it)
+ {
+ MessagePtr& message = *it;
+
+ // If this message is about to push us past our queue size, erase the oldest message
+ if (message_count_ + 1 > queue_size_)
+ {
+ messages_.pop_front();
+ --message_count_;
+
+ //printf("Removed old message, count now %d\n", message_count_);
+ }
+
+ // Add the message to our list
+ messages_.push_back(message);
+ ++message_count_;
+
+ //printf("Added message, count now %d\n", message_count_);
+ }
+ }
+
+ /**
+ * \brief Entry point into the worker thread that does all our actual work, including calling the notification callback
+ */
+ void workerThread()
+ {
+ V_Message to_notify;
+ while (!destructing_)
+ {
+ V_Message local_queue;
+
+ {
+ //printf("workerThread: locking\n");
+ boost::mutex::scoped_lock lock(queue_mutex_);
+
+ // Wait for new data to be available
+ if (message_count_ == 0 && !new_transforms_ && !new_messages_ && !destructing_)
+ {
+ //printf("workerThread: waiting\n");
+ new_data_.wait(lock);
+ }
+
+ // If we're destructing, break out of the loop
+ if (destructing_)
+ {
+ break;
+ }
+
+ local_queue.swap(new_message_queue_);
+
+ new_messages_ = false;
+ }
+
+ processNewMessages(local_queue);
+
+ // Outside the lock, gather and notify that the messages are ready
+ gatherReadyMessages(to_notify);
+
+ new_transforms_ = false;
+
+ notify(to_notify);
+ to_notify.clear();
+ }
+ }
+
+ /**
+ * \brief Subscribe to the message topic
+ */
+ void subscribeToMessage()
+ {
+ if (!topic_.empty())
+ {
+ node_->subscribe(topic_, message_, &MessageNotifier::incomingMessage, this, queue_size_);
+ }
+ }
+
+ /**
+ * \brief Unsubscribe from the message topic
+ */
+ void unsubscribeFromMessage()
+ {
+ if (!topic_.empty())
+ {
+ node_->unsubscribe(topic_, &MessageNotifier::incomingMessage, this);
+ }
+ }
+
+ /**
+ * \class MessageDeleter
+ * \brief Since we allocate with our own special functions, we need to also delete using them. This provides a deletion interface for the boost::shared_ptr
+ */
+ class MessageDeleter
+ {
+ public:
+ void operator()(Message* m)
+ {
+ m->~Message();
+ notifierDeallocate(m);
+ }
+ };
+
+ /**
+ * \brief Callback that happens when we receive a message on the message topic
+ */
+ void incomingMessage()
+ {
+ //printf("incoming message\n");
+ // Allocate our new message and placement-new it
+ Message* mem = (Message*)notifierAllocate(sizeof(Message));
+ new(mem) Message();
+
+ // Create a boost::shared_ptr from the message, with our custom deleter
+ MessagePtr message(mem, MessageDeleter());
+ // Copy the message
+ *message = message_;
+
+ {
+ boost::mutex::scoped_lock lock(queue_mutex_);
+
+ new_message_queue_.push_back(message);
+
+ //printf("Added message, count now %d\n", message_count_);
+
+ new_messages_ = true;
+
+ // Notify the worker thread that there is new data available
+ new_data_.notify_all();
+ }
+ }
+
+ /**
+ * \brief Callback that happens when we receive a message on the TF message topic
+ */
+ void incomingTFMessage()
+ {
+ // Notify the worker thread that there is new data available
+ new_data_.notify_all();
+ new_transforms_ = true;
+ }
+
+ /**
+ * \brief Callback that happens when we receive a message on the old TF TransformArray message topic
+ */
+ void incomingOldTFMessage()
+ {
+ //printf("incoming transforms\n");
+ // Notify the worker thread that there is new data available
+ new_data_.notify_all();
+ new_transforms_ = true;
+ }
+
+ Transformer* tf_; ///< The Transformer used to determine if transformation data is available
+ ros::node* node_; ///< The node used to subscribe to the topic
+ Callback callback_; ///< The callback to call when a message is ready
+ std::string target_frame_; ///< The frame we need to be able to transform to before a message is ready
+ std::string topic_; ///< The topic to listen on
+ uint32_t queue_size_; ///< The maximum number of messages we queue up
+
+ L_Message messages_; ///< The message list
+ uint32_t message_count_; ///< The number of messages in the list. Used because messages_.size() has linear cost
+
+ Message message_; ///< The incoming message
+
+ tfMessage transforms_message_; ///< The incoming TF transforms message
+ rosTF::TransformArray old_transforms_message_; ///< The incoming old TF (rosTF) TransformArray message
+
+ bool destructing_; ///< Used to notify the worker thread that it needs to shutdown
+ boost::thread* thread_handle_; ///< Thread handle for the worker thread
+ boost::condition new_data_; ///< Condition variable used for waking the worker thread
+ bool new_messages_; ///< Used to skip waiting on new_data_ if new messages have come in while calling back
+ volatile bool new_transforms_; ///< Used to skip waiting on new_data_ if new transforms have come in while calling back or transforming data
+ V_Message new_message_queue_; ///< Queues messages to later be processed by the worker thread
+ boost::mutex queue_mutex_; ///< The mutex used for locking message queue operations
+
+ boost::mutex notify_mutex_; ///< Mutex used for locking the notification process
+};
+
+
+} // namespace tf
Modified: pkg/trunk/util/tf/manifest.xml
===================================================================
--- pkg/trunk/util/tf/manifest.xml 2008-11-11 23:15:04 UTC (rev 6551)
+++ pkg/trunk/util/tf/manifest.xml 2008-11-11 23:16:51 UTC (rev 6552)
@@ -16,6 +16,9 @@
<depend package="bullet"/>
<depend package="laser_scan_utils"/>
<depend package="rosTF"/> <!-- For backwards compatibility only Fixme remove /-->
+
+<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-dev"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-thread-dev"/>
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -ltf"/>
</export>
Added: pkg/trunk/util/tf/src/message_notifier.cpp
===================================================================
--- pkg/trunk/util/tf/src/message_notifier.cpp (rev 0)
+++ pkg/trunk/util/tf/src/message_notifier.cpp 2008-11-11 23:16:51 UTC (rev 6552)
@@ -0,0 +1,47 @@
+/*
+ * 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, Inc. 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 Josh Faust */
+
+#include "tf/message_notifier.h"
+
+namespace tf
+{
+
+void* notifierAllocate(uint32_t size)
+{
+ return malloc(size);
+}
+
+void notifierDeallocate(void* p)
+{
+ free(p);
+}
+
+} // namespace tf
Modified: pkg/trunk/util/tf/test/testBroadcaster.cpp
===================================================================
--- pkg/trunk/util/tf/test/testBroadcaster.cpp 2008-11-11 23:15:04 UTC (rev 6551)
+++ pkg/trunk/util/tf/test/testBroadcaster.cpp 2008-11-11 23:16:51 UTC (rev 6552)
@@ -1,10 +1,10 @@
/*
* 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
@@ -13,7 +13,7 @@
* * Neither the name of the Willow Garage, Inc. 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
@@ -73,7 +73,7 @@
//Construct/initialize the server
testBroadcaster myTestBroadcaster;
-
+
while(myTestBroadcaster.ok())
{
//Send some data
Added: pkg/trunk/util/tf/test/test_notifier.cpp
===================================================================
--- pkg/trunk/util/tf/test/test_notifier.cpp (rev 0)
+++ pkg/trunk/util/tf/test/test_notifier.cpp 2008-11-11 23:16:51 UTC (rev 6552)
@@ -0,0 +1,381 @@
+/*
+ * 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, Inc. 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 Josh Faust */
+
+#include <tf/message_notifier.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <std_msgs/PositionStamped.h>
+#include <boost/bind.hpp>
+
+#include <gtest/gtest.h>
+
+using namespace tf;
+
+ros::node* g_node = NULL;
+TransformListener* g_tf = NULL;
+TransformBroadcaster* g_broadcaster = NULL;
+
+class Notification
+{
+public:
+ Notification(int expected_count)
+ : count_(0)
+ , expected_count_(expected_count)
+ {
+ boost::detail::thread::lock_ops<boost::timed_mutex>::lock(mutex_);
+ }
+
+ ~Notification()
+ {
+ if (count_ < expected_count_)
+ {
+ boost::detail::thread::lock_ops<boost::timed_mutex>::unlock(mutex_);
+ }
+ }
+
+ void notify(const MessageNotifier<std_msgs::PositionStamped>::MessagePtr& message)
+ {
+ ++count_;
+
+ //printf("Notify: %d\n", count_);
+
+ if (count_ == expected_count_)
+ {
+ boost::detail::thread::lock_ops<boost::timed_mutex>::unlock(mutex_);
+ }
+ }
+
+ int count_;
+ int expected_count_;
+
+ boost::timed_mutex mutex_;
+};
+
+template<class T>
+class Counter
+{
+public:
+ Counter(const std::string& topic, int expected_count)
+ : count_(0)
+ , expected_count_(expected_count)
+ , topic_(topic)
+ {
+ boost::detail::thread::lock_ops<boost::timed_mutex>::lock(mutex_);
+
+ g_node->subscribe(topic_, message_, &Counter::callback, this, 0);
+ }
+
+ ~Counter()
+ {
+ g_node->unsubscribe(topic_, &Counter::callback, this);
+
+ if (count_ < expected_count_)
+ {
+ boost::detail::thread::lock_ops<boost::timed_mutex>::unlock(mutex_);
+ }
+ }
+
+ void callback()
+ {
+ ++count_;
+
+ //printf("Counter: %d\n", count_);
+
+ if (count_ == expected_count_)
+ {
+ boost::detail::thread::lock_ops<boost::timed_mutex>::unlock(mutex_);
+ }
+ }
+
+ T message_;
+
+ int count_;
+ int expected_count_;
+ std::string topic_;
+
+ boost::timed_mutex mutex_;
+};
+
+TEST(MessageNotifier, noTransforms)
+{
+ Notification n(1);
+ MessageNotifier<std_msgs::PositionStamped>* notifier = new MessageNotifier<std_msgs::PositionStamped>(g_tf, g_node, boost::bind(&Notification::notify, &n, _1), "test_message", "frame1", 1);
+ std::auto_ptr<MessageNotifier<std_msgs::PositionStamped> > notifier_ptr(notifier);
+
+ Counter<std_msgs::PositionStamped> c("test_message", 1);
+
+ ros::Duration(0.2).sleep();
+
+ std_msgs::PositionStamped msg;
+ msg.header.stamp = ros::Time::now();
+ msg.header.frame_id = "frame2";
+ g_node->publish("test_message", msg);
+
+ ros::Duration(0.2).sleep();
+
+ EXPECT_EQ(0, n.count_);
+}
+
+TEST(MessageNotifier, preexistingTransforms)
+{
+ Notification n(1);
+ Counter<rosTF::TransformArray> c("TransformArray", 1); /// \todo Switch this to tf_message once rosTF goes away completely
+ MessageNotifier<std_msgs::PositionStamped>* notifier = new MessageNotifier<std_msgs::PositionStamped>(g_tf, g_node, boost::bind(&Notification::notify, &n, _1), "test_message", "frame1", 1);
+ std::auto_ptr<MessageNotifier<std_msgs::PositionStamped> > notifier_ptr(notifier);
+
+ ros::Duration(0.2).sleep();
+
+ ros::Time stamp = ros::Time::now();
+
+ g_broadcaster->sendTransform(btTransform(btQuaternion(0,0,0), btVector3(1,2,3)), stamp, "frame1", "frame2");
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(c.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ std_msgs::PositionStamped msg;
+ msg.header.stamp = stamp;
+ msg.header.frame_id = "frame2";
+ g_node->publish("test_message", msg);
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(n.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ EXPECT_EQ(1, n.count_);
+}
+
+TEST(MessageNotifier, postTransforms)
+{
+ Notification n(1);
+ MessageNotifier<std_msgs::PositionStamped>* notifier = new MessageNotifier<std_msgs::PositionStamped>(g_tf, g_node, boost::bind(&Notification::notify, &n, _1), "test_message", "frame3", 1);
+ std::auto_ptr<MessageNotifier<std_msgs::PositionStamped> > notifier_ptr(notifier);
+
+ Counter<std_msgs::PositionStamped> c("test_message", 1);
+
+ ros::Duration(0.2).sleep();
+
+ ros::Time stamp = ros::Time::now();
+
+ std_msgs::PositionStamped msg;
+ msg.header.stamp = stamp;
+ msg.header.frame_id = "frame4";
+ g_node->publish("test_message", msg);
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(c.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ g_broadcaster->sendTransform(btTransform(btQuaternion(0,0,0), btVector3(1,2,3)), stamp, "frame3", "frame4");
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(n.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ EXPECT_EQ(1, n.count_);
+}
+
+TEST(MessageNotifier, queueSize)
+{
+ Notification n(10);
+ MessageNotifier<std_msgs::PositionStamped>* notifier = new MessageNotifier<std_msgs::PositionStamped>(g_tf, g_node, boost::bind(&Notification::notify, &n, _1), "test_message", "frame5", 10);
+ std::auto_ptr<MessageNotifier<std_msgs::PositionStamped> > notifier_ptr(notifier);
+
+ Counter<std_msgs::PositionStamped> c("test_message", 20);
+
+ ros::Duration(0.2).sleep();
+
+ ros::Time stamp = ros::Time::now();
+
+ for (int i = 0; i < 20; ++i)
+ {
+ std_msgs::PositionStamped msg;
+ msg.header.stamp = stamp;
+ msg.header.frame_id = "frame6";
+ g_node->publish("test_message", msg);
+
+ ros::Duration(0.01).sleep();
+ }
+
+ {
+ boost::xtime xt;
+ boost::xtime_get(&xt, boost::TIME_UTC);
+ xt.sec += 10;
+
+ boost::timed_mutex::scoped_timed_lock lock(c.mutex_, xt);
+
+ EXPECT_EQ(true, lock.locked());
+ }
+
+ g_broadcaster->sendTransform(btTransform(btQuaternion(0,0,0), btVector3(1,2,3)), s...
[truncated message content] |
|
From: <jl...@us...> - 2008-11-12 01:29:41
|
Revision: 6564
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6564&view=rev
Author: jleibs
Date: 2008-11-12 01:29:38 +0000 (Wed, 12 Nov 2008)
Log Message:
-----------
TopicSynchronizer allows for optional timeout specification and timeout callback. Also callback
takes a ros::time argument to allow for intelligent determination of which messages are actually
synchronized.
Modified Paths:
--------------
pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
pkg/trunk/vision/stereo_view/stereo_view.cpp
Modified: pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
===================================================================
--- pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-12 01:26:40 UTC (rev 6563)
+++ pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-12 01:29:38 UTC (rev 6564)
@@ -77,6 +77,8 @@
} else if (rosimg.depth == "uint16") {
cvInitImageHeader(img_, cvSize(rosimg.uint16_data.layout.dim[1].size, rosimg.uint16_data.layout.dim[0].size), IPL_DEPTH_16U, 1);
cvSetData(img_, &(rosimg.uint16_data.data[0]), rosimg.uint16_data.layout.dim[1].stride*sizeof(uint16_t));
+ } else {
+ return false;
}
return true;
Modified: pkg/trunk/util/topic_synchronizer/topic_synchronizer.h
===================================================================
--- pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2008-11-12 01:26:40 UTC (rev 6563)
+++ pkg/trunk/util/topic_synchronizer/topic_synchronizer.h 2008-11-12 01:29:38 UTC (rev 6564)
@@ -40,6 +40,7 @@
#define TOPIC_SYNCHRONIZER_HH
#include "rosthread/condition.h"
+#include "ros/time.h"
//! A templated class for synchronizing incoming topics
/*!
@@ -55,8 +56,14 @@
//! A pointer to your node for calling callback
N* node_;
- //! The callback to be called
- void (N::*callback_)();
+ //! The callback to be called if successful
+ void (N::*callback_)(ros::Time);
+
+ //! Timeout duration
+ ros::Duration timeout_;
+
+ //! The callback to be called if timed out
+ void (N::*timeout_callback_)(ros::Time);
//! The condition variable used for synchronization
ros::thread::condition cond_all_;
@@ -122,6 +129,9 @@
}
//! The function called in a message cb to wait for other messages
+ /*!
+ * \param time The time that is being waited for
+ */
void wait_for_others(ros::Time* time)
{
count_ = 1;
@@ -131,14 +141,15 @@
bool timed_out = false;
while (count_ < expected_count_ && *time == waiting_time_ && !timed_out)
- if (!cond_all_.timed_wait(1))
+ if (!cond_all_.timed_wait(timeout_))
{
- printf(" Timed out waiting for other images...\n");
timed_out = true;
+ if (timeout_callback_)
+ (*node_.*timeout_callback_)(*time);
}
if (*time == waiting_time_ && !timed_out)
- (*node_.*callback_)();
+ (*node_.*callback_)(*time);
if (*time == waiting_time_)
{
@@ -155,10 +166,12 @@
/*!
* The constructor for the TopicSynchronizer
*
- * \param node A pointer to your node.
- * \param callback A pointer to the callback to invoke when all messages have arrived
+ * \param node A pointer to your node.
+ * \param callback A pointer to the callback to invoke when all messages have arrived
+ * \param timeout The duration
+ * \param timeout_callback A callback which is triggered when the timeout expires
*/
- TopicSynchronizer(N* node, void (N::*callback)()) : node_(node), callback_(callback), expected_count_(0), count_(0), done_(false)
+ TopicSynchronizer(N* node, void (N::*callback)(ros::Time), ros::Duration timeout = ros::Duration(1.0), void (N::*timeout_callback)(ros::Time) = NULL) : node_(node), callback_(callback), timeout_(timeout), timeout_callback_(timeout_callback), expected_count_(0), count_(0), done_(false)
{ }
//! Subscribe
Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-12 01:26:40 UTC (rev 6563)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-12 01:29:38 UTC (rev 6564)
@@ -61,7 +61,7 @@
TopicSynchronizer<StereoView> sync;
- StereoView() : ros::node("cv_view"), sync(this, &StereoView::image_cb_all)
+ StereoView() : ros::node("cv_view"), sync(this, &StereoView::image_cb_all, ros::Duration(0.05), &StereoView::image_cb_timeout)
{
cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
@@ -72,23 +72,39 @@
sync.subscribe("dcam/disparity", dimage, 1);
}
- void image_cb_all()
+ void image_cb_all(ros::Time t)
{
- lbridge.fromImage(limage);
- rbridge.fromImage(rimage);
- dbridge.fromImage(dimage);
+ if (lbridge.fromImage(limage))
+ cvShowImage("left", lbridge.toIpl());
- // Disparity has to be scaled to be be nicely displayable
- IplImage* disp = cvCreateImage(cvGetSize(dbridge.toIpl()), IPL_DEPTH_8U, 1);
- cvCvtScale(dbridge.toIpl(), disp, 1/4.0);
+ if (rbridge.fromImage(rimage))
+ cvShowImage("right", rbridge.toIpl());
- cvShowImage("left", lbridge.toIpl());
- cvShowImage("right", rbridge.toIpl());
- cvShowImage("disparity", disp);
+ if (dbridge.fromImage(dimage))
+ {
+ // Disparity has to be scaled to be be nicely displayable
+ IplImage* disp = cvCreateImage(cvGetSize(dbridge.toIpl()), IPL_DEPTH_8U, 1);
+ cvCvtScale(dbridge.toIpl(), disp, 1/4.0);
+ cvShowImage("disparity", disp);
+ cvReleaseImage(&disp);
+ }
cvWaitKey(5);
+ }
- cvReleaseImage(&disp);
+ void image_cb_timeout(ros::Time t)
+ {
+ if (limage.header.stamp != t)
+ printf("Timed out waiting for left image\n");
+
+ if (rimage.header.stamp != t)
+ printf("Timed out waiting for right image\n");
+
+ if (dimage.header.stamp != t)
+ printf("Timed out waiting for disparity image\n");
+
+ //Proceed to show images anyways
+ image_cb_all(t);
}
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-11-12 21:00:28
|
Revision: 6603
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6603&view=rev
Author: jleibs
Date: 2008-11-12 21:00:22 +0000 (Wed, 12 Nov 2008)
Log Message:
-----------
Fixes to dcam_node and CvBridge to support color pipeline and automated rgb 2 bgr conversion.
Modified Paths:
--------------
pkg/trunk/drivers/cam/dcam_node/CMakeLists.txt
pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp
pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
pkg/trunk/vision/stereo_view/stereo_view.cpp
Modified: pkg/trunk/drivers/cam/dcam_node/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/cam/dcam_node/CMakeLists.txt 2008-11-12 20:28:21 UTC (rev 6602)
+++ pkg/trunk/drivers/cam/dcam_node/CMakeLists.txt 2008-11-12 21:00:22 UTC (rev 6603)
@@ -1,4 +1,6 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
+set(ROS_BUILD_TYPE Release)
rospack(dcam_node)
+
rospack_add_executable(dcam_node dcam_node.cpp)
Modified: pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp 2008-11-12 20:28:21 UTC (rev 6602)
+++ pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp 2008-11-12 21:00:22 UTC (rev 6603)
@@ -126,7 +126,7 @@
string str_mode;
dc1394video_mode_t mode;
- videre_proc_mode_t videre_mode;
+ videre_proc_mode_t videre_mode = PROC_MODE_NONE;
stereo_cam_ = false;
param("~video_mode", str_mode, string("640x480_mono8"));
@@ -317,10 +317,10 @@
cam_info_.has_image = false;
}
- if (img_data->imColorType != COLOR_CODING_NONE)
+ if (img_data->imColorType != COLOR_CODING_NONE && img_data->imColorType == COLOR_CODING_RGB8)
{
fillImage(img_, "image_color",
- img_data->imHeight, img_data->imWidth, 4,
+ img_data->imHeight, img_data->imWidth, 3,
"rgba", "byte",
img_data->imColor );
@@ -344,9 +344,22 @@
cam_info_.has_image_rect = false;
}
- if (img_data->imRectColorType != COLOR_CODING_NONE)
+ if (img_data->imRectColorType != COLOR_CODING_NONE && img_data->imRectColorType == COLOR_CODING_RGB8)
{
fillImage(img_, "image_rect_color",
+ img_data->imHeight, img_data->imWidth, 3,
+ "rgb", "byte",
+ img_data->imRectColor );
+ img_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
+ publish(base_name + std::string("image_rect_color"), img_);
+ cam_info_.has_image_rect_color = true;
+ } else {
+ cam_info_.has_image_rect_color = false;
+ }
+
+ if (img_data->imRectColorType != COLOR_CODING_NONE && img_data->imRectColorType == COLOR_CODING_RGBA8)
+ {
+ fillImage(img_, "image_rect_color",
img_data->imHeight, img_data->imWidth, 4,
"rgba", "byte",
img_data->imRectColor );
Modified: pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
===================================================================
--- pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-12 20:28:21 UTC (rev 6602)
+++ pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-12 21:00:22 UTC (rev 6603)
@@ -45,21 +45,41 @@
class CvBridge
{
IplImage* img_;
- bool owns_data_;
+ IplImage* rosimg_;
+ IplImage* cvtimg_;
+ void reallocCvtIfNeeded(CvSize sz, int depth, int channels)
+ {
+ if (cvtimg_ != 0)
+ if (cvtimg_->width != sz.width ||
+ cvtimg_->height != sz.height ||
+ cvtimg_->depth != depth ||
+ cvtimg_->nChannels != channels)
+ {
+ cvReleaseImage(&cvtimg_);
+ cvtimg_ = 0;
+ }
+
+ if (cvtimg_ == 0)
+ {
+ cvtimg_ = cvCreateImage(sz, depth, channels);
+ }
+ }
+
public:
- CvBridge() : img_(0), owns_data_(false)
+ CvBridge() : img_(0), rosimg_(0), cvtimg_(0)
{
- img_ = cvCreateImageHeader( cvSize(0,0), IPL_DEPTH_8U, 1 );
+ rosimg_ = cvCreateImageHeader( cvSize(0,0), IPL_DEPTH_8U, 1 );
}
~CvBridge()
{
- if (owns_data_)
- cvReleaseData(&img_);
+ if (rosimg_)
+ cvReleaseImageHeader(&rosimg_);
- cvReleaseImageHeader(&img_);
+ if (cvtimg_)
+ cvReleaseImage(&cvtimg_);
}
@@ -68,21 +88,35 @@
return img_;
}
- bool fromImage(Image& rosimg)
+ bool fromImage(Image& rosimg, std::string encoding = "")
{
if (rosimg.depth == "byte")
{
- cvInitImageHeader(img_, cvSize(rosimg.byte_data.layout.dim[1].size, rosimg.byte_data.layout.dim[0].size), IPL_DEPTH_8U, 1);
- cvSetData(img_, &(rosimg.byte_data.data[0]), rosimg.byte_data.layout.dim[1].stride);
+ cvInitImageHeader(rosimg_, cvSize(rosimg.byte_data.layout.dim[1].size, rosimg.byte_data.layout.dim[0].size),
+ IPL_DEPTH_8U, rosimg.byte_data.layout.dim[2].size);
+ cvSetData(rosimg_, &(rosimg.byte_data.data[0]), rosimg.byte_data.layout.dim[1].stride);
+ img_ = rosimg_;
} else if (rosimg.depth == "uint16") {
- cvInitImageHeader(img_, cvSize(rosimg.uint16_data.layout.dim[1].size, rosimg.uint16_data.layout.dim[0].size), IPL_DEPTH_16U, 1);
- cvSetData(img_, &(rosimg.uint16_data.data[0]), rosimg.uint16_data.layout.dim[1].stride*sizeof(uint16_t));
+ cvInitImageHeader(rosimg_, cvSize(rosimg.uint16_data.layout.dim[1].size, rosimg.uint16_data.layout.dim[0].size),
+ IPL_DEPTH_16U, rosimg.uint16_data.layout.dim[2].size);
+ cvSetData(rosimg_, &(rosimg.uint16_data.data[0]), rosimg.uint16_data.layout.dim[1].stride*sizeof(uint16_t));
+ img_ = rosimg_;
} else {
return false;
}
-
+
+ if (encoding != "" && (encoding != rosimg.encoding))
+ {
+ if (encoding == "bgr" && rosimg.encoding == "rgb")
+ {
+ reallocCvtIfNeeded(cvGetSize(rosimg_), IPL_DEPTH_8U, 3);
+ cvCvtColor(rosimg_, cvtimg_, CV_RGB2BGR);
+ img_ = cvtimg_;
+ } else {
+ return false;
+ }
+ }
return true;
-
}
private:
Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-12 20:28:21 UTC (rev 6602)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2008-11-12 21:00:22 UTC (rev 6603)
@@ -67,17 +67,17 @@
cvNamedWindow("right", CV_WINDOW_AUTOSIZE);
cvNamedWindow("disparity", CV_WINDOW_AUTOSIZE);
- sync.subscribe("dcam/left/image_rect", limage, 1);
- sync.subscribe("dcam/right/image_rect", rimage, 1);
+ sync.subscribe("dcam/left/image_rect_color", limage, 1);
+ sync.subscribe("dcam/right/image_rect_color", rimage, 1);
sync.subscribe("dcam/disparity", dimage, 1);
}
void image_cb_all(ros::Time t)
{
- if (lbridge.fromImage(limage))
+ if (lbridge.fromImage(limage, "bgr"))
cvShowImage("left", lbridge.toIpl());
- if (rbridge.fromImage(rimage))
+ if (rbridge.fromImage(rimage, "bgr"))
cvShowImage("right", rbridge.toIpl());
if (dbridge.fromImage(dimage))
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2008-11-12 22:56:57
|
Revision: 6614
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6614&view=rev
Author: rdiankov
Date: 2008-11-12 22:56:51 +0000 (Wed, 12 Nov 2008)
Log Message:
-----------
added conversion utility to go from urdf to openrave files (openrave package also added), switched conversion of stl media files over to gazebo_robot_description in order to avoid ogre dependency
Modified Paths:
--------------
pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/wg_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/wg_robot_description/manifest.xml
Added Paths:
-----------
pkg/trunk/3rdparty/openrave/
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/3rdparty/openrave/manifest.xml
pkg/trunk/robot_descriptions/openrave_robot_description/
pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/openrave_robot_description/Makefile
pkg/trunk/robot_descriptions/openrave_robot_description/models/
pkg/trunk/robot_descriptions/openrave_robot_description/robots/
pkg/trunk/robot_descriptions/openrave_robot_description/src/
pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
Added: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile (rev 0)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-11-12 22:56:51 UTC (rev 6614)
@@ -0,0 +1,18 @@
+all: openrave
+
+SVN_DIR = openrave_svn
+SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
+include $(shell rospack find mk)/svn_checkout.mk
+
+.PHONY: openrave
+
+build: SVN_UP openrave
+
+openrave: $(SVN_DIR)
+ cd $(SVN_DIR) && svn up && make prefix=$(PWD) && make install
+
+clean:
+ make -C $(SVN_DIR) clean
+
+wipe: clean
+ rm -rf $(SVN_DIR) include lib share
Added: pkg/trunk/3rdparty/openrave/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/openrave/manifest.xml (rev 0)
+++ pkg/trunk/3rdparty/openrave/manifest.xml 2008-11-12 22:56:51 UTC (rev 6614)
@@ -0,0 +1,10 @@
+<package>
+ <description brief="OpenRAVE">
+ A planning architecture for autonomous robotics.
+ </description>
+ <license>LGPL</license>
+ <export>
+ <cpp lflags="-Wl,-rpath,${prefix}/lib `${prefix}/bin/openrave-config --libs`" cflags="`${prefix}/bin/openrave-config --cflags`"/>
+ </export>
+ <versioncontrol type="svn" url="https://openrave.svn.sourceforge.net/svnroot/openrave"/>
+</package>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt 2008-11-12 22:39:24 UTC (rev 6613)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt 2008-11-12 22:56:51 UTC (rev 6614)
@@ -8,7 +8,41 @@
find_ros_package(gazebo_robot_description)
get_target_property(urdf2gazebo_exe urdf2gazebo LOCATION)
-message(${urdf2gazebo_exe})
-message(${wg_robot_description_PACKAGE_PATH}/pr2/pr2.xml)
-message(${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml.model)
+# build the ogre mesh files
+file(GLOB pr2_stl_files ${wg_robot_description_PACKAGE_PATH}/models/pr2/*.stl)
+set(pr2_gen_files "")
+set(pr2_out_path ${CMAKE_CURRENT_SOURCE_DIR}/world/Media/models/pr2)
+
+foreach(it ${pr2_stl_files})
+ get_filename_component(basename ${it} NAME_WE)
+
+ # convert to ogre files
+ add_custom_command(
+ OUTPUT ${pr2_out_path}/${basename}.mesh
+ COMMAND rosrun
+ ARGS ogre_tools stl_to_mesh ${it} ${pr2_out_path}/${basename}.mesh
+ DEPENDS ${it})
+
+ set(pr2_gen_files ${pr2_gen_files} ${pr2_out_path}/${basename}.mesh)
+endforeach(it)
+
+add_custom_target(media_files ALL DEPENDS ${pr2_gen_files})
+
+# process all urdf files
+set(PR2_OUTPUT ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml.model)
+set(PR2_OUTPUT_NOLIMIT ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_nolimit.model)
+set(PR2_INPUT ${wg_robot_description_PACKAGE_PATH}/pr2/pr2.xml)
+add_custom_command(
+ OUTPUT ${PR2_OUTPUT}
+ COMMAND ${urdf2gazebo_exe}
+ ARGS ${PR2_INPUT} ${PR2_OUTPUT}
+ DEPENDS ${PR2_INPUT})
+add_custom_command(
+ OUTPUT ${PR2_OUTPUT_NOLIMIT}
+ COMMAND ${urdf2gazebo_exe}
+ ARGS ${PR2_INPUT} ${PR2_OUTPUT_NOLIMIT} 1
+ DEPENDS ${PR2_INPUT})
+
+add_custom_target(pr2_urdf ALL DEPENDS ${PR2_OUTPUT} ${PR2_OUTPUT_NOLIMIT})
+add_dependencies(pr2_urdf urdf2gazebo)
Added: pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt (rev 0)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt 2008-11-12 22:56:51 UTC (rev 6614)
@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(openrave_robot_description)
+rospack_add_executable(urdf2openrave src/urdf2openrave.cpp)
+
+# find needed paths
+find_ros_package(wg_robot_description)
+find_ros_package(openrave_robot_description)
+get_target_property(urdf2openrave_exe urdf2openrave LOCATION)
+
+# process all urdf files
+set(PR2_OUTPUT ${openrave_robot_description_PACKAGE_PATH}/robots/pr2.robot.xml)
+set(PR2_INPUT ${wg_robot_description_PACKAGE_PATH}/pr2/pr2.xml)
+add_custom_command(
+ OUTPUT ${PR2_OUTPUT}
+ COMMAND ${urdf2openrave_exe}
+ ARGS ${PR2_INPUT} ${PR2_OUTPUT} ../models
+ DEPENDS ${PR2_INPUT} urdf2openrave)
+
+add_custom_target(pr2_urdf ALL DEPENDS ${PR2_OUTPUT})
+add_dependencies(pr2_urdf urdf2openrave)
Added: pkg/trunk/robot_descriptions/openrave_robot_description/Makefile
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/Makefile (rev 0)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/Makefile 2008-11-12 22:56:51 UTC (rev 6614)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp (rev 0)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2008-11-12 22:56:51 UTC (rev 6614)
@@ -0,0 +1,391 @@
+/*********************************************************************
+ * 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 <cstdio>
+#include <cstdlib>
+#include <cmath>
+
+#include <vector>
+#include <string>
+#include <sstream>
+#include <fstream>
+#include <list>
+
+#include <urdf/URDF.h>
+#include <libTF/Pose3D.h>
+
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <errno.h>
+
+using namespace std;
+static list<string> s_listResourceNames;
+
+string values2str(unsigned int count, const double *values, double (*conv)(double) = NULL)
+{
+ stringstream ss;
+ for (unsigned int i = 0 ; i < count ; i++) {
+ if (i > 0)
+ ss << " ";
+ ss << (conv ? conv(values[i]) : values[i]);
+ }
+ return ss.str();
+}
+
+double rad2deg(double v)
+{
+ return v * 180.0 / M_PI;
+}
+
+void setupTransform(libTF::Pose3D &transform, const double *xyz, const double *rpy)
+{
+ transform.setFromEuler(xyz[0], xyz[1], xyz[2], rpy[2], rpy[1], rpy[0]);
+}
+
+void addKeyValue(TiXmlElement *elem, const string& key, const string &value)
+{
+ TiXmlElement *ekey = new TiXmlElement(key);
+ TiXmlText *text_ekey = new TiXmlText(value);
+ ekey->LinkEndChild(text_ekey);
+ elem->LinkEndChild(ekey);
+}
+
+void addTransform(TiXmlElement *elem, const::libTF::Pose3D& transform)
+{
+ libTF::Position pz;
+ transform.getPosition(pz);
+ double cpos[3] = { pz.x, pz.y, pz.z };
+
+ libTF::Quaternion q;
+ transform.getQuaternion(q);
+ double cquat[4] = { q.w, q.x, q.y, q.z };
+
+ // set geometry transform
+ addKeyValue(elem, "translation", values2str(3, cpos));
+ addKeyValue(elem, "quaternion", values2str(4, cquat));
+}
+
+void copyOpenraveMap(const robot_desc::URDF::Map& data, TiXmlElement *elem, const vector<string> *tags = NULL)
+{
+ vector<string> openrave_names;
+ data.getMapTagNames("openrave", openrave_names);
+ for (unsigned int k = 0 ; k < openrave_names.size() ; ++k) {
+ map<string, string> m = data.getMapTagValues("openrave", openrave_names[k]);
+ vector<string> accepted_tags;
+ if (tags)
+ accepted_tags = *tags;
+ else
+ for (map<string, 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]]);
+
+ map<string, const TiXmlElement*> x = data.getMapTagXML("openrave", openrave_names[k]);
+ for (map<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());
+ }
+ }
+}
+
+// ignore all collision data for the time being
+void convertLink(TiXmlElement *root, robot_desc::URDF::Link *link, const libTF::Pose3D &transform, bool enforce_limits)
+{
+ libTF::Pose3D currentTransform = transform;
+
+ TiXmlElement *body = new TiXmlElement("body");
+ body->SetAttribute("name", link->name);
+
+ // compute global transform
+ libTF::Pose3D localTransform;
+ setupTransform(localTransform, link->xyz, link->rpy);
+ currentTransform.multiplyPose(localTransform);
+ addTransform(body, currentTransform);
+
+ // begin create geometry node
+ TiXmlElement *geom = new TiXmlElement("geom");
+
+ string type;
+
+ switch(link->visual->geometry->type) {
+ case robot_desc::URDF::Link::Geometry::MESH: {
+ robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(link->visual->geometry->shape);
+
+ // Trim Both leading and trailing spaces
+ size_t startpos = mesh->filename.find_first_not_of(" \t");
+ size_t endpos = mesh->filename.find_last_not_of(" \t");
+
+ if(( string::npos == startpos ) || ( string::npos == endpos))
+ mesh->filename = "";
+ else
+ mesh->filename = mesh->filename.substr( startpos, endpos-startpos+1 );
+
+ if( mesh->filename.empty() ) {
+ cerr << "mesh file is empty for link " << link->name << ", adding box" << endl;
+ type = "box";
+ double extents[3] = {0.01,0.01,0.01};
+ addKeyValue(geom, "extents", values2str(3,extents));
+ }
+ else {
+ type = "trimesh";
+ stringstream ss;
+
+ s_listResourceNames.push_back(mesh->filename + "_hi.iv");
+ s_listResourceNames.push_back(mesh->filename + "_low.iv");
+
+ ss << mesh->filename << "_hi.iv " << mesh->scale[0];
+ addKeyValue(geom, "render", ss.str());
+
+ ss.str("");
+ ss << mesh->filename << "_low.iv " << mesh->scale[0];
+ addKeyValue(geom, "data", ss.str());
+ }
+ break;
+ }
+ case robot_desc::URDF::Link::Geometry::BOX: {
+ type = "box";
+ robot_desc::URDF::Link::Geometry::Box* box = static_cast<robot_desc::URDF::Link::Geometry::Box*>(link->visual->geometry->shape);
+ addKeyValue(geom, "extents", values2str(3,box->size));
+ break;
+ }
+ case robot_desc::URDF::Link::Geometry::CYLINDER: {
+ type = "cylinder";
+ robot_desc::URDF::Link::Geometry::Cylinder* cylinder = static_cast<robot_desc::URDF::Link::Geometry::Cylinder*>(link->visual->geometry->shape);
+ addKeyValue(geom,"radius",values2str(1,&cylinder->radius));
+ addKeyValue(geom,"height",values2str(1,&cylinder->length));
+ break;
+ }
+ case robot_desc::URDF::Link::Geometry::SPHERE: {
+ type = "sphere";
+ double radius = static_cast<robot_desc::URDF::Link::Geometry::Sphere*>(link->visual->geometry->shape)->radius;
+ addKeyValue(geom,"radius",values2str(1,&radius));
+ break;
+ }
+ default: {
+ fprintf(stderr,"Unknown body type: %d in geometry '%s'\n", link->visual->geometry->type, link->visual->geometry->name.c_str());
+ exit(-1);
+ }
+ }
+
+ geom->SetAttribute("type", type);
+
+ // compute the visualisation transfrom
+ // libTF::Pose3D coll;
+ // setupTransform(coll, link->collision->xyz, link->collision->rpy);
+ // coll.invert();
+
+ libTF::Pose3D vis;
+ setupTransform(vis, link->visual->xyz, link->visual->rpy);
+ // coll.multiplyPose(vis);
+
+ addTransform(geom, vis);
+
+ copyOpenraveMap(link->visual->data, geom);
+ body->LinkEndChild(geom); // end geom
+
+ // mass
+ TiXmlElement* mass = new TiXmlElement("mass");
+ mass->SetAttribute("type","custom");
+ addKeyValue(mass, "total", values2str(1,&link->inertial->mass));
+
+ double* inertia = link->inertial->inertia;
+ double Im[9] = {inertia[0],inertia[1],inertia[2],inertia[1],inertia[3],inertia[4],inertia[2],inertia[4],inertia[5]};
+ addKeyValue(mass, "inertia", values2str(9, Im));
+ addKeyValue(mass, "com", values2str(3, link->inertial->com));
+ body->LinkEndChild(mass);
+
+ copyOpenraveMap(link->data, body);
+ root->LinkEndChild(body);
+
+ /* compute the joint tag */
+ bool fixed = false;
+ string jtype;
+ switch (link->joint->type) {
+ case robot_desc::URDF::Link::Joint::REVOLUTE:
+ jtype = "hinge";
+ break;
+ case robot_desc::URDF::Link::Joint::PRISMATIC:
+ jtype = "slider";
+ break;
+ case robot_desc::URDF::Link::Joint::FLOATING:
+ case robot_desc::URDF::Link::Joint::PLANAR:
+ // sometimes used to attach the base
+ break;
+ case robot_desc::URDF::Link::Joint::FIXED:
+ jtype = "hinge";
+ fixed = true;
+ break;
+ default:
+ fprintf(stderr, "Unknown joint type: %d in link '%s'\n", link->joint->type, link->name.c_str());
+ break;
+ }
+
+ if (!jtype.empty()) {
+ TiXmlElement *joint = new TiXmlElement("joint");
+ joint->SetAttribute("type", jtype);
+ joint->SetAttribute("name", link->joint->name);
+
+ addKeyValue(joint, "body", link->name);
+ addKeyValue(joint, "body", link->parentName);
+ addKeyValue(joint, "offsetfrom", link->name);
+
+ if (fixed) {
+ addKeyValue(joint, "lowStop", "0");
+ addKeyValue(joint, "highStop", "0");
+ addKeyValue(joint, "axis", "1 0 0");
+ }
+ else {
+ addKeyValue(joint, "axis", values2str(3, link->joint->axis));
+ addKeyValue(joint, "anchor", values2str(3, link->joint->anchor));
+
+ if (enforce_limits && link->joint->isSet["limit"]) {
+ if (jtype == "slider") {
+ addKeyValue(joint, "lostop", values2str(1, link->joint->limit ));
+ addKeyValue(joint, "histop", values2str(1, link->joint->limit + 1 ));
+ }
+ else {
+ addKeyValue(joint, "lostop", values2str(1, link->joint->limit , rad2deg));
+ addKeyValue(joint, "histop", values2str(1, link->joint->limit + 1, rad2deg));
+ }
+ }
+ }
+
+ root->LinkEndChild(joint);
+ }
+
+
+ for (unsigned int i = 0 ; i < link->children.size() ; ++i)
+ convertLink(root, link->children[i], currentTransform, enforce_limits);
+}
+
+void usage(const char *progname)
+{
+ printf("\nUsage: %s URDF.xml OpenRAVE.robot.xml\n", progname);
+ printf(" where URDF.xml is the file containing a robot description in the Willow Garage format (URDF)\n");
+ printf(" and OpenRAVE.robot.xml is the file where the OpenRAVE robot model should be written\n\n");
+}
+
+int main(int argc, char **argv)
+{
+ if (argc < 3) {
+ usage(argv[0]);
+ exit(1);
+ }
+
+ bool enforce_limits = true;
+ robot_desc::URDF wgxml;
+
+ if (!wgxml.loadFile(argv[1])) {
+ fprintf(stderr,"Unable to load robot model from %s\n", argv[1]);
+ exit(2);
+ }
+
+ TiXmlDocument doc;
+
+ // extract name only
+ string robotname = argv[1];
+ size_t pos = robotname.find_last_of('/');
+ if( pos == string::npos )
+ pos = robotname.find_last_of('\\');
+ if( pos != string::npos )
+ robotname = robotname.substr(pos+1);
+ pos = robotname.find_first_of('.');
+ if( pos != string::npos )
+ robotname = robotname.substr(0,pos);
+
+ string outresdir = argv[2];
+ pos = outresdir.find_last_of('/');
+ if( pos == string::npos )
+ pos = outresdir.find_last_of('\\');
+ if( pos != string::npos )
+ outresdir = outresdir.substr(0,pos);
+
+ TiXmlDeclaration *decl = new TiXmlDeclaration("1.0", "", "");
+ doc.LinkEndChild(decl);
+
+ // create root element and define needed namespaces
+ TiXmlElement *robot = new TiXmlElement("Robot");
+ // Create a node to enclose the robot body
+ cout << "creating robot: " << robotname << endl;
+ robot->SetAttribute("name", robotname.c_str());
+
+ TiXmlElement *kinbody = new TiXmlElement("KinBody");
+ if( argc >= 4 ) {
+ if( outresdir.size() > 0 )
+ outresdir += "/";
+ outresdir += argv[3];
+ addKeyValue(kinbody,"modelsdir",argv[3]);
+ }
+
+ outresdir += "/" + robotname;
+
+ // set the transform for the whole model to identity
+ libTF::Pose3D transform;
+ for (unsigned int k = 0 ; k < wgxml.getDisjointPartCount() ; ++k)
+ convertLink(kinbody, wgxml.getDisjointPart(k), transform, enforce_limits);
+
+ /* find all data item types */
+ copyOpenraveMap(wgxml.getMap(), kinbody);
+ robot->LinkEndChild(kinbody);
+ doc.LinkEndChild(robot);
+
+ if (!doc.SaveFile(argv[2])) {
+ fprintf(stderr,"Unable to save gazebo model in %s\n", argv[2]);
+ exit(3);
+ }
+
+ string inresdir = wgxml.getResourceLocation();
+ if( inresdir.size() > 0 )
+ inresdir += "/";
+ if( outresdir.size() > 0 )
+ outresdir += "/";
+
+ int ret = mkdir(outresdir.c_str(), 0777);
+
+ if( ret != 0 && errno != EEXIST ) {
+ cerr << "failed to create resource directory " << outresdir << endl;
+ exit(-2);
+ }
+
+ cout << "copying resource files from " << inresdir << " to " << outresdir << endl;
+ for(list<string>::iterator it = s_listResourceNames.begin(); it != s_listResourceNames.end(); ++it) {
+ ifstream ifile((inresdir+*it).c_str(), ios_base::binary);
+ ofstream ofile((outresdir+*it).c_str(), ios_base::binary);
+ ofile << ifile.rdbuf();
+ }
+
+ return 0;
+}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/CMakeLists.txt 2008-11-12 22:39:24 UTC (rev 6613)
+++ pkg/trunk/robot_descriptions/wg_robot_description/CMakeLists.txt 2008-11-12 22:56:51 UTC (rev 6614)
@@ -23,15 +23,8 @@
ARGS ${it} ${basepath}/${basename}.iv
DEPENDS ${it} ivcon)
-#ogre files
-add_custom_command(
- OUTPUT ${basepath}/${basename}.mesh
- COMMAND rosrun
- ARGS ogre_tools stl_to_mesh ${it} ${basepath}/${basename}.mesh
- DEPENDS ${it})
+set(pr2_gen_files ${pr2_gen_files} ${basepath}/${basename}.iv)
-set(pr2_gen_files ${pr2_gen_files} ${basepath}/${basename}.iv ${basepath}/${basename}.mesh)
-
endforeach(it)
add_custom_target(media_files ALL DEPENDS ${pr2_gen_files})
Modified: pkg/trunk/robot_descriptions/wg_robot_description/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/manifest.xml 2008-11-12 22:39:24 UTC (rev 6613)
+++ pkg/trunk/robot_descriptions/wg_robot_description/manifest.xml 2008-11-12 22:56:51 UTC (rev 6614)
@@ -10,8 +10,6 @@
<license>BSD</license>
<depend package="wg_robot_description_parser" />
-<!-- <depend package="ogre_tools" />-->
-
<url>http://pr.willowgarage.com/wiki/RobotDescriptionFormat</url>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-11-12 23:18:02
|
Revision: 6615
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6615&view=rev
Author: isucan
Date: 2008-11-12 23:17:57 +0000 (Wed, 12 Nov 2008)
Log Message:
-----------
moved the code for dealing with resources to a separate package
Modified Paths:
--------------
pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml
pkg/trunk/robot_descriptions/wg_robot_description_parser/src/URDF.cpp
pkg/trunk/util/string_utils/include/string_utils/string_utils.h
Added Paths:
-----------
pkg/trunk/util/resource_locator/
pkg/trunk/util/resource_locator/.build-version
pkg/trunk/util/resource_locator/CMakeLists.txt
pkg/trunk/util/resource_locator/Makefile
pkg/trunk/util/resource_locator/include/
pkg/trunk/util/resource_locator/include/reslocator/
pkg/trunk/util/resource_locator/include/reslocator/reslocator.h
pkg/trunk/util/resource_locator/manifest.xml
pkg/trunk/util/resource_locator/src/
pkg/trunk/util/resource_locator/src/reslocator.cpp
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml 2008-11-12 22:56:51 UTC (rev 6614)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml 2008-11-12 23:17:57 UTC (rev 6615)
@@ -14,8 +14,6 @@
<depend package="string_utils"/>
<depend package="stl_utils" />
- <sysdepend os="ubuntu" version="8.04-hardy" package="libboost-regex-dev"/>
-
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lURDF"/>
</export>
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/src/URDF.cpp
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/src/URDF.cpp 2008-11-12 22:56:51 UTC (rev 6614)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/src/URDF.cpp 2008-11-12 23:17:57 UTC (rev 6615)
@@ -42,8 +42,8 @@
#include <fstream>
#include <sstream>
#include <queue>
-#include <boost/regex.hpp>
+
namespace robot_desc {
/* Macro to mark the fact a certain member variable was set. Also
@@ -122,40 +122,7 @@
std::string URDF::getResourceLocation(void) const
{
- // check if URL is valid
- boost::cmatch matches;
- boost::regex re("(ros-pkg|ros-param):\\/\\/((\\w+\\.)*(\\w*))\\/([\\w\\d]+\\/{0,1})+");
- if (boost::regex_match(m_resourceLocation.c_str(), matches, re) && matches.size() >= 3) {
- std::string protocol(matches[1].first, matches[1].second);
- std::string protocol_path(matches[2].first, matches[2].second);
- std::string relpath(matches[2].second,matches[matches.size()-1].second);
-
- if( protocol == std::string("ros-pkg") ) {
- // find the ROS package
- FILE* f = popen((std::string("rospack find ") + protocol_path).c_str(),"r");
- if( f == NULL )
- errorMessage(std::string("failed to launch rospack find ") + protocol_path);
- else {
- char basepath[1024];
- fgets(basepath, sizeof(basepath), f);
- pclose(f);
-
- // strip out any new lines or spaces from the end
- int len = strlen(basepath);
- char* p = basepath+len-1;
- while(len-- > 0 && (*p == ' ' || *p == '\n' || *p == '\t' || *p == '\r'))
- *p-- = 0;
- return std::string(basepath) + relpath;
- }
- }
- else if( protocol == std::string("ros-param") ) {
- errorMessage("ros-param option for the media path is not supported");
- }
- }
- else // not a url so copy directly
- return m_resourceLocation;
-
- return "";
+ return m_resourceLocation;
}
URDF::Link* URDF::getLink(const std::string &name) const
Added: pkg/trunk/util/resource_locator/.build-version
===================================================================
--- pkg/trunk/util/resource_locator/.build-version (rev 0)
+++ pkg/trunk/util/resource_locator/.build-version 2008-11-12 23:17:57 UTC (rev 6615)
@@ -0,0 +1 @@
+:
Added: pkg/trunk/util/resource_locator/CMakeLists.txt
===================================================================
--- pkg/trunk/util/resource_locator/CMakeLists.txt (rev 0)
+++ pkg/trunk/util/resource_locator/CMakeLists.txt 2008-11-12 23:17:57 UTC (rev 6615)
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(resource_locator)
+rospack_add_library(resource_locator src/reslocator.cpp)
Added: pkg/trunk/util/resource_locator/Makefile
===================================================================
--- pkg/trunk/util/resource_locator/Makefile (rev 0)
+++ pkg/trunk/util/resource_locator/Makefile 2008-11-12 23:17:57 UTC (rev 6615)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/util/resource_locator/include/reslocator/reslocator.h
===================================================================
--- pkg/trunk/util/resource_locator/include/reslocator/reslocator.h (rev 0)
+++ pkg/trunk/util/resource_locator/include/reslocator/reslocator.h 2008-11-12 23:17:57 UTC (rev 6615)
@@ -0,0 +1,52 @@
+/*********************************************************************
+ * 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 Rosen Diankov, Ioan Sucan */
+
+#ifndef ROS_RESOURCE_LOCATOR_
+#define ROS_RESOURCE_LOCATOR_
+
+#include <string>
+
+namespace res_locator
+{
+
+ /** Attempt to compute the path equivalent to a resource. This is
+ * only valid for some types of resources. The function returns
+ * an empty string if unsuccesful */
+ std::string resource2path(const std::string &resource);
+
+}
+
+#endif
Added: pkg/trunk/util/resource_locator/manifest.xml
===================================================================
--- pkg/trunk/util/resource_locator/manifest.xml (rev 0)
+++ pkg/trunk/util/resource_locator/manifest.xml 2008-11-12 23:17:57 UTC (rev 6615)
@@ -0,0 +1,19 @@
+<package>
+ <description brief="Resource Locator">
+
+ This package provides a simple library that allows locating
+ resources within ROS.
+
+ </description>
+ <author>Rosen Diankov, Ioan Sucan </author>
+ <license>BSD</license>
+
+ <depend package="rosconsole" />
+
+ <export>
+ <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lresource_locator"/>
+ </export>
+
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libboost-regex-dev"/>
+
+</package>
Added: pkg/trunk/util/resource_locator/src/reslocator.cpp
===================================================================
--- pkg/trunk/util/resource_locator/src/reslocator.cpp (rev 0)
+++ pkg/trunk/util/resource_locator/src/reslocator.cpp 2008-11-12 23:17:57 UTC (rev 6615)
@@ -0,0 +1,80 @@
+/*********************************************************************
+ * 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 Rosen Diankov, Ioan Sucan */
+
+#include <reslocator/reslocator.h>
+#include <rosconsole/rosconsole.h>
+#include <boost/regex.hpp>
+
+
+std::string res_locator::resource2path(const std::string &resource)
+{
+ // check if URL is valid
+ boost::cmatch matches;
+ boost::regex re("(ros-pkg|ros-param):\\/\\/((\\w+\\.)*(\\w*))\\/([\\w\\d]+\\/{0,1})+");
+
+ if (boost::regex_match(resource.c_str(), matches, re) && matches.size() >= 3){
+ std::string protocol(matches[1].first, matches[1].second);
+ std::string protocol_path(matches[2].first, matches[2].second);
+ std::string relpath(matches[2].second,matches[matches.size()-1].second);
+
+ if( protocol == std::string("ros-pkg") ) {
+ // find the ROS package
+ FILE* f = popen((std::string("rospack find ") + protocol_path).c_str(),"r");
+ if( f == NULL )
+ ROS_ERROR("%s\n", (std::string("failed to launch rospack find ") + protocol_path).c_str());
+ else {
+ char basepath[1024];
+ fgets(basepath, sizeof(basepath), f);
+ pclose(f);
+
+ // strip out any new lines or spaces from the end
+ int len = strlen(basepath);
+ char* p = basepath+len-1;
+ while(len-- > 0 && (*p == ' ' || *p == '\n' || *p == '\t' || *p == '\r'))
+ *p-- = 0;
+ return std::string(basepath) + relpath;
+ }
+ }
+ else if( protocol == std::string("ros-param") )
+ {
+ return "";
+ }
+ }
+ else // not a url so copy directly
+ return resource;
+
+ return "";
+}
Modified: pkg/trunk/util/string_utils/include/string_utils/string_utils.h
===================================================================
--- pkg/trunk/util/string_utils/include/string_utils/string_utils.h 2008-11-12 22:56:51 UTC (rev 6614)
+++ pkg/trunk/util/string_utils/include/string_utils/string_utils.h 2008-11-12 23:17:57 UTC (rev 6615)
@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/** \Author Ioan Sucan */
+/** \author Ioan Sucan */
#ifndef STRING_UTILS_STRING_UTILS_
#define STRING_UTILS_STRING_UTILS_
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2008-11-13 00:27:32
|
Revision: 6631
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6631&view=rev
Author: rdiankov
Date: 2008-11-13 00:27:26 +0000 (Thu, 13 Nov 2008)
Log Message:
-----------
fixed boost-regex dependency so at least it does not error anymore, added necessary packages for openrave to compile correctly
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/3rdparty/openrave/manifest.xml
pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt
pkg/trunk/util/resource_locator/CMakeLists.txt
pkg/trunk/util/resource_locator/manifest.xml
Added Paths:
-----------
pkg/trunk/3rdparty/soqt/
pkg/trunk/3rdparty/soqt/Makefile
pkg/trunk/3rdparty/soqt/manifest.xml
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-11-13 00:27:25 UTC (rev 6630)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-11-13 00:27:26 UTC (rev 6631)
@@ -9,7 +9,7 @@
build: SVN_UP openrave
openrave: $(SVN_DIR)
- cd $(SVN_DIR) && svn up && make prefix=$(PWD) && make install
+ @cd $(SVN_DIR) && PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && svn up && make prefix=$(PWD) && make install
clean:
make -C $(SVN_DIR) clean
Modified: pkg/trunk/3rdparty/openrave/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/openrave/manifest.xml 2008-11-13 00:27:25 UTC (rev 6630)
+++ pkg/trunk/3rdparty/openrave/manifest.xml 2008-11-13 00:27:26 UTC (rev 6631)
@@ -6,5 +6,7 @@
<export>
<cpp lflags="-Wl,-rpath,${prefix}/lib `${prefix}/bin/openrave-config --libs`" cflags="`${prefix}/bin/openrave-config --cflags`"/>
</export>
+ <depend package="soqt"/>
+ <depend package="opende"/>
<versioncontrol type="svn" url="https://openrave.svn.sourceforge.net/svnroot/openrave"/>
</package>
Added: pkg/trunk/3rdparty/soqt/Makefile
===================================================================
--- pkg/trunk/3rdparty/soqt/Makefile (rev 0)
+++ pkg/trunk/3rdparty/soqt/Makefile 2008-11-13 00:27:26 UTC (rev 6631)
@@ -0,0 +1,27 @@
+all: soqt
+
+TARBALL = SoQt-1.4.1.tar.gz
+TARBALL_URL = http://ftp.coin3d.org/coin/src/all/SoQt-1.4.1.tar.gz
+SOURCE_DIR = SoQt-1.4.1
+#MD5SUM_FILE = opencv-1.0.0.tar.gz.md5sum
+UNPACK_CMD = tar xzf
+include $(shell rospack find mk)/download_unpack.mk
+
+build: soqt
+
+configured:
+ cd $(SOURCE_DIR) && ./configure --prefix=$(PWD)
+ touch configured
+
+soqt: configured
+ cd $(SOURCE_DIR) && make && make install
+
+clean:
+ rm -rf $(SOURCE_DIR)
+ rm -rf configured
+
+wipe: clean
+ rm -rf $(TARBALL)
+
+$(TARBALL):
+ wget $(TARBALL_URL)
Added: pkg/trunk/3rdparty/soqt/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/soqt/manifest.xml (rev 0)
+++ pkg/trunk/3rdparty/soqt/manifest.xml 2008-11-13 00:27:26 UTC (rev 6631)
@@ -0,0 +1,17 @@
+<package>
+ <description brief="SoQt">
+ This package contains a Coin3D/Qt GUI interface.
+ </description>
+ <author>See web page for a full credits llist.</author>
+ <license url="http://www.coin3d.org/licensing/index_html#free-edition-gpl">GPL</license>
+ <url>http://www.coin3d.org/</url>
+ <export>
+ <cpp lflags="-Wl,-rpath,${prefix}/lib `${prefix}/bin/soqt-config --libs`" cflags="`${prefix}/bin/soqt-config --cppflags`"/>
+ <doxymaker external="http://developer.nvidia.com/page/documentation.html"/>
+ </export>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libcoin40-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="qt4-dev-tools"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libqt4-dev"/>
+ <sysdepend os="ubuntu" version="7.04-feisty" package="wget"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="wget"/>
+</package>
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt 2008-11-13 00:27:25 UTC (rev 6630)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt 2008-11-13 00:27:26 UTC (rev 6631)
@@ -8,6 +8,15 @@
find_ros_package(openrave_robot_description)
get_target_property(urdf2openrave_exe urdf2openrave LOCATION)
+find_package( Boost COMPONENTS regex )
+if( NOT ${Boost_regex_FOUND} )
+ message(SEND_ERROR "could not find boost-regex")
+endif()
+
+include_directories(${Boost_INCLUDE_DIRS})
+link_directories(${Boost_LIBRARY_DIRS})
+target_link_libraries(urdf2openrave ${Boost_LIBRARIES})
+
# process all urdf files
set(PR2_OUTPUT ${openrave_robot_description_PACKAGE_PATH}/robots/pr2.robot.xml)
set(PR2_INPUT ${wg_robot_description_PACKAGE_PATH}/pr2/pr2.xml)
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt 2008-11-13 00:27:25 UTC (rev 6630)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt 2008-11-13 00:27:26 UTC (rev 6631)
@@ -6,15 +6,6 @@
rospack_add_library(URDF src/URDF.cpp src/parser.cpp)
rospack_add_compile_flags(URDF -Wextra)
-find_package( Boost COMPONENTS regex )
-if( NOT ${Boost_regex_FOUND} )
- message(SEND_ERROR "could not find boost-regex")
-endif()
-
-include_directories(${Boost_INCLUDE_DIRS})
-link_directories(${Boost_LIBRARY_DIRS})
-target_link_libraries(URDF ${Boost_LIBRARIES})
-
rospack_add_executable(parse src/parse.cpp)
target_link_libraries(parse URDF)
Modified: pkg/trunk/util/resource_locator/CMakeLists.txt
===================================================================
--- pkg/trunk/util/resource_locator/CMakeLists.txt 2008-11-13 00:27:25 UTC (rev 6630)
+++ pkg/trunk/util/resource_locator/CMakeLists.txt 2008-11-13 00:27:26 UTC (rev 6631)
@@ -2,3 +2,13 @@
include(rosbuild)
rospack(resource_locator)
rospack_add_library(resource_locator src/reslocator.cpp)
+set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
+
+find_package( Boost COMPONENTS regex )
+if( NOT ${Boost_regex_FOUND} )
+ message(SEND_ERROR "could not find boost-regex")
+endif()
+
+include_directories(${Boost_INCLUDE_DIRS})
+link_directories(${Boost_LIBRARY_DIRS})
+target_link_libraries(resource_locator ${Boost_LIBRARIES})
Modified: pkg/trunk/util/resource_locator/manifest.xml
===================================================================
--- pkg/trunk/util/resource_locator/manifest.xml 2008-11-13 00:27:25 UTC (rev 6630)
+++ pkg/trunk/util/resource_locator/manifest.xml 2008-11-13 00:27:26 UTC (rev 6631)
@@ -14,6 +14,7 @@
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lresource_locator"/>
</export>
+ <sysdepend os="ubuntu" version="7.04-gutsy" package="libboost-regex-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libboost-regex-dev"/>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <pof...@us...> - 2008-11-13 00:39:24
|
Revision: 6634
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6634&view=rev
Author: poftwaresatent
Date: 2008-11-13 00:39:14 +0000 (Thu, 13 Nov 2008)
Log Message:
-----------
fixed svn co/up Makefile rules for libsunflower and nepumuk
Modified Paths:
--------------
pkg/trunk/3rdparty/libsunflower/Makefile
pkg/trunk/simulators/nepumuk/Makefile
Removed Paths:
-------------
pkg/trunk/3rdparty/libsunflower/ROS_BUILD_BLACKLIST
pkg/trunk/simulators/nepumuk/ROS_BUILD_BLACKLIST
Modified: pkg/trunk/3rdparty/libsunflower/Makefile
===================================================================
--- pkg/trunk/3rdparty/libsunflower/Makefile 2008-11-13 00:34:40 UTC (rev 6633)
+++ pkg/trunk/3rdparty/libsunflower/Makefile 2008-11-13 00:39:14 UTC (rev 6634)
@@ -43,7 +43,7 @@
$(MAKE) SOURCE_DIR=$(SVN_DIR) really-all
checkout_or_update:
- test -d $(SVN_DIR) && svn up $(SVN_DIR)
+ - test -d $(SVN_DIR) && svn up $(SVN_DIR)
test -d $(SVN_DIR) || svn co -r$(SVN_REV) $(SVN_URL) $(SVN_DIR)
.PHONY: tarball-all
Deleted: pkg/trunk/3rdparty/libsunflower/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/3rdparty/libsunflower/ROS_BUILD_BLACKLIST 2008-11-13 00:34:40 UTC (rev 6633)
+++ pkg/trunk/3rdparty/libsunflower/ROS_BUILD_BLACKLIST 2008-11-13 00:39:14 UTC (rev 6634)
@@ -1 +0,0 @@
-issues with sn checkout/update commands in nepumuk/libsunflower Makefiles
Modified: pkg/trunk/simulators/nepumuk/Makefile
===================================================================
--- pkg/trunk/simulators/nepumuk/Makefile 2008-11-13 00:34:40 UTC (rev 6633)
+++ pkg/trunk/simulators/nepumuk/Makefile 2008-11-13 00:39:14 UTC (rev 6634)
@@ -57,7 +57,7 @@
$(MAKE) symlinks
checkout_or_update:
- test -d $(SVN_DIR) && svn up $(SVN_DIR)
+ - test -d $(SVN_DIR) && svn up $(SVN_DIR)
test -d $(SVN_DIR) || svn co -r$(SVN_REV) $(SVN_URL) $(SVN_DIR)
symlinks:
Deleted: pkg/trunk/simulators/nepumuk/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/simulators/nepumuk/ROS_BUILD_BLACKLIST 2008-11-13 00:34:40 UTC (rev 6633)
+++ pkg/trunk/simulators/nepumuk/ROS_BUILD_BLACKLIST 2008-11-13 00:39:14 UTC (rev 6634)
@@ -1 +0,0 @@
-issues with sn checkout/update commands in nepumuk/libsunflower Makefiles
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2008-11-13 03:56:47
|
Revision: 6656
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6656&view=rev
Author: rdiankov
Date: 2008-11-13 03:56:36 +0000 (Thu, 13 Nov 2008)
Log Message:
-----------
fixed some urdf2openrave bugs, gripper kinematics now perfect, changed dependencies around
Modified Paths:
--------------
pkg/trunk/robot_descriptions/gazebo_robot_description/manifest.xml
pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
pkg/trunk/util/resource_locator/manifest.xml
pkg/trunk/util/resource_locator/src/reslocator.cpp
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/manifest.xml 2008-11-13 03:03:24 UTC (rev 6655)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/manifest.xml 2008-11-13 03:56:36 UTC (rev 6656)
@@ -7,5 +7,5 @@
<depend package="wg_robot_description"/>
<depend package="wg_robot_description_parser"/>
<depend package="libTF"/>
-
+ <depend package="ogre_tools"/>
</package>
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt 2008-11-13 03:03:24 UTC (rev 6655)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt 2008-11-13 03:56:36 UTC (rev 6656)
@@ -8,15 +8,6 @@
find_ros_package(openrave_robot_description)
get_target_property(urdf2openrave_exe urdf2openrave LOCATION)
-find_package( Boost COMPONENTS regex )
-if( NOT ${Boost_regex_FOUND} )
- message(SEND_ERROR "could not find boost-regex")
-endif()
-
-include_directories(${Boost_INCLUDE_DIRS})
-link_directories(${Boost_LIBRARY_DIRS})
-target_link_libraries(urdf2openrave ${Boost_LIBRARIES})
-
# process all urdf files
set(PR2_OUTPUT ${openrave_robot_description_PACKAGE_PATH}/robots/pr2.robot.xml)
set(PR2_INPUT ${wg_robot_description_PACKAGE_PATH}/pr2/pr2.xml)
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2008-11-13 03:03:24 UTC (rev 6655)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2008-11-13 03:56:36 UTC (rev 6656)
@@ -64,11 +64,6 @@
return ss.str();
}
-double rad2deg(double v)
-{
- return v * 180.0 / M_PI;
-}
-
void setupTransform(libTF::Pose3D &transform, const double *xyz, const double *rpy)
{
transform.setFromEuler(xyz[0], xyz[1], xyz[2], rpy[2], rpy[1], rpy[0]);
@@ -259,6 +254,13 @@
joint->SetAttribute("type", jtype);
joint->SetAttribute("name", link->joint->name);
+ if( link->joint->pjointMimic != NULL ) {
+ stringstream ss;
+ ss << link->joint->pjointMimic->name << " " << link->joint->fMimicMult << " " << link->joint->fMimicOffset;
+ joint->SetAttribute("mimic",ss.str());
+ joint->SetAttribute("enable","false");
+ }
+
addKeyValue(joint, "body", link->name);
addKeyValue(joint, "body", link->parentName);
addKeyValue(joint, "offsetfrom", link->name);
@@ -272,18 +274,18 @@
addKeyValue(joint, "axis", values2str(3, link->joint->axis));
addKeyValue(joint, "anchor", values2str(3, link->joint->anchor));
- if (enforce_limits && link->joint->isSet["limit"]) {
+ if (link->joint->pjointMimic == NULL && enforce_limits && link->joint->isSet["limit"]) {
if (jtype == "slider") {
addKeyValue(joint, "lostop", values2str(1, link->joint->limit ));
addKeyValue(joint, "histop", values2str(1, link->joint->limit + 1 ));
}
else {
- addKeyValue(joint, "lostop", values2str(1, link->joint->limit , rad2deg));
- addKeyValue(joint, "histop", values2str(1, link->joint->limit + 1, rad2deg));
+ addKeyValue(joint, "lostop", values2str(1, link->joint->limit));
+ addKeyValue(joint, "histop", values2str(1, link->joint->limit + 1));
}
}
}
-
+
root->LinkEndChild(joint);
}
Modified: pkg/trunk/util/resource_locator/manifest.xml
===================================================================
--- pkg/trunk/util/resource_locator/manifest.xml 2008-11-13 03:03:24 UTC (rev 6655)
+++ pkg/trunk/util/resource_locator/manifest.xml 2008-11-13 03:56:36 UTC (rev 6656)
@@ -14,7 +14,7 @@
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lresource_locator"/>
</export>
- <sysdepend os="ubuntu" version="7.04-gutsy" package="libboost-regex-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="libboost-regex-dev"/>
-
+ <sysdepend os="ubuntu" version="7.10-gutsy" package="libboost-regex-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libboost-regex-dev"/>
+ <sysdepend os="ubuntu" version="8.10-intrepid" package="libboost-regex-dev"/>
</package>
Modified: pkg/trunk/util/resource_locator/src/reslocator.cpp
===================================================================
--- pkg/trunk/util/resource_locator/src/reslocator.cpp 2008-11-13 03:03:24 UTC (rev 6655)
+++ pkg/trunk/util/resource_locator/src/reslocator.cpp 2008-11-13 03:56:36 UTC (rev 6656)
@@ -46,35 +46,35 @@
boost::regex re("(ros-pkg|ros-param):\\/\\/((\\w+\\.)*(\\w*))\\/([\\w\\d]+\\/{0,1})+");
if (boost::regex_match(resource.c_str(), matches, re) && matches.size() >= 3){
- std::string protocol(matches[1].first, matches[1].second);
- std::string protocol_path(matches[2].first, matches[2].second);
- std::string relpath(matches[2].second,matches[matches.size()-1].second);
+ std::string protocol(matches[1].first, matches[1].second);
+ std::string protocol_path(matches[2].first, matches[2].second);
+ std::string relpath(matches[2].second,matches[matches.size()-1].second);
- if( protocol == std::string("ros-pkg") ) {
- // find the ROS package
- FILE* f = popen((std::string("rospack find ") + protocol_path).c_str(),"r");
- if( f == NULL )
- ROS_ERROR("%s\n", (std::string("failed to launch rospack find ") + protocol_path).c_str());
- else {
- char basepath[1024];
- fgets(basepath, sizeof(basepath), f);
- pclose(f);
+ if( protocol == std::string("ros-pkg") ) {
+ // find the ROS package
+ FILE* f = popen((std::string("rospack find ") + protocol_path).c_str(),"r");
+ if( f == NULL )
+ ROS_ERROR("%s\n", (std::string("failed to launch rospack find ") + protocol_path).c_str());
+ else {
+ char basepath[1024];
+ fgets(basepath, sizeof(basepath), f);
+ pclose(f);
- // strip out any new lines or spaces from the end
- int len = strlen(basepath);
- char* p = basepath+len-1;
- while(len-- > 0 && (*p == ' ' || *p == '\n' || *p == '\t' || *p == '\r'))
- *p-- = 0;
- return std::string(basepath) + relpath;
- }
- }
- else if( protocol == std::string("ros-param") )
- {
- return "";
- }
+ // strip out any new lines or spaces from the end
+ int len = strlen(basepath);
+ char* p = basepath+len-1;
+ while(len-- > 0 && (*p == ' ' || *p == '\n' || *p == '\t' || *p == '\r'))
+ *p-- = 0;
+ return std::string(basepath) + relpath;
+ }
+ }
+ else if( protocol == std::string("ros-param") )
+ {
+ return "";
+ }
}
else // not a url so copy directly
- return resource;
+ return resource;
return "";
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ehb...@us...> - 2008-11-13 06:22:14
|
Revision: 6667
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6667&view=rev
Author: ehberger
Date: 2008-11-13 06:22:07 +0000 (Thu, 13 Nov 2008)
Log Message:
-----------
Moving package review status from wiki to manifests
Modified Paths:
--------------
pkg/trunk/3rdparty/Cg/manifest.xml
pkg/trunk/3rdparty/bfl_latest/manifest.xml
pkg/trunk/3rdparty/boost/manifest.xml
pkg/trunk/3rdparty/bullet/manifest.xml
pkg/trunk/3rdparty/eml/manifest.xml
pkg/trunk/3rdparty/estar/manifest.xml
pkg/trunk/3rdparty/fast_detector/manifest.xml
pkg/trunk/3rdparty/ffmpeg/manifest.xml
pkg/trunk/3rdparty/freeimage/manifest.xml
pkg/trunk/3rdparty/gazebo/manifest.xml
pkg/trunk/3rdparty/glc/manifest.xml
pkg/trunk/3rdparty/gmapping/manifest.xml
pkg/trunk/3rdparty/kdl/manifest.xml
pkg/trunk/3rdparty/kni-3.9.2/manifest.xml
pkg/trunk/3rdparty/libbk_maxflow/manifest.xml
pkg/trunk/3rdparty/libdc1394v2/manifest.xml
pkg/trunk/3rdparty/libfz_ht_superpixels/manifest.xml
pkg/trunk/3rdparty/libsunflower/manifest.xml
pkg/trunk/3rdparty/loki/manifest.xml
pkg/trunk/3rdparty/newmat10/manifest.xml
pkg/trunk/3rdparty/ogre/manifest.xml
pkg/trunk/3rdparty/opencv/manifest.xml
pkg/trunk/3rdparty/opencv_latest/manifest.xml
pkg/trunk/3rdparty/opende/manifest.xml
pkg/trunk/3rdparty/openrave/manifest.xml
pkg/trunk/3rdparty/player/manifest.xml
pkg/trunk/3rdparty/plplot/manifest.xml
pkg/trunk/3rdparty/rtnet/manifest.xml
pkg/trunk/3rdparty/scipy/manifest.xml
pkg/trunk/3rdparty/sicktoolbox/manifest.xml
pkg/trunk/3rdparty/soqt/manifest.xml
pkg/trunk/3rdparty/spacenav/manifest.xml
pkg/trunk/3rdparty/stage/manifest.xml
pkg/trunk/3rdparty/svl-1.0/manifest.xml
pkg/trunk/3rdparty/trex/manifest.xml
pkg/trunk/3rdparty/velodyne-driver/manifest.xml
pkg/trunk/3rdparty/vop/manifest.xml
pkg/trunk/3rdparty/wxPython_swig_interface/manifest.xml
pkg/trunk/3rdparty/wxmpl/manifest.xml
pkg/trunk/3rdparty/wxpropgrid/manifest.xml
pkg/trunk/3rdparty/wxswig/manifest.xml
pkg/trunk/3rdparty/xenomai/manifest.xml
pkg/trunk/audio/audio_capture/manifest.xml
pkg/trunk/calibration/kinematic_calibration/manifest.xml
pkg/trunk/controllers/control_toolbox/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/qualification_controllers/gripper_qualification_controllers/manifest.xml
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/manifest.xml
pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/curltest/manifest.xml
pkg/trunk/demos/2dnav_erratic/manifest.xml
pkg/trunk/demos/2dnav_gazebo/manifest.xml
pkg/trunk/demos/2dnav_pr2/manifest.xml
pkg/trunk/demos/2dnav_stage/manifest.xml
pkg/trunk/demos/arm_gazebo/manifest.xml
pkg/trunk/demos/examples_gazebo/manifest.xml
pkg/trunk/demos/pr2_gazebo/manifest.xml
pkg/trunk/demos/pr2_prototype1_gazebo/manifest.xml
pkg/trunk/demos/stair1-demos/manifest.xml
pkg/trunk/deprecated/display_ode_spaces/manifest.xml
pkg/trunk/deprecated/drawstuff/manifest.xml
pkg/trunk/deprecated/etherdrive_old/manifest.xml
pkg/trunk/deprecated/genericControllers/manifest.xml
pkg/trunk/deprecated/hw_interface/manifest.xml
pkg/trunk/deprecated/libKDL/manifest.xml
pkg/trunk/deprecated/old_mechanism_control/manifest.xml
pkg/trunk/deprecated/pr2Controllers/manifest.xml
pkg/trunk/deprecated/pr2Core/manifest.xml
pkg/trunk/deprecated/robot_mechanism_model/manifest.xml
pkg/trunk/deprecated/test_collision_space/manifest.xml
pkg/trunk/drivers/cam/axis_cam/manifest.xml
pkg/trunk/drivers/cam/bumblebee_bridge/manifest.xml
pkg/trunk/drivers/cam/cv_cam/manifest.xml
pkg/trunk/drivers/cam/dc1394_cam/manifest.xml
pkg/trunk/drivers/cam/dc1394_cam_server/manifest.xml
pkg/trunk/drivers/cam/dcam/manifest.xml
pkg/trunk/drivers/cam/dcam_node/manifest.xml
pkg/trunk/drivers/cam/elphel_cam/manifest.xml
pkg/trunk/drivers/cam/flea2/manifest.xml
pkg/trunk/drivers/cam/videre_cam/manifest.xml
pkg/trunk/drivers/generic/serial_port/manifest.xml
pkg/trunk/drivers/imu/3dmgx2_driver/manifest.xml
pkg/trunk/drivers/imu/imu_node/manifest.xml
pkg/trunk/drivers/input/joy/manifest.xml
pkg/trunk/drivers/input/joy_node/manifest.xml
pkg/trunk/drivers/input/joy_view/manifest.xml
pkg/trunk/drivers/laser/hokuyo_driver/manifest.xml
pkg/trunk/drivers/laser/hokuyo_node/manifest.xml
pkg/trunk/drivers/laser/laser_view/manifest.xml
pkg/trunk/drivers/laser/scan_intensity_filter/manifest.xml
pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml
pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
pkg/trunk/drivers/motor/etherdrive/manifest.xml
pkg/trunk/drivers/motor/etherdrive_hardware/manifest.xml
pkg/trunk/drivers/network/wifi_ddwrt/manifest.xml
pkg/trunk/drivers/phase_space/manifest.xml
pkg/trunk/drivers/robot/erratic_player/manifest.xml
pkg/trunk/drivers/robot/ipdcmot/manifest.xml
pkg/trunk/drivers/robot/irobot_create/manifest.xml
pkg/trunk/drivers/robot/katana/manifest.xml
pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/manifest.xml
pkg/trunk/drivers/robot/pr2/bread_board/manifest.xml
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml
pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml
pkg/trunk/drivers/robot/pr2/pr2_power_board/manifest.xml
pkg/trunk/drivers/robot/pr2/tilting_laser/manifest.xml
pkg/trunk/drivers/robot/segway_apox/manifest.xml
pkg/trunk/drivers/robot/sensor_cart/manifest.xml
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/grasp_module/grasp_learner/manifest.xml
pkg/trunk/grasp_module/grasp_module/manifest.xml
pkg/trunk/grasp_module/grasp_module_node/manifest.xml
pkg/trunk/grasp_module/grasp_planner/manifest.xml
pkg/trunk/grasp_module/object_detector/manifest.xml
pkg/trunk/hardware_test/diagnostic_updater/manifest.xml
pkg/trunk/hardware_test/qualification/manifest.xml
pkg/trunk/hardware_test/runtime_monitor/manifest.xml
pkg/trunk/hardware_test/self_test/manifest.xml
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml
pkg/trunk/highlevel/highlevel_controllers/manifest.xml
pkg/trunk/highlevel/rubot/manifest.xml
pkg/trunk/image_msgs/manifest.xml
pkg/trunk/manip/arm_trajectory/manifest.xml
pkg/trunk/manip/spacenav_node/manifest.xml
pkg/trunk/manip/teleop_arm_keyboard/manifest.xml
pkg/trunk/manip/teleop_robot/manifest.xml
pkg/trunk/manip/teleop_spacenav/manifest.xml
pkg/trunk/mechanism/hardware_interface/manifest.xml
pkg/trunk/mechanism/mechanism_bringup/manifest.xml
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/mechanism/mechanism_control_rt/manifest.xml
pkg/trunk/mechanism/mechanism_model/manifest.xml
pkg/trunk/motion_planning/collision_space/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/mp_benchmarks/manifest.xml
pkg/trunk/motion_planning/ompl/manifest.xml
pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml
pkg/trunk/motion_planning/planning_models/manifest.xml
pkg/trunk/motion_planning/planning_node_util/manifest.xml
pkg/trunk/motion_planning/planning_research/dynamic_planning/manifest.xml
pkg/trunk/motion_planning/sbpl/manifest.xml
pkg/trunk/motion_planning/sbpl_util/manifest.xml
pkg/trunk/nav/amcl_player/manifest.xml
pkg/trunk/nav/deadreckon/manifest.xml
pkg/trunk/nav/fake_localization/manifest.xml
pkg/trunk/nav/map_generator/manifest.xml
pkg/trunk/nav/nav_view/manifest.xml
pkg/trunk/nav/nav_view_sdl/manifest.xml
pkg/trunk/nav/sbpl_2dnav/manifest.xml
pkg/trunk/nav/slam_gmapping/manifest.xml
pkg/trunk/nav/teleop_base/manifest.xml
pkg/trunk/nav/teleop_base_keyboard/manifest.xml
pkg/trunk/nav/wavefront_player/manifest.xml
pkg/trunk/pr2_msgs/manifest.xml
pkg/trunk/pr2_srvs/manifest.xml
pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml
pkg/trunk/robot_control_loops/pr2_etherDrive/manifest.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/manifest.xml
pkg/trunk/robot_descriptions/openrave_robot_description/manifest.xml
pkg/trunk/robot_descriptions/pr2_alpha/manifest.xml
pkg/trunk/robot_descriptions/robo_dev_demo/manifest.xml
pkg/trunk/robot_descriptions/wg_robot_description/manifest.xml
pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml
pkg/trunk/robot_msgs/manifest.xml
pkg/trunk/robot_srvs/manifest.xml
pkg/trunk/sandbox/axis_cam2/manifest.xml
pkg/trunk/simulators/flatland/manifest.xml
pkg/trunk/simulators/gazebo_map_extruder/manifest.xml
pkg/trunk/simulators/nepumuk/manifest.xml
pkg/trunk/simulators/rosstage/manifest.xml
pkg/trunk/simulators/simulator_integration_tests/manifest.xml
pkg/trunk/trajectory_rollout/manifest.xml
pkg/trunk/vision/borg2_tuner/manifest.xml
pkg/trunk/vision/borg_laser_extractor/manifest.xml
pkg/trunk/vision/borg_tuner/manifest.xml
pkg/trunk/vision/calib_converter/manifest.xml
pkg/trunk/vision/calonder_descriptor/manifest.xml
pkg/trunk/vision/cam_viewer/manifest.xml
pkg/trunk/vision/cam_viewer_py/manifest.xml
pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/manifest.xml
pkg/trunk/vision/color_calib/manifest.xml
pkg/trunk/vision/cv_blob_tracker/manifest.xml
pkg/trunk/vision/cv_cam_calib/manifest.xml
pkg/trunk/vision/cv_movie_streamer/manifest.xml
pkg/trunk/vision/cv_view/manifest.xml
pkg/trunk/vision/dorylus/manifest.xml
pkg/trunk/vision/gmmseg/manifest.xml
pkg/trunk/vision/laser_interface/manifest.xml
pkg/trunk/vision/lasiklite/manifest.xml
pkg/trunk/vision/mrf_image_classifier/manifest.xml
pkg/trunk/vision/scan_utils/manifest.xml
pkg/trunk/vision/spectacles/manifest.xml
pkg/trunk/vision/star_detector/manifest.xml
pkg/trunk/vision/stereo_blob_tracker/manifest.xml
pkg/trunk/vision/stereo_calib/manifest.xml
pkg/trunk/vision/stereo_utils/manifest.xml
pkg/trunk/vision/stereo_view/manifest.xml
pkg/trunk/vision/urg_ipdcmot/manifest.xml
pkg/trunk/vision/videre_face_detection/manifest.xml
pkg/trunk/vision/vision_utils/manifest.xml
pkg/trunk/vision/visual_odometry/manifest.xml
pkg/trunk/visualization/cloud_viewer/manifest.xml
pkg/trunk/visualization/log_gui/manifest.xml
pkg/trunk/visualization/ogre_tools/manifest.xml
pkg/trunk/visualization/ogre_visualizer/manifest.xml
pkg/trunk/visualization/pr2_gui/manifest.xml
pkg/trunk/visualization/scene_labeler/manifest.xml
pkg/trunk/visualization/wx_camera_panel/manifest.xml
pkg/trunk/visualization/wx_rosout/manifest.xml
pkg/trunk/visualization/wx_topic_display/manifest.xml
pkg/trunk/visualization/wxpy_ros/manifest.xml
pkg/trunk/world_models/costmap_2d/manifest.xml
pkg/trunk/world_models/fake_world_3d_map/manifest.xml
pkg/trunk/world_models/map_server/manifest.xml
pkg/trunk/world_models/static_map_server/manifest.xml
pkg/trunk/world_models/topological_map/manifest.xml
pkg/trunk/world_models/voxel_grid/manifest.xml
pkg/trunk/world_models/world_3d_map/manifest.xml
Modified: pkg/trunk/3rdparty/Cg/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/Cg/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/Cg/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -7,6 +7,7 @@
</description>
<author>See web page for a full credits llist.</author>
<license url="http://developer.nvidia.com/object/legal_info.html">NVIDIA</license>
+<review status="unreviewed" notes=""/>
<url>http://developer.nvidia.com/page/cg_main.html</url>
<export>
<cpp lflags="-Wl,-rpath,${prefix}/Cg/lib -L${prefix}/Cg/lib -lCg" cflags="-I${prefix}/Cg/include/Cg"/>
Modified: pkg/trunk/3rdparty/bfl_latest/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/bfl_latest/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/bfl_latest/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -10,6 +10,7 @@
</description>
<author> Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.</author>
<license>LGPL</license>
+<review status="unreviewed" notes=""/>
<url>http://www.orocos.org/bfl</url>
<export>
<cpp cflags="-I${prefix}/bfl-boost/include/bfl" lflags="-Wl,-rpath,${prefix}/bfl-boost/lib -L${prefix}/bfl-boost/lib -lorocos-bfl"/>
Modified: pkg/trunk/3rdparty/boost/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/boost/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/boost/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -5,6 +5,7 @@
http://www.boost.org/LICENSE_1_0.txt
</description>
<license url="http://www.opensource.org/licenses/bsl1.0.html">BSL1.0</license>
+ <review status="unreviewed" notes=""/>
<author>See web site for contributors</author>
<url>http://www.boost.org</url>
<export>
Modified: pkg/trunk/3rdparty/bullet/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/bullet/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/bullet/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -6,6 +6,7 @@
</description>
<author>Erwin Coumans</author>
<license>ZLib</license>
+<review status="unreviewed" notes=""/>
<url>http://code.google.com/p/bullet/</url>
<sysdepend os="ubuntu" version="8.04-hardy" package="libXext-dev"/>
<sysdepend os="ubuntu" version="7.10-gutsy" package="libXext-dev"/>
Modified: pkg/trunk/3rdparty/eml/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/eml/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/eml/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -7,6 +7,7 @@
</description>
<author>Tom Panis</author>
<license>GPL</license>
+<review status="unreviewed" notes=""/>
<url>http://ethercatmaster.berlios.de/</url>
<depend package="rtnet"/>
<export>
Modified: pkg/trunk/3rdparty/estar/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/estar/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/estar/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -9,6 +9,7 @@
</description>
<author>Roland Philippsen</author>
<license>GPL</license>
+ <review status="unreviewed" notes=""/>
<url>http://estar.sourceforge.net/</url>
<export>
<cpp cflags="-I${prefix}/local/include" lflags="-Wl,-rpath,${prefix}/local/lib -L${prefix}/local/lib -lestar" />
Modified: pkg/trunk/3rdparty/fast_detector/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/fast_detector/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/fast_detector/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -4,5 +4,6 @@
</description>
<author>Edward Rosten and Tom Drummond</author>
<license>LGPL</license>
+ <review status="unreviewed" notes=""/>
</package>
Modified: pkg/trunk/3rdparty/ffmpeg/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/ffmpeg/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/ffmpeg/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -8,6 +8,7 @@
</description>
<author>Many; see the web page.</author>
<license>LGPL</license>
+<review status="unreviewed" notes=""/>
<url>http://ffmpeg.mplayerhq.hu</url>
<export>
<cpp cflags="-I${prefix}/ffmpeg/include" lflags="-L${prefix}/ffmpeg/lib -Wl,-rpath,${prefix}/ffmpeg/lib -lavcodec -lavdevice -lavformat -lavutil" />
Modified: pkg/trunk/3rdparty/freeimage/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/freeimage/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/freeimage/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -8,6 +8,7 @@
</description>
<author>Hervé Drolon , Floris van den Berg</author>
<license>GPL</license>
+<review status="unreviewed" notes=""/>
<url>http://freeimage.sourceforge.net</url>
<export>
<cpp cflags="-I${prefix}/freeimage/include" lflags="-Wl,-rpath,${prefix}/freeimage/lib -L${prefix}/freeimage/lib -lfreeimage"/>
Modified: pkg/trunk/3rdparty/gazebo/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/gazebo/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/gazebo/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -5,6 +5,7 @@
</description>
<author>Nate Koenig, with contributions from many others. See web page for a full credits llist.</author>
<license>LGPL</license>
+ <review status="unreviewed" notes=""/>
<depend package="opende" />
<depend package="ogre" />
<url>http://playerstage.sf.net</url>
Modified: pkg/trunk/3rdparty/glc/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/glc/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/glc/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -4,6 +4,7 @@
</description>
<author>Pyry Haulos</author>
<license url="http://nullkey.ath.cx/projects/glc/wiki/HowtoPackaging">zlib, GPL, CDDL</license>
+ <review status="unreviewed" notes=""/>
<url>http://nullkey.ath.cx/projects/glc/</url>
<sysdepend os="ubuntu" version="8.04-hardy" package="libasound2-dev"/>
</package>
Modified: pkg/trunk/3rdparty/gmapping/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/gmapping/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/gmapping/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -8,6 +8,7 @@
</description>
<author>Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard</author>
<license>CreativeCommons-by-nc-sa-2.0</license>
+<review status="unreviewed" notes=""/>
<url>http://openslam.org/</url>
<export>
<cpp lflags="-Xlinker -rpath ${prefix}/gmapping_export/lib -L${prefix}/gmapping_export/lib -lgridfastslam -lsensor_odometry -lsensor_range -lutils -lscanmatcher" cflags="-I${prefix}/gmapping_export/"/>
Modified: pkg/trunk/3rdparty/kdl/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/kdl/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/kdl/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -3,6 +3,7 @@
</description>
<author>Advait Jain, Sachin Chitta</author>
<license>BSD</license>
+<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.net</url>
<export>
<cpp cflags="-I${prefix}/kdl/include" lflags="-Wl,-rpath,${prefix}/kdl/lib -L${prefix}/kdl/lib -lorocos-kdl"/>
Modified: pkg/trunk/3rdparty/kni-3.9.2/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/kni-3.9.2/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/kni-3.9.2/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -2,6 +2,7 @@
<author>Neuronics</author>
<description>Control library and demos for the Katana arm</description>
<license>GPL</license>
+ <review status="unreviewed" notes=""/>
<depend package="boost"/>
<url>http://www.neuronics.ch</url>
<export>
Modified: pkg/trunk/3rdparty/libbk_maxflow/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/libbk_maxflow/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/libbk_maxflow/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -5,6 +5,7 @@
</description>
<author>Yuri Boykov and Vladimir Kolmogorov</author>
<license>"Research-only"</license>
+ <review status="unreviewed" notes=""/>
<url>http://www.cs.adastral.ucl.ac.uk/~vnk/software.html</url>
<export>
<cpp cflags="-I${prefix}/src" lflags=""/>
Modified: pkg/trunk/3rdparty/libdc1394v2/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/libdc1394v2/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/libdc1394v2/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -7,6 +7,7 @@
</description>
<author>Damien Douxchamps</author>
<license>LGPL</license>
+<review status="unreviewed" notes=""/>
<url>http://damien.douxchamps.net/ieee1394/libdc1394/</url>
<export>
<cpp cflags="-I${prefix}/libdc1394v2/include" lflags="-Wl,-rpath,${prefix}/libdc1394v2/lib -L${prefix}/libdc1394v2/lib/ -ldc1394 -lraw1394" />
Modified: pkg/trunk/3rdparty/libfz_ht_superpixels/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/libfz_ht_superpixels/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/libfz_ht_superpixels/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -5,6 +5,7 @@
</description>
<author>Pedro Felzenszwalb</author>
<license>GPL</license>
+ <review status="unreviewed" notes=""/>
<url>http://people.cs.uchicago.edu/~pff/segment/</url>
<export>
<cpp cflags="-I${prefix}/src"
Modified: pkg/trunk/3rdparty/libsunflower/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/libsunflower/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/libsunflower/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -9,6 +9,7 @@
</description>
<author>Roland Philippsen</author>
<license>GPL</license>
+ <review status="unreviewed" notes=""/>
<url>http://libsunflower.sourceforge.net/</url>
<export>
<cpp cflags="-I${prefix}/local/include" lflags="-Wl,-rpath,${prefix}/local/lib -L${prefix}/local/lib -lsunflower" />
Modified: pkg/trunk/3rdparty/loki/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/loki/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/loki/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -6,10 +6,12 @@
</description>
<author>Andrei Alexandrescu</author>
<license>MIT</license>
+<review status="unreviewed" notes=""/>
<url>http://loki-lib.sourceforge.net/</url>
<export>
<cpp lflags="-Wl,-rpath,${prefix}/loki-svn/lib -L${prefix}/loki-svn/lib -lloki" cflags="-I${prefix}/loki-svn/include"/>
<doxymaker external="http://loki-lib.sourceforge.net/html/modules.html"/>
+ <status api="10-9-2008" code="10-9-2008" cleared="11-10-2008"/>
</export>
<versioncontrol type="svn" url="https://loki-lib.svn.sourceforge.net/svnroot/loki-lib/trunk"/>
</package>
Modified: pkg/trunk/3rdparty/newmat10/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/newmat10/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/newmat10/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -10,6 +10,7 @@
</description>
<author>Robert Davies</author>
<license url="http://www.robertnz.net/nm10.htm#use">Zlib-style</license>
+<review status="unreviewed" notes=""/>
<url>http://www.robertnz.net/nm_intro.htm</url>
<export>
<cpp cflags="-I${prefix}/newmat" lflags="-L${prefix}/newmat/newmat10 -lnewmat" />
Modified: pkg/trunk/3rdparty/ogre/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/ogre/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/ogre/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -6,6 +6,7 @@
</description>
<author> Steve 'sinbad' Streeting, Justin Walsh, Brian Johnstone and more.</author>
<license>LGPL</license>
+<review status="unreviewed" notes=""/>
<url>http://ogre3d.org</url>
<depend package="freeimage"/>
<depend package="Cg"/>
Modified: pkg/trunk/3rdparty/opencv/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/opencv/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/opencv/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -11,6 +11,7 @@
</description>
<author>Gary Bradski and many others. See web page for a full contributor list. ROS package maintained by Morgan Quigley.</author>
<license>BSD</license>
+<review status="unreviewed" notes=""/>
<url>http://opencvlibrary.sourceforge.net</url>
<depend package="ffmpeg"/>
<export>
Modified: pkg/trunk/3rdparty/opencv_latest/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/opencv_latest/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/opencv_latest/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -9,6 +9,7 @@
</description>
<author>Gary Bradski and many others. See web page for a full contributor list. ROS package maintained by JD Chen.</author>
<license>BSD</license>
+<review status="unreviewed" notes=""/>
<url>http://opencvlibrary.sourceforge.net</url>
<export>
<cpp cflags="-I${prefix}/opencv/include -I${prefix}/opencv/include/opencv" lflags="-L${prefix}/opencv/lib -Wl,-rpath,${prefix}/opencv/lib -lcvaux -lcv -lcxcore -lhighgui -lml"/>
Modified: pkg/trunk/3rdparty/opende/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/opende/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/opende/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -7,6 +7,7 @@
</description>
<author>Russel Smith</author>
<license>LGPL</license>
+<review status="unreviewed" notes=""/>
<url>http://opende.sf.net</url>
<export>
<cpp lflags="-Wl,-rpath,${prefix}/opende/lib `${prefix}/opende/bin/ode-config --libs`" cflags="`${prefix}/opende/bin/ode-config --cflags`"/>
Modified: pkg/trunk/3rdparty/openrave/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/openrave/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/openrave/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -3,6 +3,7 @@
A planning architecture for autonomous robotics.
</description>
<license>LGPL</license>
+ <review status="unreviewed" notes=""/>
<export>
<cpp lflags="-Wl,-rpath,${prefix}/lib `${prefix}/bin/openrave-config --libs`" cflags="`${prefix}/bin/openrave-config --cflags`"/>
</export>
Modified: pkg/trunk/3rdparty/player/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/player/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/player/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -10,6 +10,7 @@
</description>
<author>Brian Gerkey, with contributions from many others. See web page for a full credits llist.</author>
<license>LGPL</license>
+<review status="unreviewed" notes=""/>
<url>http://playerstage.sf.net</url>
<export>
<cpp lflags="-Wl,-rpath,${prefix}/player/lib -L${prefix}/player/lib -lplayercore -lplayerxdr -lplayerutils -lplayererror -lplayerdrivers -Wl,-rpath,${prefix}/player-svn/build/rtk2 -L${prefix}/player-svn/build/rtk2 -lrtk" cflags="-I${prefix}/player/include/player-2.2"/>
Modified: pkg/trunk/3rdparty/plplot/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/plplot/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/plplot/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -4,6 +4,7 @@
</description>
<author>Maurice J. LeBrun and Geoffrey Furnish</author>
<license>LGPL</license>
+<review status="unreviewed" notes=""/>
<url>http://plplot.sourceforge.net/index.html</url>
<export>
<cpp lflags="`PKG_CONFIG_PATH=${prefix}/plplot/lib/pkgconfig pkg-config plplotd-c++ --libs`" cflags="`PKG_CONFIG_PATH=${prefix}/plplot/lib/pkgconfig pkg-config plplotd-c++ --cflags`"/>
Modified: pkg/trunk/3rdparty/rtnet/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/rtnet/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/rtnet/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -6,6 +6,7 @@
</description>
<author>See http://www.rts.uni-hannover.de/rtnet/team.html</author>
<license>GPL</license>
+<review status="unreviewed" notes=""/>
<url>http://www.rtnet.org/</url>
<depend package="xenomai"/>
<export>
Modified: pkg/trunk/3rdparty/scipy/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/scipy/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/scipy/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -8,6 +8,7 @@
<author>The SciPy developers sci...@sc... </author>
<license>BSD</license>
+<review status="unreviewed" notes=""/>
<url>http://www.scipy.org/</url>
<depend package="numpy"/>
<sysdepend os="osx" version="10.5.4" package="g95"/>
Modified: pkg/trunk/3rdparty/sicktoolbox/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/sicktoolbox/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/sicktoolbox/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -6,6 +6,7 @@
</description>
<author>Jason Derenik</author>
<license>BSD</license>
+<review status="unreviewed" notes=""/>
<url>http://sicktoolbox.sourceforge.net</url>
<export>
<cpp lflags="-Wl,-rpath,${prefix}/sicktoolbox/lib -L${prefix}/sicktoolbox/lib -lsicklms-1.0" cflags="-I${prefix}/sicktoolbox/include"/>
Modified: pkg/trunk/3rdparty/soqt/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/soqt/manifest.xml 2008-11-13 06:16:44 UTC (rev 6666)
+++ pkg/trunk/3rdparty/soqt/manifest.xml 2008-11-13 06:22:07 UTC (rev 6667)
@@ -4,6 +4,7 @@
</description>
<author>See web page for a full credits llist.</author>
<license url="http://www.coin3d.org/licensing/index_html#free-edi...
[truncated message content] |
|
From: <rdi...@us...> - 2008-11-13 15:51:50
|
Revision: 6672
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6672&view=rev
Author: rdiankov
Date: 2008-11-13 15:51:45 +0000 (Thu, 13 Nov 2008)
Log Message:
-----------
removed openrave stuff from autobuilds
Added Paths:
-----------
pkg/trunk/3rdparty/openrave/ROS_BUILD_BLACKLIST
pkg/trunk/robot_descriptions/openrave_robot_description/ROS_BUILD_BLACKLIST
Added: pkg/trunk/3rdparty/openrave/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/3rdparty/openrave/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/3rdparty/openrave/ROS_BUILD_BLACKLIST 2008-11-13 15:51:45 UTC (rev 6672)
@@ -0,0 +1 @@
+cannot find glew.h
\ No newline at end of file
Added: pkg/trunk/robot_descriptions/openrave_robot_description/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/ROS_BUILD_BLACKLIST 2008-11-13 15:51:45 UTC (rev 6672)
@@ -0,0 +1 @@
+needs openrave
\ No newline at end of file
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-13 18:25:52
|
Revision: 6683
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6683&view=rev
Author: hsujohnhsu
Date: 2008-11-13 18:25:49 +0000 (Thu, 13 Nov 2008)
Log Message:
-----------
updates to gazebo mimic function for joints.
updates to arm_defs for gripper.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_new.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-11-13 18:21:53 UTC (rev 6682)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-11-13 18:25:49 UTC (rev 6683)
@@ -722,6 +722,30 @@
// Settle on the new CoM pose
+Index: server/physics/Joint.hh
+===================================================================
+--- server/physics/Joint.hh (revision 7049)
++++ server/physics/Joint.hh (working copy)
+@@ -162,6 +162,19 @@
+ private: ParamT<std::string> *anchorBodyNameP;
+ private: ParamT<Vector3> *anchorOffsetP;
+ private: ParamT<bool> *provideFeedbackP;
++ private: ParamT<std::string> *mimicJointP;
++ private: ParamT<double> *mimicMultP;
++ private: ParamT<double> *mimicOffsetP;
++ private: ParamT<double> *mimicKpP;
++ private: ParamT<double> *mimicKdP;
++ private: ParamT<double> *mimicFMaxP;
++ private: Joint *mimicJoint;
++ private: bool enableMimic;
++ private: double mimicMult;
++ private: double mimicOffset;
++ private: double mimicKp;
++ private: double mimicKd;
++ private: double mimicFMax;
+
+ /// Feedback data for this joint
+ private: dJointFeedback *feedback;
Index: server/physics/ode/ODEPhysics.hh
===================================================================
--- server/physics/ode/ODEPhysics.hh (revision 7049)
@@ -847,6 +871,116 @@
contact.surface.mu = MIN(geom1->contact->mu1, geom2->contact->mu1);
contact.surface.mu2 = MIN(geom1->contact->mu2, geom2->contact->mu2);
+Index: server/physics/Joint.cc
+===================================================================
+--- server/physics/Joint.cc (revision 7049)
++++ server/physics/Joint.cc (working copy)
+@@ -30,6 +30,7 @@
+ #include "Model.hh"
+ #include "World.hh"
+ #include "Joint.hh"
++#include "HingeJoint.hh"
+
+ using namespace gazebo;
+
+@@ -41,6 +42,9 @@
+ this->visual = NULL;
+ this->model = NULL;
+
++ this->enableMimic = false;
++ this->mimicJoint = NULL;
++
+ Param::Begin(&this->parameters);
+ this->erpP = new ParamT<double>("erp",0.4,0);
+ this->cfmP = new ParamT<double>("cfm",10e-3,0);
+@@ -50,6 +54,12 @@
+ this->anchorBodyNameP = new ParamT<std::string>("anchor",std::string(),0);
+ this->anchorOffsetP = new ParamT<Vector3>("anchorOffset",Vector3(0,0,0), 0);
+ this->provideFeedbackP = new ParamT<bool>("provideFeedback", false, 0);
++ this->mimicJointP = new ParamT<std::string>("mimicJoint", std::string(), 0);
++ this->mimicMultP = new ParamT<double>("mimicMult", 1.0, 0);
++ this->mimicOffsetP = new ParamT<double>("mimicOffset", 0.0, 0);
++ this->mimicKpP = new ParamT<double>("mimicKp", 10.0, 0);
++ this->mimicKdP = new ParamT<double>("mimicKd", 0.0, 0);
++ this->mimicFMaxP = new ParamT<double>("mimicFMax", 1000.0, 0);
+ Param::End();
+ }
+
+@@ -67,6 +77,12 @@
+ delete this->anchorBodyNameP;
+ delete this->anchorOffsetP;
+ delete this->provideFeedbackP;
++ delete this->mimicJointP;
++ delete this->mimicMultP;
++ delete this->mimicOffsetP;
++ delete this->mimicKpP;
++ delete this->mimicKdP;
++ delete this->mimicFMaxP;
+ }
+
+
+@@ -92,6 +108,12 @@
+ this->cfmP->Load(node);
+ this->suspensionCfmP->Load(node);
+ this->provideFeedbackP->Load(node);
++ this->mimicJointP->Load(node);
++ this->mimicMultP->Load(node);
++ this->mimicOffsetP->Load(node);
++ this->mimicKpP->Load(node);
++ this->mimicKdP->Load(node);
++ this->mimicFMaxP->Load(node);
+
+ Body *body1 = this->model->GetBody( **(this->body1NameP));
+ Body *body2 = this->model->GetBody(**(this->body2NameP));
+@@ -144,6 +166,22 @@
+ {
+ this->SetAnchor(anchorVec);
+ }
++
++ // Check mimic option
++ this->mimicJoint = this->model->GetJoint( **(this->mimicJointP));
++ if (this->mimicJoint != NULL && this->mimicJoint != this)
++ {
++ // enforce constraint
++ this->enableMimic = true;
++ this->mimicMult = this->mimicMultP->GetValue();
++ this->mimicOffset = this->mimicOffsetP->GetValue();
++ this->mimicKp = this->mimicKpP->GetValue();
++ this->mimicKd = this->mimicKdP->GetValue();
++ this->mimicFMax = this->mimicFMaxP->GetValue();
++ std::cout << this->nameP->GetValue() << " this joint will mimic joint " << this->mimicJointP->GetValue() << std::endl;
++ if (this->type == Joint::HINGE)
++ this->SetParam(dParamFMax, this->mimicFMax);
++ }
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+@@ -181,6 +219,17 @@
+ /// Update the joint
+ void Joint::Update()
+ {
++ // enforce mimic
++ // only enforce constraint for same type hinge joints
++ if (this->enableMimic == true &&
++ this->mimicJoint->GetType() == Joint::HINGE &&
++ this->type == Joint::HINGE )
++ {
++ double error = (this->mimicMult*(((HingeJoint*)(this->mimicJoint))->GetAngle()) - this->mimicOffset - ((HingeJoint*)this)->GetAngle());
++ //std::cout << this->nameP->GetValue() << " error " << error << " " << this->mimicOffset << " " << this->mimicMult << std::endl;
++ this->SetParam(dParamVel, this->mimicKp*error);
++ }
++
+ //TODO: Evaluate impact of this code on performance
+ this->visual->SetVisible(World::Instance()->GetShowJoints());
+
+@@ -203,6 +252,7 @@
+ this->line2->SetPoint(0, start);
+ this->line2->Update();
+ }
++
+ }
+
+ //////////////////////////////////////////////////////////////////////////////
Index: server/physics/TrimeshGeom.cc
===================================================================
--- server/physics/TrimeshGeom.cc (revision 7049)
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_new.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_new.xml 2008-11-13 18:21:53 UTC (rev 6682)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_new.xml 2008-11-13 18:25:49 UTC (rev 6683)
@@ -138,8 +138,7 @@
<!-- Special gripper joint -->
<controller name="gripper_left_controller" topic="gripper_left_controller" type="JointPositionControllerNode">
<joint name="left_gripper_l_finger_joint">
- <pid p="0.001" d="0.00" i="0.00" iClamp="0.00" />
- <gripper_defaults effortLimit="10" velocityLimit="10" />
+ <pid p="1.0" d="0.00" i="0.00" iClamp="0.00" />
</joint>
</controller>
<!-- ========================================= -->
@@ -191,8 +190,7 @@
<!-- Special gripper joint -->
<controller name="gripper_right_controller" topic="gripper_right_controller" type="JointPositionControllerNode">
<joint name="right_gripper_l_finger_joint">
- <pid p="0.001" d="0.00" i="0.00" iClamp="0.00" />
- <gripper_defaults effortLimit="10" velocityLimit="10" />
+ <pid p="1.0" d="0.00" i="0.00" iClamp="0.00" />
</joint>
</controller>
<!-- head and above array -->
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml 2008-11-13 18:21:53 UTC (rev 6682)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml 2008-11-13 18:25:49 UTC (rev 6683)
@@ -5,6 +5,7 @@
<property name="shoulder_pitch_length" value="0.10" />
<property name="shoulder_pitch_radius" value="0.12" />
+ <property name="gripper_max_angle" value="0.8" />
<macro name="pr2_finger" params="prefix side parent reflect mimic_finger">
@@ -14,12 +15,17 @@
<joint name="${prefix}_${side}_finger_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <limit effort="100" velocity="1000" min="${((reflect-1)/2)*0.8}" max="${((reflect+1)/2)*0.8}" />
+ <limit effort="100" velocity="1000" min="${((reflect-1)/2)*gripper_max_angle}" max="${((reflect+1)/2)*gripper_max_angle}" />
<calibration reference_position="0" values="0 0" />
<safety_limit_min spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
<safety_limit_max spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
<joint_properties damping="0.1" />
<mimic name="${prefix}_${mimic_finger}_finger_joint" mult="${reflect}" offset="0"/>
+ <map name="${prefix}_${side}_finger_mimic" flag="gazebo">
+ <elem key="mimicKp">1000.0</elem>
+ <elem key="mimicKd"> 0.0</elem>
+ <elem key="mimicFMax">10000.0</elem>
+ </map>
</joint>
<link name="${prefix}_${side}_finger">
@@ -61,12 +67,17 @@
<joint name="${prefix}_${side}_finger_tip_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <limit effort="100" velocity="1000" min="${((-reflect-1)/2)*0.8}" max="${((-reflect+1)/2)*0.8}" />
+ <limit effort="100" velocity="1000" min="${((-reflect-1)/2)*gripper_max_angle}" max="${((-reflect+1)/2)*gripper_max_angle}" />
<calibration reference_position="0" values="0 0" />
<safety_limit_min spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
<safety_limit_max spring_constant="10.0" safety_length="0.1" damping_constant="0.1"/>
<joint_properties damping="0.1" />
<mimic name="${prefix}_${mimic_finger}_finger_joint" mult="${-reflect}" offset="0"/>
+ <map name="${prefix}_${side}_finger_tip_mimic" flag="gazebo">
+ <elem key="mimicKp">1000.0</elem>
+ <elem key="mimicKd"> 0.0</elem>
+ <elem key="mimicFMax">10000.0</elem>
+ </map>
</joint>
<link name="${prefix}_${side}_finger_tip">
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-14 02:20:25
|
Revision: 6719
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6719&view=rev
Author: hsujohnhsu
Date: 2008-11-14 02:20:20 +0000 (Fri, 14 Nov 2008)
Log Message:
-----------
* updates to new pr2.xml format with xacro.
* updates to new nameing convention:
* shoulder_pitch_left -> left_shoulder_pitch
* shoulder_pan_left -> left_shoulder_pan
* upperarm_roll_left -> left_upper_arm_roll
* elbow_flex_left -> left_elbow_flex
* forearm_roll_left -> left_forearm_roll
* wrist_flex_left -> left_wrist_flex
* gripper_roll_left -> left_wrist_roll
* added left_gripper_palm rigidly attached to left_wrist_roll
* finger_l_left -> left_gipper_l_finger
* finger_tip_l_left -> left_gipper_l_finger_tip
* ... similarly for right side.
* updates to demos accordingly.
* build gazebo .model files at maketime of wg_robot_description.
* rename single_link, dual_link and multi_link models so they do not have pr2_prefix.
Modified Paths:
--------------
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-fake_localization.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-fake_localization.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-params.xml
pkg/trunk/demos/2dnav_gazebo/manifest.xml
pkg/trunk/demos/arm_gazebo/arm.launch
pkg/trunk/demos/examples_gazebo/dual_link.xml
pkg/trunk/demos/examples_gazebo/multi_link.xml
pkg/trunk/demos/examples_gazebo/single_link.xml
pkg/trunk/demos/pr2_gazebo/pr2.launch
pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch
pkg/trunk/demos/pr2_gazebo/pr2_obs.launch
pkg/trunk/demos/pr2_gazebo/pr2_wg.launch
pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_multi_link.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_obstacle.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_single_link.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
pkg/trunk/robot_descriptions/wg_robot_description/pr2/Makefile
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups_collision.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/send_description.launch
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/controllers_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/groups_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/Makefile
pkg/trunk/visualization/ogre_visualizer/scripts/standalone_visualizer.py
Added Paths:
-----------
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/Makefile
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/send_description.launch
pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/Makefile
pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/multi_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/send_description.launch
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/Makefile
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/send_description.launch
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/Makefile
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/send_description.launch
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/single_link.xml
Removed Paths:
-------------
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/send_description.xml
pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/pr2_multi_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/send_description.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_new.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2_new.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/send_description.xml
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/pr2_single_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/send_description.xml
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.xml 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.xml 2008-11-14 02:20:20 UTC (rev 6719)
@@ -3,10 +3,13 @@
<group name="wg">
<include file="$(find pr2_prototype1_gazebo)/pr2_prototype1.launch"/>
<include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
+
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" output="screen" />
<node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
<node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
+ <node pkg="world_3d_map" type="world_3d_map" args="/robotdesc/pr2 scan:=tilt_scan cloud:=full_cloud" respawn="false" />
+ <node pkg="highlevel_controllers" type="move_base_sbpl" args="cloud:=world_3d_map" respawn="false"/>
<!-- for visualization -->
<!--node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg.xml 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg.xml 2008-11-14 02:20:20 UTC (rev 6719)
@@ -3,10 +3,13 @@
<group name="wg">
<include file="$(find pr2_prototype1_gazebo)/pr2_prototype1.launch"/>
<include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
+
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" output="screen" />
<node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
+ <node pkg="world_3d_map" type="world_3d_map" args="/robotdesc/pr2 scan:=tilt_scan cloud:=full_cloud" respawn="false" />
+ <node pkg="highlevel_controllers" type="move_base_sbpl" args="cloud:=world_3d_map" respawn="false"/>
<!-- for visualization -->
<!--node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-fake_localization.xml 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-fake_localization.xml 2008-11-14 02:20:20 UTC (rev 6719)
@@ -3,10 +3,13 @@
<group name="wg">
<include file="$(find pr2_gazebo)/pr2.launch"/>
<include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
+
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" output="screen" />
<node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
<node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
+ <node pkg="world_3d_map" type="world_3d_map" args="/robotdesc/pr2 scan:=tilt_scan cloud:=full_cloud" respawn="false" />
+ <node pkg="highlevel_controllers" type="move_base_sbpl" args="cloud:=world_3d_map" respawn="false"/>
<!-- for visualization -->
<!--node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple.xml 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple.xml 2008-11-14 02:20:20 UTC (rev 6719)
@@ -3,13 +3,16 @@
<group name="wg">
<include file="$(find pr2_gazebo)/pr2.launch"/>
<include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
+
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" output="screen" />
<node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" output="screen" />
<node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
+ <node pkg="world_3d_map" type="world_3d_map" args="/robotdesc/pr2 scan:=tilt_scan cloud:=full_cloud" respawn="false" />
+ <node pkg="highlevel_controllers" type="move_base_sbpl" args="cloud:=world_3d_map" respawn="false"/>
<!-- for visualization -->
- <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
+ <!--node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
<!-- for manual control -->
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-fake_localization.xml 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-fake_localization.xml 2008-11-14 02:20:20 UTC (rev 6719)
@@ -3,10 +3,13 @@
<group name="wg">
<include file="$(find pr2_gazebo)/pr2_wg.launch"/>
<include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
+
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" output="screen" />
<node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
<node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
+ <node pkg="world_3d_map" type="world_3d_map" args="/robotdesc/pr2 scan:=tilt_scan cloud:=full_cloud" respawn="false" />
+ <node pkg="highlevel_controllers" type="move_base_sbpl" args="cloud:=world_3d_map" respawn="false"/>
<!-- for visualization -->
<!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg.xml 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg.xml 2008-11-14 02:20:20 UTC (rev 6719)
@@ -3,10 +3,13 @@
<group name="wg">
<include file="$(find pr2_gazebo)/pr2_wg.launch"/>
<include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
+
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" output="screen" />
<node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
+ <node pkg="world_3d_map" type="world_3d_map" args="/robotdesc/pr2 scan:=tilt_scan cloud:=full_cloud" respawn="false" />
+ <node pkg="highlevel_controllers" type="move_base_sbpl" args="cloud:=world_3d_map" respawn="false"/>
<!-- for visualization -->
<node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-params.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-params.xml 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-params.xml 2008-11-14 02:20:20 UTC (rev 6719)
@@ -18,5 +18,16 @@
<param name="pf_odom_drift_xa" value="0.2"/>
<param name="pf_min_d" value="0.25"/>
<param name="pf_min_a" value="0.524"/>
+
+ <param name="world_3d_map/max_publish_frequency" type="double" value="20" /> <!-- Hz -->
+ <param name="world_3d_map/base_laser_range" type="double" value="10.0" /> <!-- Meters -->
+ <param name="world_3d_map/tilt_laser_range" type="double" value="4.0" /> <!-- Meters -->
+ <param name="world_3d_map/retain_pointcloud_fraction" type="double" value="0.25" /> <!-- percentage (between 0 and 1) -->
+ <param name="world_3d_map/retain_above_ground_threshold" type="double" value="0.03" /> <!-- double value -->
+ <param name="world_3d_map/verbosity_level" type="int" value="1" /> <!-- integer value -->
+ <param name="world_3d_map/laser_x_offset" type="double" value=".275" />
+
+ <param name="costmap_2d/dynamic_obstacle_window" type="double" value="5.0"/>
+ <param name="move_base_sbpl/controller_frequency" value="20.0" />
</launch>
Modified: pkg/trunk/demos/2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/manifest.xml 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/2dnav_gazebo/manifest.xml 2008-11-14 02:20:20 UTC (rev 6719)
@@ -15,5 +15,7 @@
<depend package="teleop_arm_keyboard"/>
<depend package="laser_view"/>
<depend package="pr2_gui"/>
+ <depend package="world_3d_map"/>
+ <depend package="highlevel_controllers"/>
<depend package="executive_trex_pr2"/>
</package>
Modified: pkg/trunk/demos/arm_gazebo/arm.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm.launch 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/arm_gazebo/arm.launch 2008-11-14 02:20:20 UTC (rev 6719)
@@ -1,9 +1,9 @@
<launch>
<group name="wg">
<!-- create model file for gazebo -->
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2_arm_test/pr2_arm.xml $(find gazebo_robot_description)/world/pr2_xml_arm.model" />
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2_arm_test/pr2_arm.xml.expanded $(find gazebo_robot_description)/world/pr2_arm.model" />
<!-- send pr2_arm.xml to param server -->
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2_arm_test/pr2_arm.xml"" />
+ <include file="$(find wg_robot_description)/pr2_arm_test/send_description.launch" />
<!-- -g flag runs gazebo in gui-less mode -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_arm_test.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
@@ -13,8 +13,8 @@
</node>
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_arm_test/controllers_arm.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
<node pkg="robot_mechanism_controllers" type="control.py" args="set gripper_left_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
- <!-- for visualization -->
- <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
+ <!-- for visualization, heavy, off by default -->
+ <!--node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
</group>
</launch>
Modified: pkg/trunk/demos/examples_gazebo/dual_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/dual_link.xml 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/examples_gazebo/dual_link.xml 2008-11-14 02:20:20 UTC (rev 6719)
@@ -1,10 +1,9 @@
<launch>
<group name="wg">
- <!-- create model file for gazebo -->
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/dual_link_test/pr2_dual_link.xml $(find gazebo_robot_description)/world/pr2_xml_dual_link.model" />
- <!-- send pr2_arm.xml to param server -->
- <!--include file="$(find wg_robot_description)/dual_link_test/send_description.xml" /-->
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/dual_link_test/pr2_dual_link.xml"" />
+ <!-- send dual_link.xml to param server -->
+ <include file="$(find wg_robot_description)/dual_link_test/send_description.launch" />
+
+ <!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_dual_link.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
Modified: pkg/trunk/demos/examples_gazebo/multi_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/multi_link.xml 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/examples_gazebo/multi_link.xml 2008-11-14 02:20:20 UTC (rev 6719)
@@ -1,10 +1,9 @@
<launch>
<group name="wg">
- <!-- create model file for gazebo -->
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/multi_link_test/pr2_multi_link.xml $(find gazebo_robot_description)/world/pr2_xml_multi_link.model" />
- <!-- send pr2_arm.xml to param server -->
- <!--include file="$(find wg_robot_description)/multi_link_test/send_description.xml" /-->
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/multi_link_test/pr2_multi_link.xml"" />
+ <!-- send multi_link.xml to param server -->
+ <include file="$(find wg_robot_description)/multi_link_test/send_description.launch" />
+
+ <!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_multi_link.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
Modified: pkg/trunk/demos/examples_gazebo/single_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/single_link.xml 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/examples_gazebo/single_link.xml 2008-11-14 02:20:20 UTC (rev 6719)
@@ -1,10 +1,9 @@
<launch>
<group name="wg">
- <!-- create model file for gazebo -->
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/single_link_test/pr2_single_link.xml $(find gazebo_robot_description)/world/pr2_xml_single_link.model" />
- <!-- send pr2_arm.xml to param server -->
- <!--include file="$(find wg_robot_description)/single_link_test/send_description.xml" /-->
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/single_link_test/pr2_single_link.xml"" />
+ <!-- send single_link.xml to param server -->
+ <include file="$(find wg_robot_description)/single_link_test/send_description.launch" />
+
+ <!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_single_link.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
Modified: pkg/trunk/demos/pr2_gazebo/pr2.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2.launch 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/pr2_gazebo/pr2.launch 2008-11-14 02:20:20 UTC (rev 6719)
@@ -1,16 +1,18 @@
<launch>
<!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/wg_robot_description/pr2 -->
<group name="wg">
- <!-- create model file for gazebo -->
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml.model" />
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml_nolimit.model 1" />
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
+ <!-- send pr2.xml to param server -->
+ <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+
+ <!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
+
+ <!-- load controllers -->
<include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch 2008-11-14 02:20:20 UTC (rev 6719)
@@ -1,9 +1,8 @@
<launch>
<group name="wg">
<!-- create model file for gazebo -->
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml.model" />
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml_nolimit.model 1" />
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
+ <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+
<!-- -g flag runs gazebo in gui-less mode -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_floorobj.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
Modified: pkg/trunk/demos/pr2_gazebo/pr2_obs.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_obs.launch 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/pr2_gazebo/pr2_obs.launch 2008-11-14 02:20:20 UTC (rev 6719)
@@ -1,14 +1,17 @@
<launch>
<group name="wg">
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml.model" />
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml_nolimit.model 1" />
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_obstacle.world" respawn="false">
+ <!-- send pr2.xml to param server -->
+ <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+
+ <!-- start gazebo -->
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_obstacle.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
+
+ <!-- load controllers -->
<include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_wg.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_wg.launch 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/demos/pr2_gazebo/pr2_wg.launch 2008-11-14 02:20:20 UTC (rev 6719)
@@ -1,16 +1,17 @@
<launch>
<group name="wg">
- <!-- create model file for gazebo -->
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml.model" />
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml_nolimit.model 1" />
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
- <!-- -g flag runs gazebo in gui-less mode -->
+ <!-- send pr2.xml to param server -->
+ <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+
+ <!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_wg.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
+
+ <!-- load controllers -->
<include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
===================================================================
--- pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-11-14 01:53:33 UTC (rev 6718)
+++ pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-11-14 02:20:20 UTC (rev 6719)
@@ -144,13 +144,13 @@
this->lArmCmd.set_margins_size(7);
this->rArmCmd.set_margins_size(7);
- this->lArmCmd.names[0] = "shoulder_pan_left_joint";
- this->lArmCmd.names[1] = "shoulder_pitch_left_joint";
- this->lArmCmd.names[2] = "upperarm_roll_left_joint";
- this->lArmCmd.names[3] = "elbow_flex_left_joint";
- this->lArmCmd.names[4] = "forearm_roll_left_joint";
- this->lArmCmd.names[5] = "wrist_flex_left_joint";
- this->lArmCmd.names[6] = "gripper_roll_left_joint";
+ this->lArmCmd.names[0] = "left_shoulder_pan_joint";
+ this->lArmCmd.names[1] = "left_shoulder_pitch_joint";
+ this->lArmCmd.names[2] = "left_upper_arm_roll_joint";
+ this->lArmCmd.names[3] = "left_elbow_flex_joint";
+ this->lArmCmd.names[4] = "left_forearm_roll_joint";
+ this->lArmCmd.names[5] = "left_wrist_flex_joint";
+ this->lArmCmd.names[6] = "left_wrist_roll_joint";
this->lArmCmd.positions[0] = 0;
this->lArmCmd.positions[1] = 0;
@@ -168,13 +168,13 @@
this->lArmCmd.margins[5] = 0;
this->lArmCmd.margins[6] = 0;
- this->rArmCmd.names[0] = "shoulder_pan_right_joint";
- this->rArmCmd.names[1] = "shoulder_pitch_right_joint";
- this->rArmCmd.names[2] = "upperarm_roll_right_joint";
- this->rArmCmd.names[3] = "elbow_flex_right_joint";
- this->rArmCmd.names[4] = "forearm_roll_right_joint";
- this->rArmCmd.names[5] = "wrist_flex_right_joint";
- this->rArmCmd.names[6] = "gripper_roll_right_joint";
+ this->rArmCmd.names[0] = "right_shoulder_pan_joint";
+ this->rArmCmd.names[1] = "right_shoulder_pitch_joint";
+ this->rArmCmd.names[2] = "right_upper_arm_roll_joint";
+ this->rArmCmd.names[3] = "right_elbow_flex_joint";
+ this->rArmCmd.names[4] = "right_forearm_roll_joint";
+ this->rArmCmd.names[5] = "right_wrist_flex_joint";
+ this->rArmCmd.names[6] = "right_wrist_roll_joint";
this->rArmCmd.positions[0] = 0;
this->rArmCmd.positions[1] = 0;
@@ -2...
[truncated message content] |
|
From: <hsu...@us...> - 2008-11-15 07:49:40
|
Revision: 6806
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6806&view=rev
Author: hsujohnhsu
Date: 2008-11-15 07:49:30 +0000 (Sat, 15 Nov 2008)
Log Message:
-----------
rename robot.world to robot_simple.world
turn off lighting for robot_obstacle.world
Modified Paths:
--------------
pkg/trunk/demos/pr2_gazebo/pr2.launch
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_obstacle.world
Added Paths:
-----------
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_simple.world
Removed Paths:
-------------
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
Modified: pkg/trunk/demos/pr2_gazebo/pr2.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2.launch 2008-11-15 07:46:31 UTC (rev 6805)
+++ pkg/trunk/demos/pr2_gazebo/pr2.launch 2008-11-15 07:49:30 UTC (rev 6806)
@@ -5,7 +5,7 @@
<include file="$(find wg_robot_description)/pr2/send_description.launch" />
<!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_simple.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-15 07:46:31 UTC (rev 6805)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-15 07:49:30 UTC (rev 6806)
@@ -1,307 +0,0 @@
-<?xml version="1.0"?>
-
-<gazebo:world
- xmlns:xi="http://www.w3.org/2001/XInclude"
- xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
- xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
- xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
- xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
- xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
- xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
- xmlns:geo="http://willowgarage.com/xmlschema/#geo"
- xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
- xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
- xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
- xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
- xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
- xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
- xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
- xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >
-
- <verbosity>5</verbosity>
-
-<!-- cfm is 1e-5 for single precision -->
-<!-- erp is typically .1-.8 -->
-<!-- here's the global contact cfm/erp -->
- <physics:ode>
- <stepTime>0.001</stepTime>
- <gravity>0 0 -9.8</gravity>
- <cfm>0.000000000001</cfm>
- <erp>0.2</erp>
- <quickStep>true</quickStep>
- <quickStepIters>3</quickStepIters>
- <quickStepW>1.3</quickStepW>
- <quickStepIters>3</quickStepIters>
- <quickStepW>1.3</quickStepW>
- </physics:ode>
-
- <geo:origin>
- <lat>37.4270909558</lat><lon>-122.077919338</lon>
- </geo:origin>
-
- <rendering:gui>
- <type>fltk</type>
- <size>1024 800</size>
- <pos>0 0</pos>
- <frames>
- <row height="100%">
- <camera width="100%">
- <xyz>0 0 20</xyz>
- <rpy>0 90 90</rpy>
- </camera>
- </row>
- </frames>
- </rendering:gui>
-
-
- <rendering:ogre>
- <ambient>1.0 1.0 1.0 1.0</ambient>
- <sky>
- <material>Gazebo/CloudySky</material>
- </sky>
- <gazeboPath>media</gazeboPath>
- <grid>false</grid>
- <maxUpdateRate>10</maxUpdateRate>
- </rendering:ogre>
-
-
- <model:physical name="gplane">
- <xyz>0 0 0</xyz>
- <rpy>0 0 0</rpy>
- <static>true</static>
-
- <body:plane name="plane">
- <geom:plane name="plane">
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <normal>0 0 1</normal>
- <size>51.3 51.3</size>
- <!-- map3.png -->
- <material>PR2/floor_texture</material>
- </geom:plane>
- </body:plane>
- </model:physical>
-
- <!--
- <model:empty name="ros_model">
- <body:empty name="ros_body">
- <controller:ros_node name="ros_node" plugin="libRos_Node.so">
- <nodeName>simulator_ros_node</nodeName>
- </controller:ros_node>
- </body:empty>
- </model:empty>
- -->
-
- <!-- The "desk"-->
- <!-- small desks -->
- <model:physical name="desk1_model">
- <xyz>2.28 -0.21 0</xyz>
- <rpy>0.0 0.0 0.0 </rpy>
- <include embedded="true">
- <!--xi:include href="desk6.model" /--> <!-- block legs -->
- <xi:include href="desk4.model" /> <!-- skinny pole legs -->
- </include>
- </model:physical>
-
- <!-- The second "desk"-->
- <model:physical name="desk2_model">
- <xyz>2.25 -3.0 0</xyz>
- <rpy>0.0 0.0 0.0 </rpy>
- <include embedded="true">
- <xi:include href="desk5.model" />
- </include>
- </model:physical>
-
- <!-- The small cylinder "cup" -->
- <model:physical name="cylinder1_model">
- <xyz> 2.5 0.0 1.5</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
- <body:cylinder name="cylinder1_body">
- <geom:cylinder name="cylinder1_geom">
- <kp>100000000.0</kp>
- <kd>0.1</kd>
- <mesh>default</mesh>
- <size>0.025 0.075</size>
- <mass> 0.05</mass>
- <visual>
- <size> 0.05 0.05 0.075</size>
- <material>Gazebo/PioneerBody</material>
- <mesh>unit_cylinder</mesh>
- </visual>
- </geom:cylinder>
- <geom:box name="cylinder1_base_geom">
- <kp>100000000.0</kp>
- <kd>0.1</kd>
- <mesh>default</mesh>
- <xyz>0.0 0.0 -0.033</xyz>
- <size>0.05 0.05 0.01</size>
- <mass> 0.01</mass>
- <visual>
- <size> 0.05 0.05 0.01</size>
- <material>Gazebo/Fish</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:cylinder>
- </model:physical>
-
- <!-- The small box "cup" -->
- <model:physical name="object1_model">
- <xyz> 0.835 -0.55 0.95</xyz>
- <rpy> 0.0 0.0 30.0</rpy>
- <body:box name="object1_body">
- <geom:box name="object1_geom">
- <kp>100000000.0</kp>
- <kd>0.1</kd>
- <mesh>default</mesh>
- <size>0.1 0.03 0.03</size>
- <mass> 0.05</mass>
- <visual>
- <size> 0.1 0.030 0.03</size>
- <material>Gazebo/PioneerBody</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:box>
-
- <controller:P3D name="p3d_object_controller" plugin="libP3D.so">
- <updateRate>100.0</updateRate>
- <bodyName>object1_body</bodyName>
- <topicName>object1_body_ground_truth</topicName>
- <frameName>object1_body_ground_truth_frame</frameName>
- <interface:position name="p3d_object_position"/>
- </controller:P3D>
-
- </model:physical>
-
-
- <!-- The small ball -->
- <model:physical name="sphere1_model">
- <xyz> 2.5 -2.8 1.5</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
- <body:sphere name="sphere1_body">
- <geom:sphere name="sphere1_geom">
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <mesh>default</mesh>
- <size> 0.15</size>
- <mass> 1.0</mass>
- <visual>
- <size> 0.3 0.3 0.3</size>
- <material>Gazebo/PioneerBody</material>
- <mesh>unit_sphere</mesh>
- </visual>
- </geom:sphere>
- </body:sphere>
- </model:physical>
-
- <!-- The large ball map3.png -->
- <model:physical name="sphere2_model">
- <xyz> 5.85 4.35 1.55</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
- <body:sphere name="sphere2_body">
- <geom:sphere name="sphere2_geom">
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <mesh>default</mesh>
- <size> 1.0</size>
- <mass> 1.0</mass>
- <visual>
- <size> 2.0 2.0 2.0</size>
- <material>Gazebo/Rocky</material>
- <mesh>unit_sphere</mesh>
- </visual>
- </geom:sphere>
- </body:sphere>
- </model:physical>
-
- <!-- The wall in front map3.png -->
- <model:physical name="wall_2_model">
- <xyz> 11.6 -1.55 1.0</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
- <static>true</static>
- <body:box name="wall_2_body">
- <geom:box name="wall_2_geom">
- <mesh>default</mesh>
- <size>2.1 32.8 2.0</size>
- <visual>
- <size>2.1 32.8 2.0</size>
- <material>Gazebo/PioneerBody</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:box>
- </model:physical>
-
- <!-- The wall behind -->
- <model:physical name="wall_1_model">
- <xyz> -11.3 -1.45 1.0</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
- <static>true</static>
- <body:box name="wall_1_body">
- <geom:box name="wall_1_geom">
- <mesh>default</mesh>
- <size>0.4 24.0 2.0</size>
- <visual>
- <size>0.4 24.0 2.0</size>
- <material>Gazebo/PioneerBody</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:box>
- </model:physical>
-
- <!-- The wall 3 -->
- <model:physical name="wall_3_model">
- <xyz> 6.7 8.05 1.0</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
- <static>true</static>
- <body:box name="wall_3_body">
- <geom:box name="wall_3_geom">
- <mesh>default</mesh>
- <size>7.5 1.2 2.0</size>
- <visual>
- <size>7.5 1.2 2.0</size>
- <material>Gazebo/Chrome</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:box>
- </model:physical>
-
-
-
- <model:physical name="robot_model1">
-
- <controller:ros_time name="ros_time" plugin="libRos_Time.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
- <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
- </controller:ros_time>
-
- <xyz>0.0 0.0 0.0408</xyz> <!-- bottom of base off the ground by this much, basically wheel height below skirt -->
- <rpy>0.0 0.0 0.0 </rpy>
-
- <!-- base, torso and arms -->
- <include embedded="true">
- <xi:include href="pr2.model" />
- </include>
-
- </model:physical>
-
-
- <!-- White Directional light -->
- <!--
- <model:renderable name="directional_white">
- <light>
- <type>directional</type>
- <direction>0 -0.5 -0.5</direction>
- <diffuseColor>0.4 0.4 0.4</diffuseColor>
- <specularColor>0.0 0.0 0.0</specularColor>
- <attenuation>1 0.0 1.0 0.4</attenuation>
- </light>
- </model:renderable>
- -->
-
-
-</gazebo:world>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_obstacle.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_obstacle.world 2008-11-15 07:46:31 UTC (rev 6805)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_obstacle.world 2008-11-15 07:49:30 UTC (rev 6806)
@@ -358,6 +358,7 @@
<!-- White Directional light -->
+ <!--
<model:renderable name="directional_white">
<light>
<type>directional</type>
@@ -367,6 +368,7 @@
<attenuation>1 0.0 1.0 0.4</attenuation>
</light>
</model:renderable>
+ -->
</gazebo:world>
Copied: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_simple.world (from rev 6805, pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world)
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_simple.world (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_simple.world 2008-11-15 07:49:30 UTC (rev 6806)
@@ -0,0 +1,307 @@
+<?xml version="1.0"?>
+
+<gazebo:world
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
+ xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geo="http://willowgarage.com/xmlschema/#geo"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >
+
+ <verbosity>5</verbosity>
+
+<!-- cfm is 1e-5 for single precision -->
+<!-- erp is typically .1-.8 -->
+<!-- here's the global contact cfm/erp -->
+ <physics:ode>
+ <stepTime>0.001</stepTime>
+ <gravity>0 0 -9.8</gravity>
+ <cfm>0.000000000001</cfm>
+ <erp>0.2</erp>
+ <quickStep>true</quickStep>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
+ </physics:ode>
+
+ <geo:origin>
+ <lat>37.4270909558</lat><lon>-122.077919338</lon>
+ </geo:origin>
+
+ <rendering:gui>
+ <type>fltk</type>
+ <size>1024 800</size>
+ <pos>0 0</pos>
+ <frames>
+ <row height="100%">
+ <camera width="100%">
+ <xyz>0 0 20</xyz>
+ <rpy>0 90 90</rpy>
+ </camera>
+ </row>
+ </frames>
+ </rendering:gui>
+
+
+ <rendering:ogre>
+ <ambient>1.0 1.0 1.0 1.0</ambient>
+ <sky>
+ <material>Gazebo/CloudySky</material>
+ </sky>
+ <gazeboPath>media</gazeboPath>
+ <grid>false</grid>
+ <maxUpdateRate>10</maxUpdateRate>
+ </rendering:ogre>
+
+
+ <model:physical name="gplane">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <static>true</static>
+
+ <body:plane name="plane">
+ <geom:plane name="plane">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
+ <normal>0 0 1</normal>
+ <size>51.3 51.3</size>
+ <!-- map3.png -->
+ <material>PR2/floor_texture</material>
+ </geom:plane>
+ </body:plane>
+ </model:physical>
+
+ <!--
+ <model:empty name="ros_model">
+ <body:empty name="ros_body">
+ <controller:ros_node name="ros_node" plugin="libRos_Node.so">
+ <nodeName>simulator_ros_node</nodeName>
+ </controller:ros_node>
+ </body:empty>
+ </model:empty>
+ -->
+
+ <!-- The "desk"-->
+ <!-- small desks -->
+ <model:physical name="desk1_model">
+ <xyz>2.28 -0.21 0</xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <include embedded="true">
+ <!--xi:include href="desk6.model" /--> <!-- block legs -->
+ <xi:include href="desk4.model" /> <!-- skinny pole legs -->
+ </include>
+ </model:physical>
+
+ <!-- The second "desk"-->
+ <model:physical name="desk2_model">
+ <xyz>2.25 -3.0 0</xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <include embedded="true">
+ <xi:include href="desk5.model" />
+ </include>
+ </model:physical>
+
+ <!-- The small cylinder "cup" -->
+ <model:physical name="cylinder1_model">
+ <xyz> 2.5 0.0 1.5</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <body:cylinder name="cylinder1_body">
+ <geom:cylinder name="cylinder1_geom">
+ <kp>100000000.0</kp>
+ <kd>0.1</kd>
+ <mesh>default</mesh>
+ <size>0.025 0.075</size>
+ <mass> 0.05</mass>
+ <visual>
+ <size> 0.05 0.05 0.075</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_cylinder</mesh>
+ </visual>
+ </geom:cylinder>
+ <geom:box name="cylinder1_base_geom">
+ <kp>100000000.0</kp>
+ <kd>0.1</kd>
+ <mesh>default</mesh>
+ <xyz>0.0 0.0 -0.033</xyz>
+ <size>0.05 0.05 0.01</size>
+ <mass> 0.01</mass>
+ <visual>
+ <size> 0.05 0.05 0.01</size>
+ <material>Gazebo/Fish</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:cylinder>
+ </model:physical>
+
+ <!-- The small box "cup" -->
+ <model:physical name="object1_model">
+ <xyz> 0.835 -0.55 0.95</xyz>
+ <rpy> 0.0 0.0 30.0</rpy>
+ <body:box name="object1_body">
+ <geom:box name="object1_geom">
+ <kp>100000000.0</kp>
+ <kd>0.1</kd>
+ <mesh>default</mesh>
+ <size>0.1 0.03 0.03</size>
+ <mass> 0.05</mass>
+ <visual>
+ <size> 0.1 0.030 0.03</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+ <controller:P3D name="p3d_object_controller" plugin="libP3D.so">
+ <updateRate>100.0</updateRate>
+ <bodyName>object1_body</bodyName>
+ <topicName>object1_body_ground_truth</topicName>
+ <frameName>object1_body_ground_truth_frame</frameName>
+ <interface:position name="p3d_object_position"/>
+ </controller:P3D>
+
+ </model:physical>
+
+
+ <!-- The small ball -->
+ <model:physical name="sphere1_model">
+ <xyz> 2.5 -2.8 1.5</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <body:sphere name="sphere1_body">
+ <geom:sphere name="sphere1_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
+ <mesh>default</mesh>
+ <size> 0.15</size>
+ <mass> 1.0</mass>
+ <visual>
+ <size> 0.3 0.3 0.3</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_sphere</mesh>
+ </visual>
+ </geom:sphere>
+ </body:sphere>
+ </model:physical>
+
+ <!-- The large ball map3.png -->
+ <model:physical name="sphere2_model">
+ <xyz> 5.85 4.35 1.55</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <body:sphere name="sphere2_body">
+ <geom:sphere name="sphere2_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
+ <mesh>default</mesh>
+ <size> 1.0</size>
+ <mass> 1.0</mass>
+ <visual>
+ <size> 2.0 2.0 2.0</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_sphere</mesh>
+ </visual>
+ </geom:sphere>
+ </body:sphere>
+ </model:physical>
+
+ <!-- The wall in front map3.png -->
+ <model:physical name="wall_2_model">
+ <xyz> 11.6 -1.55 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_2_body">
+ <geom:box name="wall_2_geom">
+ <mesh>default</mesh>
+ <size>2.1 32.8 2.0</size>
+ <visual>
+ <size>2.1 32.8 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- The wall behind -->
+ <model:physical name="wall_1_model">
+ <xyz> -11.3 -1.45 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_1_body">
+ <geom:box name="wall_1_geom">
+ <mesh>default</mesh>
+ <size>0.4 24.0 2.0</size>
+ <visual>
+ <size>0.4 24.0 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- The wall 3 -->
+ <model:physical name="wall_3_model">
+ <xyz> 6.7 8.05 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_3_body">
+ <geom:box name="wall_3_geom">
+ <mesh>default</mesh>
+ <size>7.5 1.2 2.0</size>
+ <visual>
+ <size>7.5 1.2 2.0</size>
+ <material>Gazebo/Chrome</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+
+
+ <model:physical name="robot_model1">
+
+ <controller:ros_time name="ros_time" plugin="libRos_Time.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
+ </controller:ros_time>
+
+ <xyz>0.0 0.0 0.0408</xyz> <!-- bottom of base off the ground by this much, basically wheel height below skirt -->
+ <rpy>0.0 0.0 0.0 </rpy>
+
+ <!-- base, torso and arms -->
+ <include embedded="true">
+ <xi:include href="pr2.model" />
+ </include>
+
+ </model:physical>
+
+
+ <!-- White Directional light -->
+ <!--
+ <model:renderable name="directional_white">
+ <light>
+ <type>directional</type>
+ <direction>0 -0.5 -0.5</direction>
+ <diffuseColor>0.4 0.4 0.4</diffuseColor>
+ <specularColor>0.0 0.0 0.0</specularColor>
+ <attenuation>1 0.0 1.0 0.4</attenuation>
+ </light>
+ </model:renderable>
+ -->
+
+
+</gazebo:world>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-15 21:06:04
|
Revision: 6810
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6810&view=rev
Author: hsujohnhsu
Date: 2008-11-15 21:05:59 +0000 (Sat, 15 Nov 2008)
Log Message:
-----------
updates to test worlds for gazebo plugins.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.xml
pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.xml
pkg/trunk/drivers/simulator/gazebo_plugin/test/testpendulum.xml
pkg/trunk/drivers/simulator/gazebo_plugin/test/testscan.xml
pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/testscan.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/testslide.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/teststereo.world
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.xml 2008-11-15 20:49:24 UTC (rev 6809)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.xml 2008-11-15 21:05:59 UTC (rev 6810)
@@ -1,8 +1,10 @@
<launch>
<master auto="start" />
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
- <node pkg="gazebo" type="gazebo" args="-g -n $(find gazebo_robot_description)/world/robot_headless.world" respawn="false" output="screen">
+ <!-- send single_link.xml to param server -->
+ <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+
+ <node pkg="gazebo" type="gazebo" args="-g -n $(find gazebo_robot_description)/world/robot_simple.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.xml 2008-11-15 20:49:24 UTC (rev 6809)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testcameras.xml 2008-11-15 21:05:59 UTC (rev 6810)
@@ -1,7 +1,9 @@
<launch>
<master auto="start" />
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
+ <!-- send single_link.xml to param server -->
+ <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+
<node pkg="gazebo" type="gazebo" args="-g -n $(find gazebo_robot_description)/world/testcameras.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testpendulum.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testpendulum.xml 2008-11-15 20:49:24 UTC (rev 6809)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testpendulum.xml 2008-11-15 21:05:59 UTC (rev 6810)
@@ -1,8 +1,11 @@
<launch>
<group name="wg">
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/dual_link_test/pr2_dual_link.xml"" />
- <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_dual_link.world" respawn="false" output="screen">
+
+ <!-- send single_link.xml to param server -->
+ <include file="$(find wg_robot_description)/dual_link_test/send_description.launch" />
+
+ <node pkg="gazebo" type="gazebo" args="-g -n $(find gazebo_robot_description)/world/robot_dual_link.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testscan.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testscan.xml 2008-11-15 20:49:24 UTC (rev 6809)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testscan.xml 2008-11-15 21:05:59 UTC (rev 6810)
@@ -1,7 +1,9 @@
<launch>
<master auto="start" />
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
+ <!-- send single_link.xml to param server -->
+ <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+
<node pkg="gazebo" type="gazebo" args="-g -n $(find gazebo_robot_description)/world/testscan.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.xml 2008-11-15 20:49:24 UTC (rev 6809)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.xml 2008-11-15 21:05:59 UTC (rev 6810)
@@ -1,7 +1,9 @@
<launch>
<master auto="start" />
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
+ <!-- send single_link.xml to param server -->
+ <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+
<node pkg="gazebo" type="gazebo" args="-g -n $(find gazebo_robot_description)/world/testslide.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world 2008-11-15 20:49:24 UTC (rev 6809)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world 2008-11-15 21:05:59 UTC (rev 6810)
@@ -20,9 +20,9 @@
<verbosity>5</verbosity>
-<!-- cfm is 1e-5 for single precision -->
-<!-- erp is typically .1-.8 -->
-<!-- here's the global contact cfm/erp -->
+ <!-- cfm is 1e-5 for single precision -->
+ <!-- erp is typically .1-.8 -->
+ <!-- here's the global contact cfm/erp -->
<physics:ode>
<stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world 2008-11-15 20:49:24 UTC (rev 6809)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/testcameras.world 2008-11-15 21:05:59 UTC (rev 6810)
@@ -20,15 +20,23 @@
<verbosity>5</verbosity>
-<!-- cfm is 1e-5 for single precision -->
-<!-- erp is typically .1-.8 -->
+ <!-- FIXME: pr2 mimic joints Kp setting is unstable at 0.01 -->
+ <!-- cfm is 1e-5 for single precision -->
+ <!-- erp is typically .1-.8 -->
+ <!-- here's the global contact cfm/erp -->
<physics:ode>
- <stepTime>0.01</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
- <cfm>0.0000000001</cfm>
+ <cfm>0.000000000001</cfm>
<erp>0.2</erp>
+ <quickStep>true</quickStep>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
</physics:ode>
+
<rendering:gui>
<type>fltk</type>
<size>1024 800</size>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/testscan.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/testscan.world 2008-11-15 20:49:24 UTC (rev 6809)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/testscan.world 2008-11-15 21:05:59 UTC (rev 6810)
@@ -20,13 +20,19 @@
<verbosity>5</verbosity>
-<!-- cfm is 1e-5 for single precision -->
-<!-- erp is typically .1-.8 -->
+ <!-- cfm is 1e-5 for single precision -->
+ <!-- erp is typically .1-.8 -->
+ <!-- here's the global contact cfm/erp -->
<physics:ode>
- <stepTime>0.01</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
- <cfm>0.0000000001</cfm>
+ <cfm>0.000000000001</cfm>
<erp>0.2</erp>
+ <quickStep>true</quickStep>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
</physics:ode>
<rendering:gui>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/testslide.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/testslide.world 2008-11-15 20:49:24 UTC (rev 6809)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/testslide.world 2008-11-15 21:05:59 UTC (rev 6810)
@@ -20,13 +20,19 @@
<verbosity>5</verbosity>
-<!-- cfm is 1e-5 for single precision -->
-<!-- erp is typically .1-.8 -->
+ <!-- cfm is 1e-5 for single precision -->
+ <!-- erp is typically .1-.8 -->
+ <!-- here's the global contact cfm/erp -->
<physics:ode>
- <stepTime>0.01</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
- <cfm>0.0000000001</cfm>
+ <cfm>0.000000000001</cfm>
<erp>0.2</erp>
+ <quickStep>true</quickStep>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
</physics:ode>
<geo:origin>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/teststereo.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/teststereo.world 2008-11-15 20:49:24 UTC (rev 6809)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/teststereo.world 2008-11-15 21:05:59 UTC (rev 6810)
@@ -20,13 +20,19 @@
<verbosity>5</verbosity>
-<!-- cfm is 1e-5 for single precision -->
-<!-- erp is typically .1-.8 -->
+ <!-- cfm is 1e-5 for single precision -->
+ <!-- erp is typically .1-.8 -->
+ <!-- here's the global contact cfm/erp -->
<physics:ode>
- <stepTime>0.01</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
- <cfm>0.0000000001</cfm>
+ <cfm>0.000000000001</cfm>
<erp>0.2</erp>
+ <quickStep>true</quickStep>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
</physics:ode>
<rendering:gui>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wgh...@us...> - 2008-11-18 00:46:23
|
Revision: 6849
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6849&view=rev
Author: wghassan
Date: 2008-11-18 00:46:16 +0000 (Tue, 18 Nov 2008)
Log Message:
-----------
updated Makefile for 3rdparty packages to build inside on build directory
Modified Paths:
--------------
pkg/trunk/3rdparty/boost/Makefile
pkg/trunk/3rdparty/eml/Makefile
pkg/trunk/3rdparty/estar/Makefile
pkg/trunk/3rdparty/freeimage/Makefile
pkg/trunk/3rdparty/glc/elfhacks/Makefile
pkg/trunk/3rdparty/glc/glc/Makefile
pkg/trunk/3rdparty/glc/packetstream/Makefile
pkg/trunk/3rdparty/gmapping/Makefile
pkg/trunk/3rdparty/kdl/Makefile
pkg/trunk/3rdparty/libsunflower/Makefile
pkg/trunk/3rdparty/ogre/Makefile
pkg/trunk/3rdparty/opencv/Makefile
pkg/trunk/3rdparty/scipy/Makefile
pkg/trunk/3rdparty/soqt/Makefile
pkg/trunk/3rdparty/vop/Makefile
pkg/trunk/3rdparty/wxmpl/Makefile
pkg/trunk/3rdparty/wxswig/Makefile
pkg/trunk/simulators/nepumuk/Makefile
Removed Paths:
-------------
pkg/trunk/drivers/network/wifi_ddwrt/.backup-CMakeLists.txt
pkg/trunk/drivers/network/wifi_ddwrt/.backup-manifest.xml
pkg/trunk/drivers/network/wifi_ddwrt/msg/.backup-APSignal.msg
pkg/trunk/drivers/network/wifi_ddwrt/nodes/.backup-chat
pkg/trunk/drivers/network/wifi_ddwrt/nodes/.backup-ddwrt
pkg/trunk/drivers/network/wifi_ddwrt/nodes/.backup-listener
Modified: pkg/trunk/3rdparty/boost/Makefile
===================================================================
--- pkg/trunk/3rdparty/boost/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/boost/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,18 +1,19 @@
all: boost
-TARBALL = boost_1_36_0.tar.gz
+TARBALL = build/boost_1_36_0.tar.gz
TARBALL_URL = http://pr.willowgarage.com/downloads/boost_1_36_0.tar.gz
-SOURCE_DIR = boost_1_36_0
+SOURCE_DIR = build/boost_1_36_0
+LOCAL_DIR = boost_1_36_0
UNPACK_CMD = tar zxf
MD5SUM_FILE = boost_1_36_0.tar.gz.md5sum
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
boost: $(SOURCE_DIR)
-
+ mv $(SOURCE_DIR) $(LOCAL_DIR)
clean:
rm -rf boost_1_36_0
wipe: clean
- rm -rf boost_1_36_0.tar.gz
+ rm -rf build
.PHONY : clean distclean
Modified: pkg/trunk/3rdparty/eml/Makefile
===================================================================
--- pkg/trunk/3rdparty/eml/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/eml/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,10 +1,10 @@
all: build
-TARBALL = eml-r36-patched.tar.bz2
-TARBALL_URL = http://pr.willowgarage.com/downloads/$(TARBALL)
-SOURCE_DIR = eml-svn
+TARBALL = build/eml-r36-patched.tar.bz2
+TARBALL_URL = http://pr.willowgarage.com/downloads/eml-r36-patched.tar.bz2
+SOURCE_DIR = build/eml-svn
UNPACK_CMD = tar xjf
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
build: wiped eml
@@ -28,7 +28,7 @@
rm -rf eml configured
wipe: clean
- rm -rf $(SOURCE_DIR)
+ rm -rf build
Modified: pkg/trunk/3rdparty/estar/Makefile
===================================================================
--- pkg/trunk/3rdparty/estar/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/estar/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -17,11 +17,11 @@
VERSION= r267
-TARBALL= estar-$(VERSION).tar.gz
-TARBALL_URL= http://pr.willowgarage.com/downloads/$(TARBALL)
-SOURCE_DIR= $(PWD)/estar-$(VERSION)
+TARBALL= build/estar-$(VERSION).tar.gz
+TARBALL_URL= http://pr.willowgarage.com/downloads/estar-$(VERSION).tar.gz
+SOURCE_DIR= $(PWD)/build/estar-$(VERSION)
UNPACK_CMD= tar xfz
-BUILD_DIR= $(PWD)/build
+BUILD_DIR= $(PWD)/build/build
INST_DIR= $(PWD)/local
BOOST_DIR= `rospack cflags-only-I boost`
@@ -29,7 +29,7 @@
SVN_REV= HEAD
SVN_URL= https://estar.svn.sourceforge.net/svnroot/estar/trunk/estar
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
.PHONY: reall-all
really-all: $(SOURCE_DIR)/configure $(BUILD_DIR)/config.h
Modified: pkg/trunk/3rdparty/freeimage/Makefile
===================================================================
--- pkg/trunk/3rdparty/freeimage/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/freeimage/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,15 +1,15 @@
all: freeimage
-TARBALL = FreeImage3110.zip
-TARBALL_URL = http://pr.willowgarage.com/downloads/$(TARBALL)
-SOURCE_DIR = freeimage-3.11.0
-INITIAL_DIR = FreeImage
+TARBALL = build/FreeImage3110.zip
+TARBALL_URL = http://pr.willowgarage.com/downloads/FreeImage3110.zip
+SOURCE_DIR = build/freeimage-3.11.0
+INITIAL_DIR = build/FreeImage
UNPACK_CMD = unzip
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
ROOT = $(shell rospack find freeimage)/freeimage
-freeimage: freeimage-3.11.0
+freeimage: $(SOURCE_DIR)
@echo "making it"
@echo "ROOT is: $(ROOT)"
-mkdir -p freeimage
@@ -19,14 +19,14 @@
# Fix up the makefile, which doesn't make good use of any environment variables
# We also don't need to install with -o root -g root
# We also shouldn't run ldconfig, as we don't have permission for that either.
- cd freeimage-3.11.0 ; mv Makefile.gnu Makefile.gnu-orig ; cat Makefile.gnu-orig | sed -e 's%/usr/lib%$(ROOT)/lib%g' | sed -e 's%/usr/include%$(ROOT)/include%g' | sed -e 's/-o root -g root//' | grep -v ldconfig > Makefile.gnu
+ cd $(SOURCE_DIR) ; mv Makefile.gnu Makefile.gnu-orig ; cat Makefile.gnu-orig | sed -e 's%/usr/lib%$(ROOT)/lib%g' | sed -e 's%/usr/include%$(ROOT)/include%g' | sed -e 's/-o root -g root//' | grep -v ldconfig > Makefile.gnu
# Here's some other nasty hackery to build on OS X. This will only
# work on modern Intel-based Macs, and it will not build shared
# libraries.
#
# TODO: Replace this sed madness with a proper patch
- cd freeimage-3.11.0 ; mv Makefile.osx Makefile.osx-orig ; cat Makefile.osx-orig | sed -e 's%/usr/lib%$(ROOT)/lib%g' | sed -e 's%/usr/include%$(ROOT)/include%g' | sed -e 's/-o root -g wheel//' | sed -e 's%/usr/local%$(ROOT)%g'| sed -e 's%FreeImage: $$(STATICLIB) $$(SHAREDLIB)%FreeImage: $$(STATICLIB)%' | sed -e 's%$$(STATICLIB): $$(STATICLIB)-ppc%$$(STATICLIB):%' | sed -e 's%$$(LIPO) -create $$(STATICLIB)-ppc%$$(LIPO) -create%' | sed -e 's%install -m 644 $$(SHAREDLIB)%install -m 644 %' | sed -e 's%ln -sf%#ln -sf%' | sed -e 's%all: dist%all: FreeImage%' | grep -v ldconfig > Makefile.osx
- cd freeimage-3.11.0 ; make $(PARALLEL_JOBS); make install
+ cd $(SOURCE_DIR) ; mv Makefile.osx Makefile.osx-orig ; cat Makefile.osx-orig | sed -e 's%/usr/lib%$(ROOT)/lib%g' | sed -e 's%/usr/include%$(ROOT)/include%g' | sed -e 's/-o root -g wheel//' | sed -e 's%/usr/local%$(ROOT)%g'| sed -e 's%FreeImage: $$(STATICLIB) $$(SHAREDLIB)%FreeImage: $$(STATICLIB)%' | sed -e 's%$$(STATICLIB): $$(STATICLIB)-ppc%$$(STATICLIB):%' | sed -e 's%$$(LIPO) -create $$(STATICLIB)-ppc%$$(LIPO) -create%' | sed -e 's%install -m 644 $$(SHAREDLIB)%install -m 644 %' | sed -e 's%ln -sf%#ln -sf%' | sed -e 's%all: dist%all: FreeImage%' | grep -v ldconfig > Makefile.osx
+ cd $(SOURCE_DIR) ; make $(PARALLEL_JOBS); make install
mkdir -p freeimage/lib/pkgconfig
sed 's%FREEIMAGE_PATH%$(shell rospack find freeimage)%g' freeimage.pc > freeimage/lib/pkgconfig/freeimage.pc
@@ -35,8 +35,7 @@
rm -rf freeimage
wipe:
- rm -rf freeimage-3.11.0
rm -rf freeimage
- rm -rf $(TARBALL)
+ rm -rf build
.PHONY : clean download wipe
Modified: pkg/trunk/3rdparty/glc/elfhacks/Makefile
===================================================================
--- pkg/trunk/3rdparty/glc/elfhacks/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/glc/elfhacks/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,16 +1,16 @@
all: elfhacks
-TARBALL = elfhacks-0.4.1.tar.gz
+TARBALL = build/elfhacks-0.4.1.tar.gz
TARBALL_URL = http://pr.willowgarage.com/downloads/elfhacks-0.4.1.tar.gz
SOURCE_DIR = elfhacks-0.4.1
UNPACK_CMD = tar xzf
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
elfhacks: $(SOURCE_DIR)
- -mv elfhacks elfhacks-0.4.1
+ -mv build/elfhacks $(SOURCE_DIR)
cd $(SOURCE_DIR) && cmake . -DCMAKE_INSTALL_PREFIX:PATH="`rospack find glc`" -DCMAKE_BUILD_TYPE:STRING="Release" -DCMAKE_C_FLAGS_RELEASE:STRING="-O2 -msse -mmmx -fomit-frame-pointer" -DCMAKE_INSTALL_RPATH:STRING="`rospack find glc`/lib" && make && make install
clean:
rm -rf $(SOURCE_DIR)
wipe: clean
- rm -rf $(TARBALL)
+ rm -rf $(TARBALL) build
Modified: pkg/trunk/3rdparty/glc/glc/Makefile
===================================================================
--- pkg/trunk/3rdparty/glc/glc/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/glc/glc/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,16 +1,16 @@
all: glc
-TARBALL = glc-0.5.7.tar.gz
+TARBALL = build/glc-0.5.7.tar.gz
TARBALL_URL = http://pr.willowgarage.com/downloads/glc-0.5.7.tar.gz
SOURCE_DIR = glc-0.5.7
UNPACK_CMD = tar xzf
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
glc: $(SOURCE_DIR)
- -mv glc glc-0.5.7
+ -mv build/glc $(SOURCE_DIR)
cd $(SOURCE_DIR) && cmake . -DCMAKE_INSTALL_PREFIX:PATH="`rospack find glc`" -DCMAKE_BUILD_TYPE:STRING="Release" -DCMAKE_C_FLAGS_RELEASE:STRING="-O2 -msse -mmmx -fomit-frame-pointer" -DCMAKE_INSTALL_RPATH:STRING="`rospack find glc`/lib" && make && make install
clean:
rm -rf $(SOURCE_DIR)
wipe: clean
- rm -rf $(TARBALL)
+ rm -rf $(TARBALL) build
Modified: pkg/trunk/3rdparty/glc/packetstream/Makefile
===================================================================
--- pkg/trunk/3rdparty/glc/packetstream/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/glc/packetstream/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,16 +1,16 @@
all: packetstream
-TARBALL = packetstream-0.1.4.tar.gz
+TARBALL = build/packetstream-0.1.4.tar.gz
TARBALL_URL = http://pr.willowgarage.com/downloads/packetstream-0.1.4.tar.gz
SOURCE_DIR = packetstream-0.1.4
UNPACK_CMD = tar xzf
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
packetstream: $(SOURCE_DIR)
- -mv packetstream packetstream-0.1.4
+ -mv build/packetstream packetstream-0.1.4
cd $(SOURCE_DIR) && cmake . -DCMAKE_INSTALL_PREFIX:PATH="`rospack find glc`" -DCMAKE_BUILD_TYPE:STRING="Release" -DCMAKE_C_FLAGS_RELEASE:STRING="-O2 -msse -mmmx -fomit-frame-pointer" -DCMAKE_INSTALL_RPATH:STRING="`rospack find glc`/lib" && make && make install
clean:
rm -rf $(SOURCE_DIR)
wipe: clean
- rm -rf $(TARBALL)
+ rm -rf $(TARBALL) build
Modified: pkg/trunk/3rdparty/gmapping/Makefile
===================================================================
--- pkg/trunk/3rdparty/gmapping/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/gmapping/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,17 +1,17 @@
all: build
-TARBALL = gmapping_r39.tar.gz
+TARBALL = build/gmapping_r39.tar.gz
TARBALL_URL = http://pr.willowgarage.com/downloads/gmapping_r39.tar.gz
-SOURCE_DIR = gmapping_export
+SOURCE_DIR = build/gmapping_export
UNPACK_CMD = tar xzf
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
PATCH = gmapping-r39.patch
build: wiped gmapping
configured: $(SOURCE_DIR) Makefile
- cd $(SOURCE_DIR) && patch -p0 < ../$(PATCH) && ./configure
+ cd $(SOURCE_DIR) && patch -p0 < ../../$(PATCH) && ./configure
touch configured
wiped: Makefile $(PATCH)
@@ -26,5 +26,5 @@
-cd $(SOURCE_DIR) && make clean
rm -rf gmapping configured
wipe: clean
- rm -rf $(SOURCE_DIR)
+ rm -rf build
Modified: pkg/trunk/3rdparty/kdl/Makefile
===================================================================
--- pkg/trunk/3rdparty/kdl/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/kdl/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -2,12 +2,12 @@
all: build
-TARBALL = kdl_29562.tar.gz
+TARBALL = build/kdl_29562.tar.gz
TARBALL_URL = http://pr.willowgarage.com/downloads/kdl_29562.tar.gz
-SOURCE_DIR = kdl_export
+SOURCE_DIR = build/kdl_export
#MD5SUM_FILE = opencv-1.0.0.tar.gz.md5sum
UNPACK_CMD = tar xzf
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
@@ -24,8 +24,8 @@
build: wiped kdl
configured: $(SOURCE_DIR) Makefile
- cd $(SOURCE_DIR) && patch -p0 <../$(PATCH1)
- cd $(SOURCE_DIR) && patch -p0 <../$(PATCH2)
+ cd $(SOURCE_DIR) && patch -p0 <../../$(PATCH1)
+ cd $(SOURCE_DIR) && patch -p0 <../../$(PATCH2)
mkdir -p $(SOURCE_DIR)/build
cd $(SOURCE_DIR)/build && $(CMAKE) $(CMAKE_ARGS) ..
touch configured
@@ -42,4 +42,4 @@
rm -rf configured
wipe: clean
- rm -rf $(TARBALL)
+ rm -rf $(TARBALL) build
Modified: pkg/trunk/3rdparty/libsunflower/Makefile
===================================================================
--- pkg/trunk/3rdparty/libsunflower/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/libsunflower/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -19,20 +19,20 @@
VERSION= r910
-TARBALL= libsunflower-$(VERSION).tar.gz
-TARBALL_URL= http://pr.willowgarage.com/downloads/$(TARBALL)
-SOURCE_DIR= $(PWD)/libsunflower-$(VERSION)
+TARBALL= build/libsunflower-$(VERSION).tar.gz
+TARBALL_URL= http://pr.willowgarage.com/downloads/libsunflower-$(VERSION).tar.gz
+SOURCE_DIR= $(PWD)/build/libsunflower-$(VERSION)
UNPACK_CMD= tar xfz
BUILD_DIR= $(PWD)/build
INST_DIR= $(PWD)/local
ESTAR_DIR= `rospack find estar`/local
BOOST_DIR= `rospack cflags-only-I boost`
-SVN_DIR= $(PWD)/sunflower-svn
+SVN_DIR= $(PWD)/build/sunflower-svn
SVN_REV= HEAD
SVN_URL= https://libsunflower.svn.sourceforge.net/svnroot/libsunflower/trunk/sunflower
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
.PHONY: really-all
really-all: $(SOURCE_DIR)/configure $(BUILD_DIR)/config.h
@@ -68,4 +68,4 @@
$(MAKE) -C $(BUILD_DIR) distclean
wipe:
- rm -rf $(BUILD_DIR) $(INST_DIR)
+ rm -rf $(BUILD_DIR) $(INST_DIR) build
Modified: pkg/trunk/3rdparty/ogre/Makefile
===================================================================
--- pkg/trunk/3rdparty/ogre/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/ogre/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,13 +1,13 @@
all: ogre
OGRE_VERSION = ogre-v1-4-9
-TARBALL = $(OGRE_VERSION).tar.bz2
-TARBALL_URL = http://pr.willowgarage.com/downloads/$(TARBALL)
-SOURCE_DIR = $(OGRE_VERSION)
+TARBALL = build/$(OGRE_VERSION).tar.bz2
+TARBALL_URL = http://pr.willowgarage.com/downloads/$(OGRE_VERSION).tar.bz2
+SOURCE_DIR = build/$(OGRE_VERSION)
UNPACK_CMD = tar xf
TARBALL_PATCH = warnings.patch
-INITIAL_DIR = ogre
-include $(shell rospack find mk)/download_unpack.mk
+INITIAL_DIR = build/ogre
+include $(shell rospack find mk)/download_unpack_build.mk
ROOT = $(shell rospack find ogre)
OGREROOT = $(shell rospack find ogre)/ogre
@@ -28,15 +28,15 @@
CONFIGURE_FLAGS = --with-arch=nocona --enable-release CXXFLAGS='-g -O3 -I$(FIROOT)/include -I$(CGROOT)/include' LDFLAGS='-Wl,-rpath,$(FIROOT)/lib,-rpath,$(CGROOT)/lib -L$(FIROOT)/lib -L$(CGROOT)/lib' CFLAGS='-g -O3 -I$(FIROOT)/include -I$(OISROOT)/include -I$(CGROOT)/include'
-$(OGRE_VERSION)/Makefile: $(OGRE_VERSION)
+$(SOURCE_DIR)/Makefile: $(SOURCE_DIR)
if test `uname` = Darwin ; then \
- touch $(OGRE_VERSION)/Makefile; \
+ touch $(SOURCE_DIR)/Makefile; \
else \
- cd $(OGRE_VERSION) && \
+ cd $(SOURCE_DIR) && \
export LIB="$(LFLAGS)" && \
export CFLAGS="$(CFLAGS)" && \
./configure --prefix=$(OGREROOT) --enable-threading --with-pic --with-platform=GLX --with-gui=gtk --disable-ogre-demos $(CONFIGURE_FLAGS) && \
- touch $(ROOT)/$(OGRE_VERSION)/Makefile && \
+ touch $(ROOT)/$(SOURCE_DIR)/Makefile && \
mkdir -p $(ROOT)/ogre && \
mkdir -p $(ROOT)/ogre/lib && \
mkdir -p $(ROOT)/ogre/include && \
@@ -66,10 +66,10 @@
ln -sf /usr/lib/OGRE ogre/lib/OGRE; \
fi
-ogre/lib/pkgconfig/OGRE.pc: $(OGRE_VERSION)/Makefile $(OGRE_VERSION)
+ogre/lib/pkgconfig/OGRE.pc: $(SOURCE_DIR)/Makefile $(SOURCE_DIR)
# building OGRE
if test `uname` = Darwin ; then \
- cd $(ROOT)/$(OGRE_VERSION)/Mac/Ogre && \
+ cd $(ROOT)/$(SOURCE_DIR)/Mac/Ogre && \
export CPATH=Developer/SDKs/MacOSX10.5.sdk/usr/X11/include:/Developer/SDKs/MacOSX10.5.sdk/usr/X11/include/freetype2:`rospack find freeimage`/freeimage/include:/opt/local/include && \
export LIBRARY_PATH=`rospack find freeimage`/freeimage/lib:/opt/local/lib && \
xcodebuild ARCHS=i386 -target Ogre && \
@@ -79,8 +79,8 @@
xcodebuild ARCHS=i386 -target "OctreeSceneManager Plugin" && \
xcodebuild ARCHS=i386 -target "Bundle Resources"; \
else \
- cd $(ROOT)/$(OGRE_VERSION) && make $(JOBC) $(PARALLEL_JOBS); \
- cd $(ROOT)/$(OGRE_VERSION) && make install; \
+ cd $(ROOT)/$(SOURCE_DIR) && make $(JOBC) $(PARALLEL_JOBS); \
+ cd $(ROOT)/$(SOURCE_DIR) && make install; \
fi
@echo "patch OGRE.pc to pass -Wl,-rpath,-L{libdir}"
-mv ogre/lib/pkgconfig/OGRE.pc ogre/lib/pkgconfig/OGRE.bak
@@ -91,11 +91,10 @@
.PHONY: clean wipe
clean:
- -cd $(ROOT)/$(OGRE_VERSION) && make clean
+ -cd $(ROOT)/$(SOURCE_DIR) && make clean
-rm -rf ogre
wipe:
- -rm -rf $(OGRE_VERSION)
- -rm -rf $(OGRE_VERSION).tar.bz2
+ -rm -rf build
Modified: pkg/trunk/3rdparty/opencv/Makefile
===================================================================
--- pkg/trunk/3rdparty/opencv/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/opencv/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,11 +1,11 @@
all: opencv
-TARBALL = opencv-1.0.0.tar.gz
+TARBALL = build/opencv-1.0.0.tar.gz
TARBALL_URL = http://pr.willowgarage.com/downloads/opencv-1.0.0.tar.gz
-SOURCE_DIR = opencv-1.0.0
+SOURCE_DIR = build/opencv-1.0.0
MD5SUM_FILE = opencv-1.0.0.tar.gz.md5sum
UNPACK_CMD = tar xzf
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
ffmpeg:
@if [ ! -d `rospack find ffmpeg`/ffmpeg ]; then rosmake ffmpeg; fi
@@ -14,7 +14,7 @@
configured: $(SOURCE_DIR) Makefile
@if [ ! -d /usr/include/gtk-2.0 ]; then echo "you don't appear to have the GTK+2.0 headers. On Ubuntu, this is fixed with 'sudo apt-get install libgtk2.0-dev'"; false; fi
- patch -N -p0 <opencv.patch ; true
+ patch -N -p0 <../opencv.patch ; true
export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${FFMPEG}/ffmpeg/lib && cd $(SOURCE_DIR) && ./configure --without-quicktime --with-ffmpeg --prefix=$(PWD)/opencv CFLAGS="-I${FFMPEG}/ffmpeg/include -I${FFMPEG}/ffmpeg/include/libavutils -I${FFMPEG}/include/libavformat" CPPFLAGS="-I${FFMPEG}/ffmpeg/include -I${FFMPEG}/ffmpeg/include/libavutils -I${FFMPEG}/ffmpeg/include/libavformat" LDFLAGS="-L${FFMPEG}/ffmpeg/lib" --with-gnu-ld --with-x
# hack up the opencv <=> ffmpeg interface so we can use the latest ffmpeg
touch configured
Modified: pkg/trunk/3rdparty/scipy/Makefile
===================================================================
--- pkg/trunk/3rdparty/scipy/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/scipy/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,12 +1,12 @@
all: scipy
-TARBALL = scipy-0.6.0.tar.gz
+TARBALL = build/scipy-0.6.0.tar.gz
TARBALL_URL = http://pr.willowgarage.com/downloads/scipy-0.6.0.tar.gz
-SOURCE_DIR = scipy-0.6.0
-MD5SUM_FILE = scipy-0.6.0.tar.gz.md5sum
+SOURCE_DIR = build/scipy-0.6.0
+MD5SUM_FILE = scipy-0.6.0.tar.gz.md5sum
UNPACK_CMD = tar xzf
NUMPY_PATH = `rospack find numpy`/src
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
scipy: $(SOURCE_DIR)
mkdir -p src
@@ -17,6 +17,6 @@
clean:
rm -rf src scipy $(SOURCE_DIR)
wipe: clean
- rm -rf $(TARBALL)
+ rm -rf build
all: scipy
Modified: pkg/trunk/3rdparty/soqt/Makefile
===================================================================
--- pkg/trunk/3rdparty/soqt/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/soqt/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,16 +1,16 @@
all: soqt
-TARBALL = SoQt-1.4.1.tar.gz
+TARBALL = build/SoQt-1.4.1.tar.gz
TARBALL_URL = http://pr.willowgarage.com/downloads/SoQt-1.4.1.tar.gz
-SOURCE_DIR = SoQt-1.4.1
+SOURCE_DIR = build/SoQt-1.4.1
MD5SUM_FILE = SoQt-1.4.1.tar.gz.md5sum
UNPACK_CMD = tar xzf
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
PATCH=soqt_qt4.patch
configured: $(SOURCE_DIR)
- cd $(SOURCE_DIR) && patch -s -N -p1 < ../$(PATCH) || echo
+ cd $(SOURCE_DIR) && patch -s -N -p1 < ../../$(PATCH) || echo
cd $(SOURCE_DIR) && autoconf && ./configure --prefix=$(PWD)
touch configured
Modified: pkg/trunk/3rdparty/vop/Makefile
===================================================================
--- pkg/trunk/3rdparty/vop/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/vop/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,17 +1,17 @@
all: vop
-TARBALL = vop-1.0.tar.gz
+TARBALL = build/vop-1.0.tar.gz
TARBALL_URL = http://www.excamera.com/files/vop-1.0.tar.gz
-SOURCE_DIR = vop-1.0
+SOURCE_DIR = build/vop-1.0
UNPACK_CMD = tar xzf
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
vop: $(SOURCE_DIR)
-rm -rf src
mkdir -p src
cd $(SOURCE_DIR) && \
python setup.py build && \
- python setup.py install --prefix ../src
+ python setup.py install --prefix ../../src
mv src/lib/python2.5/site-packages/* src/
rm -rf src/bin src/lib
touch vop
@@ -20,4 +20,4 @@
rm -rf src vop $(SOURCE_DIR)
wipe: clean
- rm -rf $(TARBALL)
+ rm -rf $(TARBALL) build
Modified: pkg/trunk/3rdparty/wxmpl/Makefile
===================================================================
--- pkg/trunk/3rdparty/wxmpl/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/wxmpl/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,10 +1,10 @@
all: wxmpl
-TARBALL = wxmpl-1.2.9.tar.gz
+TARBALL = build/wxmpl-1.2.9.tar.gz
TARBALL_URL = http://pr.willowgarage.com/downloads/wxmpl-1.2.9.tar.gz
-SOURCE_DIR = wxmpl-1.2.9
+SOURCE_DIR = build/wxmpl-1.2.9
UNPACK_CMD = tar xzf
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
wxmpl: $(SOURCE_DIR)
-mkdir src
@@ -14,4 +14,4 @@
clean:
rm -rf src wxmpl
wipe: clean
- rm -rf $(SOURCE_DIR) $(TARBALL)
+ rm -rf $(SOURCE_DIR) $(TARBALL) build
Modified: pkg/trunk/3rdparty/wxswig/Makefile
===================================================================
--- pkg/trunk/3rdparty/wxswig/Makefile 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/3rdparty/wxswig/Makefile 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,10 +1,11 @@
all: wxswig
-TARBALL = SWIG-1.3.29-wx.tar.gz
-TARBALL_URL = http://pr.willowgarage.com/downloads/$(TARBALL)
-SOURCE_DIR = SWIG-1.3.29
+PKGVERSION = SWIG-1.3.29
+TARBALL = build/$(PKGVERSION)-wx.tar.gz
+TARBALL_URL = http://pr.willowgarage.com/downloads/$(PKGVERSION)-wx.tar.gz
+SOURCE_DIR = build/SWIG-1.3.29
UNPACK_CMD = tar xzf
-include $(shell rospack find mk)/download_unpack.mk
+include $(shell rospack find mk)/download_unpack_build.mk
$(VERSION)/Makefile: $(VERSION)/Makefile.in $(VERSION)/configure $(VERSION)/configure.in
cd $(VERSION) && \
@@ -13,9 +14,9 @@
wxswig: $(SOURCE_DIR)
cd $(SOURCE_DIR) && ./configure --prefix=$(shell rospack find wxswig) && make $(PARALLEL_JOBS) && make install
touch wxswig
-
+
clean:
rm -rf bin share $(SOURCE_DIR)
-
+
wipe: clean
- rm -rf $(TARBALL)
+ rm -rf build
Deleted: pkg/trunk/drivers/network/wifi_ddwrt/.backup-CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/network/wifi_ddwrt/.backup-CMakeLists.txt 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/drivers/network/wifi_ddwrt/.backup-CMakeLists.txt 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,5 +0,0 @@
-cmake_minimum_required(VERSION 2.6)
-include(rosbuild)
-rospack(rospy_demo)
-genmsg()
-rospack_add_rostest(test/talker-listener-test.xml)
Deleted: pkg/trunk/drivers/network/wifi_ddwrt/.backup-manifest.xml
===================================================================
--- pkg/trunk/drivers/network/wifi_ddwrt/.backup-manifest.xml 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/drivers/network/wifi_ddwrt/.backup-manifest.xml 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,11 +0,0 @@
-<package>
- <description brief="dd-wrt wifi ap">
- Access to the DD-WRT wifi
- </description>
- <author>Scott Hasasn/ha...@wi...</author>
- <license>BSD</license>
- <url>http://pr.willowgarage.com/wiki/wifi_ddwrt</url>
- <depend package="rospy" />
- <depend package="std_msgs" />
- <depend package="rostest" />
-</package>
Deleted: pkg/trunk/drivers/network/wifi_ddwrt/msg/.backup-APSignal.msg
===================================================================
--- pkg/trunk/drivers/network/wifi_ddwrt/msg/.backup-APSignal.msg 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/drivers/network/wifi_ddwrt/msg/.backup-APSignal.msg 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,2 +0,0 @@
-string name
-string msg
\ No newline at end of file
Deleted: pkg/trunk/drivers/network/wifi_ddwrt/nodes/.backup-chat
===================================================================
--- pkg/trunk/drivers/network/wifi_ddwrt/nodes/.backup-chat 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/drivers/network/wifi_ddwrt/nodes/.backup-chat 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,69 +0,0 @@
-#!/usr/bin/env python
-# 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.
-#
-# Revision $Id: gossipbot.py 1013 2008-05-21 01:08:56Z sfkwc $
-
-## chat is a simple IM-like test node. It demonstrates publishing and
-## subscribing to the same topic.
-
-PKG = 'rospy_demo'
-import rostools; rostools.update_path(PKG)
-
-import sys
-
-import rospy
-from rospy_demo.msg import *
-
-def chat(name):
- def callback(msg):
- print "%s: %s"%(msg.name, msg.msg)
- rospy.Subscriber("chat", Msg, callback)
- pub = rospy.Publisher("chat", Msg)
- rospy.init_node(name)
- while not rospy.is_shutdown():
- utter = sys.stdin.readline().strip()
- if utter:
- pub.publish(Msg(name, utter))
-
-if __name__ == '__main__':
- name = None
- try:
- print "Please type your name:"
- while not name:
- name = sys.stdin.readline().strip()
- chat(name)
- except KeyboardInterrupt, k:
- pass
- print "exiting"
-
-
Deleted: pkg/trunk/drivers/network/wifi_ddwrt/nodes/.backup-ddwrt
===================================================================
--- pkg/trunk/drivers/network/wifi_ddwrt/nodes/.backup-ddwrt 2008-11-18 00:39:58 UTC (rev 6848)
+++ pkg/trunk/drivers/network/wifi_ddwrt/nodes/.backup-ddwrt 2008-11-18 00:46:16 UTC (rev 6849)
@@ -1,110 +0,0 @@
-#!/usr/bin/env python
-# 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 AD...
[truncated message content] |
|
From: <rdi...@us...> - 2008-11-18 05:57:25
|
Revision: 6870
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6870&view=rev
Author: rdiankov
Date: 2008-11-18 05:57:20 +0000 (Tue, 18 Nov 2008)
Log Message:
-----------
openrave updates
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-11-18 03:50:59 UTC (rev 6869)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-11-18 05:57:20 UTC (rev 6870)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 457
+SVN_REVISION = -r 464
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2008-11-18 03:50:59 UTC (rev 6869)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2008-11-18 05:57:20 UTC (rev 6870)
@@ -64,6 +64,11 @@
return ss.str();
}
+double rad2deg(double f)
+{
+ return f / 3.1415926 * 180.0f;
+}
+
void setupTransform(libTF::Pose3D &transform, const double *xyz, const double *rpy)
{
transform.setFromEuler(xyz[0], xyz[1], xyz[2], rpy[2], rpy[1], rpy[0]);
@@ -280,8 +285,8 @@
addKeyValue(joint, "histop", values2str(1, link->joint->limit + 1 ));
}
else {
- addKeyValue(joint, "lostop", values2str(1, link->joint->limit));
- addKeyValue(joint, "histop", values2str(1, link->joint->limit + 1));
+ addKeyValue(joint, "lostop", values2str(1, link->joint->limit,rad2deg));
+ addKeyValue(joint, "histop", values2str(1, link->joint->limit + 1,rad2deg));
}
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-11-18 08:41:56
|
Revision: 6878
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6878&view=rev
Author: jleibs
Date: 2008-11-18 08:41:47 +0000 (Tue, 18 Nov 2008)
Log Message:
-----------
Making API changes to Hokuyo and Hokuyo node.
Modified Paths:
--------------
pkg/trunk/demos/2dnav_pr2/2dnav_pr2_pspace.xml
pkg/trunk/drivers/laser/hokuyo_driver/hokuyo.cpp
pkg/trunk/drivers/laser/hokuyo_driver/hokuyo.h
pkg/trunk/drivers/laser/hokuyo_node/hokuyo_node.cpp
pkg/trunk/hardware_test/qualification/src/qualification/tests/hokuyo_test/hokuyo_test.xml
pkg/trunk/robot_descriptions/pr2_alpha/pre.launch
pkg/trunk/robot_descriptions/pr2_alpha/prf.launch
pkg/trunk/robot_descriptions/pr2_alpha/prg.launch
pkg/trunk/robot_descriptions/robo_dev_demo/demo.launch
Modified: pkg/trunk/demos/2dnav_pr2/2dnav_pr2_pspace.xml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/2dnav_pr2_pspace.xml 2008-11-18 08:08:42 UTC (rev 6877)
+++ pkg/trunk/demos/2dnav_pr2/2dnav_pr2_pspace.xml 2008-11-18 08:41:47 UTC (rev 6878)
@@ -5,8 +5,8 @@
<node pkg="hokuyo_node" type="hokuyo_node" name="base_hokuyo_node" args="scan:=base_scan" machine="base_laser_machine">
<param name="port" type="string" value="/dev/ttyACM0" />
<param name="frameid" type="string" value="base_laser" />
- <param name="min_ang" type="double" value="-100.0" />
- <param name="max_ang" type="double" value="100.0" />
+ <param name="min_ang_degrees" type="double" value="-100.0" />
+ <param name="max_ang_degrees" type="double" value="100.0" />
</node>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/phase_space_05cm.pgm 0.05" respawn="false" />
<include file="$(find highlevel_controllers)/test/launch_world_3d_map.xml"/>
Modified: pkg/trunk/drivers/laser/hokuyo_driver/hokuyo.cpp
===================================================================
--- pkg/trunk/drivers/laser/hokuyo_driver/hokuyo.cpp 2008-11-18 08:08:42 UTC (rev 6877)
+++ pkg/trunk/drivers/laser/hokuyo_driver/hokuyo.cpp 2008-11-18 08:41:47 UTC (rev 6878)
@@ -53,16 +53,16 @@
//! Helper function for querying the system time
-static unsigned long long timeHelper()
+static uint64_t timeHelper()
{
#if POSIX_TIMERS > 0
struct timespec curtime;
clock_gettime(CLOCK_REALTIME, &curtime);
- return (unsigned long long)(curtime.tv_sec) * 1000000000 + (unsigned long long)(curtime.tv_nsec);
+ return (uint64_t)(curtime.tv_sec) * 1000000000 + (uint64_t)(curtime.tv_nsec);
#else
struct timeval timeofday;
gettimeofday(&timeofday,NULL);
- return (unsigned long long)(timeofday.tv_sec) * 1000000000 + (unsigned long long)(timeofday.tv_usec) * 1000;
+ return (uint64_t)(timeofday.tv_sec) * 1000000000 + (uint64_t)(timeofday.tv_usec) * 1000;
#endif
}
@@ -85,7 +85,7 @@
///////////////////////////////////////////////////////////////////////////////
void
-hokuyo::Laser::open(const char * port_name, bool use_serial, int baud)
+hokuyo::Laser::open(const char * port_name)
{
if (portOpen())
close();
@@ -101,34 +101,19 @@
if (laser_fd_ == -1)
HOKUYO_EXCEPT_ARGS(hokuyo::Exception, "Failed to get file descriptor -- error = %d: %s", errno, strerror(errno));
- int bauds[] = {B115200, B57600, B19200};
+ // Settings for USB?
+ struct termios newtio;
+ memset (&newtio, 0, sizeof (newtio));
+ newtio.c_cflag = CS8 | CLOCAL | CREAD;
+ newtio.c_iflag = IGNPAR;
+ newtio.c_oflag = 0;
+ newtio.c_lflag = ICANON;
- if (use_serial)
- {
- int i = 0;
- for (i = 0; i < 3; i++) {
- if (changeBaud(bauds[i], baud, 100))
- break;
- }
- if (i == 3)
- HOKUYO_EXCEPT(hokuyo::Exception, "Failed to connect at any baud rate");
- }
- else
- {
- // Settings for USB?
- struct termios newtio;
- memset (&newtio, 0, sizeof (newtio));
- newtio.c_cflag = CS8 | CLOCAL | CREAD;
- newtio.c_iflag = IGNPAR;
- newtio.c_oflag = 0;
- newtio.c_lflag = ICANON;
+ // activate new settings
+ tcflush (laser_fd_, TCIFLUSH);
+ tcsetattr (laser_fd_, TCSANOW, &newtio);
+ usleep (200000);
- // activate new settings
- tcflush (laser_fd_, TCIFLUSH);
- tcsetattr (laser_fd_, TCSANOW, &newtio);
- usleep (200000);
- }
-
// Just in case a previous failure mode has left our Hokuyo
// spewing data, we send the TM2 and QT commands to be safe.
laserFlush();
@@ -325,69 +310,9 @@
return;
}
+
///////////////////////////////////////////////////////////////////////////////
bool
-hokuyo::Laser::changeBaud (int curr_baud, int new_baud, int timeout)
-{
- if (!portOpen())
- HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
-
- struct termios newtio;
- int fd;
- fd = fileno (laser_port_);
-
- if (tcgetattr (fd, &newtio) < 0)
- HOKUYO_EXCEPT_ARGS(hokuyo::Exception, "tcgetattr failed -- error = %d: %s\n", errno, strerror(errno));
-
- cfmakeraw (&newtio);
- cfsetispeed (&newtio, curr_baud);
- cfsetospeed (&newtio, curr_baud);
-
- if (tcsetattr (fd, TCSAFLUSH, &newtio) < 0 )
- HOKUYO_EXCEPT_ARGS(hokuyo::Exception, "tcsetattr failed -- error = %d: %s\n", errno, strerror(errno));
-
- char buf[100];
- memset (buf,0,sizeof (buf));
-
- switch (new_baud)
- {
- case B19200:
- sprintf(buf,"%s","S019200");
- break;
- case B57600:
- sprintf(buf,"%s","S057600");
- break;
- case B115200:
- sprintf(buf,"%s","S115200");
- break;
- default:
- HOKUYO_EXCEPT_ARGS(hokuyo::Exception, "unknown baud rate: %d",new_baud);
- }
-
- try
- {
- if (sendCmd(buf, timeout) != 0) {
- return false;
- }
- } catch (hokuyo::TimeoutException& e) { }
-
- if (tcgetattr (fd, &newtio) < 0)
- HOKUYO_EXCEPT_ARGS(hokuyo::Exception, "tcgetattr failed -- error = %d: %s\n", errno, strerror(errno));
-
- cfmakeraw (&newtio);
- cfsetispeed (&newtio, new_baud);
- cfsetospeed (&newtio, new_baud);
-
- if (tcsetattr (fd, TCSAFLUSH, &newtio) < 0 )
- HOKUYO_EXCEPT_ARGS(hokuyo::Exception, "tcsetattr failed -- error = %d: %s\n", errno, strerror(errno));
-
- usleep (200000);
- return (0);
-
-}
-
-
-bool
hokuyo::Laser::checkSum(const char* buf, int buf_len)
{
char sum = 0;
@@ -401,7 +326,8 @@
}
-unsigned long long
+///////////////////////////////////////////////////////////////////////////////
+uint64_t
hokuyo::Laser::readTime(int timeout)
{
char buf[100];
@@ -419,13 +345,16 @@
last_time_ = laser_time;
- return (unsigned long long)((wrapped_ << 24) | laser_time)*(unsigned long long)(1000000);
+ return (uint64_t)((wrapped_ << 24) | laser_time)*(uint64_t)(1000000);
}
+
+///////////////////////////////////////////////////////////////////////////////
void
-hokuyo::Laser::readData(hokuyo::LaserScan* scan, bool has_intensity, int timeout)
+hokuyo::Laser::readData(hokuyo::LaserScan& scan, bool has_intensity, int timeout)
{
- scan->num_readings = 0;
+ scan.ranges.clear();
+ scan.intensities.clear();
int data_size = 3;
if (has_intensity)
@@ -435,10 +364,13 @@
int ind = 0;
- scan->self_time_stamp = readTime(timeout);
+ scan.self_time_stamp = readTime(timeout);
int bytes;
+ float range;
+ float intensity;
+
for (;;)
{
bytes = laserReadline(&buf[ind], 100 - ind, timeout);
@@ -454,19 +386,21 @@
// Read as many ranges as we can get
for (int j = 0; j < bytes - (bytes % data_size); j+=data_size)
{
- if (scan->num_readings < MAX_READINGS)
+ if (scan.ranges.size() < MAX_READINGS)
{
- scan->ranges[scan->num_readings] = (((buf[j]-0x30) << 12) | ((buf[j+1]-0x30) << 6) | (buf[j+2]-0x30)) / 1000.0;
-
+ range = (((buf[j]-0x30) << 12) | ((buf[j+1]-0x30) << 6) | (buf[j+2]-0x30)) / 1000.0;
+ scan.ranges.push_back(range);
+
if (has_intensity)
- scan->intensities[scan->num_readings] = (((buf[j+3]-0x30) << 12) | ((buf[j+4]-0x30) << 6) | (buf[j+5]-0x30));
- else
- scan->intensities[scan->num_readings] = 0;
-
- scan->num_readings++;
+ {
+ intensity = (((buf[j+3]-0x30) << 12) | ((buf[j+4]-0x30) << 6) | (buf[j+5]-0x30));
+ scan.intensities.push_back(intensity);
+ }
}
else
+ {
HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Got more readings than expected");
+ }
}
// Shuffle remaining bytes to front of buffer to get them on the next loop
ind = 0;
@@ -478,16 +412,18 @@
///////////////////////////////////////////////////////////////////////////////
int
-hokuyo::Laser::pollScan(hokuyo::LaserScan* scan, double min_ang, double max_ang, int cluster, int timeout)
+hokuyo::Laser::pollScan(hokuyo::LaserScan& scan, double min_ang, double max_ang, int cluster, int timeout)
{
if (!portOpen())
HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
int status;
- // Always set num_readings to 0 so we can return easily in case of erro
- scan->num_readings = 0;
+ // Always clear ranges/intensities so we can return easily in case of erro
+ scan.ranges.clear();
+ scan.intensities.clear();
+ // clustering of 0 and 1 are actually the same
if (cluster == 0)
cluster = 1;
@@ -500,26 +436,26 @@
status = sendCmd(cmdbuf, timeout);
- scan->system_time_stamp = timeHelper() + offset_;
+ scan.system_time_stamp = timeHelper() + offset_;
if (status != 0)
return status;
// Populate configuration
- scan->config.min_angle = (min_i - afrt_) * (2.0*M_PI)/(ares_);
- scan->config.max_angle = (max_i - afrt_) * (2.0*M_PI)/(ares_);
- scan->config.ang_increment = cluster*(2.0*M_PI)/(ares_);
- scan->config.time_increment = (60.0)/(double)(rate_ * ares_);
- scan->config.scan_time = 0.0;
- scan->config.min_range = dmin_ / 1000.0;
- scan->config.max_range = dmax_ / 1000.0;
+ scan.config.min_angle = (min_i - afrt_) * (2.0*M_PI)/(ares_);
+ scan.config.max_angle = (max_i - afrt_) * (2.0*M_PI)/(ares_);
+ scan.config.ang_increment = cluster*(2.0*M_PI)/(ares_);
+ scan.config.time_increment = (60.0)/(double)(rate_ * ares_);
+ scan.config.scan_time = 0.0;
+ scan.config.min_range = dmin_ / 1000.0;
+ scan.config.max_range = dmax_ / 1000.0;
readData(scan, false, timeout);
- long long inc = (long long)(min_i * scan->config.time_increment * 1000000000);
+ long long inc = (long long)(min_i * scan.config.time_increment * 1000000000);
- scan->system_time_stamp += inc;
- scan->self_time_stamp += inc;
+ scan.system_time_stamp += inc;
+ scan.self_time_stamp += inc;
return 0;
}
@@ -572,13 +508,14 @@
int
-hokuyo::Laser::serviceScan(hokuyo::LaserScan* scan, int timeout)
+hokuyo::Laser::serviceScan(hokuyo::LaserScan& scan, int timeout)
{
if (!portOpen())
HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
- // Always set num_readings to 0 so we can return easily in case of error
- scan->num_readings = 0;
+ // Always clear ranges/intensities so we can return easily in case of erro
+ scan.ranges.clear();
+ scan.intensities.clear();
char buf[100];
@@ -595,7 +532,7 @@
do {
ind = laserReadlineAfter(buf, 100, "M",timeout);
- scan->system_time_stamp = timeHelper() + offset_;
+ scan.system_time_stamp = timeHelper() + offset_;
if (ind[0] == 'D')
intensity = false;
@@ -621,20 +558,20 @@
} while(status != 99);
- scan->config.min_angle = (min_i - afrt_) * (2.0*M_PI)/(ares_);
- scan->config.max_angle = (max_i - afrt_) * (2.0*M_PI)/(ares_);
- scan->config.ang_increment = cluster*(2.0*M_PI)/(ares_);
- scan->config.time_increment = (60.0)/(double)(rate_ * ares_);
- scan->config.scan_time = (60.0 * (skip + 1))/((double)(rate_));
- scan->config.min_range = dmin_ / 1000.0;
- scan->config.max_range = dmax_ / 1000.0;
+ scan.config.min_angle = (min_i - afrt_) * (2.0*M_PI)/(ares_);
+ scan.config.max_angle = (max_i - afrt_) * (2.0*M_PI)/(ares_);
+ scan.config.ang_increment = cluster*(2.0*M_PI)/(ares_);
+ scan.config.time_increment = (60.0)/(double)(rate_ * ares_);
+ scan.config.scan_time = (60.0 * (skip + 1))/((double)(rate_));
+ scan.config.min_range = dmin_ / 1000.0;
+ scan.config.max_range = dmax_ / 1000.0;
readData(scan, intensity, timeout);
- long long inc = (long long)(min_i * scan->config.time_increment * 1000000000);
+ long long inc = (long long)(min_i * scan.config.time_increment * 1000000000);
- scan->system_time_stamp += inc;
- scan->self_time_stamp += inc;
+ scan.system_time_stamp += inc;
+ scan.self_time_stamp += inc;
return 0;
}
@@ -688,8 +625,8 @@
offset_ = 0;
- unsigned long long comp_time = 0;
- unsigned long long laser_time = 0;
+ uint64_t comp_time = 0;
+ uint64_t laser_time = 0;
long long diff_time = 0;
long long drift_time = 0;
long long tmp_offset1 = 0;
@@ -712,7 +649,7 @@
tmp_offset1 += diff_time / count;
}
- unsigned long long start_time = timeHelper();
+ uint64_t start_time = timeHelper();
usleep(5000000);
sendCmd("TM1;a",timeout);
sendCmd("TM1;b",timeout);
@@ -734,7 +671,7 @@
{
try
{
- serviceScan(&scan, 1000);
+ serviceScan(scan, 1000);
} catch (hokuyo::CorruptedDataException &e) {
continue;
}
Modified: pkg/trunk/drivers/laser/hokuyo_driver/hokuyo.h
===================================================================
--- pkg/trunk/drivers/laser/hokuyo_driver/hokuyo.h 2008-11-18 08:08:42 UTC (rev 6877)
+++ pkg/trunk/drivers/laser/hokuyo_driver/hokuyo.h 2008-11-18 08:41:47 UTC (rev 6878)
@@ -28,12 +28,13 @@
#include <stdexcept>
#include <termios.h>
#include <string>
+#include <vector>
//! A namespace containing the hokuyo device driver
namespace hokuyo
{
//! The maximum number of readings that can be returned from a hokuyo
- const int MAX_READINGS = 1128;
+ const uint32_t MAX_READINGS = 1128;
//! The maximum length a command will ever be
const int MAX_CMD_LEN = 100;
@@ -62,9 +63,9 @@
//! A struct for returning configuration from the Hokuyo
struct LaserConfig
{
- //! Start angle for the laser scan [rad].
+ //! Start angle for the laser scan [rad]. 0 is forward and angles are measured clockwise when viewing hokuyo from the top.
float min_angle;
- //! Stop angle for the laser scan [rad].
+ //! Stop angle for the laser scan [rad]. 0 is forward and angles are measured clockwise when viewing hokuyo from the top.
float max_angle;
//! Scan resolution [rad].
float ang_increment;
@@ -85,29 +86,34 @@
struct LaserScan
{
//! Array of ranges
- float ranges[MAX_READINGS];
+ std::vector<float> ranges;
//! Array of intensities
- float intensities[MAX_READINGS];
- //! Number of readings
- int num_readings;
+ std::vector<float> intensities;
//! Self reported time stamp in nanoseconds
- unsigned long long self_time_stamp;
+ uint64_t self_time_stamp;
//! System time when first range was measured in nanoseconds
- unsigned long long system_time_stamp;
+ uint64_t system_time_stamp;
//! Configuration of scan
LaserConfig config;
};
//! A class for interfacing to an SCIP2.0-compliant Hokuyo laser device
- /*!
- * NOTE: Just about all methods of this class may throw an exception
- * of type hokuyo::exception in the event of an error when
- * communicating with the device. However, many methods which wrap
- * commands that are sent to the hokuyo will return a hokuyo-supplied
- * status code. This code may indicate some form of failure on the
- * part of the device itself. For more information, consult the hokuyo
- * manual.
+ /*!
+ * Almost all methods of this class may throw an exception of type
+ * hokuyo::Exception in the event of an error when communicating
+ * with the device. This primarily include read/write problems
+ * (hokuyo::Exception), communication time-outs
+ * (hokuyo:TimeoutException), and corrupted data
+ * (hokuyo::CorruptedDataException). However, many methods which
+ * wrap commands that are sent to the hokuyo will return a
+ * hokuyo-supplied status code as well. These are not
+ * "exceptional," in that the device is operating correctly from a
+ * communication standpoint, although the return code may still
+ * indicate that the device has undergone some sort of failure. In
+ * these cases, return codes of 0 are normal operation. For more
+ * information on the possible device error codes, consult the
+ * hokuyo manual.
*/
class Laser
{
@@ -124,11 +130,9 @@
* wraps fopen, with some additional calls to tcsetattr.
*
* \param port_name A character array containing the name of the port
- * \param use_serial Set to 1 if using serial instead of USB
- * \param baud Baud rate to use when working over serial
*
*/
- void open(const char * port_name, bool use_serial = 0, int baud = B115200);
+ void open(const char * port_name);
//! Close the port
/*!
@@ -139,6 +143,7 @@
//! Check whether the port is open
bool portOpen() { return laser_port_ != NULL; }
+
//! Sends an SCIP2.0 command to the hokuyo device
/*!
* This function allows commands to be sent directly to the hokuyo.
@@ -153,23 +158,10 @@
*/
int sendCmd(const char* cmd, int timeout = -1);
- //! Change the baud rate
- /*!
- * Baud rates are specified using defines in termios:
- *
- * B19200 - B250000
- *
- * \param curr_baud The current baud rate
- * \param new_baud The desired baud rate
- * \param timeout Timeout in milliseconds.
- *
- * \return Returns true on success, false on failure
- */
- bool changeBaud(int curr_baud, int new_baud, int timeout = -1);
//! Retrieve a single scan from the hokuyo
/*!
- * \param scan A pointer to an LaserScan which will be populated
+ * \param scan A reference to the LaserScan which will be populated
* \param min_ang Minimal angle of the scan
* \param max_ang Maximum angle of the scan
* \param clustering Number of adjascent ranges to be clustered into a single measurement.
@@ -177,7 +169,7 @@
*
* \return Status code returned from hokuyo device.
*/
- int pollScan(LaserScan * scan, double min_ang, double max_ang, int clustering = 0, int timeout = -1);
+ int pollScan(LaserScan& scan, double min_ang, double max_ang, int clustering = 0, int timeout = -1);
//! Request a sequence of scans from the hokuyo
/*!
@@ -200,12 +192,12 @@
//! Retrieve a scan if they have been requested
/*!
- * \param scan A pointer to an LaserScan which will be populated.
+ * \param scan A reference to the LaserScan which will be populated.
* \param timeout Timeout in milliseconds.
*
* \return 0 on succes, Status code returned from hokuyo device on failure. (Normally 99 is used for success)
*/
- int serviceScan(LaserScan * scan, int timeout = -1);
+ int serviceScan(LaserScan& scan, int timeout = -1);
//! Turn the laser off
/*!
@@ -283,10 +275,10 @@
bool checkSum(const char* buf, int buf_len);
//! Read in a time value
- unsigned long long readTime(int timeout = -1);
+ uint64_t readTime(int timeout = -1);
//! Read in a scan
- void readData(LaserScan* scan, bool has_intensity, int timout = -1);
+ void readData(LaserScan& scan, bool has_intensity, int timout = -1);
int dmin_;
int dmax_;
Modified: pkg/trunk/drivers/laser/hokuyo_node/hokuyo_node.cpp
===================================================================
--- pkg/trunk/drivers/laser/hokuyo_node/hokuyo_node.cpp 2008-11-18 08:08:42 UTC (rev 6877)
+++ pkg/trunk/drivers/laser/hokuyo_node/hokuyo_node.cpp 2008-11-18 08:41:47 UTC (rev 6878)
@@ -83,15 +83,16 @@
Reads the following parameters from the parameter server
-- @b "~min_ang" : @b [double] the angle of the first range measurement in degrees (Default: -90.0)
-- @b "~max_ang" : @b [double] the angle of the last range measurement in degrees (Default: 90.0)
-- @b "~cluster" : @b [int] the number of adjascent range measurements to cluster into a single reading (Default: 1)
-- @b "~skip" : @b [int] the number of scans to skip between each measured scan (Default: 1)
-- @b "~port" : @b [string] the port where the hokuyo device can be found (Default: "/dev/ttyACM0")
-- @b "~autostart" : @b [bool] whether the node should automatically start the hokuyo (Default: true)
-- @b "~calibrate_time" : @b [bool] whether the node should calibrate the hokuyo's time offset (Default: true)
-- @b "~frame_id" : @b [string] the frame in which laser scans will be returned (Default: "FRAMEID_LASER")
-
+- @b "~min_ang_degrees" : @b [double] the angle of the first range measurement in degrees (Default: -90.0)
+- @b "~max_ang_degrees" : @b [double] the angle of the last range measurement in degrees (Default: 90.0)
+- @b "~min_ang" : @b [double] the angle of the first range measurement in radians (Default: -pi/2)
+- @b "~max_ang" : @b [double] the angle of the last range measurement in radians (Default: pi/2)
+- @b "~cluster" : @b [int] the number of adjascent range measurements to cluster into a single reading (Default: 1)
+- @b "~skip" : @b [int] the number of scans to skip between each measured scan (Default: 1)
+- @b "~port" : @b [string] the port where the hokuyo device can be found (Default: "/dev/ttyACM0")
+- @b "~autostart" : @b [bool] whether the node should automatically start the hokuyo (Default: true)
+- @b "~calibrate_time" : @b [bool] whether the node should calibrate the hokuyo's time offset (Default: true)
+- @b "~frame_id" : @b [string] the frame in which laser scans will be returned (Default: "FRAMEID_LASER")
**/
#include <assert.h>
@@ -147,10 +148,48 @@
{
advertise<std_msgs::LaserScan>("scan", 100);
- param("~min_ang", min_ang_, -90.0);
- min_ang_ *= M_PI/180;
- param("~max_ang", max_ang_, 90.0);
- max_ang_ *= M_PI/180;
+ if (has_param("~min_ang_degrees") && has_param("~min_ang"))
+ {
+ ROS_FATAL("Minimum angle is specified in both radians and degrees");
+ self_destruct();
+ }
+
+ if (has_param("~max_ang_degrees") && has_param("~max_ang"))
+ {
+ ROS_FATAL("Maximum angle is specified in both radians and degrees");
+ self_destruct();
+ }
+
+ if (has_param("~min_ang_degrees"))
+ {
+ get_param("~min_ang_degrees", min_ang_);
+ min_ang_ *= M_PI/180;
+ }
+ else if (has_param("~min_ang"))
+ {
+ get_param("~min_ang", min_ang_);
+ }
+ else
+ {
+ min_ang_ = -M_PI/2.0;
+ }
+
+ if (has_param("~max_ang_degrees"))
+ {
+ get_param("~max_ang_degrees", max_ang_);
+ max_ang_ *= M_PI/180;
+ }
+ else if (has_param("~max_ang"))
+ {
+ get_param("~max_ang", max_ang_);
+ }
+ else
+ {
+ max_ang_ = M_PI/2.0;
+ }
+
+ printf("%f --> %f\n", min_ang_, max_ang_);
+
param("~cluster", cluster_, 1);
param("~skip", skip_, 1);
param("~port", port_, string("/dev/ttyACM0"));
@@ -239,7 +278,7 @@
{
try
{
- int status = laser_.serviceScan(&scan_);
+ int status = laser_.serviceScan(scan_);
if(status != 0)
{
@@ -262,17 +301,11 @@
scan_msg_.scan_time = scan_.config.scan_time;
scan_msg_.range_min = scan_.config.min_range;
scan_msg_.range_max = scan_.config.max_range;
- scan_msg_.set_ranges_size(scan_.num_readings);
- scan_msg_.set_intensities_size(scan_.num_readings);
+ scan_msg_.ranges = scan_.ranges;
+ scan_msg_.intensities = scan_.intensities;
scan_msg_.header.stamp = ros::Time((uint64_t)scan_.system_time_stamp);
scan_msg_.header.frame_id = frameid_;
- for(int i = 0; i < scan_.num_readings; ++i)
- {
- scan_msg_.ranges[i] = scan_.ranges[i];
- scan_msg_.intensities[i] = scan_.intensities[i];
- }
-
publish("scan", scan_msg_);
count_++;
@@ -451,7 +484,7 @@
hokuyo::LaserScan scan;
- int res = laser_.pollScan(&scan, min_ang_, max_ang_, cluster_, 1000);
+ int res = laser_.pollScan(scan, min_ang_, max_ang_, cluster_, 1000);
if (res != 0)
{
@@ -485,7 +518,7 @@
for (int i = 0; i < 99; i++)
{
- laser_.serviceScan(&scan, 1000);
+ laser_.serviceScan(scan, 1000);
}
status.level = 0;
@@ -516,7 +549,7 @@
for (int i = 0; i < 99; i++)
{
try {
- laser_.serviceScan(&scan, 1000);
+ laser_.serviceScan(scan, 1000);
} catch (hokuyo::CorruptedDataException &e) {
corrupted_data++;
}
Modified: pkg/trunk/hardware_test/qualification/src/qualification/tests/hokuyo_test/hokuyo_test.xml
===================================================================
--- pkg/trunk/hardware_test/qualification/src/qualification/tests/hokuyo_test/hokuyo_test.xml 2008-11-18 08:08:42 UTC (rev 6877)
+++ pkg/trunk/hardware_test/qualification/src/qualification/tests/hokuyo_test/hokuyo_test.xml 2008-11-18 08:41:47 UTC (rev 6878)
@@ -1,6 +1,6 @@
<launch>
<param name="hokuyo/calibrate_time" value="False" type="bool"/>
- <param name="hokuyo/min_ang" value="-15.0" type="double"/>
- <param name="hokuyo/max_ang" value="15.0" type="double"/>
+ <param name="hokuyo/min_ang_degrees" value="-15.0" type="double"/>
+ <param name="hokuyo/max_ang_degrees" value="15.0" type="double"/>
<node pkg="hokuyo_node" type="hokuyo_node"/>
</launch>
Modified: pkg/trunk/robot_descriptions/pr2_alpha/pre.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2_alpha/pre.launch 2008-11-18 08:08:42 UTC (rev 6877)
+++ pkg/trunk/robot_descriptions/pr2_alpha/pre.launch 2008-11-18 08:41:47 UTC (rev 6878)
@@ -24,16 +24,16 @@
<node machine="xenomai" pkg="hokuyo_node" type="hokuyo_node" name="base_hokuyo_node" args="scan:=base_scan">
<param name="port" type="string" value="/dev/ttyACM0" />
<param name="frameid" type="string" value="base_laser" />
- <param name="min_ang" type="double" value="-100.0" />
- <param name="max_ang" type="double" value="100.0" />
+ <param name="min_ang_degrees" type="double" value="-100.0" />
+ <param name="max_ang_degrees" type="double" value="100.0" />
</node>
<!-- Tilt Laser
<node machine="xenomai" pkg="hokuyo_node" type="hokuyo_node" name="tilt_hokuyo_node" args="scan:=tilt_scan">
<param name="port" type="string" value="/dev/ttyACM1" />
<param name="frameid" type="string" value="tilt_laser" />
- <param name="min_ang" type="double" value="-80.0" />
- <param name="max_ang" type="double" value="80.0" />
+ <param name="min_ang_degrees" type="double" value="-80.0" />
+ <param name="max_ang_degrees" type="double" value="80.0" />
</node>
-->
<!-- Runtime Diagnostics Logging -->
Modified: pkg/trunk/robot_descriptions/pr2_alpha/prf.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2_alpha/prf.launch 2008-11-18 08:08:42 UTC (rev 6877)
+++ pkg/trunk/robot_descriptions/pr2_alpha/prf.launch 2008-11-18 08:41:47 UTC (rev 6878)
@@ -24,16 +24,16 @@
<node machine="two" pkg="hokuyo_node" type="hokuyo_node" name="base_hokuyo_node" args="scan:=base_scan">
<param name="port" type="string" value="/dev/ttyACM0" />
<param name="frameid" type="string" value="base_laser" />
- <param name="min_ang" type="double" value="-100.0" />
- <param name="max_ang" type="double" value="100.0" />
+ <param name="min_ang_degrees" type="double" value="-100.0" />
+ <param name="max_ang_degrees" type="double" value="100.0" />
</node>
<!-- Tilt Laser -->
<node machine="three" pkg="hokuyo_node" type="hokuyo_node" name="tilt_hokuyo_node" args="scan:=tilt_scan">
<param name="port" type="string" value="/dev/ttyACM0" />
<param name="frameid" type="string" value="tilt_laser" />
- <param name="min_ang" type="double" value="-80.0" />
- <param name="max_ang" type="double" value="80.0" />
+ <param name="min_ang_degrees" type="double" value="-80.0" />
+ <param name="max_ang_degrees" type="double" value="80.0" />
</node>
<!-- Runtime Diagnostics Logging -->
Modified: pkg/trunk/robot_descriptions/pr2_alpha/prg.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2_alpha/prg.launch 2008-11-18 08:08:42 UTC (rev 6877)
+++ pkg/trunk/robot_descriptions/pr2_alpha/prg.launch 2008-11-18 08:41:47 UTC (rev 6878)
@@ -24,16 +24,16 @@
<node machine="xenomai" pkg="hokuyo_node" type="hokuyo_node" name="base_hokuyo_node" args="scan:=base_scan">
<param name="port" type="string" value="/dev/ttyACM0" />
<param name="frameid" type="string" value="base_laser" />
- <param name="min_ang" type="double" value="-100.0" />
- <param name="max_ang" type="double" value="100.0" />
+ <param name="min_ang_degrees" type="double" value="-100.0" />
+ <param name="max_ang_degrees" type="double" value="100.0" />
</node>
<!-- Tilt Laser -->
<node machine="xenomai" pkg="hokuyo_node" type="hokuyo_node" name="tilt_hokuyo_node" args="scan:=tilt_scan">
<param name="port" type="string" value="/dev/ttyACM0" />
<param name="frameid" type="string" value="tilt_laser" />
- <param name="min_ang" type="double" value="-80.0" />
- <param name="max_ang" type="double" value="80.0" />
+ <param name="min_ang_degrees" type="double" value="-80.0" />
+ <param name="max_ang_degrees" type="double" value="80.0" />
</node>
<!-- Runtime Diagnostics Logging -->
Modified: pkg/trunk/robot_descriptions/robo_dev_demo/demo.launch
===================================================================
--- pkg/trunk/robot_descriptions/robo_dev_demo/demo.launch 2008-11-18 08:08:42 UTC (rev 6877)
+++ pkg/trunk/robot_descriptions/robo_dev_demo/demo.launch 2008-11-18 08:41:47 UTC (rev 6878)
@@ -20,8 +20,8 @@
<node machine="xenomai" pkg="hokuyo_node" type="hokuyo_node" name="tilt_hokuyo_node" args="scan:=tilt_scan">
<param name="port" ...
[truncated message content] |
|
From: <mc...@us...> - 2008-11-18 14:29:18
|
Revision: 6881
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6881&view=rev
Author: mcgann
Date: 2008-11-18 14:29:12 +0000 (Tue, 18 Nov 2008)
Log Message:
-----------
Added observation buffers for multiple sources
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/highlevel/highlevel_controllers/test/launch_move_base.xml
pkg/trunk/highlevel/highlevel_controllers/test/launch_stage_5cm.xml
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
Modified: pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-18 14:25:33 UTC (rev 6880)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-18 14:29:12 UTC (rev 6881)
@@ -63,6 +63,38 @@
namespace ros {
namespace highlevel_controllers {
+ /**
+ * @brief Extend base class to handle buffering until a transform is available, and to support locking for mult-threaded
+ * access
+ */
+ class ObservationBuffer: public costmap_2d::ObservationBuffer {
+ public:
+ ObservationBuffer(const std::string& frame_id, rosTFClient& tf, ros::Duration keepAlive, double robotRadius, double minZ, double maxZ);
+
+ void buffer_cloud(const std_msgs::PointCloud& local_cloud);
+
+ virtual void get_observations(std::vector<Observation>& observations);
+
+ private:
+
+ /**
+ * @brief Test if point in the square footprint of the robot
+ */
+ bool inFootprint(double x, double y) const;
+
+ /**
+ * @brief Provide a filtered set of points based on the extraction of the robot footprint and the
+ * filter based on the min and max z values
+ */
+ std_msgs::PointCloud * extractFootprintAndGround(const std_msgs::PointCloud& baseFrameCloud) const;
+
+ const std::string frame_id_;
+ rosTFClient& tf_;
+ std::deque<std_msgs::PointCloud> point_clouds_; /**< Buffer point clouds until a transform is available */
+ ros::thread::mutex buffer_mutex_;
+ const double robotRadius_, minZ_, maxZ_; /**< Constraints for filtering points */
+ };
+
class MoveBase : public HighlevelController<std_msgs::Planner2DState, std_msgs::Planner2DGoal> {
public:
@@ -149,22 +181,9 @@
*/
void odomCallback();
- void bufferData(const std_msgs::PointCloud& local_cloud);
-
- /**
- * @brief Process point cloud data
- * @todo migrate to costmap
- */
- void processData();
-
void updateGlobalPose();
/**
- * @brief Helper method to update the costmap and conduct other book-keeping
- */
- void updateDynamicObstacles(const std::vector<std_msgs::PointCloud*>& clouds);
-
- /**
* @brief Issue zero velocity commands
*/
void stopRobot();
@@ -190,17 +209,6 @@
bool withinDistance(double x1, double y1, double th1, double x2, double y2, double th2) const ;
/**
- * @brief Test if point in the square footprint of the robot
- */
- bool inFootprint(double x, double y) const;
-
- /**
- * @brief Provide a filtered set of points based on the extraction of the robot footprint and the
- * filter based on the min and max z values
- */
- std_msgs::PointCloud * extractFootprintAndGround(const std_msgs::PointCloud& baseFrameCloud) const;
-
- /**
* @brief Watchdog Handling
*/
void petTheWatchDog(const ros::Time& t);
@@ -220,6 +228,11 @@
rosTFClient tf_; /**< Used to do transforms */
+ // Observation Buffers
+ ObservationBuffer* baseScanBuffer_;
+ ObservationBuffer* tiltScanBuffer_;
+ ObservationBuffer* stereoCloudBuffer_;
+
/** Should encapsulate as a controller wrapper that is not resident in the trajectory rollout package */
VelocityController* controller_;
@@ -237,9 +250,6 @@
std::list<std_msgs::Pose2DFloat32> plan_; /**< The 2D plan in grid co-ordinates of the cost map */
- std::deque<std_msgs::PointCloud> point_clouds_; /**< Buffer point clouds until a transform is available */
- ros::thread::mutex bufferMutex_;
-
// Filter parameters
double minZ_;
double maxZ_;
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-18 14:25:33 UTC (rev 6880)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-18 14:29:12 UTC (rev 6881)
@@ -46,14 +46,153 @@
namespace ros {
namespace highlevel_controllers {
+ ObservationBuffer::ObservationBuffer(const std::string& frame_id, rosTFClient& tf, ros::Duration keepAlive,
+ double robotRadius, double minZ, double maxZ)
+ : costmap_2d::ObservationBuffer(keepAlive), frame_id_(frame_id), tf_(tf), robotRadius_(robotRadius), minZ_(minZ), maxZ_(maxZ){}
+
+ void ObservationBuffer::buffer_cloud(const std_msgs::PointCloud& local_cloud)
+ {
+ static const ros::Duration max_transform_delay(10, 0); // max time we will wait for a transform before chucking out the data
+ point_clouds_.push_back(local_cloud);
+
+ std_msgs::PointCloud * newData = NULL;
+ std_msgs::PointCloud * map_cloud = NULL;
+
+ while(!point_clouds_.empty()){
+ const std_msgs::PointCloud& point_cloud = point_clouds_.front();
+ std_msgs::Point origin;
+
+ // Throw out point clouds that are old.
+ if((last_updated_ - point_cloud.header.stamp) > max_transform_delay){
+ ROS_INFO("Discarding stale point cloud\n");
+ point_clouds_.pop_front();
+ continue;
+ }
+
+ libTF::TFPoint map_origin;
+ std_msgs::PointCloud base_cloud;
+
+ /* Transform to the base frame */
+ try
+ {
+ // First we want the origin for the sensor
+ libTF::TFPoint local_origin;
+ local_origin.x = 0;
+ local_origin.y = 0;
+ local_origin.z = 0;
+ local_origin.time = point_cloud.header.stamp.toNSec();
+ local_origin.frame = frame_id_;
+ map_origin = tf_.transformPoint("map", local_origin);
+
+ tf_.transformPointCloud("base", base_cloud, point_cloud);
+ newData = extractFootprintAndGround(base_cloud);
+ map_cloud = new std_msgs::PointCloud();
+ tf_.transformPointCloud("map", *map_cloud, *newData);
+
+ ROS_INFO("Buffering cloud for %s at origin <%f, %f, %f>\n", frame_id_.c_str(), map_origin.x, map_origin.y, map_origin.z);
+ }
+ catch(libTF::TransformReference::LookupException& ex)
+ {
+ ROS_ERROR("Lookup exception: %s\n", ex.what());
+ break;
+ }
+ catch(libTF::TransformReference::ExtrapolateException& ex)
+ {
+ ROS_DEBUG("No transform available yet - have to try later: %s . Buffer size is %d\n", ex.what(), point_clouds_.size());
+ break;
+ }
+ catch(libTF::TransformReference::ConnectivityException& ex)
+ {
+ ROS_ERROR("Connectivity exception: %s\n", ex.what());
+ break;
+ }
+ catch(...)
+ {
+ ROS_ERROR("Exception in point cloud computation\n");
+ break;
+ }
+
+ point_clouds_.pop_front();
+
+ if (newData != NULL){
+ delete newData;
+ newData = NULL;
+ }
+
+ // Get the point from whihc we ray trace
+ std_msgs::Point o;
+ o.x = map_origin.x;
+ o.y = map_origin.y;
+ o.z = map_origin.z;
+
+ // Allocate and buffer the observation
+ Observation obs(o, map_cloud);
+ buffer_observation(obs);
+ map_cloud = NULL;
+ }
+
+ // In case we get thrown out on the second transform - clean up
+ if (newData != NULL){
+ delete newData;
+ newData = NULL;
+ }
+
+ if(map_cloud != NULL){
+ delete map_cloud;
+ map_cloud = NULL;
+ } }
+
+ void ObservationBuffer::get_observations(std::vector<costmap_2d::Observation>& observations){
+ costmap_2d::ObservationBuffer::get_observations(observations);
+ }
+
+ /**
+ * The point is in the footprint if its x and y values are in the range [0 robotWidth] in
+ * the base frame.
+ */
+ bool ObservationBuffer::inFootprint(double x, double y) const {
+ bool result = fabs(x) <= robotRadius_ && fabs(y) <= robotRadius_;
+
+ if(result){
+ ROS_DEBUG("Discarding point <%f, %f> in footprint\n", x, y);
+ }
+
+ return result;
+ }
+
+ std_msgs::PointCloud * ObservationBuffer::extractFootprintAndGround(const std_msgs::PointCloud& baseFrameCloud) const {
+ std_msgs::PointCloud *copy = new std_msgs::PointCloud();
+ copy->header = baseFrameCloud.header;
+
+ unsigned int n = baseFrameCloud.get_pts_size();
+ unsigned int j = 0;
+ copy->set_pts_size(n);
+
+ for (unsigned int k = 0 ; k < n ; ++k){
+ bool ok = baseFrameCloud.pts[k].z > minZ_ && baseFrameCloud.pts[k].z < maxZ_;
+ if (ok && !inFootprint(baseFrameCloud.pts[k].x, baseFrameCloud.pts[k].y)){
+ copy->pts[j++] = baseFrameCloud.pts[k];
+ }
+ else {
+ ROS_DEBUG("Discarding <%f, %f, %f>\n", baseFrameCloud.pts[k].x, baseFrameCloud.pts[k].y, baseFrameCloud.pts[k].z);
+ }
+ }
+
+ copy->set_pts_size(j);
+
+ ROS_DEBUG("Filter discarded %d points (%d left) \n", n - j, j);
+
+ return copy;
+ }
+
MoveBase::MoveBase()
: HighlevelController<std_msgs::Planner2DState, std_msgs::Planner2DGoal>("move_base", "state", "goal"),
- tf_(*this, true, 10000000000ULL, 0ULL), // cache for 10 sec, no extrapolation
- controller_(NULL),
- costMap_(NULL),
- ma_(NULL),
- baseLaserMaxRange_(10.0),
- tiltLaserMaxRange_(10.0),
+ tf_(*this, true, 10000000000ULL, 0ULL), // cache for 10 sec, no extrapolation
+ controller_(NULL),
+ costMap_(NULL),
+ ma_(NULL),
+ baseLaserMaxRange_(10.0),
+ tiltLaserMaxRange_(10.0),
minZ_(0.03), maxZ_(2.0), robotWidth_(0.0), active_(true) , map_update_frequency_(10.0)
{
// Initialize global pose. Will be set in control loop based on actual data.
@@ -66,7 +205,7 @@
base_odom_.vel.y = 0;
base_odom_.vel.th = 0;
- // Initialize state message parameters that are unsused
+ // Initialize state message parameters that are unused
stateMsg.waypoint.x = 0.0;
stateMsg.waypoint.y = 0.0;
stateMsg.waypoint.th = 0.0;
@@ -104,6 +243,11 @@
robotWidth_ = inscribedRadius * 2;
+ // Allocate observation buffers
+ baseScanBuffer_ = new ObservationBuffer(std::string("base_laser"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
+ tiltScanBuffer_ = new ObservationBuffer(std::string("tilt_laser"), tf_, ros::Duration(1, 0), inscribedRadius, minZ_, maxZ_);
+ stereoCloudBuffer_ = new ObservationBuffer(std::string("stereo"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
+
// get map via RPC
std_srvs::StaticMap::request req;
std_srvs::StaticMap::response resp;
@@ -128,7 +272,7 @@
// Now allocate the cost map and its sliding window used by the controller
costMap_ = new CostMap2D((unsigned int)resp.map.width, (unsigned int)resp.map.height,
inputData , resp.map.resolution,
- lethalObstacleThreshold, maxZ_, freeSpaceProjectionHeight,
+ lethalObstacleThreshold, maxZ_, 0.0, freeSpaceProjectionHeight,
inflationRadius, circumscribedRadius, inscribedRadius, weight);
// Allocate Velocity Controller
@@ -204,7 +348,7 @@
// Advertize message to publish velocity cmds
advertise<std_msgs::BaseVel>("cmd_vel", 1);
- //Advertize message to publis local goal for head to track
+ //Advertize message to publish local goal for head to track
advertise<std_msgs::PointStamped>("head_controller/head_track_point", 1);
// The cost map is populated with either laser scans in the case that we are unable to use a
@@ -238,6 +382,9 @@
if(costMap_ != NULL)
delete costMap_;
+ delete baseScanBuffer_;
+ delete tiltScanBuffer_;
+ delete stereoCloudBuffer_;
}
void MoveBase::updateGlobalPose(){
@@ -309,13 +456,16 @@
void MoveBase::baseScanCallback()
{
- ROS_INFO("Base Scan Callback");
+ ROS_DEBUG("Base Scan Callback");
// Project laser into point cloud
std_msgs::PointCloud local_cloud;
local_cloud.header = baseScanMsg_.header;
projector_.projectLaser(baseScanMsg_, local_cloud, baseLaserMaxRange_);
- ROS_INFO("Projected");
- bufferData(local_cloud);
+ petTheWatchDog(local_cloud.header.stamp);
+ ROS_DEBUG("Projected");
+ lock();
+ baseScanBuffer_->buffer_cloud(local_cloud);
+ unlock();
}
void MoveBase::tiltScanCallback()
@@ -324,144 +474,19 @@
std_msgs::PointCloud local_cloud;
local_cloud.header = tiltScanMsg_.header;
projector_.projectLaser(tiltScanMsg_, local_cloud, tiltLaserMaxRange_);
- bufferData(local_cloud);
+ lock();
+ tiltScanBuffer_->buffer_cloud(local_cloud);
+ unlock();
}
void MoveBase::stereoCloudCallback()
{
- bufferData(stereoCloudMsg_);
+ lock();
+ stereoCloudBuffer_->buffer_cloud(stereoCloudMsg_);
+ unlock();
}
- void MoveBase::bufferData(const std_msgs::PointCloud& local_cloud)
- {
- bufferMutex_.lock();
- point_clouds_.push_back(local_cloud);
- petTheWatchDog(local_cloud.header.stamp);
- bufferMutex_.unlock();
- }
-
- void MoveBase::processData()
- {
- // Setting timing values to
- const ros::Duration maxDuration(10, 0);
-
- bufferMutex_.lock();
-
- std_msgs::PointCloud * newData = NULL;
- std_msgs::PointCloud * map_cloud = NULL;
- std::vector<std_msgs::PointCloud*> pending_updates;
-
- while(!point_clouds_.empty()){
- const std_msgs::PointCloud& point_cloud = point_clouds_.front();
-
- // Throw out point clouds that are old.
- if(last_time_stamp_ - point_cloud.header.stamp > maxDuration){
- ROS_INFO("Discarding stale point cloud\n");
- point_clouds_.pop_front();
- continue;
- }
-
- std_msgs::PointCloud base_cloud;
-
- /* Transform to the base frame */
- try
- {
- tf_.transformPointCloud("base", base_cloud, point_cloud);
- newData = extractFootprintAndGround(base_cloud);
- map_cloud = new std_msgs::PointCloud();
- tf_.transformPointCloud("map", *map_cloud, *newData);
- }
- catch(libTF::TransformReference::LookupException& ex)
- {
- ROS_ERROR("Lookup exception: %s\n", ex.what());
- break;
- }
- catch(libTF::TransformReference::ExtrapolateException& ex)
- {
- ROS_DEBUG("No transform available yet - have to try later: %s . Buffer size is %d\n", ex.what(), point_clouds_.size());
- break;
- }
- catch(libTF::TransformReference::ConnectivityException& ex)
- {
- ROS_ERROR("Connectivity exception: %s\n", ex.what());
- break;
- }
- catch(...)
- {
- ROS_ERROR("Exception in point cloud computation\n");
- break;
- }
-
- point_clouds_.pop_front();
-
- if (newData != NULL){
- delete newData;
- newData = NULL;
- }
-
- pending_updates.push_back(map_cloud);
- map_cloud = NULL;
- }
-
- // In case we get thrown out on the second transform - clean up
- if (newData != NULL){
- delete newData;
- newData = NULL;
- }
-
- if(map_cloud != NULL){
- delete map_cloud;
- map_cloud = NULL;
- }
-
- updateDynamicObstacles(pending_updates);
-
- // Now clean up retrieved clouds
- for(std::vector<std_msgs::PointCloud*>::const_iterator it = pending_updates.begin(); it != pending_updates.end(); ++it)
- delete *it;
-
- bufferMutex_.unlock();
- }
-
/**
- * The point is in the footprint if its x and y values are in the range [0 robotWidth] in
- * the base frame.
- */
- bool MoveBase::inFootprint(double x, double y) const {
- bool result = fabs(x) <= robotWidth_/2 && fabs(y) <= robotWidth_/2;
-
- if(result){
- ROS_DEBUG("Discarding point <%f, %f> in footprint\n", x, y);
- }
- return result;
- }
-
- std_msgs::PointCloud * MoveBase::extractFootprintAndGround(const std_msgs::PointCloud& baseFrameCloud) const {
- std_msgs::PointCloud *copy = new std_msgs::PointCloud();
- copy->header = baseFrameCloud.header;
-
- unsigned int n = baseFrameCloud.get_pts_size();
- unsigned int j = 0;
- copy->set_pts_size(n);
-
- for (unsigned int k = 0 ; k < n ; ++k){
- bool ok = baseFrameCloud.pts[k].z > minZ_ && baseFrameCloud.pts[k].z < maxZ_;
- if (ok && !inFootprint(baseFrameCloud.pts[k].x, baseFrameCloud.pts[k].y)){
- copy->pts[j++] = baseFrameCloud.pts[k];
- }
- else {
- ROS_DEBUG("Discarding <%f, %f, %f>\n", baseFrameCloud.pts[k].x, baseFrameCloud.pts[k].y, baseFrameCloud.pts[k].z);
- }
- }
-
- copy->set_pts_size(j);
-
- ROS_DEBUG("Filter discarded %d points (%d left) \n", n - j, j);
-
- return copy;
- }
-
- /**
* The odomMsg_ will be updates and we will do the transform to update the odom in the base frame
*/
void MoveBase::odomCallback(){
@@ -674,7 +699,6 @@
// The plan is bogus if it is empty
if(planOk && plan_.empty()){
planOk = false;
- ROS_ASSERT(inCollision());
ROS_DEBUG("No path points in local window.\n");
}
@@ -694,11 +718,12 @@
ROS_DEBUG("Velocity Controller could not find a valid trajectory.\n");
planOk = false;
}
+
gettimeofday(&end,NULL);
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
t_diff = end_t - start_t;
- ROS_DEBUG("Cycle Time: %.3f\n", t_diff);
+ ROS_INFO("Cycle Time: %.3f\n", t_diff);
if(!planOk){
// Zero out the velocities
@@ -858,7 +883,7 @@
}
void MoveBase::stopRobot(){
- ROS_INFO("Stopping the robot now!\n");
+ ROS_DEBUG("Stopping the robot now!\n");
std_msgs::BaseVel cmdVel; // Commanded velocities
cmdVel.vx = 0.0;
cmdVel.vy = 0.0;
@@ -870,41 +895,48 @@
stopRobot();
}
- void MoveBase::updateDynamicObstacles(const std::vector<std_msgs::PointCloud*>& clouds){
- //Avoids laser race conditions.
- if (!isInitialized()) {
- return;
- }
-
- std::vector<unsigned int> updates;
- lock();
- struct timeval curr;
- gettimeofday(&curr,NULL);
- double curr_t, last_t, t_diff;
- curr_t = curr.tv_sec + curr.tv_usec / 1e6;
- costMap_->updateDynamicObstacles(global_pose_.x, global_pose_.y, clouds, updates);
- gettimeofday(&curr,NULL);
- last_t = curr.tv_sec + curr.tv_usec / 1e6;
- t_diff = last_t - curr_t;
- handleMapUpdates(updates);
- publishLocalCostMap();
- unlock();
- ROS_INFO("Updated map in %f seconds/n", t_diff);
- }
-
MoveBase::footprint_t const & MoveBase::getFootprint() const{
return footprint_;
}
-
+ /**
+ * Each update loop will query all observations and aggregate them and then apply
+ * a batch update to the cost map
+ */
void MoveBase::mapUpdateLoop()
{
ros::Duration *d = new ros::Duration(1.0/map_update_frequency_);
while (active_){
- processData();
+ //Avoids laser race conditions.
+ if (isInitialized()) {
+
+ ROS_INFO("Starting cost map update/n");
+ lock();
+ // Aggregate buffered observations across 3 sources
+ std::vector<costmap_2d::Observation> observations;
+ baseScanBuffer_->get_observations(observations);
+ tiltScanBuffer_->get_observations(observations);
+ stereoCloudBuffer_->get_observations(observations);
+
+ ROS_INFO("Applying update with %d observations/n", observations.size());
+ // Apply to cost map
+ struct timeval curr;
+ gettimeofday(&curr,NULL);
+ double curr_t, last_t, t_diff;
+ curr_t = curr.tv_sec + curr.tv_usec / 1e6;
+ costMap_->updateDynamicObstacles(global_pose_.x, global_pose_.y, observations);
+ gettimeofday(&curr,NULL);
+ last_t = curr.tv_sec + curr.tv_usec / 1e6;
+ t_diff = last_t - curr_t;
+ publishLocalCostMap();
+ unlock();
+ ROS_INFO("Updated map in %f seconds for %d observations/n", t_diff, observations.size());
+ }
+
d->sleep();
}
+
delete d;
}
}
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2008-11-18 14:25:33 UTC (rev 6880)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2008-11-18 14:29:12 UTC (rev 6881)
@@ -106,12 +106,6 @@
protected:
/**
- * @brief Called during update of cost map. Will just buffer and handle in batch.
- * @see applyMapUpdates
- */
- virtual void handleMapUpdates(const std::vector<unsigned int>& updates);
-
- /**
* @brief Builds a plan from current state to goal state
*/
virtual bool makePlan();
@@ -248,20 +242,6 @@
delete pMgr_;
}
- /**
- * @brief This is called during a cost map update. Will insert new updates, possibly overwriting prior values
- */
- void MoveBaseSBPL::handleMapUpdates(const std::vector<unsigned int>& updates){
-
- const CostMap2D& cm = getCostMap();
-
- for(std::vector<unsigned int>::const_iterator it = updates.begin(); it != updates.end(); ++it){
- unsigned int x, y; // Cell coordinates
- cm.IND_MC(*it, x, y);
- env_->UpdateCost(x, y, (unsigned char) cm.getCost(x, y));
- }
- }
-
bool MoveBaseSBPL::isMapDataOK() {
const CostMap2D& cm = getCostMap();
@@ -287,8 +267,20 @@
ompl::SBPLPlannerStatistics::entry & statsEntry(pStat_.top());
try {
+ // Update costs
+ lock();
const CostMap2D& cm = getCostMap();
-
+ unsigned int x = cm.getWidth();
+ while(x > 0){
+ x--;
+ unsigned int y = cm.getHeight();
+ while(y > 0){
+ y--;
+ env_->UpdateCost(x, y, (unsigned char) cm.getCost(x, y));
+ }
+ }
+ unlock();
+
// Copy out start and goal states to minimize locking requirement. Lock was not previously required because the
// planner and controller were running on the same thread and the only contention was for cost map updates on call
// backs. Note that cost map queries here are const methods that merely do co-ordinate transformations, so we do not need
@@ -369,7 +361,7 @@
prevth = th;
#warning 'add the cumulation of delta(waypoint.th) now that we can have 3D plans'
}
-
+
plan.push_back(waypoint);
}
// probably we should add the delta from the last theta to
Modified: pkg/trunk/highlevel/highlevel_controllers/test/launch_move_base.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/test/launch_move_base.xml 2008-11-18 14:25:33 UTC (rev 6880)
+++ pkg/trunk/highlevel/highlevel_controllers/test/launch_move_base.xml 2008-11-18 14:29:12 UTC (rev 6881)
@@ -1,7 +1,6 @@
<launch>
<group name="wg">
- <!--param name="costmap_2d/dynamic_obstacle_window" value="255.0"/-->
<!-- for the moment use e.g.: ./shparam set move_base/plannerType string ADPlanner -->
<param name="move_base/environmentList" value="2D,3DKIN"/> <!-- future extension -->
<param name="move_base/plannerList" value="ADPlanner,ARAPlanner"/> <!-- future extension -->
@@ -10,15 +9,16 @@
<param name="move_base/controller_frequency" value="20.0"/>
<param name="move_base/planner_frequency" value="0.0"/>
<param name="move_base/plannerTimeLimit" value="10.0"/>
- <node pkg="highlevel_controllers" type="move_base_sbpl" args="" respawn="false" />
-
- <!-- Set parameters for mailing -->
- <param name="recharge/email_addresses" value="mc...@wi..."/>
- <param name="recharge/subject_plugin" value="Robot Needs To Be Plugged In"/>
- <param name="recharge/subject_unplug" value="Robot Needs To Be Unplugged"/>
- <param name="recharge/body_plugin" value="Hello, could you please plug me in?\nThanks, PR2"/>
- <param name="recharge/body_unplug" value="Hello, could you please unplug me?\nThanks, PR2"/>
- <param name="recharge/mail_client" value="mailx -s"/>
- <node pkg="highlevel_controllers" type="recharge_controller" args="battery_state:=bogus_battery_state" respawn="false" />
+ <param name="/costmap_2d/base_laser_max_range" value="10.0"/>
+ <param name="/costmap_2d/tilt_laser_max_range" value="10.0"/>
+ <param name="/costmap_2d/lethal_obstacle_threshold" value="100.0"/>
+ <param name="/costmap_2d/no_information_value" value="255"/>
+ <param name="/costmap_2d/z_threshold" value="2.0"/>
+ <param name="/costmap_2d/freespace_projection_height" value="2.0"/>
+ <param name="/costmap_2d/inflation_radius" value="0.55"/>
+ <param name="/costmap_2d/circumscribed_radius" value="0.46"/>
+ <param name="/costmap_2d/inscribed_radius" value="0.325"/>
+ <param name="/costmap_2d/weight" value="0.1"/>
+ <node pkg="highlevel_controllers" type="move_base_sbpl" args="base_scan:=base_scan tilt_scan:=tilt_scan" respawn="false" />
</group>
</launch>
Modified: pkg/trunk/highlevel/highlevel_controllers/test/launch_stage_5cm.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/test/launch_stage_5cm.xml 2008-11-18 14:25:33 UTC (rev 6880)
+++ pkg/trunk/highlevel/highlevel_controllers/test/launch_stage_5cm.xml 2008-11-18 14:29:12 UTC (rev 6881)
@@ -11,17 +11,8 @@
<node pkg="fake_localization" type="fake_localization" respawn="false" />
<node pkg="nav_view" type="nav_view" respawn="false"/>
- <!--param name="costmap_2d/dynamic_obstacle_window" value="255.0"/-->
- <!-- for the moment use e.g.: ./shparam set move_base/plannerType string ADPlanner -->
- <param name="move_base/environmentList" value="2D,3DKIN"/> <!-- future extension -->
- <param name="move_base/plannerList" value="ADPlanner,ARAPlanner"/> <!-- future extension -->
- <param name="move_base/plannerType" value="ARAPlanner"/>
- <param name="move_base/environmentType" value="2D"/>
- <param name="move_base/controller_frequency" value="20.0"/>
- <param name="move_base/planner_frequency" value="0.0"/>
- <param name="move_base/plannerTimeLimit" value="10.0"/>
- <param name="costmap_2d/weight" value="0.1"/>
- <node pkg="highlevel_controllers" type="move_base_sbpl" args="scan:=base_scan" respawn="false" />
+ <!-- Now launch controller node required -->
+ <include file="$(find highlevel_controllers)/test/launch_move_base.xml"/>
<!-- Set parameters for mailing -->
<param name="recharge/email_addresses" value="mc...@wi..."/>
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-18 14:25:33 UTC (rev 6880)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-18 14:29:12 UTC (rev 6881)
@@ -56,6 +56,8 @@
// Base class
#include "obstacle_map_accessor.h"
+#include <std_msgs/Point.h>
+#include <std_msgs/PointCloud.h>
//c++
#include <list>
@@ -63,12 +65,8 @@
#include <map>
#include <set>
#include <string>
-#include <map>
#include <queue>
-// For point clouds <Could Make it a template>
-#include "std_msgs/PointCloud.h"
-
namespace costmap_2d {
typedef unsigned char TICK;
@@ -98,6 +96,50 @@
typedef std::priority_queue< QueueElement*, std::vector<QueueElement*>, QueueElementComparator > QUEUE;
+ /**
+ * @brief Stores an observation in terms of a point cloud and the origin of the source
+ * @note Tried to make members and constructor arguments const but the compiler would not accept the default
+ * assignment operator for vector insertion!
+ */
+ class Observation {
+ public:
+ // Structors
+ Observation(std_msgs::Point& p, std_msgs::PointCloud* cloud): origin_(p), cloud_(cloud) {}
+ Observation(const Observation& org): origin_(org.origin_), cloud_(org.cloud_){}
+
+ std_msgs::Point origin_;
+ std_msgs::PointCloud* cloud_;
+ };
+
+ /**
+ * @brief Base class for buffering observations with a time to live property
+ * The inputs are in the map frame
+ */
+ class ObservationBuffer {
+ public:
+ ObservationBuffer(ros::Duration keep_alive):keep_alive_(keep_alive){}
+
+ virtual ~ObservationBuffer();
+
+ /**
+ * @brief Buffer a current observation
+ * @return true if succeded, false if not (which might occur if there was no transform available for example)
+ */
+ bool buffer_observation(const Observation& observation);
+
+ /**
+ * @brief Queries for current observations. Any observations that are no longer
+ * current will be deleted. Will append observations to the input vector
+ */
+ virtual void get_observations(std::vector<Observation>& observations);
+
+ protected:
+ std::list<Observation> buffer_;
+ const ros::Duration keep_alive_;
+ ros::Time last_updated_;
+ };
+
+
class CostMap2D: public ObstacleMapAccessor
{
public:
@@ -126,7 +168,7 @@
*/
CostMap2D(unsigned int width, unsigned int height, const std::vector<unsigned char>& data,
double resolution, u...
[truncated message content] |
|
From: <mc...@us...> - 2008-11-18 16:28:09
|
Revision: 6883
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6883&view=rev
Author: mcgann
Date: 2008-11-18 16:28:03 +0000 (Tue, 18 Nov 2008)
Log Message:
-----------
Further performance improvement and sreamlining by putting cost data in the base class
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/obstacle_map_accessor.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/obstacle_map_accessor.cc
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-18 15:19:18 UTC (rev 6882)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-18 16:28:03 UTC (rev 6883)
@@ -407,9 +407,6 @@
catch(libTF::TransformReference::ExtrapolateException& ex){
ROS_INFO("Extrapolation Error\n");
}
-
- // Update the cost map window
- ma_->updateForRobotPosition(global_pose_.x, global_pose_.y);
}
@@ -670,6 +667,9 @@
bool planOk = checkWatchDog() && isValid();
std_msgs::BaseVel cmdVel; // Commanded velocities
+ // Update the cost map window
+ ma_->updateForRobotPosition(global_pose_.x, global_pose_.y);
+
// if we have achieved all our waypoints but have yet to achieve the goal, then we know that we wish to accomplish our desired
// orientation
if(plan_.empty()){
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-18 15:19:18 UTC (rev 6882)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-11-18 16:28:03 UTC (rev 6883)
@@ -210,45 +210,15 @@
void updateDynamicObstacles(double wx, double wy, const std::vector<Observation>& observations);
/**
- * @brief Get pointer into the obstacle map (which contains both static
- * and dynamic obstacles)
- */
- const unsigned char* getMap() const;
-
- /**
* @brief Obtain the collection of all occupied cells: the union of static and dynamic obstacles
*/
void getOccupiedCellDataIndexList(std::vector<unsigned int>& results) const;
/**
- * @brief Accessor for contents of full map cell by cell index
- */
- unsigned char operator [](unsigned int ind) const;
-
- /**
- * @brief Accessor by map coordinates stored cost value integrating dynamic and static data
- * @param mx the x map index. mx must be in [0, width-1]
- * @param my the y map index. my must be in [0, height-1]
- * @return A cost value in the range [0 255]
- * @note The inputs should always be in bounds. We could add a check for these values to avoid out of range errors
- * but that should only arise as an error condition and should not burden the code for a common accessor
- */
- unsigned char getCost(unsigned int mx, unsigned int my) const{
- ROS_ASSERT(mx < width_);
- ROS_ASSERT(my < height_);
- return costData_[MC_IND(mx, my)];
- }
-
- /**
* @brief The weight for scaling in the cost function
*/
double getWeight() const {return weight_;}
- /**
- * @brief Utility for debugging
- */
- std::string toString() const;
-
private:
/**
@@ -304,10 +274,7 @@
const double weight_; /**< The weighting to apply to a normalized cost value */
unsigned char* staticData_; /**< initial static map */
- unsigned char* costData_; /**< the full map data that has both static and obstacle data */
bool* xy_markers_; /**< Records time remaining in ticks before expiration of the observation */
- unsigned int mx_; /** < The x position of the robot in the grid */
- unsigned int my_; /** < The y position of the robot in the grid */
QUEUE queue_; /**< Used for cost propagation */
double** cachedDistances; /**< Cached distances indexed by dx, dy */
@@ -327,10 +294,6 @@
*/
CostMapAccessor(const CostMap2D& costMap, double maxSize, double pose_x, double pose_y);
- /** Implementation for base class interface **/
- virtual unsigned char operator[](unsigned int ind) const;
- virtual unsigned char getCost(unsigned int mx, unsigned int my) const;
-
/**
* @brief Set the pose for the robot. Will adjust other parameters accordingly.
*/
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/obstacle_map_accessor.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/obstacle_map_accessor.h 2008-11-18 15:19:18 UTC (rev 6882)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/obstacle_map_accessor.h 2008-11-18 16:28:03 UTC (rev 6883)
@@ -62,17 +62,36 @@
static const unsigned char INSCRIBED_INFLATED_OBSTACLE;
/**
- * @brief Access the cost value of the cell at the given index
- * @see MC_IND
+ * @brief Get pointer into the obstacle map (which contains both static
+ * and dynamic obstacles)
*/
- virtual unsigned char operator[](unsigned int ind) const = 0;
+ const unsigned char* getMap() const {
+ return costData_;
+ }
/**
- * @brief Get the cost for the cell given in map coordinates
+ * @brief Accessor for contents of cost map cell by cell index
*/
- virtual unsigned char getCost(unsigned int mx, unsigned int my) const = 0;
+ unsigned char operator [](unsigned int ind) const{
+ return costData_[ind];
+ }
/**
+ * @brief Accessor by map coordinates stored cost value integrating dynamic and static data
+ * @param mx the x map index. mx must be in [0, width-1]
+ * @param my the y map index. my must be in [0, height-1]
+ * @return A cost value in the range [0 255]
+ * @note The inputs should always be in bounds. We could add a check for these values to avoid out of range errors
+ * but that should only arise as an error condition and should not burden the code for a common accessor
+ */
+ unsigned char getCost(unsigned int mx, unsigned int my) const{
+ ROS_ASSERT(mx < width_);
+ ROS_ASSERT(my < height_);
+ unsigned int ind = mx + my * width_;
+ return costData_[ind];
+ }
+
+ /**
* @brief Test if the given cell is necessarily in the footprint of the robot. Note that a negative result
* does not mean it is not in the footprint of the robot, it just means that we are not certain.
* @param mx the x map index. mx must bi in [0, width-1]
@@ -113,7 +132,6 @@
*/
double getResolution() const {return resolution_;}
-
/**
* @brief Sets the bound for testing if in circumscribed circle overflow
*/
@@ -193,10 +211,39 @@
* @param mx map x index return value
* @param my map y index return value
*/
- bool WC_MC(double wx, double wy, unsigned int& mx, unsigned int& my) const;
+ inline bool WC_MC(double wx, double wy, unsigned int& mx, unsigned int& my) const{
+ if(wx < 0 || wy < 0) {
+ mx = 0;
+ my = 0;
+ return false;
+ }
- virtual ~ObstacleMapAccessor(){}
+ //not much for now
+ mx = (int) ((wx - origin_x_)/resolution_);
+ my = (int) ((wy - origin_y_)/resolution_);
+ //printf("x: %.2f y: %.2f or_x: %.2f, or_y: %.2f, resolution: %.2f\n ", wx, wy, origin_x_, origin_y_, resolution_);
+ if(mx >= width_) {
+ mx = 0;
+ return false;
+ }
+
+ if(my >= height_) {
+ //printf("WC_MC converted %d greater than height %d\n", my, height_);
+ my = 0;
+ return false;
+ }
+
+ return true;
+ }
+
+ virtual ~ObstacleMapAccessor();
+
+ /**
+ * @brief Utility for debugging
+ */
+ std::string toString() const;
+
protected:
/**
@@ -214,6 +261,7 @@
unsigned int width_;
unsigned int height_;
double resolution_;
+ unsigned char* costData_; /**< the full cost map data */
private:
unsigned char costLB_; /**< The cost value for the lowest cost cell in the circumscribed radius.*/
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-11-18 15:19:18 UTC (rev 6882)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-11-18 16:28:03 UTC (rev 6883)
@@ -120,7 +120,7 @@
circumscribedRadius_(toCellDistance(circumscribedRadius, inflationRadius_, resolution)),
inscribedRadius_(toCellDistance(inscribedRadius, circumscribedRadius_, resolution)),
weight_(std::max(0.0, std::min(weight, 1.0))),
- staticData_(NULL), costData_(NULL), xy_markers_(NULL), mx_(0), my_(0)
+ staticData_(NULL), xy_markers_(NULL)
{
if(weight != weight_){
ROS_INFO("Warning - input weight %f is invalid and has been set to %f\n", weight, weight_);
@@ -128,9 +128,7 @@
unsigned int i, j;
staticData_ = new unsigned char[width_*height_];
- costData_ = new unsigned char[width_*height_];
xy_markers_ = new bool[width_*height_];
- memset(costData_, 0, width_*height_);
memset(xy_markers_, 0, width_*height_);
cachedDistances = new double*[inflationRadius_+1];
@@ -173,7 +171,6 @@
CostMap2D::~CostMap2D() {
if(staticData_ != NULL) delete[] staticData_;
- if(costData_ != NULL) delete[] costData_;
if(xy_markers_ != NULL) delete[] xy_markers_;
if(cachedDistances != NULL){
for (unsigned int i=0; i<=inflationRadius_; i++)
@@ -225,12 +222,10 @@
/**
* @brief Update the cost map based on aggregate observations
+ * @todo Deprecate use of wx and wy here
*/
void CostMap2D::updateDynamicObstacles(double wx, double wy, const std::vector<Observation>& observations)
{
- // Update current grid position
- WC_MC(wx, wy, mx_, my_);
-
// Revert to initial state
memset(xy_markers_, 0, width_ * height_);
memcpy(costData_, staticData_, width_ * height_);
@@ -282,10 +277,6 @@
}
}
- const unsigned char* CostMap2D::getMap() const {
- return costData_;
- }
-
void CostMap2D::getOccupiedCellDataIndexList(std::vector<unsigned int>& results) const {
results.clear();
unsigned int maxCellCount = getWidth() * getHeight();
@@ -295,37 +286,6 @@
}
}
- unsigned char CostMap2D::operator [](unsigned int ind) const{
- return costData_[ind];
- }
-
- std::string CostMap2D::toString() const {
- std::stringstream ss;
- ss << std::endl;
- for(unsigned j = 0; j < getHeight(); j++){
- for (unsigned int i = 0; i < getWidth(); i++){
- unsigned char cost = getCost(i, j);
- if(cost == LETHAL_OBSTACLE)
- ss << "O";
- else if (cost == INSCRIBED_INFLATED_OBSTACLE)
- ss << "I";
- else if (isCircumscribedCell(i, j))
- ss << "C";
- else if (cost == NO_INFORMATION)
- ss << "?";
- else if (cost > 0)
- ss << "f";
- else
- ss << "F";
-
- ss << ",";
- }
-
- ss << std::endl;
- }
- return ss.str();
- }
-
void CostMap2D::propagateCosts(){
while(!queue_.empty()){
QueueElement* c = queue_.top();
@@ -407,7 +367,7 @@
/**
* Utilizes Eitan's implementation of Bresenhams ray tracing algorithm to iterate over the cells between the
- * current position (mx_, my_).
+ * origin and the sightline.
*/
void CostMap2D::updateFreeSpace(const std_msgs::Point& origin, double wx, double wy){
unsigned int x, y, x1, y1;
@@ -419,7 +379,7 @@
unsigned int xinc1, xinc2, yinc1, yinc2;
unsigned int den, num, numadd, numpixels;
- if (x1 >= mx_) // The x-values are increasing
+ if (x1 >= x) // The x-values are increasing
{
xinc1 = 1;
xinc2 = 1;
@@ -430,7 +390,7 @@
xinc2 = -1;
}
- if (y1 >= my_) // The y-values are increasing
+ if (y1 >= y) // The y-values are increasing
{
yinc1 = 1;
yinc2 = 1;
@@ -489,23 +449,12 @@
setCircumscribedCostLowerBound(costMap.getCircumscribedCostLowerBound());
- // The origin locates this grid. Convert from world coordinates to cell co-ordinates
- // to get the cell coordinates of the origin
- costMap_.WC_MC(origin_x_, origin_y_, mx_0_, my_0_);
+ // Set robot position
+ updateForRobotPosition(poseX, poseY);
ROS_DEBUG_NAMED("costmap_2d", "Creating Local %d X %d Map\n", getWidth(), getHeight());
}
- unsigned char CostMapAccessor::operator[](unsigned int ind) const {
- unsigned int mx = ind % width_;
- unsigned int my = (unsigned int) ind / height_;
- return getCost(mx, my);
- }
-
- unsigned char CostMapAccessor::getCost(unsigned int mx, unsigned int my) const {
- return costMap_.getCost(mx_0_ + mx, my_0_ + my);
- }
-
void CostMapAccessor::updateForRobotPosition(double wx, double wy){
if(wx < 0 || wx > costMap_.getResolution() * costMap_.getWidth())
return;
@@ -520,6 +469,15 @@
ROS_DEBUG_NAMED("costmap_2d",
"Moving map to locate at <%f, %f> and size of %f meters for position <%f, %f>\n",
origin_x_, origin_y_, maxSize_, wx, wy);
+
+ // Now update all the cells from the cost map
+ for(unsigned int x = 0; x < width_; x++){
+ for (unsigned int y = 0; y < height_; y++){
+ unsigned int ind = x + y * width_;
+ unsigned int global_ind = mx_0_ + x + (my_0_ + y) * costMap_.getWidth();
+ costData_[ind] = costMap_[global_ind];
+ }
+ }
}
double CostMapAccessor::computeWX(const CostMap2D& costMap, double maxSize, double wx, double wy){
Modified: pkg/trunk/world_models/costmap_2d/src/obstacle_map_accessor.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/obstacle_map_accessor.cc 2008-11-18 15:19:18 UTC (rev 6882)
+++ pkg/trunk/world_models/costmap_2d/src/obstacle_map_accessor.cc 2008-11-18 16:28:03 UTC (rev 6883)
@@ -44,8 +44,17 @@
const unsigned char ObstacleMapAccessor::INSCRIBED_INFLATED_OBSTACLE(253);
ObstacleMapAccessor::ObstacleMapAccessor(double origin_x, double origin_y, unsigned int width, unsigned int height, double resolution)
- : origin_x_(origin_x), origin_y_(origin_y), width_(width), height_(height), resolution_(resolution), costLB_(0){}
+ : origin_x_(origin_x), origin_y_(origin_y), width_(width), height_(height), resolution_(resolution), costData_(NULL), costLB_(0){
+ costData_ = new unsigned char[width_*height_];
+ memset(costData_, 0, width_*height_);
+ }
+
+ ObstacleMapAccessor::~ObstacleMapAccessor(){
+ if(costData_ != NULL)
+ delete[] costData_;
+ }
+
void ObstacleMapAccessor::getOriginInWorldCoordinates(double& wx, double& wy) const{
wx = origin_x_;
wy = origin_y_;
@@ -56,38 +65,30 @@
return cost < INSCRIBED_INFLATED_OBSTACLE && cost >= costLB_;
}
- /**
- * @brief Get index of given world (x,y) point in map indexes
- *
- * @param wx world x location of the cell
- * @param wy world y location of the cell
- * @param mx map x index return value
- * @param my map y index return value
- */
- bool ObstacleMapAccessor::WC_MC(double wx, double wy, unsigned int& mx, unsigned int& my) const {
+ std::string ObstacleMapAccessor::toString() const {
+ std::stringstream ss;
+ ss << std::endl;
+ for(unsigned j = 0; j < getHeight(); j++){
+ for (unsigned int i = 0; i < getWidth(); i++){
+ unsigned char cost = getCost(i, j);
+ if(cost == LETHAL_OBSTACLE)
+ ss << "O";
+ else if (cost == INSCRIBED_INFLATED_OBSTACLE)
+ ss << "I";
+ else if (isCircumscribedCell(i, j))
+ ss << "C";
+ else if (cost == NO_INFORMATION)
+ ss << "?";
+ else if (cost > 0)
+ ss << "f";
+ else
+ ss << "F";
- if(wx < 0 || wy < 0) {
- mx = 0;
- my = 0;
- return false;
- }
+ ss << ",";
+ }
- //not much for now
- mx = (int) ((wx - origin_x_)/resolution_);
- my = (int) ((wy - origin_y_)/resolution_);
-
- //printf("x: %.2f y: %.2f or_x: %.2f, or_y: %.2f, resolution: %.2f\n ", wx, wy, origin_x_, origin_y_, resolution_);
- if(mx >= width_) {
- mx = 0;
- return false;
- }
-
- if(my >= height_) {
- //printf("WC_MC converted %d greater than height %d\n", my, height_);
- my = 0;
- return false;
+ ss << std::endl;
}
-
- return true;
+ return ss.str();
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-18 18:26:11
|
Revision: 6891
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6891&view=rev
Author: hsujohnhsu
Date: 2008-11-18 18:26:05 +0000 (Tue, 18 Nov 2008)
Log Message:
-----------
updates per math_utils API review.
* rename math_utils to angles package
* move MathExpressiosn to math_expr package
* move exponentialSmoothing and clamp to control_toolbox
* updated dependent packages accordingly
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_qualification.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_autotuner.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_pd_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_smoothing_controller.cpp
pkg/trunk/deprecated/genericControllers/include/genericControllers/JointController.h
pkg/trunk/deprecated/genericControllers/src/JointController.cpp
pkg/trunk/deprecated/libKDL/manifest.xml
pkg/trunk/deprecated/pr2Controllers/manifest.xml
pkg/trunk/deprecated/pr2Core/include/pr2Core/pr2Misc.h
pkg/trunk/deprecated/pr2Core/manifest.xml
pkg/trunk/deprecated/rosControllers/manifest.xml.old
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
pkg/trunk/motion_planning/ompl/manifest.xml
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/nav/fake_localization/manifest.xml
pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml
pkg/trunk/robot_descriptions/wg_robot_description_parser/src/URDF.cpp
pkg/trunk/robot_descriptions/wg_robot_description_parser/src/parser.cpp
pkg/trunk/util/angles/test/utest.cpp
pkg/trunk/util/kinematics/robot_kinematics/manifest.xml
pkg/trunk/util/libTF/manifest.xml
pkg/trunk/util/libTF/src/test/testMatrixQuaternion.cc
pkg/trunk/util/libTF/test/libTF_unittest.cpp
pkg/trunk/util/libTF/test/pose3d_unittest.cpp
pkg/trunk/util/tf/manifest.xml
pkg/trunk/util/tf/test/cache_unittest.cpp
pkg/trunk/util/tf/test/tf_unittest.cpp
pkg/trunk/util/tf/test/tf_unittest_future.cpp
Added Paths:
-----------
pkg/trunk/controllers/control_toolbox/include/control_toolbox/filters.h
pkg/trunk/util/angles/
pkg/trunk/util/angles/CMakeLists.txt
pkg/trunk/util/angles/Makefile
pkg/trunk/util/angles/doc.dox
pkg/trunk/util/angles/include/
pkg/trunk/util/angles/manifest.xml
pkg/trunk/util/angles/src/
pkg/trunk/util/angles/test/
pkg/trunk/util/math_expr/
pkg/trunk/util/math_expr/CMakeLists.txt
pkg/trunk/util/math_expr/Makefile
pkg/trunk/util/math_expr/doc.dox
pkg/trunk/util/math_expr/include/
pkg/trunk/util/math_expr/include/math_expr/
pkg/trunk/util/math_expr/include/math_expr/MathExpression.h
pkg/trunk/util/math_expr/manifest.xml
pkg/trunk/util/math_expr/src/
pkg/trunk/util/math_expr/src/MathExpression.cpp
pkg/trunk/util/math_expr/test/
pkg/trunk/util/math_expr/test/utest.cpp
Removed Paths:
-------------
pkg/trunk/util/angles/CMakeLists.txt
pkg/trunk/util/angles/Makefile
pkg/trunk/util/angles/doc.dox
pkg/trunk/util/angles/include/
pkg/trunk/util/angles/manifest.xml
pkg/trunk/util/angles/src/
pkg/trunk/util/angles/test/
pkg/trunk/util/math_utils/
Copied: pkg/trunk/controllers/control_toolbox/include/control_toolbox/filters.h (from rev 6882, pkg/trunk/util/math_utils/include/math_utils/math_utils.h)
===================================================================
--- pkg/trunk/controllers/control_toolbox/include/control_toolbox/filters.h (rev 0)
+++ pkg/trunk/controllers/control_toolbox/include/control_toolbox/filters.h 2008-11-18 18:26:05 UTC (rev 6891)
@@ -0,0 +1,58 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef FILTERS_H
+#define FILTERS_H
+
+#include <algorithm>
+
+namespace filters
+{
+
+ /** Clamp value a between b and c */
+ template<typename T>
+ static inline const T& clamp(const T &a, const T &b, const T &c)
+ {
+ return std::min<T>(std::max<T>(b, a), c);
+ }
+
+ /** Exponential smoothing filter. Alpha is between 0 and 1. Values closer to 0 weight the last smoothed value more heavily */
+
+ static inline double exponentialSmoothing(double current_raw_value, double last_smoothed_value, double alpha)
+ {
+ return alpha*current_raw_value + (1-alpha)*last_smoothed_value;
+ }
+ }
+
+#endif
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h 2008-11-18 18:26:05 UTC (rev 6891)
@@ -53,9 +53,6 @@
//Kinematics
#include <robot_kinematics/robot_kinematics.h>
-// Math utils
-#include <math_utils/angles.h>
-
// #include <libTF/Pose3D.h>
// #include <urdf/URDF.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h 2008-11-18 18:26:05 UTC (rev 6891)
@@ -52,7 +52,6 @@
#include <std_msgs/VisualizationMarker.h>
// Math utils
-#include <math_utils/angles.h>
#include <math.h>
#include <tf/transform_listener.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2008-11-18 18:26:05 UTC (rev 6891)
@@ -52,7 +52,6 @@
#include <std_msgs/VisualizationMarker.h>
// Math utils
-#include <math_utils/angles.h>
#include <math.h>
#include <tf/transform_listener.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2008-11-18 18:26:05 UTC (rev 6891)
@@ -16,11 +16,12 @@
<depend package="rosTF" />
<depend package="std_msgs" />
<depend package="pr2_msgs" />
- <depend package="math_utils" />
<depend package="robot_kinematics" />
<depend package="rosout" />
<depend package="trajectory" />
<depend package="rosthread" />
+ <depend package="angles" />
+ <depend package="control_toolbox" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp 2008-11-18 18:26:05 UTC (rev 6891)
@@ -36,6 +36,9 @@
#include "pr2_mechanism_controllers/arm_dynamics_controller.h"
+// Math utils
+#include <angles/angles.h>
+
using namespace controller;
using namespace std;
@@ -216,12 +219,12 @@
j_type = joint_effort_controllers_[i]->joint_state_->joint_->type_;
if(j_type == mechanism::JOINT_ROTARY)
{
- if(!math_utils::shortest_angular_distance_with_limits(command, actual, joint_effort_controllers_[i]->joint_state_->joint_->joint_limit_min_, joint_effort_controllers_[i]->joint_state_->joint_->joint_limit_max_,error))
+ if(!angles::shortest_angular_distance_with_limits(command, actual, joint_effort_controllers_[i]->joint_state_->joint_->joint_limit_min_, joint_effort_controllers_[i]->joint_state_->joint_->joint_limit_max_,error))
error = 0;
}
else if(j_type == mechanism::JOINT_CONTINUOUS)
{
- error = math_utils::shortest_angular_distance(command, actual);
+ error = angles::shortest_angular_distance(command, actual);
}
else
{
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp 2008-11-18 18:26:05 UTC (rev 6891)
@@ -33,8 +33,8 @@
*********************************************************************/
#include <pr2_mechanism_controllers/base_controller.h>
-#include <math_utils/angles.h>
-#include <math_utils/math_utils.h>
+#include <angles/angles.h>
+#include <control_toolbox/filters.h>
#include "ros/node.h"
#define NUM_TRANSFORMS 2
@@ -49,7 +49,6 @@
using namespace control_toolbox;
using namespace libTF;
using namespace NEWMAT;
-using namespace math_utils;
ROS_REGISTER_CONTROLLER(BaseController)
@@ -107,9 +106,9 @@
{
pthread_mutex_lock(&base_controller_lock_);
- cmd_vel_t_.x = clamp(cmd_vel.x,-max_vel_.x, max_vel_.x);
- cmd_vel_t_.y = clamp(cmd_vel.y,-max_vel_.y, max_vel_.y);
- cmd_vel_t_.z = clamp(cmd_vel.z,-max_vel_.z, max_vel_.z);
+ cmd_vel_t_.x = filters::clamp(cmd_vel.x,-max_vel_.x, max_vel_.x);
+ cmd_vel_t_.y = filters::clamp(cmd_vel.y,-max_vel_.y, max_vel_.y);
+ cmd_vel_t_.z = filters::clamp(cmd_vel.z,-max_vel_.z, max_vel_.z);
cmd_received_timestamp_ = robot_state_->hw_->current_time_;
#if 0
@@ -132,9 +131,9 @@
libTF::Vector BaseController::interpolateCommand(libTF::Vector start, libTF::Vector end, libTF::Vector max_rate, double dT)
{
libTF::Vector result;
- result.x = start.x + clamp(end.x - start.x,-max_rate.x*dT,max_rate.x*dT);
- result.y = start.y + clamp(end.y - start.y,-max_rate.y*dT,max_rate.y*dT);
- result.z = start.z + clamp(end.z - start.z,-max_rate.z*dT,max_rate.z*dT);
+ result.x = start.x + filters::clamp(end.x - start.x,-max_rate.x*dT,max_rate.x*dT);
+ result.y = start.y + filters::clamp(end.y - start.y,-max_rate.y*dT,max_rate.y*dT);
+ result.z = start.z + filters::clamp(end.z - start.z,-max_rate.z*dT,max_rate.z*dT);
return result;
}
*/
@@ -476,10 +475,10 @@
steer_angle_desired = atan2(result.y,result.x);
steer_angle_stored_[i] = steer_angle_desired;
}
- steer_angle_desired_m_pi = normalize_angle(steer_angle_desired+M_PI);
+ steer_angle_desired_m_pi = angles::normalize_angle(steer_angle_desired+M_PI);
- error_steer = shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
- error_steer_m_pi = shortest_angular_distance(steer_angle_desired_m_pi, steer_angle_actual_[i]);
+ error_steer = angles::shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
+ error_steer_m_pi = angles::shortest_angular_distance(steer_angle_desired_m_pi, steer_angle_actual_[i]);
if(fabs(error_steer_m_pi) < fabs(error_steer))
{
@@ -546,7 +545,7 @@
odom_msg_.pos.x = base_odom_position_.x;
odom_msg_.pos.y = base_odom_position_.y;
- odom_msg_.pos.th = math_utils::normalize_angle(base_odom_position_.z);
+ odom_msg_.pos.th = angles::normalize_angle(base_odom_position_.z);
odom_msg_.vel.x = base_odom_velocity_.x;
odom_msg_.vel.y = base_odom_velocity_.y;
@@ -814,7 +813,7 @@
out.z = 0;
out.roll = 0;
out.pitch = 0;
- out.yaw = math_utils::normalize_angle(-yaw);
+ out.yaw = angles::normalize_angle(-yaw);
rosTF::TransformEuler &out2 = transform_publisher_->msg_.eulers[1];
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp 2008-11-18 18:26:05 UTC (rev 6891)
@@ -35,6 +35,8 @@
// Original version: Melonee Wise <mw...@wi...>
#include "pr2_mechanism_controllers/head_servoing_controller.h"
+// Math utils
+#include <angles/angles.h>
using namespace controller;
using namespace std;
@@ -160,7 +162,7 @@
for(unsigned int i=0; i < num_joints_;++i)
{
- math_utils::shortest_angular_distance_with_limits(joint_velocity_controllers_[i]->joint_state_->position_,set_pts_[i], joint_velocity_controllers_[i]->joint_state_->joint_->joint_limit_min_, joint_velocity_controllers_[i]->joint_state_->joint_->joint_limit_max_, error);
+ angles::shortest_angular_distance_with_limits(joint_velocity_controllers_[i]->joint_state_->position_,set_pts_[i], joint_velocity_controllers_[i]->joint_state_->joint_->joint_limit_min_, joint_velocity_controllers_[i]->joint_state_->joint_->joint_limit_max_, error);
error=((gain_*error)<max_velocity_)?error:max_velocity_;
joint_velocity_controllers_[i]->setCommand(error);
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2008-11-18 18:26:05 UTC (rev 6891)
@@ -33,7 +33,6 @@
*********************************************************************/
#include <algorithm>
#include <pr2_mechanism_controllers/laser_scanner_controller.h>
-#include <math_utils/angles.h>
using namespace std;
using namespace controller;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_qualification.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_qualification.cpp 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_qualification.cpp 2008-11-18 18:26:05 UTC (rev 6891)
@@ -44,7 +44,6 @@
#include <algorithm>
#include <pr2_mechanism_controllers/laser_scanner_qualification.h>
-#include <math_utils/angles.h>
using namespace std;
using namespace controller;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2008-11-18 18:26:05 UTC (rev 6891)
@@ -33,7 +33,7 @@
*********************************************************************/
#include <algorithm>
#include <pr2_mechanism_controllers/laser_scanner_traj_controller.h>
-#include <math_utils/angles.h>
+#include <angles/angles.h>
#include <math.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp 2008-11-18 18:26:05 UTC (rev 6891)
@@ -34,8 +34,7 @@
#include <algorithm>
#include <pr2_mechanism_controllers/laser_scanner_velocity_controller.h>
-#include <math_utils/angles.h>
-#include <math_utils/velocity.h>
+
using namespace std;
using namespace controller;
#define MIN_MOVING_SPEED 0.3
Modified: pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2008-11-18 18:26:05 UTC (rev 6891)
@@ -16,6 +16,7 @@
<depend package="robot_srvs" />
<depend package="robot_msgs" />
<depend package="tf" />
+ <depend package="angles" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_autotuner.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_autotuner.cpp 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_autotuner.cpp 2008-11-18 18:26:05 UTC (rev 6891)
@@ -33,7 +33,6 @@
*********************************************************************/
#include <robot_mechanism_controllers/joint_autotuner.h>
-#include <math_utils/angles.h>
#include <fstream>
using namespace std;
using namespace controller;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_pd_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_pd_controller.cpp 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_pd_controller.cpp 2008-11-18 18:26:05 UTC (rev 6891)
@@ -33,7 +33,6 @@
*********************************************************************/
#include <robot_mechanism_controllers/joint_pd_controller.h>
-#include <math_utils/angles.h>
using namespace std;
using namespace controller;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp 2008-11-18 18:26:05 UTC (rev 6891)
@@ -33,7 +33,7 @@
*********************************************************************/
#include <robot_mechanism_controllers/joint_position_controller.h>
-#include <math_utils/angles.h>
+#include <angles/angles.h>
using namespace std;
using namespace controller;
@@ -134,12 +134,12 @@
if(joint_state_->joint_->type_ == mechanism::JOINT_ROTARY)
{
- math_utils::shortest_angular_distance_with_limits(command_, joint_state_->position_, joint_state_->joint_->joint_limit_min_, joint_state_->joint_->joint_limit_max_,error);
+ angles::shortest_angular_distance_with_limits(command_, joint_state_->position_, joint_state_->joint_->joint_limit_min_, joint_state_->joint_->joint_limit_max_,error);
}
else if(joint_state_->joint_->type_ == mechanism::JOINT_CONTINUOUS)
{
- error = math_utils::shortest_angular_distance(command_, joint_state_->position_);
+ error = angles::shortest_angular_distance(command_, joint_state_->position_);
}
else //prismatic
{
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_smoothing_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_smoothing_controller.cpp 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_smoothing_controller.cpp 2008-11-18 18:26:05 UTC (rev 6891)
@@ -33,7 +33,7 @@
*********************************************************************/
#include <robot_mechanism_controllers/joint_position_smoothing_controller.h>
-#include <math_utils/angles.h>
+#include <angles/angles.h>
using namespace std;
using namespace controller;
@@ -145,12 +145,12 @@
if(joint_state_->joint_->type_ == mechanism::JOINT_ROTARY)
{
- math_utils::shortest_angular_distance_with_limits(command_, joint_state_->position_, joint_state_->joint_->joint_limit_min_, joint_state_->joint_->joint_limit_max_,error);
+ angles::shortest_angular_distance_with_limits(command_, joint_state_->position_, joint_state_->joint_->joint_limit_min_, joint_state_->joint_->joint_limit_max_,error);
}
else if(joint_state_->joint_->type_ == mechanism::JOINT_CONTINUOUS)
{
- error = math_utils::shortest_angular_distance(command_, joint_state_->position_);
+ error = angles::shortest_angular_distance(command_, joint_state_->position_);
}
else //prismatic
{
Modified: pkg/trunk/deprecated/genericControllers/include/genericControllers/JointController.h
===================================================================
--- pkg/trunk/deprecated/genericControllers/include/genericControllers/JointController.h 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/deprecated/genericControllers/include/genericControllers/JointController.h 2008-11-18 18:26:05 UTC (rev 6891)
@@ -73,7 +73,7 @@
#include <genericControllers/Controller.h>
#include <genericControllers/Pid.h>
-#include <math_utils/angles.h>
+#include <angles/angles.h>
#include <mechanism_model/joint.h>
#include <string>
#include <math.h>
Modified: pkg/trunk/deprecated/genericControllers/src/JointController.cpp
===================================================================
--- pkg/trunk/deprecated/genericControllers/src/JointController.cpp 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/deprecated/genericControllers/src/JointController.cpp 2008-11-18 18:26:05 UTC (rev 6891)
@@ -284,8 +284,8 @@
double JointController::getMaxVelocity(){
double disToMin,disToMax,closestLimit;
- disToMin = fabs(math_utils::shortest_angular_distance(joint->position, joint->jointLimitMin));
- disToMax = fabs(math_utils::shortest_angular_distance(joint->position, joint->jointLimitMax));
+ disToMin = fabs(angles::shortest_angular_distance(joint->position, joint->jointLimitMin));
+ disToMax = fabs(angles::shortest_angular_distance(joint->position, joint->jointLimitMax));
closestLimit = (disToMin<disToMax)?disToMin:disToMax; //min
//std::cout<<"Dis to min"<<disToMin<<" Dist to Max"<<disToMax<<" Closest limit"<<closestLimit<<std::endl;
return sqrt(fabs(closestLimit*maxAccel));
@@ -316,7 +316,7 @@
case CONTROLLER_POSITION: //Close the loop around position
if(joint->type == mechanism::JOINT_ROTARY || joint->type == mechanism::JOINT_CONTINUOUS)
- error = math_utils::shortest_angular_distance(cmdPos, joint->position);
+ error = angles::shortest_angular_distance(cmdPos, joint->position);
else
error = joint->position - cmdPos;
currentTorqueCmd = pidController.UpdatePid(error,time-lastTime);
@@ -384,7 +384,7 @@
case CONTROLLER_POSITION: //Close the loop around position
if(joint->type == mechanism::JOINT_ROTARY || joint->type == mechanism::JOINT_CONTINUOUS)
- error = math_utils::shortest_angular_distance(cmdPos, joint->position);
+ error = angles::shortest_angular_distance(cmdPos, joint->position);
else
error = joint->position - cmdPos;
currentTorqueCmd = pidController.UpdatePid(error,time-lastTime);
Modified: pkg/trunk/deprecated/libKDL/manifest.xml
===================================================================
--- pkg/trunk/deprecated/libKDL/manifest.xml 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/deprecated/libKDL/manifest.xml 2008-11-18 18:26:05 UTC (rev 6891)
@@ -8,7 +8,7 @@
<depend package="pr2Core"/>
<depend package="wg_robot_description_parser"/>
<depend package="rosTF"/>
-<depend package="math_utils"/>
+<depend package="angles"/>
<depend package="kdl"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lKDL"/>
Modified: pkg/trunk/deprecated/pr2Controllers/manifest.xml
===================================================================
--- pkg/trunk/deprecated/pr2Controllers/manifest.xml 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/deprecated/pr2Controllers/manifest.xml 2008-11-18 18:26:05 UTC (rev 6891)
@@ -14,7 +14,7 @@
<depend package="libKDL" />
<depend package="wg_robot_description_parser" />
<depend package="joy" />
- <depend package="math_utils" />
+ <depend package="angles" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Modified: pkg/trunk/deprecated/pr2Core/include/pr2Core/pr2Misc.h
===================================================================
--- pkg/trunk/deprecated/pr2Core/include/pr2Core/pr2Misc.h 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/deprecated/pr2Core/include/pr2Core/pr2Misc.h 2008-11-18 18:26:05 UTC (rev 6891)
@@ -3,7 +3,7 @@
#include <cmath>
#include <cstdio>
-#include <math_utils/angles.h>
+#include <angles/angles.h>
namespace PR2
{
Modified: pkg/trunk/deprecated/pr2Core/manifest.xml
===================================================================
--- pkg/trunk/deprecated/pr2Core/manifest.xml 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/deprecated/pr2Core/manifest.xml 2008-11-18 18:26:05 UTC (rev 6891)
@@ -8,7 +8,7 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com</url>
-<depend package="math_utils" />
+<depend package="angles" />
<export>
<cpp cflags="-I${prefix}/include" lflags=""/>
</export>
Modified: pkg/trunk/deprecated/rosControllers/manifest.xml.old
===================================================================
--- pkg/trunk/deprecated/rosControllers/manifest.xml.old 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/deprecated/rosControllers/manifest.xml.old 2008-11-18 18:26:05 UTC (rev 6891)
@@ -10,7 +10,7 @@
<depend package="std_msgs" />
<depend package="rosthread" />
<depend package="rosTF" />
- <depend package="math_utils" />
+ <depend package="angles" />
<depend package="string_utils" />
<depend package="libKDL" />
<depend package="robot_mechanism_model" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2008-11-18 18:26:05 UTC (rev 6891)
@@ -18,7 +18,7 @@
<depend package="robot_msgs" />
<depend package="rosthread"/>
<depend package="stl_utils" />
- <depend package="math_utils" />
+ <depend package="angles" />
<depend package="gazebo_robot_description"/>
<depend package="hardware_interface" />
<depend package="mechanism_control" />
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2008-11-18 18:15:54 UTC (rev 6890)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceI...
[truncated message content] |
|
From: <ge...@us...> - 2008-11-18 19:29:41
|
Revision: 6900
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6900&view=rev
Author: gerkey
Date: 2008-11-18 19:29:39 +0000 (Tue, 18 Nov 2008)
Log Message:
-----------
blacklisted packages that depend on calonder_descriptor
Modified Paths:
--------------
pkg/trunk/vision/dorylus/ROS_BUILD_BLACKLIST
Added Paths:
-----------
pkg/trunk/util/point_cloud_utils/ROS_BUILD_BLACKLIST
pkg/trunk/vision/stereo_calib/ROS_BUILD_BLACKLIST
pkg/trunk/vision/videre_face_detection/ROS_BUILD_BLACKLIST
pkg/trunk/vision/visual_odometry/ROS_BUILD_BLACKLIST
Added: pkg/trunk/util/point_cloud_utils/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/util/point_cloud_utils/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/util/point_cloud_utils/ROS_BUILD_BLACKLIST 2008-11-18 19:29:39 UTC (rev 6900)
@@ -0,0 +1 @@
+Depends on calonder_descriptor, which is blacklisted
Modified: pkg/trunk/vision/dorylus/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/dorylus/ROS_BUILD_BLACKLIST 2008-11-18 19:26:09 UTC (rev 6899)
+++ pkg/trunk/vision/dorylus/ROS_BUILD_BLACKLIST 2008-11-18 19:29:39 UTC (rev 6900)
@@ -1,3 +1,5 @@
+Depends on calonder_descriptor, which is blacklisted
+
There seems to be a problem with the inclusion of the opencv type 'schar'
Added: pkg/trunk/vision/stereo_calib/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/stereo_calib/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/vision/stereo_calib/ROS_BUILD_BLACKLIST 2008-11-18 19:29:39 UTC (rev 6900)
@@ -0,0 +1 @@
+Depends on calonder_descriptor, which is blacklisted
Added: pkg/trunk/vision/videre_face_detection/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/videre_face_detection/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/vision/videre_face_detection/ROS_BUILD_BLACKLIST 2008-11-18 19:29:39 UTC (rev 6900)
@@ -0,0 +1 @@
+Depends on calonder_descriptor, which is blacklisted
Added: pkg/trunk/vision/visual_odometry/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/visual_odometry/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/vision/visual_odometry/ROS_BUILD_BLACKLIST 2008-11-18 19:29:39 UTC (rev 6900)
@@ -0,0 +1 @@
+Depends on calonder_descriptor, which is blacklisted
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|