Menu
▾
▴
personalrobots-commit
|
From: <hsu...@us...> - 2008-09-10 01:36:17
|
Revision: 4131
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4131&view=rev
Author: hsujohnhsu
Date: 2008-09-10 01:36:28 +0000 (Wed, 10 Sep 2008)
Log Message:
-----------
final touches to upgrading gazebo again.
fancier gui window, view body properties.
fix to pass IsConnected() call from controllers back to sensor UpdateChild() calls.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/3rdparty/opende/Makefile
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-09-10 01:26:30 UTC (rev 4130)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-09-10 01:36:28 UTC (rev 4131)
@@ -997,6 +997,17 @@
{
if (lastUpdate + updatePeriod <= Simulator::Instance()->GetSimTime())
{
+@@ -210,6 +219,10 @@
+ {
+ std::vector<Iface*>::const_iterator iter;
+
++ // if the alwaysOn flag is true, this controller is connected
++ if (this->alwaysOnP->GetValue())
++ return true;
++
+ for (iter=this->ifaces.begin(); iter!=this->ifaces.end(); iter++)
+ {
+ if ((*iter)->GetOpenCount() > 0)
Index: server/controllers/ptz/generic/Generic_PTZ.cc
===================================================================
--- server/controllers/ptz/generic/Generic_PTZ.cc (revision 7016)
Modified: pkg/trunk/3rdparty/opende/Makefile
===================================================================
--- pkg/trunk/3rdparty/opende/Makefile 2008-09-10 01:26:30 UTC (rev 4130)
+++ pkg/trunk/3rdparty/opende/Makefile 2008-09-10 01:36:28 UTC (rev 4131)
@@ -2,7 +2,7 @@
WITH_DRAWSTUFF = yes
CFG_OPTIONS = --with-arch=nocona --enable-release --disable-asserts --with-pic --enable-double-precision
-SVN_REVISION = -r 1542
+SVN_REVISION = -r 1550
ifeq ($(WITH_DRAWSTUFF), yes)
CFG_OPTIONS += --with-drawstuff=X11
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc 2008-09-10 01:26:30 UTC (rev 4130)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc 2008-09-10 01:36:28 UTC (rev 4131)
@@ -107,7 +107,9 @@
void Ros_Camera::UpdateChild()
{
- this->PutCameraData();
+ // do this first so there's chance for sensor to run 1 frame after activate
+ if (this->myParent->IsActive())
+ this->PutCameraData();
#if 0
// do this first so there's chance for sensor to run 1 frame after activate
if (this->myParent->IsActive())
@@ -181,6 +183,7 @@
src = this->myParent->GetImageData(0);
dst = data->image;
+ //std::cout << " updating camera " << this->topicName << " " << data->image_size << std::endl;
// TODO: can skip copy to Iface if Iface is not used
if (src)
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-10 02:39:22
|
Revision: 4133
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4133&view=rev
Author: hsujohnhsu
Date: 2008-09-10 02:39:32 +0000 (Wed, 10 Sep 2008)
Log Message:
-----------
new Ros Stereo node. Not yet tested, thus, commented out in pr2.xml for now.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
Added Paths:
-----------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Stereo_Camera.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Stereo_Camera.cc
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-09-10 02:38:14 UTC (rev 4132)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-09-10 02:39:32 UTC (rev 4133)
@@ -9,6 +9,7 @@
add_definitions(-Wall)
#rospack_add_library(Pr2_Actarray src/Pr2_Actarray.cc)
#rospack_add_library(Pr2_Gripper src/Pr2_Gripper.cc)
+rospack_add_library(Ros_Stereo_Camera src/Ros_Stereo_Camera.cc)
rospack_add_library(Ros_Camera src/Ros_Camera.cc)
rospack_add_library(Ros_Laser src/Ros_Laser.cc)
#rospack_add_library(Ros_Node src/Ros_Node.cc)
Added: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Stereo_Camera.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Stereo_Camera.hh (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Stereo_Camera.hh 2008-09-10 02:39:32 UTC (rev 4133)
@@ -0,0 +1,154 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/*
+ * Desc: A stereo camera controller
+ * Author: Nathan Koenig
+ * Date: 06 April 2008
+ * SVN: $Id:$
+ */
+
+#ifndef ROS_STEREO_CAMERA_HH
+#define ROS_STEREO_CAMERA_HH
+
+#include <map>
+
+#include <ros/node.h>
+#include <std_msgs/PointCloud.h>
+#include <std_msgs/Image.h>
+
+#include <Generic_Camera.hh>
+#include <gazebo/gazebo.h>
+#include <gazebo/Param.hh>
+#include <gazebo/Controller.hh>
+#include <StereoCameraSensor.hh>
+
+namespace gazebo
+{
+ class CameraIface;
+ class StereoCameraIface;
+
+/// @addtogroup gazebo_controller
+/// @{
+/** \defgroup stereocamera stereo camera
+
+ \brief Stereo camera controller.
+
+ This is a controller that collects data from a Stereo Camera Sensor and populates a libgazebo stereo camera interfaace. This controller should only be used as a child of a stereo camera sensor
+
+ \verbatim
+ <model:physical name="camera_model">
+ <body:empty name="camera_body">
+ <sensor:stereocamera name="stereo_camera_sensor">
+
+ <controller:stereo_camera name="controller-name">
+ <interface:stereocamera name="iface-name"/>
+ </controller:stereo_camera>
+
+ </sensor:stereocamera>
+ </body:empty>
+ </model:phyiscal>
+ \endverbatim
+
+\{
+*/
+
+/// \brief Stereo camera controller.
+///
+/// This is a controller that simulates a stereo camera
+class Ros_Stereo_Camera : public Controller
+{
+ /// \brief Constructor
+ /// \param parent The parent entity, must be a Model or a Sensor
+ public: Ros_Stereo_Camera(Entity *parent);
+
+ /// \brief Destructor
+ public: virtual ~Ros_Stereo_Camera();
+
+ /// \brief True if a stereo iface is connected
+ public: bool StereoIfaceConnected() const;
+
+ /// \brief Load the controller
+ /// \param node XML config node
+ /// \return 0 on success
+ protected: virtual void LoadChild(XMLConfigNode *node);
+
+ /// \brief Save the controller.
+ /// \stream Output stream
+ protected: void SaveChild(std::string &prefix, std::ostream &stream);
+
+ /// \brief Init the controller
+ /// \return 0 on success
+ protected: virtual void InitChild();
+
+ /// \brief Update the controller
+ /// \return 0 on success
+ protected: virtual void UpdateChild();
+
+ /// \brief Finalize the controller
+ /// \return 0 on success
+ protected: virtual void FiniChild();
+
+ /// \brief Put stereo data to the iface
+ private: void PutStereoData();
+
+ /// \brief Put camera data to the iface
+ private: void PutCameraData( unsigned int camera);
+
+ /// The camera interface
+ private: StereoCameraIface *stereoIface;
+ private: std::map< std::string, CameraIface*> cameraIfaces;
+
+ private: ParamT<std::string> *leftCameraNameP;
+ private: ParamT<std::string> *rightCameraNameP;
+
+ /// The parent sensor
+ private: StereoCameraSensor *myParent;
+
+ // pointer to ros node
+ private: ros::node *rosnode;
+
+ // ros message
+ private: std_msgs::PointCloud cloudMsg;
+ // ros message
+ private: std_msgs::Image imageMsg[2];
+
+ // topic name
+ private: std::string topicName;
+ private: std::string leftTopicName;
+ private: std::string rightTopicName;
+
+ // frame transform name, should match link name
+ // FIXME: extract link name directly?
+ private: std::string frameName;
+
+ // A mutex to lock access to fields that are used in message callbacks
+ private: ros::thread::mutex lock;
+
+
+};
+
+/** /} */
+/// @}
+
+}
+
+#endif
+
Added: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Stereo_Camera.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Stereo_Camera.cc (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Stereo_Camera.cc 2008-09-10 02:39:32 UTC (rev 4133)
@@ -0,0 +1,277 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/*
+ * Desc: Stereo camera controller.
+ * Author: Nathan Koenig
+ * Date: 06 April 2008
+ * SVN info: $Id: Ros_Stereo_Camera.cc 4436 2008-03-24 17:42:45Z robotos $
+ */
+
+#include <algorithm>
+#include <assert.h>
+
+#include <gazebo_plugin/Ros_Stereo_Camera.hh>
+#include <gazebo/Sensor.hh>
+#include <gazebo/Global.hh>
+#include <gazebo/XMLConfig.hh>
+#include <gazebo/Simulator.hh>
+#include <gazebo/gazebo.h>
+#include <gazebo/GazeboError.hh>
+#include <gazebo/ControllerFactory.hh>
+
+using namespace gazebo;
+
+GZ_REGISTER_DYNAMIC_CONTROLLER("ros_stereocamera", Ros_Stereo_Camera);
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+Ros_Stereo_Camera::Ros_Stereo_Camera(Entity *parent)
+ : Controller(parent)
+{
+ this->myParent = dynamic_cast<StereoCameraSensor*>(this->parent);
+
+ Param::Begin(&this->parameters);
+ this->leftCameraNameP = new ParamT<std::string>("leftcamera","", 1);
+ this->rightCameraNameP = new ParamT<std::string>("rightcamera","", 1);
+ Param::End();
+
+ if (!this->myParent)
+ gzthrow("Ros_Stereo_Camera controller requires a Stereo Camera Sensor as its parent");
+
+ // set parent sensor to active automatically
+ this->myParent->SetActive(true);
+
+ rosnode = ros::g_node; // comes from where?
+ int argc = 0;
+ char** argv = NULL;
+ if (rosnode == NULL)
+ {
+ // this only works for a single camera.
+ ros::init(argc,argv);
+ rosnode = new ros::node("ros_gazebo",ros::node::DONT_HANDLE_SIGINT);
+ printf("-------------------- starting node in stereo camera \n");
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Destructor
+Ros_Stereo_Camera::~Ros_Stereo_Camera()
+{
+ delete this->leftCameraNameP;
+ delete this->rightCameraNameP;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Load the controller
+void Ros_Stereo_Camera::LoadChild(XMLConfigNode *node)
+{
+ std::vector<Iface*>::iterator iter;
+
+ for (iter = this->ifaces.begin(); iter != this->ifaces.end(); iter++)
+ {
+ if ((*iter)->GetType() == "stereo")
+ this->stereoIface = dynamic_cast<StereoCameraIface*>(*iter);
+ else if ((*iter)->GetType() == "camera")
+ {
+ CameraIface *ciface = dynamic_cast<CameraIface*>(*iter);
+ this->cameraIfaces[ciface->GetId()] = ciface;
+ }
+ }
+
+ this->leftCameraNameP->Load(node);
+ this->rightCameraNameP->Load(node);
+
+ if (!this->stereoIface)
+ gzthrow("Ros_Stereo_Camera controller requires a StereoCameraIface");
+
+ this->topicName = node->GetString("topicName","default_ros_stereocamera",0); //read from xml file
+ this->leftTopicName = node->GetString("leftTopicName","default_ros_stereocamera_left_image",0); //read from xml file
+ this->rightTopicName = node->GetString("rightTopicName","default_ros_stereocamera_right_image",0); //read from xml file
+ this->frameName = node->GetString("frameName","default_ros_stereocamera",0); //read from xml file
+
+ std::cout << "================= " << this->topicName << std::endl;
+ rosnode->advertise<std_msgs::PointCloud>(this->topicName);
+ rosnode->advertise<std_msgs::Image>(this->leftTopicName);
+ rosnode->advertise<std_msgs::Image>(this->rightTopicName);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Save the controller.
+void Ros_Stereo_Camera::SaveChild(std::string &prefix, std::ostream &stream)
+{
+ stream << prefix << *(this->leftCameraNameP) << "\n";
+ stream << prefix << *(this->rightCameraNameP) << "\n";
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Initialize the controller
+void Ros_Stereo_Camera::InitChild()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// True if a stereo iface is connected
+bool Ros_Stereo_Camera::StereoIfaceConnected() const
+{
+ // always on
+ return true;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Update the controller
+void Ros_Stereo_Camera::UpdateChild()
+{
+
+ this->PutCameraData( 0 );
+ this->PutCameraData( 1 );
+
+ this->PutStereoData();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Finalize the controller
+void Ros_Stereo_Camera::FiniChild()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Put stereo data to the interface
+void Ros_Stereo_Camera::PutStereoData()
+{
+ StereoCameraData* stereo_data = new StereoCameraData();
+
+ const float *disp_src;
+ float *disp_dst;
+
+ // Data timestamp
+ stereo_data->head.time = Simulator::Instance()->GetSimTime();
+
+ stereo_data->width = this->myParent->GetImageWidth();
+ stereo_data->height = this->myParent->GetImageHeight();
+ stereo_data->farClip = this->myParent->GetFarClip();
+ stereo_data->nearClip = this->myParent->GetNearClip();
+
+ stereo_data->hfov = *(this->myParent->GetHFOV());
+ stereo_data->vfov = *(this->myParent->GetVFOV());
+
+ stereo_data->right_depth_size = stereo_data->width * stereo_data->height * sizeof(float);
+ stereo_data->left_depth_size = stereo_data->width * stereo_data->height * sizeof(float);
+
+ assert (stereo_data->right_depth_size <= sizeof(stereo_data->right_depth));
+ assert (stereo_data->left_depth_size <= sizeof(stereo_data->left_depth));
+
+ // Copy the left depth data to the interface
+ disp_src = this->myParent->GetDepthData(0);
+ disp_dst = stereo_data->left_depth;
+ memcpy(disp_dst, disp_src, stereo_data->left_depth_size);
+
+ // Copy the right depth data to the interface
+ disp_src = this->myParent->GetDepthData(1);
+ disp_dst = stereo_data->right_depth;
+ memcpy(disp_dst, disp_src, stereo_data->right_depth_size);
+
+ //FIXME: the does nothing except get time for now
+ if (disp_src)
+ {
+ this->lock.lock();
+ // copy data into point cloud
+ this->cloudMsg.header.frame_id = this->frameName;
+ this->cloudMsg.header.stamp.sec = (unsigned long)floor(stereo_data->head.time);
+ this->cloudMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( stereo_data->head.time - this->cloudMsg.header.stamp.sec) );
+ //this->cloudMsg.set_data_size(10000);
+ //this->cloudMsg.data = (unsigned char*)NULL;
+ // publish to ros
+ rosnode->publish(this->topicName,this->cloudMsg);
+ this->lock.unlock();
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Put camera data to the interface
+void Ros_Stereo_Camera::PutCameraData(unsigned int camera)
+{
+ CameraData *camera_data = new CameraData();
+ const unsigned char *rgb_src = NULL;
+ unsigned char *rgb_dst = NULL;
+ Pose3d cameraPose;
+
+ camera_data->head.time = Simulator::Instance()->GetSimTime();
+
+ camera_data->width = this->myParent->GetImageWidth();
+ camera_data->height = this->myParent->GetImageHeight();
+ camera_data->image_size = camera_data->width * camera_data->height * 3;
+ assert (camera_data->image_size <= sizeof(camera_data->image));
+
+ camera_data->hfov = *(this->myParent->GetHFOV());
+ camera_data->vfov = *(this->myParent->GetVFOV());
+
+ // Set the pose of the camera
+ cameraPose = this->myParent->GetWorldPose();
+ camera_data->camera_pose.pos.x = cameraPose.pos.x;
+ camera_data->camera_pose.pos.y = cameraPose.pos.y;
+ camera_data->camera_pose.pos.z = cameraPose.pos.z;
+ camera_data->camera_pose.roll = cameraPose.rot.GetRoll();
+ camera_data->camera_pose.pitch = cameraPose.rot.GetPitch();
+ camera_data->camera_pose.yaw = cameraPose.rot.GetYaw();
+
+ // Copy the pixel data to the interface
+ rgb_src = this->myParent->GetImageData(camera);
+ rgb_dst = camera_data->image;
+
+ memcpy(rgb_dst, rgb_src, camera_data->image_size);
+
+
+ if (rgb_src)
+ {
+ this->lock.lock();
+ // copy data into image
+ this->imageMsg[camera].header.frame_id = this->frameName;
+ this->imageMsg[camera].header.stamp.sec = (unsigned long)floor(camera_data->head.time);
+ this->imageMsg[camera].header.stamp.nsec = (unsigned long)floor( 1e9 * ( camera_data->head.time - this->imageMsg[camera].header.stamp.sec) );
+
+ int width = this->myParent->GetImageWidth();
+ int height = this->myParent->GetImageHeight();
+ int depth = 3;
+
+ this->imageMsg[camera].width = width;
+ this->imageMsg[camera].height = height;
+ this->imageMsg[camera].compression = "raw";
+ this->imageMsg[camera].colorspace = "rgb24";
+
+ // on first pass, the sensor does not update after cameraIface is opened.
+ uint32_t buf_size = (width) * (height) * (depth);
+
+ this->imageMsg[camera].set_data_size(buf_size);
+ this->imageMsg[camera].data = (unsigned char*)rgb_src;
+
+ // publish to ros
+ if (camera==0)
+ rosnode->publish(this->leftTopicName,this->imageMsg[camera]);
+ else
+ rosnode->publish(this->rightTopicName,this->imageMsg[camera]);
+
+ this->lock.unlock();
+ }
+
+
+
+}
+
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-09-10 02:38:14 UTC (rev 4132)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-09-10 02:39:32 UTC (rev 4133)
@@ -2305,7 +2305,7 @@
<saveFramePath>frames</saveFramePath>
<baseline>0.2</baseline>
<updateRate>15.0</updateRate>
- <controller:stereocamera name="stereo_camera_controller">
+ <controller:ros_stereocamera name="stereo_camera_controller" plugin="libRos_Stereo_Camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>15.0</updateRate>
<interface:stereocamera name="stereo_iface_0" />
@@ -2313,7 +2313,11 @@
<interface:camera name="camera_iface_1" />
<leftcamera>camera_iface_0</leftcamera>
<rightcamera>camera_iface_1</rightcamera>
- </controller:stereocamera>
+ <topicName>stereo_cloud</topicName>
+ <leftTopicName>stereo_left_image</leftTopicName>
+ <rightTopicName>stereo_right_image</rightTopicName>
+ <frameName>stereo</frameName>
+ </controller:ros_stereocamera>
</sensor:stereocamera>
-->
</verbatim>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-10 19:49:47
|
Revision: 4143
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4143&view=rev
Author: hsujohnhsu
Date: 2008-09-10 19:49:56 +0000 (Wed, 10 Sep 2008)
Log Message:
-----------
* fixed PTZ initialization command for pan/tilt
* update PTZ axis in pr2.xml so it matches axis camera.
* added offsets for pose in P3D, allows initialization of robot location for fake localization. updates to pr2.xml accordingly <xyzOffsets> <rpyOffsets> tags.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/manifest.xml
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/src/Ros_PTZ.cc
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
Modified: pkg/trunk/3rdparty/gazebo/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/gazebo/manifest.xml 2008-09-10 17:57:59 UTC (rev 4142)
+++ pkg/trunk/3rdparty/gazebo/manifest.xml 2008-09-10 19:49:56 UTC (rev 4143)
@@ -11,7 +11,7 @@
<depend package="opende" />
<depend package="ogre" />
<export>
- <cpp lflags="`xml2-config --libs` -Wl,-rpath,${prefix}/gazebo/lib -L${prefix}/gazebo/lib -lgazebo -lgazeboServer" cflags="`xml2-config --cflags` -I${prefix}/gazebo/include -I${prefix}/gazebo-svn/server/physics -I${prefix}/gazebo-svn/server/sensors -I${prefix}/gazebo-svn/server/sensors/ray -I${prefix}/gazebo-svn/server/sensors/camera -I${prefix}/gazebo-svn/server -I${prefix}/gazebo-svn/server/rendering"/>
+ <cpp lflags="`xml2-config --libs` -Wl,-rpath,${prefix}/gazebo/lib -L${prefix}/gazebo/lib -lgazebo -lgazeboServer" cflags="`xml2-config --cflags` -I${prefix}/gazebo/include -I${prefix}/gazebo-svn/server/controllers -I${prefix}/gazebo-svn/server/controllers/camera/generic -I${prefix}/gazebo-svn/server/physics -I${prefix}/gazebo-svn/server/sensors -I${prefix}/gazebo-svn/server/sensors/ray -I${prefix}/gazebo-svn/server/sensors/camera -I${prefix}/gazebo-svn/server -I${prefix}/gazebo-svn/server/rendering"/>
<doxymaker external="http://playerstage.sourceforge.net/doc/Gazebo-manual-svn-html/"/>
</export>
<versioncontrol type="svn" url="https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/gazebo/trunk"/>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-09-10 17:57:59 UTC (rev 4142)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-09-10 19:49:56 UTC (rev 4143)
@@ -103,6 +103,10 @@
// FIXME: extract link name directly?
private: std::string frameName;
+ // allow specifying constant xyz and rpy offsets
+ private: Vector3 xyzOffsets;
+ private: Vector3 rpyOffsets;
+
// A mutex to lock access to fields that are used in message callbacks
private: ros::thread::mutex lock;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-09-10 17:57:59 UTC (rev 4142)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-09-10 19:49:56 UTC (rev 4143)
@@ -85,6 +85,8 @@
this->topicName = node->GetString("topicName", "", 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));
std::cout << "==== topic name for P3D ======== " << this->topicName << std::endl;
rosnode->advertise<std_msgs::Pose3DStamped>(this->topicName);
@@ -102,11 +104,20 @@
{
Quatern rot;
Vector3 pos;
+
+ // apply xyz offsets
+ pos = this->myBody->GetPosition() + this->xyzOffsets;
+
+ // apply rpy offsets
+ Vector3 rpyTotal;
+ rot = this->myBody->GetRotation();
+ rpyTotal.x = this->rpyOffsets.x + rot.GetRoll();
+ rpyTotal.y = this->rpyOffsets.y + rot.GetPitch();
+ rpyTotal.z = this->rpyOffsets.z + rot.GetYaw();
+ rot.SetFromEuler(rpyTotal);
+
this->myIface->Lock(1);
this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
-
- pos = this->myBody->GetPosition();
- rot = this->myBody->GetRotation();
this->myIface->data->pose.pos.x = pos.x;
this->myIface->data->pose.pos.y = pos.y;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_PTZ.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_PTZ.cc 2008-09-10 17:57:59 UTC (rev 4142)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_PTZ.cc 2008-09-10 19:49:56 UTC (rev 4143)
@@ -162,6 +162,9 @@
// Initialize the controller
void Ros_PTZ::InitChild()
{
+ // initialize pan/tilt angles
+ this->cmdPan = 0.0;
+ this->cmdTilt = 0.0;
}
////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml 2008-09-10 17:57:59 UTC (rev 4142)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml 2008-09-10 19:49:56 UTC (rev 4143)
@@ -75,6 +75,8 @@
<bodyName>base</bodyName>
<topicName>base_pose</topicName>
<frameName>base_frame</frameName>
+ <xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
+ <rpyOffsets>0 0 0</rpyOffsets>
<interface:position name="p3d_base_position"/>
</controller:P3D>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-09-10 17:57:59 UTC (rev 4142)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-09-10 19:49:56 UTC (rev 4143)
@@ -826,22 +826,22 @@
<anchor xyz="0 0 0" /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
</joint>
<joint name="ptz_pan_left_joint" type="revolute">
- <axis xyz=" 0 0 1 " /> <!-- direction of the joint in a local coordinate frame -->
+ <axis xyz=" 0 0 -1 " /> <!-- direction of the joint in a local coordinate frame, axis ptz uses opposite sign than our robot frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<limit min=" ptz_pan_min_limit" max="ptz_pan_max_limit" effort="10" velocity="5" />
</joint>
<joint name="ptz_tilt_left_joint" type="revolute">
- <axis xyz=" 0 1 0 " /> <!-- direction of the joint in a local coordinate frame -->
+ <axis xyz=" 0 -1 0 " /> <!-- direction of the joint in a local coordinate frame, axis ptz uses opposite sign than our robot frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<limit min=" ptz_tilt_min_limit" max="ptz_tilt_max_limit" effort="10" velocity="5" />
</joint>
<joint name="ptz_pan_right_joint" type="revolute">
- <axis xyz=" 0 0 1 " /> <!-- direction of the joint in a local coordinate frame -->
+ <axis xyz=" 0 0 -1 " /> <!-- direction of the joint in a local coordinate frame, axis ptz uses opposite sign than our robot frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<limit min=" ptz_pan_min_limit" max="ptz_pan_max_limit" effort="10" velocity="5" />
</joint>
<joint name="ptz_tilt_right_joint" type="revolute">
- <axis xyz=" 0 1 0 " /> <!-- direction of the joint in a local coordinate frame -->
+ <axis xyz=" 0 -1 0 " /> <!-- direction of the joint in a local coordinate frame, axis ptz uses opposite sign than our robot frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<limit min=" ptz_tilt_min_limit" max="ptz_tilt_max_limit" effort="10" velocity="5" />
</joint>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-10 21:32:40
|
Revision: 4152
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4152&view=rev
Author: hsujohnhsu
Date: 2008-09-10 21:32:47 +0000 (Wed, 10 Sep 2008)
Log Message:
-----------
updated P3D constant offsets for base_pose for all pr2 demos.
updated demo with more complex maps of willow garage.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_test_wg.world
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test_headless.xml
Added Paths:
-----------
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless-fake_localization.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_test_wg.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_test_wg_headless.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_test_wg_headless.world
Added: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless-fake_localization.xml (rev 0)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless-fake_localization.xml 2008-09-10 21:32:47 UTC (rev 4152)
@@ -0,0 +1,21 @@
+<launch>
+ <group name="wg">
+ <include file="$(find gazebo_robot_description)/pr2_test_headless.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" />
+
+ <!-- for visualization -->
+ <node pkg="wx_camera_panel" type="camera_test" args="" respawn="true" output="screen" />
+ <!--node pkg="wx_camera_panel" type="standalone_camera.py" args="-i PTZL_image -s PTZL_state -c PTZL_cmd" respawn="true" output="screen" /-->
+ <node pkg="laser_view" type="laser_view" args="scan:=tilt_scan" respawn="true" output="screen" />
+ <node pkg="ogre_visualizer" type="visualizer_test" respawn="true" output="screen" />
+ <!--node pkg="wxpy_ros" type="ros_panel" respawn="false" output="screen" /--><!-- does not start correctly-->
+
+ <!-- for manual control -->
+ <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ <node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" />
+ </group>
+</launch>
+
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless.xml 2008-09-10 21:12:13 UTC (rev 4151)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless.xml 2008-09-10 21:32:47 UTC (rev 4152)
@@ -8,6 +8,7 @@
<!-- for visualization -->
<node pkg="wx_camera_panel" type="camera_test" args="" respawn="true" output="screen" />
+ <!--node pkg="wx_camera_panel" type="standalone_camera.py" args="-i PTZL_image -s PTZL_state -c PTZL_cmd" respawn="true" output="screen" /--> <!-- not working yet -->
<node pkg="laser_view" type="laser_view" args="scan:=tilt_scan" respawn="true" output="screen" />
<node pkg="ogre_visualizer" type="visualizer_test" respawn="true" output="screen" />
<!--node pkg="wxpy_ros" type="ros_panel" respawn="false" output="screen" /--><!-- does not start correctly-->
Added: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml (rev 0)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml 2008-09-10 21:32:47 UTC (rev 4152)
@@ -0,0 +1,21 @@
+<launch>
+ <group name="wg">
+ <include file="$(find gazebo_robot_description)/pr2_test_wg.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" />
+
+ <!-- for visualization -->
+ <node pkg="wx_camera_panel" type="camera_test" args="" respawn="true" output="screen" />
+ <!--node pkg="wx_camera_panel" type="standalone_camera.py" args="-i PTZL_image -s PTZL_state -c PTZL_cmd" respawn="true" output="screen" /-->
+ <node pkg="laser_view" type="laser_view" args="scan:=tilt_scan" respawn="true" output="screen" />
+ <node pkg="ogre_visualizer" type="visualizer_test" respawn="true" output="screen" />
+ <!--node pkg="wxpy_ros" type="ros_panel" respawn="false" output="screen" /--><!-- does not start correctly-->
+
+ <!-- for manual control -->
+ <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ <node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" />
+ </group>
+</launch>
+
Added: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml (rev 0)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml 2008-09-10 21:32:47 UTC (rev 4152)
@@ -0,0 +1,21 @@
+<launch>
+ <group name="wg">
+ <include file="$(find gazebo_robot_description)/pr2_test_wg_headless.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" />
+
+ <!-- for visualization -->
+ <node pkg="wx_camera_panel" type="camera_test" args="" respawn="true" output="screen" />
+ <!--node pkg="wx_camera_panel" type="standalone_camera.py" args="-i PTZL_image -s PTZL_state -c PTZL_cmd" respawn="true" output="screen" /--> <!-- not working yet -->
+ <node pkg="laser_view" type="laser_view" args="scan:=tilt_scan" respawn="true" output="screen" />
+ <node pkg="ogre_visualizer" type="visualizer_test" respawn="true" output="screen" />
+ <!--node pkg="wxpy_ros" type="ros_panel" respawn="false" output="screen" /--><!-- does not start correctly-->
+
+ <!-- for manual control -->
+ <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ <node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" />
+ </group>
+</launch>
+
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml 2008-09-10 21:12:13 UTC (rev 4151)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml 2008-09-10 21:32:47 UTC (rev 4152)
@@ -1,9 +1,7 @@
<launch>
<group name="wg">
- <include file="$(find wg_robot_description)/send.xml"/>
- <node pkg="gazebo" type="run-gazebo.sh" args="$(find gazebo_robot_description)/world/robot_rosgazebo_wg.world" respawn="true" />
+ <include file="$(find gazebo_robot_description)/pr2_test_wg.xml"/>
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" />
- <node pkg="pr2_gazebo" type="run-pr2_gazebo.sh" args="" respawn="true" />
<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" />
<node pkg="nav_view" type="nav_view" respawn="true" />
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_test_wg.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_test_wg.xml (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_test_wg.xml 2008-09-10 21:32:47 UTC (rev 4152)
@@ -0,0 +1,8 @@
+<launch>
+ <group name="wg">
+ <include file="$(find wg_robot_description)/send_test.xml"/>
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="run-gazebo.sh" args="$(find gazebo_robot_description)/world/robot_test_wg.world" respawn="false" output="screen" />
+ </group>
+</launch>
+
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_test_wg_headless.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_test_wg_headless.xml (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_test_wg_headless.xml 2008-09-10 21:32:47 UTC (rev 4152)
@@ -0,0 +1,8 @@
+<launch>
+ <group name="wg">
+ <include file="$(find wg_robot_description)/send_test_headless.xml"/>
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="run-gazebo.sh" args="-g $(find gazebo_robot_description)/world/robot_test_wg_headless.world" respawn="false" output="screen" />
+ </group>
+</launch>
+
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_test_wg.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_test_wg.world 2008-09-10 21:12:13 UTC (rev 4151)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_test_wg.world 2008-09-10 21:32:47 UTC (rev 4152)
@@ -67,18 +67,17 @@
</body:plane>
</model:physical>
-<!--
<model:physical name="walls">
<include embedded="false">
- <xi:include href="willow-walls.model" />
+ <xi:include href="tests/willow-walls.model" />
</include>
</model:physical>
--->
- <model:physical name="map">
+<!--
+ <model:physical name="willow_map">
<xyz>-25.65 25.65 1.0</xyz>
<rpy>180 0 0</rpy>
- <body:map name="map_body">
- <geom:map name="map_geom">
+ <body:map name="willow_map_body">
+ <geom:map name="willow_map_geom">
<image>willowMap.png</image>
<threshold>200</threshold>
<granularity>2</granularity>
@@ -89,6 +88,7 @@
</geom:map>
</body:map>
</model:physical>
+-->
<model:physical name="robot_model1">
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_test_wg_headless.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_test_wg_headless.world (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_test_wg_headless.world 2008-09-10 21:32:47 UTC (rev 4152)
@@ -0,0 +1,120 @@
+<?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.0000000001</cfm>
+ <erp>0.2</erp>
+ </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>
+ </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>100</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>
+ <material>Gazebo/GrassFloor</material>
+ </geom:plane>
+ </body:plane>
+ </model:physical>
+
+ <model:physical name="walls">
+ <include embedded="false">
+ <xi:include href="tests/willow-walls.model" />
+ </include>
+ </model:physical>
+<!--
+ <model:physical name="map">
+ <xyz>-25.65 25.65 1.0</xyz>
+ <rpy>180 0 0</rpy>
+ <body:map name="map_body">
+ <geom:map name="map_geom">
+ <image>willowMap.png</image>
+ <threshold>200</threshold>
+ <granularity>2</granularity>
+ <negative>false</negative>
+ <scale>0.1</scale>
+ <offset>0 0 0</offset>
+ <material>Gazebo/Rocky</material>
+ </geom:map>
+ </body:map>
+ </model:physical>
+-->
+
+
+ <model:physical name="robot_model1">
+ <xyz>0.0 0.0 0.02</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_xml_test.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/wg_robot_description/pr2/controllers_gazebo.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml 2008-09-10 21:12:13 UTC (rev 4151)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml 2008-09-10 21:32:47 UTC (rev 4152)
@@ -66,6 +66,8 @@
<bodyName>base</bodyName>
<topicName>base_pose</topicName>
<frameName>base_frame</frameName>
+ <xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
+ <rpyOffsets>0 0 0</rpyOffsets>
<interface:position name="p3d_base_position"/>
</controller:P3D>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test_headless.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test_headless.xml 2008-09-10 21:12:13 UTC (rev 4151)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test_headless.xml 2008-09-10 21:32:47 UTC (rev 4152)
@@ -75,6 +75,8 @@
<bodyName>base</bodyName>
<topicName>base_pose</topicName>
<frameName>base_frame</frameName>
+ <xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
+ <rpyOffsets>0 0 0</rpyOffsets>
<interface:position name="p3d_base_position"/>
</controller:P3D>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-09-10 21:39:15
|
Revision: 4153
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4153&view=rev
Author: isucan
Date: 2008-09-10 21:39:21 +0000 (Wed, 10 Sep 2008)
Log Message:
-----------
added EST (Expansive Space Trees) algorithm
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/RKPModel.h
pkg/trunk/motion_planning/kinematic_planning/include/RKPSBLSetup.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/CMakeLists.txt
pkg/trunk/motion_planning/ompl/code/examples/samplingbased/kinematic/grid/grid.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/sbl/SBL.h
pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
Added Paths:
-----------
pkg/trunk/motion_planning/kinematic_planning/include/RKPESTSetup.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/EST.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/src/
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/src/EST.cpp
Added: pkg/trunk/motion_planning/kinematic_planning/include/RKPESTSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPESTSetup.h (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPESTSetup.h 2008-09-10 21:39:21 UTC (rev 4153)
@@ -0,0 +1,137 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#ifndef KINEMATIC_PLANNING_RKP_EST_SETUP_
+#define KINEMATIC_PLANNING_RKP_EST_SETUP_
+
+#include "RKPPlannerSetup.h"
+#include <ompl/extension/samplingbased/kinematic/extension/est/EST.h>
+
+class RKPESTSetup : public RKPPlannerSetup
+{
+ public:
+
+ RKPESTSetup(void) : RKPPlannerSetup()
+ {
+ }
+
+ virtual ~RKPESTSetup(void)
+ {
+ if (dynamic_cast<ompl::EST_t>(mp))
+ {
+ ompl::ProjectionEvaluator_t pe = dynamic_cast<ompl::EST_t>(mp)->getProjectionEvaluator();
+ if (pe)
+ delete pe;
+ }
+ }
+
+ virtual bool setup(RKPModelBase *model, std::map<std::string, std::string> &options)
+ {
+ std::cout << "Adding EST instance for motion planning: " << model->groupName << std::endl;
+
+ si = new SpaceInformationRKPModel(model);
+ svc = new StateValidityPredicate(model);
+ si->setStateValidityChecker(svc);
+
+ smoother = new ompl::PathSmootherKinematic(si);
+ smoother->setMaxSteps(50);
+ smoother->setMaxEmptySteps(4);
+
+ ompl::EST_t sbl = new ompl::EST(si);
+ mp = sbl;
+
+ bool setDim = false;
+ bool setProj = false;
+
+ if (options.find("range") != options.end())
+ {
+ double range = string_utils::fromString<double>(options["range"]);
+ sbl->setRange(range);
+ std::cout << "Range is set to " << range << std::endl;
+ }
+
+ if (options.find("projection") != options.end())
+ {
+ std::string proj = options["projection"];
+ std::vector<unsigned int> projection;
+ std::stringstream ss(proj);
+ while (ss.good())
+ {
+ unsigned int comp;
+ ss >> comp;
+ projection.push_back(comp);
+ }
+
+ sbl->setProjectionEvaluator(new ompl::OrthogonalProjectionEvaluator(projection));
+
+ std::cout << "Projection is set to " << proj << std::endl;
+ setProj = true;
+ }
+
+ if (options.find("celldim") != options.end())
+ {
+ std::string celldim = options["celldim"];
+ std::vector<double> cdim;
+ std::stringstream ss(celldim);
+ while (ss.good())
+ {
+ double comp;
+ ss >> comp;
+ cdim.push_back(comp);
+ }
+
+ sbl->setCellDimensions(cdim);
+ setDim = true;
+ std::cout << "Cell dimensions set to " << celldim << std::endl;
+ }
+
+ if (!setDim || !setProj)
+ {
+ std::cout << "Adding EST failed: need to set both 'projection' and 'cellldim' for " << model->groupName << std::endl;
+ return false;
+ }
+ else
+ {
+ setupDistanceEvaluators();
+ si->setup();
+ mp->setup();
+ return true;
+ }
+ }
+
+};
+
+#endif
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RKPModel.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPModel.h 2008-09-10 21:32:47 UTC (rev 4152)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPModel.h 2008-09-10 21:39:21 UTC (rev 4153)
@@ -42,6 +42,7 @@
#include "RKPRRTSetup.h"
#include "RKPLazyRRTSetup.h"
#include "RKPSBLSetup.h"
+#include "RKPESTSetup.h"
#include <string>
#include <map>
@@ -78,6 +79,14 @@
delete rrt;
}
+ void addEST(std::map<std::string, std::string> &options)
+ {
+ RKPPlannerSetup *est = new RKPESTSetup();
+ if (est->setup(dynamic_cast<RKPModelBase*>(this), options))
+ planners["EST"] = est;
+ else
+ delete est;
+ }
void addSBL(std::map<std::string, std::string> &options)
{
RKPPlannerSetup *sbl = new RKPSBLSetup();
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RKPSBLSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPSBLSetup.h 2008-09-10 21:32:47 UTC (rev 4152)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPSBLSetup.h 2008-09-10 21:39:21 UTC (rev 4153)
@@ -52,7 +52,7 @@
{
if (dynamic_cast<ompl::SBL_t>(mp))
{
- ompl::SBL::ProjectionEvaluator_t pe = dynamic_cast<ompl::SBL_t>(mp)->getProjectionEvaluator();
+ ompl::ProjectionEvaluator_t pe = dynamic_cast<ompl::SBL_t>(mp)->getProjectionEvaluator();
if (pe)
delete pe;
}
@@ -95,7 +95,7 @@
projection.push_back(comp);
}
- sbl->setProjectionEvaluator(new ompl::SBL::OrthogonalProjectionEvaluator(projection));
+ sbl->setProjectionEvaluator(new ompl::OrthogonalProjectionEvaluator(projection));
std::cout << "Projection is set to " << proj << std::endl;
setProj = true;
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-09-10 21:32:47 UTC (rev 4152)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-09-10 21:39:21 UTC (rev 4153)
@@ -228,6 +228,14 @@
options = data.getMapTagValues("planning", "SBL");
}
model->addSBL(options);
+
+ options.clear();
+ if (group)
+ {
+ const robot_desc::URDF::Map &data = group->data;
+ options = data.getMapTagValues("planning", "EST");
+ }
+ model->addEST(options);
}
ModelMap m_models;
Modified: pkg/trunk/motion_planning/ompl/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/ompl/CMakeLists.txt 2008-09-10 21:32:47 UTC (rev 4152)
+++ pkg/trunk/motion_planning/ompl/CMakeLists.txt 2008-09-10 21:39:21 UTC (rev 4153)
@@ -8,6 +8,7 @@
code/ompl/extension/samplingbased/kinematic/src/PathSmootherKinematic.cpp
code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
code/ompl/extension/samplingbased/kinematic/extension/rrt/src/LazyRRT.cpp
+ code/ompl/extension/samplingbased/kinematic/extension/est/src/EST.cpp
code/ompl/extension/samplingbased/kinematic/extension/sbl/src/SBL.cpp
)
rospack_add_compile_flags(ompl -Wextra)
Modified: pkg/trunk/motion_planning/ompl/code/examples/samplingbased/kinematic/grid/grid.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/examples/samplingbased/kinematic/grid/grid.cpp 2008-09-10 21:32:47 UTC (rev 4152)
+++ pkg/trunk/motion_planning/ompl/code/examples/samplingbased/kinematic/grid/grid.cpp 2008-09-10 21:39:21 UTC (rev 4153)
@@ -315,13 +315,13 @@
std::vector<unsigned int> projection;
projection.push_back(0);
projection.push_back(1);
- ope = new SBL::OrthogonalProjectionEvaluator(projection);
+ ope = new OrthogonalProjectionEvaluator(projection);
sbl->setProjectionEvaluator(ope);
return sbl;
}
- SBL::OrthogonalProjectionEvaluator_t ope;
+ OrthogonalProjectionEvaluator_t ope;
};
Added: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h (rev 0)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h 2008-09-10 21:39:21 UTC (rev 4153)
@@ -0,0 +1,96 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* \author Ioan Sucan */
+
+#ifndef OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_PROJECTION_EVALUATOR_
+#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_PROJECTION_EVALUATOR_
+
+#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
+
+namespace ompl
+{
+
+ /** Forward class declaration */
+ ForwardClassDeclaration(ProjectionEvaluator);
+
+ /** Abstract definition for a class computing projections */
+ class ProjectionEvaluator
+ {
+ public:
+ /** Destructor */
+ virtual ~ProjectionEvaluator(void)
+ {
+ }
+
+ /** Return the dimension of the projection defined by this evaluator */
+ virtual unsigned int getDimension(void) const = 0;
+
+ /** Compute the projection as an array of double values */
+ virtual void operator()(const SpaceInformationKinematic::StateKinematic_t state, double *projection) = 0;
+ };
+
+ /** Forward class declaration */
+ ForwardClassDeclaration(OrthogonalProjectionEvaluator);
+
+ /** Definition for a class computing orthogonal projections */
+ class OrthogonalProjectionEvaluator : public ProjectionEvaluator
+ {
+ public:
+
+ OrthogonalProjectionEvaluator(const std::vector<unsigned int> &components) : ProjectionEvaluator()
+ {
+ m_components = components;
+ }
+
+ virtual unsigned int getDimension(void) const
+ {
+ return m_components.size();
+ }
+
+ virtual void operator()(const SpaceInformationKinematic::StateKinematic_t state, double *projection)
+ {
+ for (unsigned int i = 0 ; i < m_components.size() ; ++i)
+ projection[i] = state->values[m_components[i]];
+ }
+
+ protected:
+
+ std::vector<unsigned int> m_components;
+
+ };
+
+}
+
+#endif
Added: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/EST.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/EST.h (rev 0)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/EST.h 2008-09-10 21:39:21 UTC (rev 4153)
@@ -0,0 +1,237 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* \author Ioan Sucan */
+
+#ifndef OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_EST_
+#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_EST_
+
+#include "ompl/base/Planner.h"
+#include "ompl/datastructures/Grid.h"
+#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
+#include "ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h"
+#include <vector>
+
+namespace ompl
+{
+
+ /** Forward class declaration */
+ ForwardClassDeclaration(EST);
+
+ /**
+ @subsubsection EST Expansive Space Trees (EST)
+
+ @par Short description
+
+
+ */
+ class EST : public Planner
+ {
+ public:
+
+ EST(SpaceInformation_t si) : Planner(si)
+ {
+ m_type = PLAN_TO_GOAL_STATE | PLAN_TO_GOAL_REGION;
+ random_utils::init(&m_rngState);
+ m_projectionEvaluator = NULL;
+ m_projectionDimension = 0;
+ m_goalBias = 0.05;
+ m_rho = 0.5;
+ }
+
+ virtual ~EST(void)
+ {
+ freeMemory();
+ }
+
+ virtual bool solve(double solveTime);
+
+ virtual void clear(void)
+ {
+ freeMemory();
+ m_tree.grid.clear();
+ m_tree.size = 0;
+ }
+
+ /** In the process of randomly selecting states in the state
+ space to attempt to go towards, the algorithm may in fact
+ choose the actual goal state, if it knows it, with some
+ probability. This probability is a real number between 0.0
+ and 1.0; its value should usually be around 0.05 and
+ should not be too large. It is probably a good idea to use
+ the default value. */
+ void setGoalBias(double goalBias)
+ {
+ m_goalBias = goalBias;
+ }
+
+ /** Get the goal bias the planner is using */
+ double getGoalBias(void) const
+ {
+ return m_goalBias;
+ }
+
+ /** Set the range the planner is supposed to use. This
+ parameter greatly influences the runtime of the
+ algorithm. It is probably a good idea to find what a good
+ value is for each model the planner is used for. The range
+ parameter influences how this @b qm along the path between
+ @b qc and @b qr is chosen. @b qr may be too far, and it
+ may not be best to have @b qm = @b qr all the time (range
+ = 1.0 implies @b qm = @b qr. range should be less than
+ 1.0). However, in a large space, it is also good to leave
+ the neighborhood of @b qc (range = 0.0 implies @b qm = @b
+ qc and no progress is made. rande should be larger than
+ 0.0). Multiple values of this range parameter should be
+ tried until a suitable one is found. */
+ void setRange(double rho)
+ {
+ m_rho = rho;
+ }
+
+ /** Get the range the planner is using */
+ double getRange(void) const
+ {
+ return m_rho;
+ }
+
+ /** Set the projection evaluator. This class is able to
+ compute the projection of a given state. The simplest
+ option is to use an orthogonal projection; see
+ OrthogonalProjectionEvaluator */
+ void setProjectionEvaluator(ProjectionEvaluator_t projectionEvaluator)
+ {
+ m_projectionEvaluator = projectionEvaluator;
+ }
+
+ ProjectionEvaluator_t getProjectionEvaluator(void) const
+ {
+ return m_projectionEvaluator;
+ }
+
+ /** Define the dimension (each component) of a grid cell. The
+ number of dimensions set here must be the same as the
+ dimension of the projection computed by the projection
+ evaluator. */
+ void setCellDimensions(const std::vector<double> &cellDimensions)
+ {
+ m_cellDimensions = cellDimensions;
+ }
+
+ void getCellDimensions(std::vector<double> &cellDimensions) const
+ {
+ cellDimensions = m_cellDimensions;
+ }
+
+ virtual void setup(void)
+ {
+ assert(m_projectionEvaluator);
+ m_projectionDimension = m_projectionEvaluator->getDimension();
+ assert(m_projectionDimension > 0);
+ assert(m_cellDimensions.size() == m_projectionDimension);
+ m_tree.grid.setDimension(m_projectionDimension);
+ Planner::setup();
+ }
+
+ protected:
+
+ ForwardClassDeclaration(Motion);
+
+ class Motion
+ {
+ public:
+
+ Motion(void)
+ {
+ parent = NULL;
+ state = NULL;
+ }
+
+ Motion(unsigned int dimension)
+ {
+ state = new SpaceInformationKinematic::StateKinematic(dimension);
+ parent = NULL;
+ }
+
+ virtual ~Motion(void)
+ {
+ if (state)
+ delete state;
+ }
+
+ SpaceInformationKinematic::StateKinematic_t state;
+ Motion_t parent;
+
+ };
+
+ typedef std::vector<Motion_t> MotionSet;
+
+ struct TreeData
+ {
+ TreeData(void)
+ {
+ size = 0;
+ }
+
+ Grid<MotionSet> grid;
+ unsigned int size;
+ };
+
+ void freeMemory(void)
+ {
+ for (Grid<MotionSet>::iterator it = m_tree.grid.begin(); it != m_tree.grid.end() ; ++it)
+ {
+ for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
+ delete it->second->data[i];
+ }
+ }
+
+ void addMotion(Motion_t motion);
+ Motion_t selectMotion(void);
+ void computeCoordinates(const Motion_t motion, Grid<MotionSet>::Coord &coord);
+
+ TreeData m_tree;
+
+ ProjectionEvaluator *m_projectionEvaluator;
+ unsigned int m_projectionDimension;
+ std::vector<double> m_cellDimensions;
+
+ double m_goalBias;
+ double m_rho;
+ random_utils::rngState m_rngState;
+ };
+
+}
+
+#endif
Added: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/src/EST.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/src/EST.cpp (rev 0)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/src/EST.cpp 2008-09-10 21:39:21 UTC (rev 4153)
@@ -0,0 +1,202 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* \author Ioan Sucan */
+
+#include "ompl/extension/samplingbased/kinematic/extension/est/EST.h"
+
+bool ompl::EST::solve(double solveTime)
+{
+ SpaceInformationKinematic_t si = dynamic_cast<SpaceInformationKinematic_t>(m_si);
+ SpaceInformationKinematic::GoalRegionKinematic_t goal_r = dynamic_cast<SpaceInformationKinematic::GoalRegionKinematic_t>(si->getGoal());
+ SpaceInformationKinematic::GoalStateKinematic_t goal_s = dynamic_cast<SpaceInformationKinematic::GoalStateKinematic_t>(si->getGoal());
+ unsigned int dim = si->getStateDimension();
+
+ if (!goal_s && !goal_r)
+ {
+ m_msg.error("Unknown type of goal (or goal undefined)");
+ return false;
+ }
+
+ time_utils::Time endTime = time_utils::Time::now() + time_utils::Duration(solveTime);
+
+ if (m_tree.grid.size() == 0)
+ {
+ for (unsigned int i = 0 ; i < m_si->getStartStateCount() ; ++i)
+ {
+ Motion_t motion = new Motion(dim);
+ si->copyState(motion->state, dynamic_cast<SpaceInformationKinematic::StateKinematic_t>(si->getStartState(i)));
+ if (si->isValid(motion->state))
+ addMotion(motion);
+ else
+ {
+ m_msg.error("Initial state is in collision!");
+ delete motion;
+ }
+ }
+ }
+
+ if (m_tree.grid.size() == 0)
+ {
+ m_msg.error("There are no valid initial states!");
+ return false;
+ }
+
+ m_msg.inform("Starting with %u states", m_tree.size);
+
+ std::vector<double> range(dim);
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ range[i] = m_rho * (si->getStateComponent(i).maxValue - si->getStateComponent(i).minValue);
+
+ Motion_t solution = NULL;
+ Motion_t approxsol = NULL;
+ double approxdif = INFINITY;
+ SpaceInformationKinematic::StateKinematic_t xstate = new SpaceInformationKinematic::StateKinematic(dim);
+
+ while (time_utils::Time::now() < endTime)
+ {
+ /* Decide on a state to expand from */
+ Motion_t existing = selectMotion();
+ assert(existing);
+
+ /* sample random state (with goal biasing) */
+ if (goal_s && random_utils::uniform(&m_rngState, 0.0, 1.0) < m_goalBias)
+ si->copyState(xstate, goal_s->state);
+ else
+ si->sampleNear(xstate, existing->state, range);
+
+ if (si->checkMotionSubdivision(existing->state, xstate))
+ {
+ /* create a motion */
+ Motion_t motion = new Motion(dim);
+ si->copyState(motion->state, xstate);
+ motion->parent = existing;
+
+ addMotion(motion);
+ double dist = 0.0;
+ bool solved = goal_r->isSatisfied(motion->state, &dist);
+ if (solved)
+ {
+ approxdif = dist;
+ solution = motion;
+ break;
+ }
+ if (dist < approxdif)
+ {
+ approxdif = dist;
+ approxsol = motion;
+ }
+ }
+ }
+
+ bool approximate = false;
+ if (solution == NULL)
+ {
+ solution = approxsol;
+ approximate = true;
+ }
+
+ if (solution != NULL)
+ {
+ /* construct the solution path */
+ std::vector<Motion_t> mpath;
+ while (solution != NULL)
+ {
+ mpath.push_back(solution);
+ solution = solution->parent;
+ }
+
+ /* set the solution path */
+ SpaceInformationKinematic::PathKinematic_t path = new SpaceInformationKinematic::PathKinematic(m_si);
+ for (int i = mpath.size() - 1 ; i >= 0 ; --i)
+ {
+ SpaceInformationKinematic::StateKinematic_t st = new SpaceInformationKinematic::StateKinematic(dim);
+ si->copyState(st, mpath[i]->state);
+ path->states.push_back(st);
+ }
+ goal_r->setDifference(approxdif);
+ goal_r->setSolutionPath(path, approximate);
+ }
+
+ delete xstate;
+
+ m_msg.inform("Created %u states in %u cells", m_tree.size, m_tree.grid.size());
+
+ return goal_r->isAchieved();
+}
+
+ompl::EST::Motion_t ompl::EST::selectMotion(void)
+{
+ double sum = 0.0;
+ Grid<MotionSet>::Cell_t cell = NULL;
+ double prob = random_utils::uniform(&m_rngState) * (m_tree.grid.size() - 1);
+ for (Grid<MotionSet>::iterator it = m_tree.grid.begin(); it != m_tree.grid.end() ; ++it)
+ {
+ sum += (double)(m_tree.size - it->second->data.size()) / (double)m_tree.size;
+ if (prob < sum)
+ {
+ cell = it->second;
+ break;
+ }
+ }
+ if (!cell && m_tree.grid.size() > 0)
+ cell = m_tree.grid.begin()->second;
+ return cell && !cell->data.empty() ? cell->data[random_utils::uniformInt(&m_rngState, 0, cell->data.size() - 1)] : NULL;
+}
+
+void ompl::EST::computeCoordinates(const Motion_t motion, Grid<MotionSet>::Coord &coord)
+{
+ coord.resize(m_projectionDimension);
+ double projection[m_projectionDimension];
+ (*m_projectionEvaluator)(motion->state, projection);
+
+ for (unsigned int i = 0 ; i < m_projectionDimension; ++i)
+ coord[i] = (int)trunc(projection[i]/m_cellDimensions[i]);
+}
+
+void ompl::EST::addMotion(Motion_t motion)
+{
+ Grid<MotionSet>::Coord coord;
+ computeCoordinates(motion, coord);
+ Grid<MotionSet>::Cell_t cell = m_tree.grid.getCell(coord);
+ if (cell)
+ cell->data.push_back(motion);
+ else
+ {
+ cell = m_tree.grid.create(coord);
+ cell->data.push_back(motion);
+ m_tree.grid.add(cell);
+ }
+ m_tree.size++;
+}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/sbl/SBL.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/sbl/SBL.h 2008-09-10 21:32:47 UTC (rev 4152)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/sbl/SBL.h 2008-09-10 21:39:21 UTC (rev 4153)
@@ -40,6 +40,7 @@
#include "ompl/base/Planner.h"
#include "ompl/datastructures/Grid.h"
#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
+#include "ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h"
#include <vector>
/** Main namespace */
@@ -83,57 +84,6 @@
{
public:
- /** Forward class declaration */
- ForwardClassDeclaration(ProjectionEvaluator);
-
-
- /** Abstract definition for a class comp...
[truncated message content] |
|
From: <is...@us...> - 2008-09-11 07:22:58
|
Revision: 4197
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4197&view=rev
Author: isucan
Date: 2008-09-11 07:23:09 +0000 (Thu, 11 Sep 2008)
Log Message:
-----------
code for demo....
Modified Paths:
--------------
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/world_models/fake_world_3d_map/src/fake_world_3d_map.cpp
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-09-11 07:19:55 UTC (rev 4196)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-09-11 07:23:09 UTC (rev 4197)
@@ -44,7 +44,7 @@
#include <robot_msgs/DisplayKinematicPath.h>
#include <robot_srvs/ValidateKinematicPath.h>
-#include <pr2_controllers/SetJointPathCmd.h>
+#include <pr2_controllers/SetJointTarget.h>
#include <cassert>
@@ -86,11 +86,17 @@
req.goal_state.set_vals_size(3);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
req.goal_state.vals[i] = m_basePos[i];
-
+ /*
req.goal_state.vals[0] += 7.5;
req.goal_state.vals[1] += 0.5;
req.goal_state.vals[2] = -M_PI/2.0;
+ */
+ req.goal_state.vals[0] += 5.5;
+ req.goal_state.vals[1] += 0.5;
+ req.goal_state.vals[2] = -M_PI/2.0;
+
+
req.allowed_time = 15.0;
req.params.volumeMin.x = -10.0 + m_basePos[0]; req.params.volumeMin.y = -10.0 + m_basePos[1]; req.params.volumeMin.z = 0.0;
@@ -139,10 +145,10 @@
initialState(req.start_state);
- req.goal_state.set_vals_size(7);
+ req.goal_state.set_vals_size(4);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
req.goal_state.vals[i] = 0.0;
- req.goal_state.vals[0] = -1.0;
+ req.goal_state.vals[0] = -1.3;
req.allowed_time = 30.0;
@@ -185,21 +191,6 @@
performCall(req);
}
- void performCall(robot_srvs::KinematicPlanState::request &req)
- {
- robot_srvs::KinematicPlanState::response res;
-
- if (ros::service::call("plan_kinematic_path_state", req, res))
- {
- printPath(res.path, res.distance);
- sendDisplay(res.path, req.params.model_id);
- verifyPath(req.start_state, req.constraints, res.path, req.params.model_id);
- sendCommand(res.path, req.params.model_id);
- }
- else
- fprintf(stderr, "Service 'plan_kinematic_path_state' failed\n");
- }
-
void runTestLeftEEf(void)
{
robot_srvs::KinematicPlanLinkPosition::request req;
@@ -208,7 +199,7 @@
req.params.distance_metric = "L2Square";
req.params.planner_id = "RRT";
req.interpolate = 1;
- req.times = 4;
+ req.times = 3;
initialState(req.start_state);
@@ -235,7 +226,7 @@
req.constraints.pose[0].pose.position.z = 0.74;
req.constraints.pose[0].position_distance = 0.01;
*/
- req.allowed_time = 0.1;
+ req.allowed_time = 0.3;
req.params.volumeMin.x = -5.0 + m_basePos[0]; req.params.volumeMin.y = -5.0 + m_basePos[1]; req.params.volumeMin.z = 0.0;
req.params.volumeMax.x = 5.0 + m_basePos[0]; req.params.volumeMax.y = 5.0 + m_basePos[1]; req.params.volumeMax.z = 0.0;
@@ -258,10 +249,25 @@
fprintf(stderr, "Service 'plan_kinematic_path_position' failed\n");
}
+ void performCall(robot_srvs::KinematicPlanState::request &req)
+ {
+ robot_srvs::KinematicPlanState::response res;
+
+ if (ros::service::call("plan_kinematic_path_state", req, res))
+ {
+ printPath(res.path, res.distance);
+ sendDisplay(res.path, req.params.model_id);
+ verifyPath(req.start_state, req.constraints, res.path, req.params.model_id);
+ sendCommand(res.path, req.params.model_id);
+ }
+ else
+ fprintf(stderr, "Service 'plan_kinematic_path_state' failed\n");
+ }
+
void printPath(robot_msgs::KinematicPath &path, const double distance)
{
unsigned int nstates = path.get_states_size();
- printf("Obtained ssolution path with %u states, distance to goal = %f\n", nstates, distance);
+ printf("Obtained solution path with %u states, distance to goal = %f\n", nstates, distance);
for (unsigned int i = 0 ; i < nstates ; ++i)
{
@@ -304,12 +310,13 @@
void sendCommand(robot_msgs::KinematicPath &path, const std::string &model)
{
// create the service request
-
+ // return;
+
const double margin_fraction = 0.1;
planning_models::KinematicModel::StateParams *state = m_kmodel->newStateParams();
- pr2_controllers::SetJointPathCmd::request req;
- req.set_path_size(path.get_states_size());
+ pr2_controllers::SetJointTarget::request req;
+ req.set_positions_size(path.get_states_size());
int groupID = m_kmodel->getGroupID(model);
@@ -327,20 +334,20 @@
state->setParams(path.states[i].vals, groupID);
- req.path[i].set_names_size(joints.size());
- req.path[i].set_positions_size(joints.size());
- req.path[i].set_margins_size(joints.size());
- req.path[i].timeout = 1.0;
+ req.positions[i].set_names_size(joints.size());
+ req.positions[i].set_positions_size(joints.size());
+ req.positions[i].set_margins_size(joints.size());
+ req.positions[i].timeout = 10.0;
for (unsigned int j = 0 ; j < joints.size() ; ++j)
{
- req.path[i].names[j] = joints[j];
+ req.positions[i].names[j] = joints[j];
- state->copyParams(&(req.path[i].positions[j]), joints[j]);
+ state->copyParams(&(req.positions[i].positions[j]), joints[j]);
int pos = state->getPos(joints[j]);
assert(pos >= 0);
- req.path[i].margins[j] = margin_fraction * (m_kmodel->stateBounds[2 * pos + 1] - m_kmodel->stateBounds[2 * pos]);
+ req.positions[i].margins[j] = 0.25; // margin_fraction * (m_kmodel->stateBounds[2 * pos + 1] - m_kmodel->stateBounds[2 * pos]);
}
}
@@ -348,9 +355,9 @@
// perform the service call
- pr2_controllers::SetJointPathCmd::response res;
+ pr2_controllers::SetJointTarget::response res;
- if (ros::service::call("left_arm_controller/set_target", req, res))
+ if (ros::service::call("/left_arm_controller/set_target", req, res))
{
}
Modified: pkg/trunk/world_models/fake_world_3d_map/src/fake_world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/fake_world_3d_map/src/fake_world_3d_map.cpp 2008-09-11 07:19:55 UTC (rev 4196)
+++ pkg/trunk/world_models/fake_world_3d_map/src/fake_world_3d_map.cpp 2008-09-11 07:23:09 UTC (rev 4197)
@@ -108,6 +108,9 @@
case 1:
oneObstacle();
break;
+ case 2:
+ verticalObstacle();
+ break;
case 3:
planeX();
break;
@@ -130,6 +133,33 @@
m_toPublish.pts[0].z = 0.75;
}
+ void verticalObstacle(void)
+ {
+ const int N = 30;
+ const int M = 3;
+ const int L = 3;
+
+ m_toPublish.set_pts_size(N * M * L);
+
+ for (int k = 0 ; k < L ; ++k)
+ {
+ double x = 0.30 + 0.02 * k;
+
+ for (int j = 0 ; j < M ; ++j)
+ {
+ double y = -0.30 - 0.02 * j;
+
+ for (int i = 0 ; i < N ; ++i)
+ {
+ double z = 0.1 + i * 0.05;
+ m_toPublish.pts[i + j*N + k*N*M].x = x;
+ m_toPublish.pts[i + j*N + k*N*M].y = y;
+ m_toPublish.pts[i + j*N + k*N*M].z = z;
+ }
+ }
+ }
+ }
+
void planeX(void)
{
// for (int i = 0 ; i < 20 ; ++i)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-09-11 21:07:41
|
Revision: 4206
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4206&view=rev
Author: isucan
Date: 2008-09-11 21:07:50 +0000 (Thu, 11 Sep 2008)
Log Message:
-----------
update display paths so we can reset the state of the robot
Modified Paths:
--------------
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/robot_msgs/msg/DisplayKinematicPath.msg
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-09-11 20:56:51 UTC (rev 4205)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-09-11 21:07:50 UTC (rev 4206)
@@ -241,7 +241,7 @@
if (ros::service::call("plan_kinematic_path_position", req, res))
{
printPath(res.path, res.distance);
- sendDisplay(res.path, req.params.model_id);
+ sendDisplay(req.start_state, res.path, req.params.model_id);
verifyPath(req.start_state, req.constraints, res.path, req.params.model_id);
sendCommand(res.path, req.params.model_id);
}
@@ -256,7 +256,7 @@
if (ros::service::call("plan_kinematic_path_state", req, res))
{
printPath(res.path, res.distance);
- sendDisplay(res.path, req.params.model_id);
+ sendDisplay(req.start_state, res.path, req.params.model_id);
verifyPath(req.start_state, req.constraints, res.path, req.params.model_id);
sendCommand(res.path, req.params.model_id);
}
@@ -298,11 +298,12 @@
}
}
- void sendDisplay(robot_msgs::KinematicPath &path, const std::string &model)
+ void sendDisplay(robot_msgs::KinematicState &start, robot_msgs::KinematicPath &path, const std::string &model)
{
robot_msgs::DisplayKinematicPath dpath;
dpath.frame_id = "FRAMEID_MAP";
- dpath.name = model;
+ dpath.model_name = model;
+ dpath.start_state = start;
dpath.path = path;
publish("display_kinematic_path", dpath);
}
@@ -312,7 +313,7 @@
// create the service request
// return;
- const double margin_fraction = 0.1;
+ // const double margin_fraction = 0.1;
planning_models::KinematicModel::StateParams *state = m_kmodel->newStateParams();
pr2_controllers::SetJointTarget::request req;
Modified: pkg/trunk/robot_msgs/msg/DisplayKinematicPath.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/DisplayKinematicPath.msg 2008-09-11 20:56:51 UTC (rev 4205)
+++ pkg/trunk/robot_msgs/msg/DisplayKinematicPath.msg 2008-09-11 21:07:50 UTC (rev 4206)
@@ -1,5 +1,6 @@
# The definition of a kinematic path that has a name
# The name identifies the part of the robot the path is for
string frame_id
-string name
+string model_name
+KinematicState start_state
KinematicPath path
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-11 21:20:24
|
Revision: 4208
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4208&view=rev
Author: hsujohnhsu
Date: 2008-09-11 21:20:30 +0000 (Thu, 11 Sep 2008)
Log Message:
-----------
reverted broken checkin for laser scanner, renamed it in same directory.
moved set profile service to pr2_controllers.
reenabled laser scanner in gazebo.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_controllers/CMakeLists.txt
pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
Added Paths:
-----------
pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/laser_scanner_controller.h
pkg/trunk/controllers/pr2_controllers/src/laser_scanner_controller.cpp
pkg/trunk/controllers/pr2_controllers/srv/SetProfile.srv
Removed Paths:
-------------
pkg/trunk/controllers/generic_controllers/srv/SetProfile.srv
Deleted: pkg/trunk/controllers/generic_controllers/srv/SetProfile.srv
===================================================================
--- pkg/trunk/controllers/generic_controllers/srv/SetProfile.srv 2008-09-11 21:14:58 UTC (rev 4207)
+++ pkg/trunk/controllers/generic_controllers/srv/SetProfile.srv 2008-09-11 21:20:30 UTC (rev 4208)
@@ -1,7 +0,0 @@
-float64 UpperTurnaround
-float64 LowerTurnaround
-float64 upperDecelBuffer
-float64 lowerDecelBuffer
----
-float64 time
-
Modified: pkg/trunk/controllers/pr2_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_controllers/CMakeLists.txt 2008-09-11 21:14:58 UTC (rev 4207)
+++ pkg/trunk/controllers/pr2_controllers/CMakeLists.txt 2008-09-11 21:20:30 UTC (rev 4208)
@@ -3,9 +3,6 @@
rospack(pr2_controllers)
genmsg()
gensrv()
-rospack_add_library(pr2_controllers src/base_controller.cpp
-# src/laser_scanner_controller.cpp
- src/arm_position_controller.cpp
- src/arm_velocity_controller.cpp)
+rospack_add_library(pr2_controllers src/base_controller.cpp src/laser_scanner_controller.cpp src/arm_position_controller.cpp src/arm_velocity_controller.cpp)
rospack_add_executable(testBaseController test/test_base_controller.cpp)
target_link_libraries(testBaseController pr2_controllers)
Added: pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/laser_scanner_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/laser_scanner_controller.h (rev 0)
+++ pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/laser_scanner_controller.h 2008-09-11 21:20:30 UTC (rev 4208)
@@ -0,0 +1,240 @@
+/*********************************************************************
+ * 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.
+ *********************************************************************/
+
+#pragma once
+
+#include <ros/node.h>
+
+#include <generic_controllers/controller.h>
+#include <generic_controllers/joint_position_controller.h>
+
+// Services
+#include <generic_controllers/SetCommand.h>
+#include <generic_controllers/GetCommand.h>
+#include <pr2_controllers/SetProfile.h>
+
+namespace controller
+{
+
+class LaserScannerController : public Controller
+{
+public:
+
+ enum LaserControllerMode
+ {
+ MANUAL,SAWTOOTH,SINEWAVE,DYNAMIC_SAWTOOTH,DYNAMIC_SINEWAVE,AUTO_LEVEL
+ };
+
+ /*!
+ * \brief Default Constructor of the JointController class.
+ *
+ */
+ LaserScannerController();
+
+ /*!
+ * \brief Destructor of the JointController class.
+ */
+ ~LaserScannerController();
+
+ /*!
+ * \brief Functional way to initialize limits and gains.
+ *
+ */
+ void init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::RobotState *robot);
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+ /*!
+ * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
+ *
+ * \param double pos Position command to issue
+ */
+ void setCommand(double command);
+
+ /*!
+ * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
+ */
+ double getCommand();
+
+ /*!
+ * \brief Read the torque of the motor
+ */
+ double getMeasuredPosition();
+
+ /*!
+ * \brief Issues commands to the joint. Should be called at regular intervals
+ */
+
+ virtual void update();
+
+ /*!
+ * \brief Set automatic profile to sawtooth
+ *\param double period Period of signal
+ *\param double amplitude Peak to peak amplitude of signal
+ *\param int num_elements Number of points along one period of sawtooth wave
+ *\param double offset Offset of minimum point of signal to zero
+ *\param double current_time Used to determine start of cycle
+ */
+
+ void setSawtoothProfile(double period, double amplitude, int num_elements, double offset);
+
+ /*!
+ * \brief Set automatic profile to sawtooth, dynamically calculate desired position at each timestep
+ *\param double period Period of signal
+ *\param double amplitude Peak to peak amplitude of signal
+ *\param double offset Offset of minimum point of signal to zero
+ *\param double current_time Used to determine start of cycle
+ */
+ void setSawtoothProfile(double period, double amplitude, double offset);
+
+ /*!
+ * \brief Set automatic profile to sinewave
+ *\param double period Period of signal
+ *\param double amplitude Peak to peak amplitude of signal
+ *\param int num_elements Number of points along one period of sine wave
+ *\param double offset Offset of minimum point of signal to zero
+ *\param double current_time Used to determine start of cycle
+ */
+ void setSinewaveProfile(double period, double amplitude, int num_elements, double offset);
+
+ /*!
+ * \brief Set automatic profile to sinewave, dynamically calculate desired position at each timestep
+ *\param double period Period of signal
+ *\param double amplitude Peak to peak amplitude of signal
+ *\param double offset Offset of minimum point of signal to zero
+ *\param double current_time Used to determine start of cycle
+ */
+ void setSinewaveProfile(double period, double amplitude,double offset);
+
+ /*!
+ * \brief Starts the process of auto-leveling
+ */
+ void startAutoLevelSequence();
+
+ /*!
+ * \brief Returns a value indicating whether auto leveling has finished
+ */
+ bool checkAutoLevelStatus();
+
+ /*!
+ * \brief Returns whether auto level completed successfully
+ */
+ bool checkAutoLevelResult();
+
+ double getTime();
+private:
+ /*!
+ * \brief Actually issue torque set command of the joint motor.
+ */
+ void setJointEffort(double torque);
+
+ /*!
+ * \brief Get dynamically calculated sinewave position based on time
+ *\param double time_from_start Time elapsed since beginning of current period
+ */
+ void setDynamicSinewave(double time_from_start);
+
+ /*!
+ * \brief Get dynamically calculated sawtooth position based on time
+ *\param double time_from_start Time elapsed since beginning of current period
+ */
+ void setDynamicSawtooth(double time_from_start);
+
+
+ mechanism::JointState* joint_; /*!< Joint we're controlling>*/
+ JointPositionController joint_position_controller_; /*!< Internal PID controller>*/
+ double last_time_; /*!< Last time stamp of update> */
+ double command_; /*!< Last commanded position> */
+ mechanism::RobotState *robot_; /*!< Pointer to robot structure>*/
+ double* profile_locations_; /**<Contains locations for profile>*/
+ double* profile_dt_; /**<Contains timesteps for profile locations>*/
+ int profile_index_; /**<Track location in profile>*/
+ int profile_length_; /**<Number of points in one cycle>*/
+ double cycle_start_time_; //**<Start of the last cycle for profile>*/
+
+ double time_of_last_point_;/*!<Time of last setpoint>*/
+ double period_;/*!<Period for use in dynamic profile calculation>*/
+ double amplitude_;/*!<Amplitude for use in dynamic profile calculation>*/
+ double offset_;/*!<Offset for use in dynamic profile calculation>*/
+
+
+ LaserControllerMode current_mode_; /*!<Indicates the current status of the controller>*/
+ bool auto_level_result_; /*!<Indicates whether the auto_level_routine finished correct>*/
+
+};
+
+class LaserScannerControllerNode : public Controller
+{
+public:
+ /*!
+ * \brief Default Constructor
+ *
+ */
+ LaserScannerControllerNode();
+
+ /*!
+ * \brief Destructor
+ */
+ ~LaserScannerControllerNode();
+
+ double getMeasuredPosition();
+
+ void update();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+ // Services
+ bool setCommand(generic_controllers::SetCommand::request &req,
+ generic_controllers::SetCommand::response &resp);
+
+ bool getCommand(generic_controllers::GetCommand::request &req,
+ generic_controllers::GetCommand::response &resp);
+
+ bool getActual(generic_controllers::GetActual::request &req,
+ generic_controllers::GetActual::response &resp);
+
+ bool setProfileCall(pr2_controllers::SetProfile::request &req,
+ pr2_controllers::SetProfile::response &resp);
+
+ void setCommand(double command);
+
+ void setProfile(LaserScannerController::LaserControllerMode profile, double period, double amplitude, int num_elements=0, double offset=0.0);
+
+ double getCommand();
+
+private:
+ LaserScannerController *c_;
+};
+}
+
+
Added: pkg/trunk/controllers/pr2_controllers/src/laser_scanner_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/src/laser_scanner_controller.cpp (rev 0)
+++ pkg/trunk/controllers/pr2_controllers/src/laser_scanner_controller.cpp 2008-09-11 21:20:30 UTC (rev 4208)
@@ -0,0 +1,513 @@
+/*********************************************************************
+ * 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 <algorithm>
+
+#include <pr2_controllers/laser_scanner_controller.h>
+#include <math_utils/angles.h>
+
+using namespace std;
+using namespace controller;
+
+ROS_REGISTER_CONTROLLER(LaserScannerController)
+
+LaserScannerController::LaserScannerController()
+{
+ robot_ = NULL;
+ joint_ = NULL;
+
+ command_ = 0;
+ last_time_ = 0;
+ profile_index_ = 0;
+ profile_length_ = 0;
+
+ //Clear arrays
+ profile_locations_ = NULL;
+ profile_dt_ = NULL;
+ current_mode_ = MANUAL;
+}
+
+LaserScannerController::~LaserScannerController()
+{
+ //Free memory in profile if needed
+ if(profile_locations_!=NULL) delete[] profile_locations_;
+ if(profile_dt_!=NULL) delete[] profile_dt_;
+
+}
+
+void LaserScannerController::init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::RobotState *robot)
+{
+ robot_ = robot;
+ joint_ = robot->getJointState(name);
+
+ // TODO
+ abort(); //joint_position_controller_.init( p_gain, i_gain, d_gain, windup, time, name, robot);
+ command_= 0;
+ last_time_= time;
+}
+
+bool LaserScannerController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ assert(robot);
+ robot_ = robot;
+ last_time_ = robot->hw_->current_time_;
+
+ //Perform checks at highest level to give the most informative error message possible
+ TiXmlElement *j = config->FirstChildElement("joint");
+ if (!j)
+ {
+ fprintf(stderr, "LaserScannerController was not given a joint\n");
+ return false;
+ }
+
+ const char *joint_name = j->Attribute("name");
+ joint_ = joint_name ? robot->getJointState(joint_name) : NULL;
+ if (!joint_)
+ {
+ fprintf(stderr, "LaserScannerController could not find joint named \"%s\"\n", joint_name);
+ return false;
+ }
+/*
+ TiXmlElement *p = j->FirstChildElement("pid");
+ if (!p)
+ fprintf(stderr, "LaserScannerController's config did not specify the default pid parameters.\n");
+*/
+ joint_position_controller_.initXml(robot,config); //Pass down XML snippet to encapsulated joint_position_controller_
+ return true;
+
+
+ }
+
+// Set the joint position command
+void LaserScannerController::setCommand(double command)
+{
+ command_ = command;
+ current_mode_ = MANUAL;
+}
+
+// Return the current position command
+double LaserScannerController::getCommand()
+{
+ return command_;
+}
+
+// Return the measured joint position
+double LaserScannerController::getMeasuredPosition()
+{
+ return joint_->position_;
+}
+
+double LaserScannerController::getTime()
+{
+ return robot_->hw_->current_time_;
+}
+
+void LaserScannerController::update()
+{
+ double time = robot_->hw_->current_time_;
+
+ switch(current_mode_)
+ {
+ case MANUAL:
+ joint_position_controller_.setCommand(command_);
+ break;
+ case SAWTOOTH:
+ case SINEWAVE:
+ joint_position_controller_.setCommand(profile_locations_[profile_index_]); //Issue position command
+
+ //Check if enough time has elapsed to move to next set point
+ if(time-time_of_last_point_ >= profile_dt_[profile_index_])
+ {
+ #ifdef DEBUG
+ printf("DLL : %f %f\n",profile_locations_[profile_index_],time- time_of_last_point_);
+ #endif
+ time_of_last_point_ = time;
+
+ //Advance time index
+ if(profile_index_ == (profile_length_-1))
+ {
+ profile_index_ = 0; //Restart profile
+ cycle_start_time_ = time;
+ } else profile_index_++;
+ }
+ break;
+ case DYNAMIC_SAWTOOTH:
+ //Advance to next period
+ if(time-cycle_start_time_>period_) cycle_start_time_ = time;
+
+ //Issue command based on time from start of cycle
+ setDynamicSawtooth(time-cycle_start_time_);
+ break;
+ case DYNAMIC_SINEWAVE:
+ //Advance to next period
+ if(time-cycle_start_time_>period_) cycle_start_time_ = time;
+
+ //Issue command based on time from start of cycle
+ setDynamicSinewave(time-cycle_start_time_);
+ break;
+ case AUTO_LEVEL:
+ break;
+ default:
+ break;
+ }
+ joint_position_controller_.update(); //Update lower controller
+
+ last_time_ = time; //Keep track of last time for update
+
+}
+
+//Set mode to use sawtooth profile
+void LaserScannerController::setSawtoothProfile(double period, double amplitude, int num_elements, double offset)
+{
+ int smaller_num_elements = num_elements/4; //Number of elements in a single quadrant
+ int total_elements = smaller_num_elements*4; //track actual number of elements after int truncation
+ double delta = amplitude/smaller_num_elements; //Scale first, then determine step size
+ double dt = period/total_elements;
+ double current = 0;
+ double newvalue = 0;
+
+ //Error checking
+ if(total_elements<=0) return;
+
+ //Clear arrays
+ if(profile_locations_ !=NULL) delete[] profile_locations_;
+ if(profile_dt_ !=NULL) delete[] profile_dt_;
+
+ profile_locations_ = new double[total_elements];
+ profile_dt_ = new double[total_elements];
+
+ //Construct evenly spaced elements in distance along sine wave
+ for(int i = 0;i<total_elements;i++)
+ {
+ profile_locations_[i] = current + offset; //set current point
+ profile_dt_[i] = dt; //Constant dt because of linear relationship
+
+ newvalue = current + delta; //Calculate next value
+ if(i == smaller_num_elements) //Shift from quadrant 1 to 2
+ {
+ delta = -delta;
+ newvalue = current + delta;
+ }
+ else if (i == smaller_num_elements*3)//Shift from quadrant 3-4
+ {
+ delta = -delta;
+ newvalue = current + delta;
+ }
+ current = newvalue;
+ }
+
+ //Reset profile settings
+ profile_length_ = total_elements; //Keep track of profile length
+ profile_index_= 0; //Start at beginning
+ time_of_last_point_ = robot_->hw_->current_time_;
+
+ current_mode_ = SAWTOOTH;
+}
+
+//Set mode to use sawtooth profile
+void LaserScannerController::setSawtoothProfile(double period, double amplitude, double offset)
+{
+ period_ = period;
+ amplitude_ = amplitude;
+ offset_ = offset;
+
+ //Reset profile settings
+ profile_length_ = 0;
+ profile_index_= 0;
+ cycle_start_time_ = robot_->hw_->current_time_;
+
+ current_mode_ = DYNAMIC_SAWTOOTH;
+}
+
+//Set mode to use Sinewave profile
+void LaserScannerController::setSinewaveProfile(double period, double amplitude, int num_elements, double offset)
+{
+ int smaller_num_elements = num_elements/4; //Number of elements in a single quadrant
+ int total_elements = smaller_num_elements*4; //track actual number of elements after int truncation
+ double delta = 1.0/smaller_num_elements;
+ double current = 0;
+ double newvalue = 0;
+ double temp_value = 0.0;
+ double last_temp_value = 0.0;
+
+ //Error checking
+ if(total_elements<=0) return;
+
+ //Clear arrays
+ if(profile_locations_ !=NULL) delete[] profile_locations_;
+ if(profile_dt_ !=NULL) delete[] profile_dt_;
+
+ profile_locations_ = new double[total_elements];
+ profile_dt_ = new double[total_elements];
+
+ //Construct evenly spaced elements in distance along sine wave
+ for(int i = 0;i<total_elements;i++)
+ {
+ profile_locations_[i] = current; //set current point
+ newvalue = current + delta; //Calculate next value
+ if(i == smaller_num_elements) //Shift from quadrant 1 to 2
+ {
+ delta = -delta;
+ newvalue = current + delta;
+ }
+ else if (i == smaller_num_elements*3)//Shift from quadrant 3-4
+ {
+ delta = -delta;
+ newvalue = current + delta;
+ }
+
+ current = newvalue;
+ current = min(max(current, -1.0), 1.0); //Make sure asin doesn't fail
+ }
+
+ profile_locations_[0] = offset; //set first value
+
+ //At time 0, we wish for our location to be at offset. Start indexing at 1, but associate dt with previous value
+ for(int i = 1;i<total_elements;i++)
+ {
+ temp_value = asin(profile_locations_[i]); //Calculate time
+ profile_dt_[i-1] = fabs(temp_value-last_temp_value)*period/(2*M_PI); //Calculate dt, scale by period
+ profile_locations_[i] = temp_value*amplitude + offset; //Scale goal location by amplitude
+ last_temp_value = temp_value;
+
+ #ifdef DEBUG
+ printf("*** test %u %f %f\n",i,profile_dt_[i-1],profile_locations_[i]);
+ #endif
+ }
+
+ profile_dt_[total_elements-1] = profile_dt_[0]; //Make symmetric
+
+ #ifdef DEBUG
+ for(int i = 0;i<total_elements;i++)
+ {
+ printf("** test %u %f %f\n",i,profile_dt_[i],profile_locations_[i]);
+ }
+ #endif
+
+ //Reset profile settings
+ profile_length_ = total_elements; //Keep track of profile length
+ profile_index_= 0; //Start at beginning
+ time_of_last_point_ = robot_->hw_->current_time_;
+ cycle_start_time_ = robot_->hw_->current_time_;
+
+ current_mode_ = SINEWAVE;
+}
+
+//Set mode to use Sinewave profile
+void LaserScannerController::setSinewaveProfile(double period, double amplitude,double offset)
+{
+ //Reset profile settings
+ profile_length_ = 0;
+ profile_index_= 0;
+ cycle_start_time_ = robot_->hw_->current_time_;
+ current_mode_ = DYNAMIC_SINEWAVE;
+
+ period_ = period;
+ amplitude_ = amplitude;
+ offset_ = offset;
+}
+
+void LaserScannerController::startAutoLevelSequence()
+{
+ current_mode_=AUTO_LEVEL;
+}
+
+bool LaserScannerController::checkAutoLevelStatus()
+{
+ return (current_mode_==AUTO_LEVEL);
+}
+
+bool LaserScannerController::checkAutoLevelResult()
+{
+ return auto_level_result_;
+}
+
+void LaserScannerController::setJointEffort(double effort)
+{
+ joint_->commanded_effort_ = effort;
+}
+
+//Get sinewave based on current time
+void LaserScannerController::setDynamicSinewave(double time_from_start)
+{
+ double command = sin(2*M_PI*time_from_start/period_)*amplitude_+offset_;
+ joint_position_controller_.setCommand(command);
+
+}
+
+//Set mode to use sawtooth profile
+void LaserScannerController::setDynamicSawtooth(double time_from_start)
+{
+ double time_from_peak = fmod(time_from_start,(period_/4));
+ double command = (time_from_peak)/(period_/4)*amplitude_;
+
+ if(time_from_start<period_/4.0) //Quadrant I
+ {
+ command = command + offset_;
+ }
+ else if (time_from_start>period_/4.0 && time_from_start<period_/2.0) //Quadrant II
+ {
+ command = amplitude_-command + offset_;
+ }
+ else if (time_from_start>period_/2.0 && time_from_start<period_*3/4.0) //Quadrant III
+ {
+ command = -command + offset_;
+ }
+ else if (time_from_start>period_*3/4.0 && time_from_start<period_) //Quadrant IV
+ {
+ command = -amplitude_ + command + offset_;
+ }
+
+ joint_position_controller_.setCommand(command);
+}
+
+ROS_REGISTER_CONTROLLER(LaserScannerControllerNode)
+LaserScannerControllerNode::LaserScannerControllerNode()
+{
+ c_ = new LaserScannerController();
+}
+
+
+LaserScannerControllerNode::~LaserScannerControllerNode()
+{
+ delete c_;
+}
+
+void LaserScannerControllerNode::update()
+{
+ c_->update();
+}
+
+// Return the measured joint position
+double LaserScannerControllerNode::getMeasuredPosition()
+{
+ return c_->getMeasuredPosition();
+}
+
+bool LaserScannerControllerNode::setCommand(
+ generic_controllers::SetCommand::request &req,
+ generic_controllers::SetCommand::response &resp)
+{
+ c_->setCommand(req.command);
+ resp.command = c_->getCommand();
+
+ //FIXME: Backdoor method to issue command set
+ if(req.command==41)c_->setSawtoothProfile(1,0.5,100,0);
+ else if(req.command==42)c_->setSawtoothProfile(2,0.5,100,0);
+ else if(req.command==43)c_->setSawtoothProfile(2,0.5,100,0.5);
+ else if(req.command==44)c_->setSawtoothProfile(0.5,0.5,100,0);
+ else if(req.command==45)c_->setSawtoothProfile(1,0.5,0);
+ else if(req.command==-41)c_->setSinewaveProfile(2,0.5,100,0.5);
+ else if(req.command==-42)c_->setSinewaveProfile(2,0.5,100,0);
+ else if(req.command==-43)c_->setSinewaveProfile(1,0.5,100,0);
+ else if(req.command==-44)c_->setSinewaveProfile(4,0.5,100,0);
+ else if (req.command==-45)c_->setSinewaveProfile(1,0.5,0);
+ else if (req.command==-46)c_->setSinewaveProfile(3,0.5,0);
+ else if (req.command==-47)c_->setSinewaveProfile(20,0.7,0);
+ return true;
+}
+
+bool LaserScannerControllerNode::getCommand(
+ generic_controllers::GetCommand::request &req,
+ generic_controllers::GetCommand::response &resp)
+{
+ resp.command = c_->getCommand();
+
+ return true;
+}
+
+bool LaserScannerControllerNode::setProfileCall(
+ pr2_controllers::SetProfile::request &req,
+ pr2_controllers::SetProfile::response &resp)
+{
+ setProfile(LaserScannerController::LaserControllerMode(req.profile),req.period,req.amplitude,req.offset);
+ resp.time = c_->getTime();
+ return true;
+}
+
+bool LaserScannerControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ ros::node *node = ros::node::instance();
+ string prefix = config->Attribute("name");
+
+ if (!c_->initXml(robot, config))
+ return false;
+ node->advertise_service(prefix + "/set_command", &LaserScannerControllerNode::setCommand, this);
+ node->advertise_service(prefix + "/get_command", &LaserScannerControllerNode::getCommand, this);
+ node->advertise_service(prefix + "/set_profile", &LaserScannerControllerNode::setProfileCall, this);
+ return true;
+}
+bool LaserScannerControllerNode::getActual(
+ generic_controllers::GetActual::request &req,
+ generic_controllers::GetActual::response &resp)
+{
+ resp.command = c_->getMeasuredPosition();
+ resp.time = c_->getTime();
+ return true;
+}
+
+void LaserScannerControllerNode::setCommand(double command)
+{
+ c_->setCommand(command);
+}
+
+void LaserScannerControllerNode::setProfile(LaserScannerController::LaserControllerMode profile, double period, double amplitude, int num_elements, double offset)
+{
+ //Instruct the controller to perform an automatic profile
+ switch(profile)
+ {
+ case LaserScannerController::SINEWAVE:
+ c_->setSinewaveProfile(period, amplitude, num_elements, offset);
+ break;
+ case LaserScannerController::DYNAMIC_SINEWAVE:
+ c_->setSinewaveProfile(period, amplitude,offset);
+ break;
+ case LaserScannerController::SAWTOOTH:
+ c_->setSawtoothProfile(period, amplitude, num_elements, offset);
+ break;
+ case LaserScannerController::DYNAMIC_SAWTOOTH:
+ c_->setSawtoothProfile(period, amplitude,offset);
+ break;
+ default:
+ break;
+ }
+}
+
+// Return the current position command
+double LaserScannerControllerNode::getCommand()
+{
+ return c_->getCommand();
+}
+
+
+
Copied: pkg/trunk/controllers/pr2_controllers/srv/SetProfile.srv (from rev 4203, pkg/trunk/controllers/generic_controllers/srv/SetProfile.srv)
===================================================================
--- pkg/trunk/controllers/pr2_controllers/srv/SetProfile.srv (rev 0)
+++ pkg/trunk/controllers/pr2_controllers/srv/SetProfile.srv 2008-09-11 21:20:30 UTC (rev 4208)
@@ -0,0 +1,11 @@
+float64 UpperTurnaround
+float64 LowerTurnaround
+float64 upperDecelBuffer
+float64 lowerDecelBuffer
+float64 profile
+float64 period
+float64 amplitude
+float64 offset
+---
+float64 time
+
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-09-11 21:14:58 UTC (rev 4207)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-09-11 21:20:30 UTC (rev 4208)
@@ -125,7 +125,7 @@
std::cout << " initializing tile laser scanner\n" << std::endl;
//Typical scan of 100 degrees yields the following amplitudes
//tlcn->setProfile(controller::LaserScannerController::SINEWAVE, 20, 0.872, 100, 0.3475);
- //tlcn->setProfile(controller::LaserScannerController::SAWTOOTH, 20, 0.872, 100, 0.3475); //FIXME: sawtooth disappeared in r4180!!
+ tlcn->setProfile(controller::LaserScannerController::SAWTOOTH, 20, 0.872, 100, 0.3475); //FIXME: sawtooth disappeared in r4180, I reverted it for now.
}
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-09-11 21:14:58 UTC (rev 4207)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-09-11 21:20:30 UTC (rev 4208)
@@ -192,7 +192,13 @@
</joint>
</controller>
- <!-- laser scanner controller seems to be broken
+ <controller name="tilt_laser_controller" topic="tilt_laser_controller" type="LaserScannerControllerNode">
+ <joint name="tilt_laser_joint" >
+ <pid p="0.4" d="0" i="0" iClamp="0.1" />
+ </joint>
+ </controller>
+
+ <!-- this version of laser scanner controller seems to be broken
<controller name="tilt_laser_controller" topic="laser_test" type="LaserScannerControllerNode">
<velocity>
<velocityFilter smoothing="0.2"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-09-11 22:25:26
|
Revision: 4213
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4213&view=rev
Author: isucan
Date: 2008-09-11 22:25:36 +0000 (Thu, 11 Sep 2008)
Log Message:
-----------
deprecating some packages
Added Paths:
-----------
pkg/trunk/deprecated/display_ode_spaces/
pkg/trunk/deprecated/test_collision_space/
Removed Paths:
-------------
pkg/trunk/motion_planning/test_collision_space/
pkg/trunk/util/display_ode_spaces/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-12 00:08:58
|
Revision: 4226
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4226&view=rev
Author: hsujohnhsu
Date: 2008-09-12 00:09:08 +0000 (Fri, 12 Sep 2008)
Log Message:
-----------
updates to gazebo revision 7019, with contact sensor template.
updates to pr2.xml comment for contact sensor, not yet functional.
timing outputs for gazebo. uncomment #define TIMING in Global.hh patch.
Revision Links:
--------------
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7019&view=rev
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/Makefile
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
Added Paths:
-----------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Bumper.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Bumper.cc
Modified: pkg/trunk/3rdparty/gazebo/Makefile
===================================================================
--- pkg/trunk/3rdparty/gazebo/Makefile 2008-09-11 23:40:05 UTC (rev 4225)
+++ pkg/trunk/3rdparty/gazebo/Makefile 2008-09-12 00:09:08 UTC (rev 4226)
@@ -2,7 +2,7 @@
SVN_DIR = gazebo-svn
SVN_URL = https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/gazebo/trunk
-SVN_REVISION = -r 7016
+SVN_REVISION = -r 7019
SVN_PATCH = gazebo_patch.diff
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-09-11 23:40:05 UTC (rev 4225)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-09-12 00:09:08 UTC (rev 4226)
@@ -1,6 +1,6 @@
Index: player/SConscript
===================================================================
---- player/SConscript (revision 7016)
+--- player/SConscript (revision 7019)
+++ player/SConscript (working copy)
@@ -1,7 +1,8 @@
import os
@@ -14,7 +14,7 @@
'GazeboClient.cc',
Index: libgazebo/gazebo.h
===================================================================
---- libgazebo/gazebo.h (revision 7016)
+--- libgazebo/gazebo.h (revision 7019)
+++ libgazebo/gazebo.h (working copy)
@@ -556,7 +556,7 @@
@@ -183,7 +183,7 @@
};
Index: server/physics/SphereGeom.cc
===================================================================
---- server/physics/SphereGeom.cc (revision 7016)
+--- server/physics/SphereGeom.cc (revision 7019)
+++ server/physics/SphereGeom.cc (working copy)
@@ -66,11 +66,18 @@
this->radiusP->SetValue( radius );
@@ -209,7 +209,7 @@
////////////////////////////////////////////////////////////////////////////////
Index: server/physics/BoxGeom.cc
===================================================================
---- server/physics/BoxGeom.cc (revision 7016)
+--- server/physics/BoxGeom.cc (revision 7019)
+++ server/physics/BoxGeom.cc (working copy)
@@ -66,9 +66,18 @@
this->sizeP->SetValue( size );
@@ -235,9 +235,9 @@
// Create a box geometry with box mass matrix
Index: server/physics/Geom.hh
===================================================================
---- server/physics/Geom.hh (revision 7016)
+--- server/physics/Geom.hh (revision 7019)
+++ server/physics/Geom.hh (working copy)
-@@ -171,6 +171,20 @@
+@@ -180,6 +180,20 @@
/// Mass as a double
protected: ParamT<double> *massP;
@@ -260,7 +260,7 @@
Index: server/physics/Body.hh
===================================================================
---- server/physics/Body.hh (revision 7016)
+--- server/physics/Body.hh (revision 7019)
+++ server/physics/Body.hh (working copy)
@@ -180,6 +180,7 @@
@@ -272,7 +272,7 @@
/// \}
Index: server/physics/HingeJoint.hh
===================================================================
---- server/physics/HingeJoint.hh (revision 7016)
+--- server/physics/HingeJoint.hh (revision 7019)
+++ server/physics/HingeJoint.hh (working copy)
@@ -126,6 +126,7 @@
private: ParamT<Vector3> *axisP;
@@ -284,7 +284,7 @@
Index: server/physics/CylinderGeom.cc
===================================================================
---- server/physics/CylinderGeom.cc (revision 7016)
+--- server/physics/CylinderGeom.cc (revision 7019)
+++ server/physics/CylinderGeom.cc (working copy)
@@ -64,11 +64,21 @@
this->sizeP->SetValue( size );
@@ -312,7 +312,7 @@
//////////////////////////////////////////////////////////////////////////////
Index: server/physics/Geom.cc
===================================================================
---- server/physics/Geom.cc (revision 7016)
+--- server/physics/Geom.cc (revision 7019)
+++ server/physics/Geom.cc (working copy)
@@ -73,6 +73,17 @@
@@ -406,7 +406,7 @@
dMassRotate(&this->bodyMass, r);
Index: server/physics/Body.cc
===================================================================
---- server/physics/Body.cc (revision 7016)
+--- server/physics/Body.cc (revision 7019)
+++ server/physics/Body.cc (working copy)
@@ -70,6 +70,7 @@
@@ -437,7 +437,7 @@
Index: server/physics/HingeJoint.cc
===================================================================
---- server/physics/HingeJoint.cc (revision 7016)
+--- server/physics/HingeJoint.cc (revision 7019)
+++ server/physics/HingeJoint.cc (working copy)
@@ -44,6 +44,7 @@
this->axisP = new ParamT<Vector3>("axis",Vector3(0,1,0), 1);
@@ -466,7 +466,7 @@
Index: server/physics/ode/ODEPhysics.hh
===================================================================
---- server/physics/ode/ODEPhysics.hh (revision 7016)
+--- server/physics/ode/ODEPhysics.hh (revision 7019)
+++ server/physics/ode/ODEPhysics.hh (working copy)
@@ -133,6 +133,7 @@
@@ -478,9 +478,20 @@
/** \}*/
Index: server/physics/ode/ODEPhysics.cc
===================================================================
---- server/physics/ode/ODEPhysics.cc (revision 7016)
+--- server/physics/ode/ODEPhysics.cc (revision 7019)
+++ server/physics/ode/ODEPhysics.cc (working copy)
-@@ -70,6 +70,7 @@
+@@ -44,6 +44,10 @@
+ #include "XMLConfig.hh"
+ #include "ODEPhysics.hh"
+
++#ifdef TIMING
++#include "Simulator.hh"// for timing
++#endif
++
+ using namespace gazebo;
+
+ ////////////////////////////////////////////////////////////////////////////////
+@@ -70,6 +74,7 @@
Param::Begin(&this->parameters);
this->globalCFMP = new ParamT<double>("cfm", 10e-5, 0);
this->globalERPP = new ParamT<double>("erp", 0.2, 0);
@@ -488,7 +499,7 @@
Param::End();
}
-@@ -88,6 +89,7 @@
+@@ -88,6 +93,7 @@
delete this->globalCFMP;
delete this->globalERPP;
@@ -496,7 +507,7 @@
}
////////////////////////////////////////////////////////////////////////////////
-@@ -103,6 +105,7 @@
+@@ -103,6 +109,7 @@
this->updateRateP->Load(cnode);
this->globalCFMP->Load(cnode);
this->globalERPP->Load(cnode);
@@ -504,7 +515,7 @@
}
////////////////////////////////////////////////////////////////////////////////
-@@ -115,6 +118,7 @@
+@@ -115,6 +122,7 @@
stream << prefix << " " << *(this->updateRateP) << "\n";
stream << prefix << " " << *(this->globalCFMP) << "\n";
stream << prefix << " " << *(this->globalERPP) << "\n";
@@ -512,9 +523,22 @@
stream << prefix << "</physics:ode>\n";
}
-@@ -137,8 +141,10 @@
+@@ -133,13 +141,29 @@
+ // Update the ODE engine
+ void ODEPhysics::Update()
+ {
++#ifdef TIMING
++ double tmpT1 = Simulator::Instance()->GetWallTime();
++#endif
++
+ // Do collision detection; this will add contacts to the contact group
dSpaceCollide( this->spaceId, this, CollisionCallback );
++#ifdef TIMING
++ double tmpT2 = Simulator::Instance()->GetWallTime();
++ std::cout << " collision dt (" << tmpT2-tmpT1 << ")" << std::endl;
++#endif
++
// Update the dynamical model
- dWorldStep( this->worldId, this->stepTimeP->GetValue() );
- //dWorldQuickStep(this->worldId, this->stepTime);
@@ -523,9 +547,15 @@
+ else
+ dWorldStep( this->worldId, this->stepTimeP->GetValue() );
++#ifdef TIMING
++ double tmpT3 = Simulator::Instance()->GetWallTime();
++ std::cout << " ode step dt (" << tmpT3-tmpT2 << ")" << std::endl;
++#endif
++
// Very important to clear out the contact group
dJointGroupEmpty( this->contactGroup );
-@@ -266,15 +272,16 @@
+
+@@ -266,15 +290,16 @@
contact.surface.mode = dContactSlip1 | dContactSlip2 |
dContactSoftERP | dContactSoftCFM |
dContactBounce | dContactMu2 | dContactApprox1;
@@ -546,7 +576,7 @@
contact.surface.mu2 = MIN(geom1->contact->mu2, geom2->contact->mu2);
Index: server/physics/TrimeshGeom.cc
===================================================================
---- server/physics/TrimeshGeom.cc (revision 7016)
+--- server/physics/TrimeshGeom.cc (revision 7019)
+++ server/physics/TrimeshGeom.cc (working copy)
@@ -208,7 +208,13 @@
@@ -565,7 +595,7 @@
this->SetGeom(this->geomId, true);
Index: server/sensors/Sensor.hh
===================================================================
---- server/sensors/Sensor.hh (revision 7016)
+--- server/sensors/Sensor.hh (revision 7019)
+++ server/sensors/Sensor.hh (working copy)
@@ -70,6 +70,7 @@
@@ -585,7 +615,7 @@
#endif
Index: server/sensors/ray/RaySensor.cc
===================================================================
---- server/sensors/ray/RaySensor.cc (revision 7016)
+--- server/sensors/ray/RaySensor.cc (revision 7019)
+++ server/sensors/ray/RaySensor.cc (working copy)
@@ -273,7 +273,7 @@
// Update the sensor information
@@ -598,7 +628,7 @@
Pose3d poseDelta;
Index: server/sensors/Sensor.cc
===================================================================
---- server/sensors/Sensor.cc (revision 7016)
+--- server/sensors/Sensor.cc (revision 7019)
+++ server/sensors/Sensor.cc (working copy)
@@ -33,6 +33,7 @@
#include "ControllerFactory.hh"
@@ -635,9 +665,21 @@
+}
+
+Index: server/Global.hh
+===================================================================
+--- server/Global.hh (revision 7019)
++++ server/Global.hh (working copy)
+@@ -88,4 +88,7 @@
+
+ #define GZ_DELETE(p) { if(p) { delete (p); (p)=NULL; } }
+
++// Timing Debug
++//#define TIMING
++
+ #endif
Index: server/Simulator.cc
===================================================================
---- server/Simulator.cc (revision 7016)
+--- server/Simulator.cc (revision 7019)
+++ server/Simulator.cc (working copy)
@@ -72,6 +72,7 @@
timeout(-1),
@@ -647,29 +689,44 @@
}
////////////////////////////////////////////////////////////////////////////////
-@@ -291,6 +292,9 @@
+@@ -291,6 +292,11 @@
{
currTime = this->GetRealTime();
-+ //double tmpT1 = this->GetWallTime();
-+ //fprintf(stderr, " before World::Instance()->Update() sim: %.5f t %.5f",this->simTime , tmpT1);
++#ifdef TIMING
++ double tmpT1 = this->GetWallTime();
++ std::cout << "CURRENT simTime(" << this->simTime << ") current world time (" << tmpT1 << ")" << std::endl;
++#endif
+
if (physicsUpdateRate == 0 ||
currTime - this->prevPhysicsTime >= physicsUpdatePeriod)
{
-@@ -315,6 +319,9 @@
+@@ -315,6 +321,11 @@
World::Instance()->Update();
}
-+ //double tmpT2 = this->GetWallTime();
-+ //fprintf(stderr, " world dt %.5f",tmpT2-tmpT1);
++#ifdef TIMING
++ double tmpT2 = this->GetWallTime();
++ std::cout << " World::Instance() TOTAL DT(" << tmpT2-tmpT1 << ")" << std::endl;
++#endif
+
// Update the rendering
if (renderUpdateRate == 0 ||
currTime - this->prevRenderTime >= renderUpdatePeriod)
+@@ -327,6 +338,10 @@
+ if (this->gui)
+ {
+ this->gui->Update();
++#ifdef TIMING
++ double tmpT3 = this->GetWallTime();
++ std::cout << " GUI dt(" << tmpT3-tmpT2 << ")" << std::endl;
++#endif
+ }
+
+ elapsedTime = (this->GetRealTime() - currTime);
Index: server/XMLConfig.cc
===================================================================
---- server/XMLConfig.cc (revision 7016)
+--- server/XMLConfig.cc (revision 7019)
+++ server/XMLConfig.cc (working copy)
@@ -513,29 +513,59 @@
///////////////////////////////////////////////////////////////////////////
@@ -747,7 +804,7 @@
Index: server/GazeboConfig.cc
===================================================================
---- server/GazeboConfig.cc (revision 7016)
+--- server/GazeboConfig.cc (revision 7019)
+++ server/GazeboConfig.cc (working copy)
@@ -67,31 +67,34 @@
this->gazeboPaths.push_back(gazebo_resource_path);
@@ -821,9 +878,80 @@
this->RTTMode="PBuffer";
}
}
+Index: server/Model.cc
+===================================================================
+--- server/Model.cc (revision 7019)
++++ server/Model.cc (working copy)
+@@ -47,6 +47,10 @@
+ #include "IfaceFactory.hh"
+ #include "Model.hh"
+
++#ifdef TIMING
++#include "Simulator.hh"// for timing
++#endif
++
+ using namespace gazebo;
+
+ uint Model::lightNumber = 0;
+@@ -305,7 +309,7 @@
+
+ return this->InitChild();
+ }
+-
++
+ ////////////////////////////////////////////////////////////////////////////////
+ // Update the model
+ int Model::Update()
+@@ -316,6 +320,10 @@
+
+ Pose3d bodyPose, newPose, oldPose;
+
++#ifdef TIMING
++ double tmpT1 = Simulator::Instance()->GetWallTime();
++#endif
++
+ for (bodyIter=this->bodies.begin(); bodyIter!=this->bodies.end(); bodyIter++)
+ {
+ if (bodyIter->second)
+@@ -324,6 +332,11 @@
+ }
+ }
+
++#ifdef TIMING
++ double tmpT2 = Simulator::Instance()->GetWallTime();
++ std::cout << " bodies dt (" << tmpT2-tmpT1 << ")";
++#endif
++
+ for (contIter=this->controllers.begin();
+ contIter!=this->controllers.end(); contIter++)
+ {
+@@ -332,6 +345,11 @@
+ contIter->second->Update();
+ }
+
++#ifdef TIMING
++ double tmpT3 = Simulator::Instance()->GetWallTime();
++ std::cout << " controllers dt (" << tmpT3-tmpT2 << ")";
++#endif
++
+ for (jointIter = this->joints.begin(); jointIter != this->joints.end(); jointIter++)
+ {
+ jointIter->second->Update();
+@@ -350,6 +368,11 @@
+ this->rpyP->SetValue(this->pose.rot);
+ }
+
++#ifdef TIMING
++ double tmpT4 = Simulator::Instance()->GetWallTime();
++ std::cout << " joints/canonical body dt (" << tmpT4-tmpT3 << ")" << std::endl;
++#endif
++
+ return this->UpdateChild();
+ }
+
Index: server/gui/StatusBar.cc
===================================================================
---- server/gui/StatusBar.cc (revision 7016)
+--- server/gui/StatusBar.cc (revision 7019)
+++ server/gui/StatusBar.cc (working copy)
@@ -25,6 +25,7 @@
*/
@@ -835,7 +963,7 @@
#include <FL/Fl_Button.H>
Index: server/gui/GLWindow.cc
===================================================================
---- server/gui/GLWindow.cc (revision 7016)
+--- server/gui/GLWindow.cc (revision 7019)
+++ server/gui/GLWindow.cc (working copy)
@@ -227,21 +227,10 @@
}
@@ -865,7 +993,7 @@
{
Index: server/World.hh
===================================================================
---- server/World.hh (revision 7016)
+--- server/World.hh (revision 7019)
+++ server/World.hh (working copy)
@@ -92,6 +92,26 @@
/// \return Pointer to the physics engine
@@ -906,7 +1034,7 @@
};
Index: server/controllers/Controller.hh
===================================================================
---- server/controllers/Controller.hh (revision 7016)
+--- server/controllers/Controller.hh (revision 7019)
+++ server/controllers/Controller.hh (working copy)
@@ -105,6 +105,9 @@
/// \brief The entity that owns this controller
@@ -920,7 +1048,7 @@
protected: ParamT<double> *updatePeriodP;
Index: server/controllers/camera/generic/Generic_Camera.cc
===================================================================
---- server/controllers/camera/generic/Generic_Camera.cc (revision 7016)
+--- server/controllers/camera/generic/Generic_Camera.cc (revision 7019)
+++ server/controllers/camera/generic/Generic_Camera.cc (working copy)
@@ -86,7 +86,24 @@
// Update the controller
@@ -950,7 +1078,7 @@
////////////////////////////////////////////////////////////////////////////////
Index: server/controllers/Controller.cc
===================================================================
---- server/controllers/Controller.cc (revision 7016)
+--- server/controllers/Controller.cc (revision 7019)
+++ server/controllers/Controller.cc (working copy)
@@ -43,6 +43,7 @@
{
@@ -1010,7 +1138,7 @@
if ((*iter)->GetOpenCount() > 0)
Index: server/controllers/ptz/generic/Generic_PTZ.cc
===================================================================
---- server/controllers/ptz/generic/Generic_PTZ.cc (revision 7016)
+--- server/controllers/ptz/generic/Generic_PTZ.cc (revision 7019)
+++ server/controllers/ptz/generic/Generic_PTZ.cc (working copy)
@@ -70,10 +70,10 @@
// Destructor
@@ -1029,7 +1157,7 @@
this->tiltJoint = NULL;
Index: server/World.cc
===================================================================
---- server/World.cc (revision 7016)
+--- server/World.cc (revision 7019)
+++ server/World.cc (working copy)
@@ -27,6 +27,7 @@
#include <assert.h>
@@ -1057,24 +1185,27 @@
this->toAddModels.clear();
this->toDeleteModels.clear();
-@@ -185,6 +190,11 @@
+@@ -185,6 +190,12 @@
std::vector< Model* >::iterator miter;
std::vector< Model* >::iterator miter2;
+ this->simTime += this->physicsEngine->GetStepTime();
+
-+ //double tmpT1 = this->GetWallTime();
-+ //fprintf(stderr, " | sim ");
++#ifdef TIMING
++ double tmpT1 = this->GetWallTime();
++#endif
+
// Update all the models
for (miter=this->models.begin(); miter!=this->models.end(); miter++)
{
-@@ -194,14 +204,27 @@
+@@ -194,14 +205,33 @@
}
}
-+ //double tmpT2 = this->GetWallTime();
-+ //fprintf(stderr, " model dt: %.5f",tmpT2-tmpT1);
++#ifdef TIMING
++ double tmpT2 = this->GetWallTime();
++ std::cout << " models update dt(" << tmpT2-tmpT1 << ")" << std::endl;
++#endif
+
if (!Simulator::Instance()->IsPaused() &&
Simulator::Instance()->GetPhysicsEnabled())
@@ -1086,28 +1217,34 @@
+ this->pauseTime += this->physicsEngine->GetStepTime();
+ }
-+ //double tmpT3 = this->GetWallTime();
-+ //fprintf(stderr, " physics dt: %.5f",tmpT3-tmpT2);
++#ifdef TIMING
++ double tmpT3 = this->GetWallTime();
++ std::cout << " physics engine dt(" << tmpT3-tmpT2 << ")" << std::endl;
++#endif
+
this->UpdateSimulationIface();
-+ //double tmpT4 = this->GetWallTime();
-+ //fprintf(stderr, " Iface dt: %.5f",tmpT4-tmpT3);
++#ifdef TIMING
++ double tmpT4 = this->GetWallTime();
++ std::cout << " sim Iface dt(" << tmpT4-tmpT3 << ")" << std::endl;
++#endif
+
// Copy the newly created models into the main model vector
std::copy(this->toAddModels.begin(), this->toAddModels.end(),
std::back_inserter(this->models));
-@@ -219,6 +242,9 @@
+@@ -219,6 +249,11 @@
this->toDeleteModels.clear();
-+ //double tmpT5 = this->GetWallTime();
-+ //fprintf(stderr, " add/del models dt: %.5f | ",tmpT5-tmpT4);
++#ifdef TIMING
++ double tmpT5 = this->GetWallTime();
++ std::cout << " add/del models dt(" << tmpT5-tmpT4 << ")" << std::endl;
++#endif
+
return 0;
}
-@@ -273,6 +299,41 @@
+@@ -273,6 +308,41 @@
return this->physicsEngine;
}
@@ -1151,7 +1288,7 @@
int World::LoadEntities(XMLConfigNode *node, Model *parent)
Index: SConstruct
===================================================================
---- SConstruct (revision 7016)
+--- SConstruct (revision 7019)
+++ SConstruct (working copy)
@@ -22,8 +22,9 @@
# 3rd party packages
Added: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Bumper.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Bumper.hh (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Bumper.hh 2008-09-12 00:09:08 UTC (rev 4226)
@@ -0,0 +1,86 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/*
+ * Desc: Bumper Controller
+ * Author: Nate Koenig
+ * Date: 09 Sept 2008
+ */
+#ifndef GENERICBUMPER_CONTROLLER_HH
+#define GENERICBUMPER_CONTROLLER_HH
+
+#include <sys/time.h>
+
+#include "Controller.hh"
+#include "Entity.hh"
+
+namespace gazebo
+{
+ class ContactSensor;
+
+ /// \addtogroup gazebo_controller
+ /// \{
+ /** \defgroup bumper bumper
+
+ \brief A controller that returns bump contacts
+
+ \{
+ */
+
+ /// \brief A Bumper controller
+ class Generic_Bumper : public Controller
+ {
+ /// Constructor
+ public: Generic_Bumper(Entity *parent );
+
+ /// Destructor
+ public: virtual ~Generic_Bumper();
+
+ /// Load the controller
+ /// \param node XML config node
+ /// \return 0 on success
+ protected: virtual void LoadChild(XMLConfigNode *node);
+
+ /// Init the controller
+ /// \return 0 on success
+ protected: virtual void InitChild();
+
+ /// Update the controller
+ /// \return 0 on success
+ protected: virtual void UpdateChild();
+
+ /// Finalize the controller
+ /// \return 0 on success
+ protected: virtual void FiniChild();
+
+ /// The parent Model
+ private: ContactSensor *myParent;
+
+ /// The Iface.
+ private: BumperIface *myIface;
+ };
+
+ /** \} */
+ /// \}
+
+}
+
+#endif
+
Added: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Bumper.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Bumper.cc (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Bumper.cc 2008-09-12 00:09:08 UTC (rev 4226)
@@ -0,0 +1,99 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/*
+ * Desc: Bumper controller
+ * Author: Nate Koenig
+ * Date: 09 Setp. 2008
+ */
+
+#include "Global.hh"
+#include "XMLConfig.hh"
+#include "ContactSensor.hh"
+#include "World.hh"
+#include "gazebo.h"
+#include "GazeboError.hh"
+#include "ControllerFactory.hh"
+#include "Simulator.hh"
+#include "Generic_Bumper.hh"
+
+using namespace gazebo;
+
+GZ_REGISTER_STATIC_CONTROLLER("bumper", Generic_Bumper);
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+Generic_Bumper::Generic_Bumper(Entity *parent )
+ : Controller(parent)
+{
+ this->myParent = dynamic_cast<ContactSensor*>(this->parent);
+
+ if (!this->myParent)
+ gzthrow("Bumper controller requires a Contact Sensor as its parent");
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Destructor
+Generic_Bumper::~Generic_Bumper()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Load the controller
+void Generic_Bumper::LoadChild(XMLConfigNode *node)
+{
+ this->myIface = dynamic_cast<BumperIface*>(this->ifaces[0]);
+
+ if (!this->myIface)
+ {
+ gzthrow("Generic_Bumper controller requires an BumperIface");
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Initialize the controller
+void Generic_Bumper::InitChild()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Update the controller
+void Generic_Bumper::UpdateChild()
+{
+ this->myIface->Lock(1);
+
+ this->myIface->data->bumper_count = this->myParent->GetContactCount();
+
+ for (unsigned int i=0; i < this->myParent->GetContactCount(); i++)
+ {
+ this->myIface->data->head.time = this->myParent->GetContactTime(i);
+ this->myIface->data->bumpers[i] = this->myParent->GetContactState(i);
+ }
+
+ this->myParent->ResetContactStates();
+
+ this->myIface->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Finalize the controller
+void Generic_Bumper::FiniChild()
+{
+}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-09-11 23:40:05 UTC (rev 4225)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-09-12 00:09:08 UTC (rev 4226)
@@ -1465,7 +1465,7 @@
<map name="finger_l_left_gravity" flag="gazebo">
<elem key="turnGravityOff">true</elem>
</map>
- </link>
+ </link>
<!-- End left hand l finger proximal digit definition -->
<!-- Begin left hand l finger tip (distal digit) definition -->
<link name="finger_tip_l_left"><!-- link specifying the shoulder of the robot -->
@@ -1487,6 +1487,23 @@
<map name="finger_tip_l_left_gravity" flag="gazebo">
<elem key="turnGravityOff">true</elem>
</map>
+ <!--
+ <map name="sensor" flag="gazebo">
+ <verbatim key="contact_sensor">
+ <sensor:contact name="finger_tip_l_left_contact_sensor">
+ <updateRate>15.0</updateRate>
+ <geom>pr2_finger_tip_l_collision_geom</geom>
+ <controller:ros_bumper name="finger_tip_l_contact_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>15.0</updateRate>
+ <topicName>finger_tip_l_contact</topicName>
+ <frameName>finger_tip_l_contact</frameName>
+ <interface:bumper name="dummy_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+ -->
</link>
<!-- End left hand l finger tip (distal digit) definition -->
<!-- Begin left hand r finger proximal digit definition -->
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-12 22:21:59
|
Revision: 4248
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4248&view=rev
Author: hsujohnhsu
Date: 2008-09-12 20:11:33 +0000 (Fri, 12 Sep 2008)
Log Message:
-----------
migrate tests to trex and gazebo_plugin from rosgazebo.
Added Paths:
-----------
pkg/trunk/drivers/simulator/gazebo_plugin/test/
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp
Removed Paths:
-------------
pkg/trunk/drivers/simulator/gazebo_plugin/test/groundtruthtransform.cpp
Deleted: pkg/trunk/drivers/simulator/gazebo_plugin/test/groundtruthtransform.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/test/groundtruthtransform.cpp 2008-09-08 16:14:19 UTC (rev 4033)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/groundtruthtransform.cpp 2008-09-12 20:11:33 UTC (rev 4248)
@@ -1,83 +0,0 @@
-
-#include "ros/node.h"
-#include "std_msgs/Point3DFloat32.h"
-#include <rosTF/rosTF.h>
-
-using std::string;
-
-/**
- * This is a hack to get the position of the robot. TfPy is not here yet, so this is just to get the tests working.
- */
-class GroundTruthTransform : public ros::node {
-public:
- std_msgs::Point3DFloat32 msg;
- rosTFClient tf;
-
- GroundTruthTransform() : ros::node("GroundTruthTransform"), tf(*this, false) {
- advertise<std_msgs::Point3DFloat32>("groundtruthposition");
- }
-
- void speak() {
- libTF::TFPose robotPose, global_pose;
- robotPose.x = 0;
- robotPose.y = 0;
- robotPose.z = 0;
- robotPose.yaw = 0;
- robotPose.pitch = 0;
- robotPose.roll = 0;
- robotPose.time = 0;
- try {
- robotPose.frame = "FRAMEID_ROBOT";
- } catch(libTF::TransformReference::LookupException& ex) {
- std::cerr << "LookupException in lookup(\"FRAMEID_ROBOT\"): " << ex.what() << "\n";
- std::cout << "LookupException in lookup(\"FRAMEID_ROBOT\"): " << ex.what();
- return;
- } catch(libTF::TransformReference::ExtrapolateException& ex) {
- std::cerr << "ExtrapolateException in lookup(\"FRAMEID_ROBOT\"): " << ex.what() << "\n";
- std::cout << "ExtrapolateException in lookup(\"FRAMEID_ROBOT\"): " << ex.what();
- return;
- } catch(libTF::TransformReference::ConnectivityException& ex) {
- std::cerr << "ConnectivityException in lookup(\"FRAMEID_ROBOT\"): " << ex.what() << "\n";
- std::cout << "ConnectivityException in lookup(\"FRAMEID_ROBOT\"): " << ex.what();
- return;
- }
-
-
- try {
- global_pose = this->tf.transformPose("FRAMEID_ODOM", robotPose);
- } catch(libTF::TransformReference::LookupException& ex) {
- std::cerr << tf.viewFrames();
- std::cerr << "LookupException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what() << "\n";
- std::cout << "LookupException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what();
- return;
- } catch(libTF::TransformReference::ExtrapolateException& ex) {
- std::cerr << "ExtrapolateException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what() << "\n";
- std::cout << "ExtrapolateException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what();
- return;
- } catch(libTF::TransformReference::ConnectivityException& ex) {
- std::cerr << "ConnectivityException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what() << "\n";
- std::cout << "ConnectivityException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what();
- return;
- }
- msg.x = global_pose.x;
- msg.y = global_pose.y;
- msg.z = global_pose.z;
- std::cout << msg.x << ", " << msg.y << ", " << msg.z;
- publish("groundtruthposition", msg);
- }
-};
-
-int main(int argc, char **argv) {
- ros::init(argc, argv);
- std::cout << "Starting...\n";
- GroundTruthTransform t;
- while (t.ok())
- {
- usleep(100000);
- std::cout << "Transform: ";
- t.speak();
- std::cout << std::endl;
- }
- ros::fini();
- return 0;
-}
Copied: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp (from rev 4033, pkg/trunk/simulators/rosgazebo/test/groundtruthtransform.cpp)
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp (rev 0)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp 2008-09-12 20:11:33 UTC (rev 4248)
@@ -0,0 +1,83 @@
+
+#include "ros/node.h"
+#include "std_msgs/Point3DFloat32.h"
+#include <rosTF/rosTF.h>
+
+using std::string;
+
+/**
+ * This is a hack to get the position of the robot. TfPy is not here yet, so this is just to get the tests working.
+ */
+class GroundTruthTransform : public ros::node {
+public:
+ std_msgs::Point3DFloat32 msg;
+ rosTFClient tf;
+
+ GroundTruthTransform() : ros::node("GroundTruthTransform"), tf(*this, false) {
+ advertise<std_msgs::Point3DFloat32>("groundtruthposition");
+ }
+
+ void speak() {
+ libTF::TFPose robotPose, global_pose;
+ robotPose.x = 0;
+ robotPose.y = 0;
+ robotPose.z = 0;
+ robotPose.yaw = 0;
+ robotPose.pitch = 0;
+ robotPose.roll = 0;
+ robotPose.time = 0;
+ try {
+ robotPose.frame = "FRAMEID_ROBOT";
+ } catch(libTF::TransformReference::LookupException& ex) {
+ std::cerr << "LookupException in lookup(\"FRAMEID_ROBOT\"): " << ex.what() << "\n";
+ std::cout << "LookupException in lookup(\"FRAMEID_ROBOT\"): " << ex.what();
+ return;
+ } catch(libTF::TransformReference::ExtrapolateException& ex) {
+ std::cerr << "ExtrapolateException in lookup(\"FRAMEID_ROBOT\"): " << ex.what() << "\n";
+ std::cout << "ExtrapolateException in lookup(\"FRAMEID_ROBOT\"): " << ex.what();
+ return;
+ } catch(libTF::TransformReference::ConnectivityException& ex) {
+ std::cerr << "ConnectivityException in lookup(\"FRAMEID_ROBOT\"): " << ex.what() << "\n";
+ std::cout << "ConnectivityException in lookup(\"FRAMEID_ROBOT\"): " << ex.what();
+ return;
+ }
+
+
+ try {
+ global_pose = this->tf.transformPose("FRAMEID_ODOM", robotPose);
+ } catch(libTF::TransformReference::LookupException& ex) {
+ std::cerr << tf.viewFrames();
+ std::cerr << "LookupException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what() << "\n";
+ std::cout << "LookupException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what();
+ return;
+ } catch(libTF::TransformReference::ExtrapolateException& ex) {
+ std::cerr << "ExtrapolateException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what() << "\n";
+ std::cout << "ExtrapolateException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what();
+ return;
+ } catch(libTF::TransformReference::ConnectivityException& ex) {
+ std::cerr << "ConnectivityException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what() << "\n";
+ std::cout << "ConnectivityException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what();
+ return;
+ }
+ msg.x = global_pose.x;
+ msg.y = global_pose.y;
+ msg.z = global_pose.z;
+ std::cout << msg.x << ", " << msg.y << ", " << msg.z;
+ publish("groundtruthposition", msg);
+ }
+};
+
+int main(int argc, char **argv) {
+ ros::init(argc, argv);
+ std::cout << "Starting...\n";
+ GroundTruthTransform t;
+ while (t.ok())
+ {
+ usleep(100000);
+ std::cout << "Transform: ";
+ t.speak();
+ std::cout << std::endl;
+ }
+ ros::fini();
+ return 0;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-15 20:03:07
|
Revision: 4290
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4290&view=rev
Author: hsujohnhsu
Date: 2008-09-15 20:03:11 +0000 (Mon, 15 Sep 2008)
Log Message:
-----------
moved cmd_vel subscription to base controller.
commentd out arm message stuff from test_actuators.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h
pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
Modified: pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h 2008-09-15 19:14:42 UTC (rev 4289)
+++ pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h 2008-09-15 20:03:11 UTC (rev 4290)
@@ -53,6 +53,7 @@
#include <newmat10/newmatap.h>
#include <std_msgs/RobotBase2DOdom.h>
+#include <std_msgs/BaseVel.h>
#include <pthread.h>
@@ -155,7 +156,6 @@
*/
pthread_mutex_t base_controller_lock_;
-
/*!
* \brief URDF representation of the robot model
*/
@@ -346,6 +346,17 @@
BaseController *c_;
+ /*!
+ * \brief deal with cmd_vel command from 2dnav stack
+ */
+ void CmdBaseVelReceived();
+ std_msgs::BaseVel velMsg;
+
+ /*!
+ * \brief mutex lock for setting and getting ros messages
+ */
+ ros::thread::mutex ros_lock_;
+
};
/** \brief A namespace ostream overload for displaying parameters */
Modified: pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-09-15 19:14:42 UTC (rev 4289)
+++ pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-09-15 20:03:11 UTC (rev 4290)
@@ -433,9 +433,19 @@
node->advertise<std_msgs::RobotBase2DOdom>("odom");
+ // receive messages from 2dnav stack
+ node->subscribe("cmd_vel", velMsg, &BaseControllerNode::CmdBaseVelReceived, this);
+
return true;
}
+void BaseControllerNode::CmdBaseVelReceived()
+{
+ this->ros_lock_.lock();
+ this->setCommand(velMsg.vx,0.0,velMsg.vw);
+ this->ros_lock_.unlock();
+}
+
void BaseControllerNode::getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw)
{
c_->getOdometry(x,y,w,vx,vy,vw);
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h 2008-09-15 19:14:42 UTC (rev 4289)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h 2008-09-15 20:03:11 UTC (rev 4290)
@@ -49,10 +49,10 @@
// roscpp - used for broadcasting time over ros
#include <rostools/Time.h>
// ros messages
-#include <std_msgs/RobotBase2DOdom.h>
-#include <std_msgs/BaseVel.h>
-#include <std_msgs/PR2Arm.h>
-#include <pr2_msgs/EndEffectorState.h>
+//#include <std_msgs/RobotBase2DOdom.h>
+//#include <std_msgs/BaseVel.h>
+//#include <std_msgs/PR2Arm.h>
+//#include <pr2_msgs/EndEffectorState.h>
// Ioan's parser
#include <urdf/URDF.h>
@@ -85,11 +85,11 @@
virtual void FiniChild();
// Callbacks for subscribed messages
- void CmdBaseVelReceived();
- void CmdLeftarmconfigReceived();
- void CmdRightarmconfigReceived();
- void CmdLeftarmcartesianReceived();
- void CmdRightarmcartesianReceived();
+ //void CmdBaseVelReceived();
+ //void CmdLeftarmconfigReceived();
+ //void CmdRightarmconfigReceived();
+ //void CmdLeftarmcartesianReceived();
+ //void CmdRightarmcartesianReceived();
#if 0
bool reset_IK_guess(gazebo_plugin::VoidVoid::request &req, gazebo_plugin::VoidVoid::response &res);
bool SetRightArmCartesian(gazebo_plugin::MoveCartesian::request &req, gazebo_plugin::MoveCartesian::response &res);
@@ -108,13 +108,13 @@
private:
- std_msgs::BaseVel velMsg;
- // arm joint data
- std_msgs::PR2Arm leftarmMsg;
- std_msgs::PR2Arm rightarmMsg;
+ //std_msgs::BaseVel velMsg;
+ // arm joint data
+ //std_msgs::PR2Arm leftarmMsg;
+ //std_msgs::PR2Arm rightarmMsg;
// end effector cmds
- pr2_msgs::EndEffectorState leftarmcartesianMsg;
- pr2_msgs::EndEffectorState rightarmcartesianMsg;
+ //pr2_msgs::EndEffectorState leftarmcartesianMsg;
+ //pr2_msgs::EndEffectorState rightarmcartesianMsg;
rostools::Time timeMsg;
@@ -163,7 +163,7 @@
// for storing reverse transmission results
//mechanism::Robot* reverse_mech_robot_;
- std_msgs::PR2Arm larm,rarm;
+ //std_msgs::PR2Arm larm,rarm;
// for storing controller xml
struct Gazebo_joint_
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-09-15 19:14:42 UTC (rev 4289)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-09-15 20:03:11 UTC (rev 4290)
@@ -441,29 +441,29 @@
rosnode_->publish("time",timeMsg);
- // FIXME: move to arm controller
+ // FIXEDME: deprecated by mechanism state messages!!!
/* get left arm position */
- if( mc_.state_->getJointState("shoulder_pan_left_joint") ) larm.turretAngle = mc_.state_->getJointState("shoulder_pan_left_joint") ->position_;
- if( mc_.state_->getJointState("shoulder_pitch_left_joint") ) larm.shoulderLiftAngle = mc_.state_->getJointState("shoulder_pitch_left_joint")->position_;
- if( mc_.state_->getJointState("upperarm_roll_left_joint") ) larm.upperarmRollAngle = mc_.state_->getJointState("upperarm_roll_left_joint") ->position_;
- if( mc_.state_->getJointState("elbow_flex_left_joint") ) larm.elbowAngle = mc_.state_->getJointState("elbow_flex_left_joint") ->position_;
- if( mc_.state_->getJointState("forearm_roll_left_joint") ) larm.forearmRollAngle = mc_.state_->getJointState("forearm_roll_left_joint") ->position_;
- if( mc_.state_->getJointState("wrist_flex_left_joint") ) larm.wristPitchAngle = mc_.state_->getJointState("wrist_flex_left_joint") ->position_;
- if( mc_.state_->getJointState("gripper_roll_left_joint") ) larm.wristRollAngle = mc_.state_->getJointState("gripper_roll_left_joint") ->position_;
- if( mc_.state_->getJointState("gripper_left_joint") ) larm.gripperForceCmd = mc_.state_->getJointState("gripper_left_joint") ->applied_effort_;
- if( mc_.state_->getJointState("gripper_left_joint") ) larm.gripperGapCmd = mc_.state_->getJointState("gripper_left_joint") ->position_;
- rosnode_->publish("left_pr2arm_pos", larm);
- /* get right arm position */
- if( mc_.state_->getJointState("shoulder_pan_right_joint") ) rarm.turretAngle = mc_.state_->getJointState("shoulder_pan_right_joint") ->position_;
- if( mc_.state_->getJointState("shoulder_pitch_right_joint") ) rarm.shoulderLiftAngle = mc_.state_->getJointState("shoulder_pitch_right_joint")->position_;
- if( mc_.state_->getJointState("upperarm_roll_right_joint") ) rarm.upperarmRollAngle = mc_.state_->getJointState("upperarm_roll_right_joint") ->position_;
- if( mc_.state_->getJointState("elbow_flex_right_joint") ) rarm.elbowAngle = mc_.state_->getJointState("elbow_flex_right_joint") ->position_;
- if( mc_.state_->getJointState("forearm_roll_right_joint") ) rarm.forearmRollAngle = mc_.state_->getJointState("forearm_roll_right_joint") ->position_;
- if( mc_.state_->getJointState("wrist_flex_right_joint") ) rarm.wristPitchAngle = mc_.state_->getJointState("wrist_flex_right_joint") ->position_;
- if( mc_.state_->getJointState("gripper_roll_right_joint") ) rarm.wristRollAngle = mc_.state_->getJointState("gripper_roll_right_joint") ->position_;
- if( mc_.state_->getJointState("gripper_right_joint") ) rarm.gripperForceCmd = mc_.state_->getJointState("gripper_right_joint") ->applied_effort_;
- if( mc_.state_->getJointState("gripper_right_joint") ) rarm.gripperGapCmd = mc_.state_->getJointState("gripper_right_joint") ->position_;
- rosnode_->publish("right_pr2arm_pos", rarm);
+ // if( mc_.state_->getJointState("shoulder_pan_left_joint") ) larm.turretAngle = mc_.state_->getJointState("shoulder_pan_left_joint") ->position_;
+ // if( mc_.state_->getJointState("shoulder_pitch_left_joint") ) larm.shoulderLiftAngle = mc_.state_->getJointState("shoulder_pitch_left_joint")->position_;
+ // if( mc_.state_->getJointState("upperarm_roll_left_joint") ) larm.upperarmRollAngle = mc_.state_->getJointState("upperarm_roll_left_joint") ->position_;
+ // if( mc_.state_->getJointState("elbow_flex_left_joint") ) larm.elbowAngle = mc_.state_->getJointState("elbow_flex_left_joint") ->position_;
+ // if( mc_.state_->getJointState("forearm_roll_left_joint") ) larm.forearmRollAngle = mc_.state_->getJointState("forearm_roll_left_joint") ->position_;
+ // if( mc_.state_->getJointState("wrist_flex_left_joint") ) larm.wristPitchAngle = mc_.state_->getJointState("wrist_flex_left_joint") ->position_;
+ // if( mc_.state_->getJointState("gripper_roll_left_joint") ) larm.wristRollAngle = mc_.state_->getJointState("gripper_roll_left_joint") ->position_;
+ // if( mc_.state_->getJointState("gripper_left_joint") ) larm.gripperForceCmd = mc_.state_->getJointState("gripper_left_joint") ->applied_effort_;
+ // if( mc_.state_->getJointState("gripper_left_joint") ) larm.gripperGapCmd = mc_.state_->getJointState("gripper_left_joint") ->position_;
+ // rosnode_->publish("left_pr2arm_pos", larm);
+ // /* get right arm position */
+ // if( mc_.state_->getJointState("shoulder_pan_right_joint") ) rarm.turretAngle = mc_.state_->getJointState("shoulder_pan_right_joint") ->position_;
+ // if( mc_.state_->getJointState("shoulder_pitch_right_joint") ) rarm.shoulderLiftAngle = mc_.state_->getJointState("shoulder_pitch_right_joint")->position_;
+ // if( mc_.state_->getJointState("upperarm_roll_right_joint") ) rarm.upperarmRollAngle = mc_.state_->getJointState("upperarm_roll_right_joint") ->position_;
+ // if( mc_.state_->getJointState("elbow_flex_right_joint") ) rarm.elbowAngle = mc_.state_->getJointState("elbow_flex_right_joint") ->position_;
+ // if( mc_.state_->getJointState("forearm_roll_right_joint") ) rarm.forearmRollAngle = mc_.state_->getJointState("forearm_roll_right_joint") ->position_;
+ // if( mc_.state_->getJointState("wrist_flex_right_joint") ) rarm.wristPitchAngle = mc_.state_->getJointState("wrist_flex_right_joint") ->position_;
+ // if( mc_.state_->getJointState("gripper_roll_right_joint") ) rarm.wristRollAngle = mc_.state_->getJointState("gripper_roll_right_joint") ->position_;
+ // if( mc_.state_->getJointState("gripper_right_joint") ) rarm.gripperForceCmd = mc_.state_->getJointState("gripper_right_joint") ->applied_effort_;
+ // if( mc_.state_->getJointState("gripper_right_joint") ) rarm.gripperGapCmd = mc_.state_->getJointState("gripper_right_joint") ->position_;
+ // rosnode_->publish("right_pr2arm_pos", rarm);
PublishFrameTransforms();
@@ -550,13 +550,13 @@
int
TestActuators::AdvertiseSubscribeMessages()
{
- rosnode_->advertise<std_msgs::PR2Arm>("left_pr2arm_pos");
- rosnode_->advertise<std_msgs::PR2Arm>("right_pr2arm_pos");
+ //rosnode_->advertise<std_msgs::PR2Arm>("left_pr2arm_pos");
+ //rosnode_->advertise<std_msgs::PR2Arm>("right_pr2arm_pos");
rosnode_->advertise<rostools::Time>("time");
- rosnode_->subscribe("cmd_vel", velMsg, &TestActuators::CmdBaseVelReceived, this);
- rosnode_->subscribe("cmd_leftarmconfig", leftarmMsg, &TestActuators::CmdLeftarmconfigReceived,this);
- rosnode_->subscribe("cmd_rightarmconfig", rightarmMsg, &TestActuators::CmdRightarmconfigReceived,this);
+ //rosnode_->subscribe("cmd_vel", velMsg, &TestActuators::CmdBaseVelReceived, this);
+ //rosnode_->subscribe("cmd_leftarmconfig", leftarmMsg, &TestActuators::CmdLeftarmconfigReceived,this);
+ //rosnode_->subscribe("cmd_rightarmconfig", rightarmMsg, &TestActuators::CmdRightarmconfigReceived,this);
//rosnode_->subscribe("cmd_leftarm_cartesian", leftarmcartesianMsg, &TestActuators::CmdLeftarmcartesianReceived);
//rosnode_->subscribe("cmd_rightarm_cartesian", rightarmcartesianMsg, &TestActuators::CmdRightarmcartesianReceived);
@@ -676,7 +676,7 @@
-
+#if 0
void
TestActuators::CmdLeftarmconfigReceived()
{
@@ -767,7 +767,6 @@
-#if 0
void
TestActuators::CmdLeftarmcartesianReceived()
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-09-15 20:31:53
|
Revision: 4293
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4293&view=rev
Author: jleibs
Date: 2008-09-15 20:32:00 +0000 (Mon, 15 Sep 2008)
Log Message:
-----------
Checking in a minimal curl test case. Hopefully I can remove this
once bug is resolved. If it can be made more deterministic, it might
serve as the basis for an eventual unit test that ought to be added
to roscpp.
Added Paths:
-----------
pkg/trunk/curltest/
pkg/trunk/curltest/CMakeLists.txt
pkg/trunk/curltest/Makefile
pkg/trunk/curltest/curlnode.cpp
pkg/trunk/curltest/curltest.cpp
pkg/trunk/curltest/curltest.h
pkg/trunk/curltest/manifest.xml
Added: pkg/trunk/curltest/CMakeLists.txt
===================================================================
--- pkg/trunk/curltest/CMakeLists.txt (rev 0)
+++ pkg/trunk/curltest/CMakeLists.txt 2008-09-15 20:32:00 UTC (rev 4293)
@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(curltest)
+
+find_library(curl REQUIRED)
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+
+rospack_add_executable(curltest curlnode.cpp curltest.cpp)
+target_link_libraries(curltest curl)
+
+
+
Added: pkg/trunk/curltest/Makefile
===================================================================
--- pkg/trunk/curltest/Makefile (rev 0)
+++ pkg/trunk/curltest/Makefile 2008-09-15 20:32:00 UTC (rev 4293)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/curltest/curlnode.cpp
===================================================================
--- pkg/trunk/curltest/curlnode.cpp (rev 0)
+++ pkg/trunk/curltest/curlnode.cpp 2008-09-15 20:32:00 UTC (rev 4293)
@@ -0,0 +1,81 @@
+///////////////////////////////////////////////////////////////////////////////
+// The axis_cam package provides a library that talks to Axis IP-based cameras
+// as well as ROS nodes which use these libraries
+//
+// Copyright (C) 2008, Morgan Quigley
+//
+// 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 Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include "ros/node.h"
+#include "curltest.h"
+#include "std_msgs/String.h"
+
+class CurlNode : public ros::node
+{
+public:
+ CurlTest *curl;
+
+ std_msgs::String msg_out;
+
+ CurlNode() : node("curltest"), curl(NULL)
+ {
+ advertise<std_msgs::String>("chatter",1);
+
+ curl = new CurlTest(string("axis-00408c7dfe2b.local"));
+ }
+
+ virtual ~CurlNode()
+ {
+ if (curl)
+ delete curl;
+ }
+
+ bool spin()
+ {
+ while (ok())
+ {
+ if (curl->get())
+ log(ros::ERROR,"Get failed.");
+
+ msg_out.data = (char*)(curl->buf);
+ publish("chatter", msg_out);
+ }
+ return true;
+ }
+
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ CurlNode n;
+
+ n.spin();
+
+ ros::fini();
+
+ return 0;
+}
+
Added: pkg/trunk/curltest/curltest.cpp
===================================================================
--- pkg/trunk/curltest/curltest.cpp (rev 0)
+++ pkg/trunk/curltest/curltest.cpp 2008-09-15 20:32:00 UTC (rev 4293)
@@ -0,0 +1,118 @@
+///////////////////////////////////////////////////////////////////////////////
+// The axis_cam package provides a library that talks to Axis IP-based cameras
+// as well as ROS nodes which use these libraries
+//
+// Copyright (C) 2008, Morgan Quigley, Stanford Univerity
+// Jeremy Leibs, Willow Garage
+//
+// 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 Stanford University, 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 "curltest.h"
+
+#define RETURN_CURL_ERR(curl, code) \
+ fprintf(stderr, "curl error: [%s]\n", curl_easy_strerror(code)); \
+ if (code == 22) \
+ { \
+ int http_code; \
+ curl_easy_getinfo(curl, CURLINFO_RESPONSE_CODE, &http_code); \
+ fprintf(stderr, "HTTP response: %d\n", http_code); \
+ return http_code; \
+ } \
+ return code; \
+
+CurlTest::CurlTest(string ip) : ip(ip)
+{
+ buf = NULL;
+ buf_size = 0;
+
+ ostringstream oss;
+
+ oss << "http://" << ip << "/axis-cgi/com/ptz.cgi";
+ url = new char[oss.str().length()+1];
+ strcpy(url, oss.str().c_str());
+
+ curl_global_init(0);
+
+ get_curl = curl_easy_init();
+
+ curl_easy_setopt(get_curl, CURLOPT_URL, url);
+ curl_easy_setopt(get_curl, CURLOPT_WRITEFUNCTION, CurlTest::buf_write);
+ curl_easy_setopt(get_curl, CURLOPT_WRITEDATA, this);
+ curl_easy_setopt(get_curl, CURLOPT_POSTFIELDS, "query=position");
+ curl_easy_setopt(get_curl, CURLOPT_TIMEOUT, 1);
+ curl_easy_setopt(get_curl, CURLOPT_FAILONERROR, 1);
+}
+
+CurlTest::~CurlTest()
+{
+ if (buf)
+ delete[] buf;
+
+ buf = NULL;
+ curl_global_cleanup();
+}
+
+size_t CurlTest::buf_write(void *buf, size_t size, size_t nmemb, void *userp)
+{
+ if (size * nmemb == 0)
+ return 0;
+
+ CurlTest *a = (CurlTest *)userp;
+
+ if (a->buf_file_size + size*nmemb >= a->buf_size)
+ {
+ a->buf_size = 2 * (a->buf_file_size + (size*nmemb));
+
+ uint8_t* tmp = new uint8_t[a->buf_size];
+
+ if (a->buf)
+ {
+ memcpy(tmp, a->buf, a->buf_file_size);
+ delete[] a->buf;
+ } else {
+ memset(tmp, 0, a->buf_size);
+ }
+ a->buf = tmp;
+ }
+ memcpy(a->buf + a->buf_file_size, buf, size*nmemb);
+ a->buf_file_size += size*nmemb;
+ return size*nmemb;
+}
+
+int CurlTest::get()
+{
+
+ CURLcode code;
+
+ buf_file_size = 0;
+
+ if ((code = curl_easy_perform(get_curl)))
+ {
+ RETURN_CURL_ERR(get_curl, code);
+ }
+
+ printf("Read: %s\n", buf);
+
+ return 0;
+}
Added: pkg/trunk/curltest/curltest.h
===================================================================
--- pkg/trunk/curltest/curltest.h (rev 0)
+++ pkg/trunk/curltest/curltest.h 2008-09-15 20:32:00 UTC (rev 4293)
@@ -0,0 +1,59 @@
+///////////////////////////////////////////////////////////////////////////////
+// The axis_cam package provides a library that talks to Axis IP-based cameras
+// as well as ROS nodes which use these libraries
+//
+// Copyright (C) 2008, Morgan Quigley
+//
+// 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 Stanford University 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 AXIS_CAM_AXIS_CAM_H
+#define AXIS_CAM_AXIS_CAM_H
+
+#include <curl/curl.h>
+#include <string>
+#include <sstream>
+#include "rosthread/mutex.h"
+
+using namespace std;
+
+class CurlTest
+{
+public:
+ CurlTest(string ip);
+ ~CurlTest();
+
+ string ip;
+
+ uint8_t *buf;
+ uint32_t buf_size, buf_file_size;
+ CURL *get_curl;
+ char *url;
+
+ int get();
+
+ static size_t buf_write(void *buf, size_t size, size_t nmemb, void *userp);
+};
+
+#endif
+
Added: pkg/trunk/curltest/manifest.xml
===================================================================
--- pkg/trunk/curltest/manifest.xml (rev 0)
+++ pkg/trunk/curltest/manifest.xml 2008-09-15 20:32:00 UTC (rev 4293)
@@ -0,0 +1,13 @@
+<package>
+<description>
+A minimal package to try debugging single-threaded ros and curl interference.
+</description>
+<author>Jeremy Leibs</author>
+<license>BSD</license>
+<url></url>
+<depend package="roscpp"/>
+<depend package="std_msgs"/>
+<sysdepend package="libcurl3-openssl-dev" os="ubuntu" version="7.04-feisty"/>
+<sysdepend package="libcurl4-openssl-dev" os="ubuntu" version="8.04-hardy"/>
+</package>
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-15 20:54:36
|
Revision: 4294
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4294&view=rev
Author: hsujohnhsu
Date: 2008-09-15 20:54:42 +0000 (Mon, 15 Sep 2008)
Log Message:
-----------
moved odometry frame publishing to base controller.
moved odometry message publishing from BaseController to BaseControllerNode.
removed corresponding functions from test_actuators.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h
pkg/trunk/controllers/pr2_controllers/manifest.xml
pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
Modified: pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h 2008-09-15 20:32:00 UTC (rev 4293)
+++ pkg/trunk/controllers/pr2_controllers/include/pr2_controllers/base_controller.h 2008-09-15 20:54:42 UTC (rev 4294)
@@ -55,6 +55,8 @@
#include <std_msgs/RobotBase2DOdom.h>
#include <std_msgs/BaseVel.h>
+#include <rosTF/rosTF.h>
+
#include <pthread.h>
namespace controller
@@ -163,16 +165,15 @@
/*!
- * \brief Set the publish count (number of update ticks between odometry message publishing).
+ * \brief returns odometry data
*/
- void setPublishCount(int publish_count);
+ void getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw);
/*!
- * \brief returns odometry data
+ * \brief set odometry message
*/
- void getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw);
+ void setOdomMessage(std_msgs::RobotBase2DOdom &odom_msg_);
-
private:
/*!
@@ -186,15 +187,7 @@
*/
int num_casters_;
-
/*!
- * \brief number of update ticks to wait before publishing ROS odom message
- * defaults to 10.
- */
- int odom_publish_count_;
-
-
- /*!
* \brief local gain used for speed control of the caster (to achieve resultant position control)
*/
double kp_speed_;
@@ -297,21 +290,9 @@
std::vector<double> steer_angle_actual_; /** vector of actual caster steer angles */
-
std::vector<double> wheel_speed_actual_; /** vector of actual wheel speeds */
-
- /*!
- * \brief std_msgs representation of an odometry message
- */
- std_msgs::RobotBase2DOdom odom_msg_;
-
-
double last_time_; /** time corresponding to when update was last called */
-
-
- int odom_publish_counter_; /** counter - when this exceeds odom_publish_count_, the odomeetry message will be published on ROS */
-
};
class BaseControllerNode : public Controller
@@ -350,13 +331,36 @@
* \brief deal with cmd_vel command from 2dnav stack
*/
void CmdBaseVelReceived();
- std_msgs::BaseVel velMsg;
+ std_msgs::BaseVel baseVelMsg;
/*!
* \brief mutex lock for setting and getting ros messages
*/
ros::thread::mutex ros_lock_;
+ /*!
+ * \brief frame transform server for publishing base odometry frame
+ */
+ private: rosTFServer *tfs;
+
+ /*!
+ * \brief Set the publish count (number of update ticks between odometry message publishing).
+ */
+ void setPublishCount(int publish_count); //FIXME: use time rather than count
+
+ /*!
+ * \brief std_msgs representation of an odometry message
+ */
+ std_msgs::RobotBase2DOdom odom_msg_;
+
+ /*!
+ * \brief number of update ticks to wait before publishing ROS odom message
+ * defaults to 10.
+ */
+ int odom_publish_count_; //FIXME: use time rather than count
+
+
+ int odom_publish_counter_; /** counter - when this exceeds odom_publish_count_, the odomeetry message will be published on ROS */ //FIXME: use time rather than count
};
/** \brief A namespace ostream overload for displaying parameters */
Modified: pkg/trunk/controllers/pr2_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_controllers/manifest.xml 2008-09-15 20:32:00 UTC (rev 4293)
+++ pkg/trunk/controllers/pr2_controllers/manifest.xml 2008-09-15 20:54:42 UTC (rev 4294)
@@ -11,6 +11,7 @@
<depend package="generic_controllers" />
<depend package="rospy" />
<depend package="libTF" />
+ <depend package="rosTF" />
<depend package="std_msgs" />
<depend package="math_utils" />
<depend package="robot_kinematics" />
Modified: pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-09-15 20:32:00 UTC (rev 4293)
+++ pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-09-15 20:54:42 UTC (rev 4294)
@@ -43,7 +43,7 @@
ROS_REGISTER_CONTROLLER(BaseController)
-BaseController::BaseController() : num_wheels_(0), num_casters_(0), odom_publish_count_(10), odom_publish_counter_(0)
+BaseController::BaseController() : num_wheels_(0), num_casters_(0)
{
cmd_vel_.x = 0;
cmd_vel_.y = 0;
@@ -88,19 +88,12 @@
return cmd_vel;
}
-void BaseController::setPublishCount(int publish_count)
-{
- odom_publish_count_ = publish_count;
-};
-
-
void BaseController::init(std::vector<JointControlParam> jcp, mechanism::RobotState *robot_state)
{
std::vector<JointControlParam>::iterator jcp_iter;
robot_desc::URDF::Link *link;
std::string joint_name;
-// ros::g_node->advertise<std_msgs::RobotBase2DOdom>("odom");
std::string xml_content;
(ros::g_node)->get_param("robotdesc/pr2",xml_content);
if(!urdf_model_.loadString(xml_content.c_str()))
@@ -291,17 +284,9 @@
computeOdometry(current_time);
- if(odom_publish_counter_ > odom_publish_count_) // FIXME: time based rate limiting
- {
- (ros::g_node)->publish("odom", odom_msg_);
- odom_publish_counter_ = 0;
- }
-
updateJointControllers();
last_time_ = current_time;
-
- odom_publish_counter_++;
}
void BaseController::computeAndSetCasterSteer()
@@ -362,9 +347,26 @@
base_casters_[i].controller_.update();
}
+void BaseController::setOdomMessage(std_msgs::RobotBase2DOdom &odom_msg_)
+{
+ odom_msg_.header.frame_id = "FRAMEID_ODOM";
+ odom_msg_.header.stamp.sec = (unsigned long)floor(robot_state_->hw_->current_time_);
+ odom_msg_.header.stamp.nsec = (unsigned long)floor( 1e9 * ( robot_state_->hw_->current_time_ - odom_msg_.header.stamp.sec) );
+ odom_msg_.pos.x = base_odom_position_.x;
+ odom_msg_.pos.y = base_odom_position_.y;
+ odom_msg_.pos.th = base_odom_position_.z;
+
+ odom_msg_.vel.x = base_odom_velocity_.x;
+ odom_msg_.vel.y = base_odom_velocity_.y;
+ odom_msg_.vel.th = base_odom_velocity_.z;
+
+// cout << "Base Odometry: Velocity " << base_odom_velocity_;
+// cout << "Base Odometry: Position " << base_odom_position_;
+}
+
ROS_REGISTER_CONTROLLER(BaseControllerNode)
- BaseControllerNode::BaseControllerNode()
+ BaseControllerNode::BaseControllerNode() : odom_publish_count_(10), odom_publish_counter_(0)
{
c_ = new BaseController();
}
@@ -374,9 +376,65 @@
delete c_;
}
+void BaseControllerNode::setPublishCount(int publish_count)
+{
+ odom_publish_count_ = publish_count;
+};
+
void BaseControllerNode::update()
{
c_->update();
+
+ c_->setOdomMessage(odom_msg_);
+
+ if(odom_publish_counter_ > odom_publish_count_) // FIXME: switch to time based rate limiting
+ {
+ (ros::g_node)->publish("odom", odom_msg_);
+ odom_publish_counter_ = 0;
+
+ // FIXME: should this be in here?
+ /***************************************************************/
+ /* */
+ /* frame transforms */
+ /* */
+ /* TODO: should we send z, roll, pitch, yaw? seems to confuse */
+ /* localization */
+ /* */
+ /***************************************************************/
+ double x=0,y=0,z=0,roll=0,pitch=0,yaw=0,vx,vy,vyaw;
+ this->getOdometry(x,y,yaw,vx,vy,vyaw);
+ // FIXME: z, roll, pitch not accounted for
+ this->tfs->sendInverseEuler("FRAMEID_ODOM",
+ "base",
+ x,
+ y,
+ 0, //z - base_center_offset_z, /* get infor from xml: half height of base box */
+ yaw,
+ pitch,
+ roll,
+ odom_msg_.header.stamp);
+
+ /***************************************************************/
+ /* */
+ /* frame transforms */
+ /* */
+ /* x,y,z,yaw,pitch,roll */
+ /* */
+ /***************************************************************/
+ this->tfs->sendEuler("base",
+ "FRAMEID_ROBOT",
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0,
+ odom_msg_.header.stamp);
+
+ }
+
+ odom_publish_counter_++;
+
}
bool BaseControllerNode::setCommand(
@@ -434,15 +492,18 @@
node->advertise<std_msgs::RobotBase2DOdom>("odom");
// receive messages from 2dnav stack
- node->subscribe("cmd_vel", velMsg, &BaseControllerNode::CmdBaseVelReceived, this);
+ node->subscribe("cmd_vel", baseVelMsg, &BaseControllerNode::CmdBaseVelReceived, this);
+ // for publishing odometry frame transforms FRAMEID_ODOM
+ this->tfs = new rosTFServer(*node); //, true, 1 * 1000000000ULL, 0ULL);
+
return true;
}
void BaseControllerNode::CmdBaseVelReceived()
{
this->ros_lock_.lock();
- this->setCommand(velMsg.vx,0.0,velMsg.vw);
+ this->setCommand(baseVelMsg.vx,0.0,baseVelMsg.vw);
this->ros_lock_.unlock();
}
@@ -497,21 +558,6 @@
base_odom_delta.z = base_odom_velocity_.z * dt;
base_odom_position_ += base_odom_delta;
-
- odom_msg_.header.frame_id = "FRAMEID_ODOM";
- odom_msg_.header.stamp.sec = (unsigned long)floor(robot_state_->hw_->current_time_);
- odom_msg_.header.stamp.nsec = (unsigned long)floor( 1e9 * ( robot_state_->hw_->current_time_ - odom_msg_.header.stamp.sec) );
-
- odom_msg_.pos.x = base_odom_position_.x;
- odom_msg_.pos.y = base_odom_position_.y;
- odom_msg_.pos.th = base_odom_position_.z;
-
- odom_msg_.vel.x = base_odom_velocity_.x;
- odom_msg_.vel.y = base_odom_velocity_.y;
- odom_msg_.vel.th = base_odom_velocity_.z;
-
-// cout << "Base Odometry: Velocity " << base_odom_velocity_;
-// cout << "Base Odometry: Position " << base_odom_position_;
}
void BaseController::computeBaseVelocity()
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h 2008-09-15 20:32:00 UTC (rev 4293)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h 2008-09-15 20:54:42 UTC (rev 4294)
@@ -49,8 +49,6 @@
// roscpp - used for broadcasting time over ros
#include <rostools/Time.h>
// ros messages
-//#include <std_msgs/RobotBase2DOdom.h>
-//#include <std_msgs/BaseVel.h>
//#include <std_msgs/PR2Arm.h>
//#include <pr2_msgs/EndEffectorState.h>
@@ -85,7 +83,6 @@
virtual void FiniChild();
// Callbacks for subscribed messages
- //void CmdBaseVelReceived();
//void CmdLeftarmconfigReceived();
//void CmdRightarmconfigReceived();
//void CmdLeftarmcartesianReceived();
@@ -108,7 +105,6 @@
private:
- //std_msgs::BaseVel velMsg;
// arm joint data
//std_msgs::PR2Arm leftarmMsg;
//std_msgs::PR2Arm rightarmMsg;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-09-15 20:32:00 UTC (rev 4293)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-09-15 20:54:42 UTC (rev 4294)
@@ -554,7 +554,6 @@
//rosnode_->advertise<std_msgs::PR2Arm>("right_pr2arm_pos");
rosnode_->advertise<rostools::Time>("time");
- //rosnode_->subscribe("cmd_vel", velMsg, &TestActuators::CmdBaseVelReceived, this);
//rosnode_->subscribe("cmd_leftarmconfig", leftarmMsg, &TestActuators::CmdLeftarmconfigReceived,this);
//rosnode_->subscribe("cmd_rightarmconfig", rightarmMsg, &TestActuators::CmdRightarmconfigReceived,this);
//rosnode_->subscribe("cmd_leftarm_cartesian", leftarmcartesianMsg, &TestActuators::CmdLeftarmcartesianReceived);
@@ -600,51 +599,15 @@
{
- // FIXME: the frame transforms should be published by individual mechanism joints, not here
- // FIXME: the frame transforms should be published by individual mechanism joints, not here
- // FIXME: the frame transforms should be published by individual mechanism joints, not here
- /***************************************************************/
- /* */
- /* frame transforms */
- /* */
- /* TODO: should we send z, roll, pitch, yaw? seems to confuse */
- /* localization */
- /* */
- /***************************************************************/
- double x=0,y=0,z=0,roll=0,pitch=0,yaw=0,vx,vy,vyaw;
- controller::Controller* bc = mc_.getControllerByName( "base_controller" );
- controller::BaseControllerNode* bcn = dynamic_cast<controller::BaseControllerNode*>(bc);
- if (bcn) bcn->getOdometry(x,y,yaw,vx,vy,vyaw);
- // FIXME: z, roll, pitch not accounted for
- tfs->sendInverseEuler("FRAMEID_ODOM",
- "base",
- x,
- y,
- z, //z - base_center_offset_z, /* get infor from xml: half height of base box */
- yaw,
- pitch,
- roll,
- timeMsg.rostime);
-
- /***************************************************************/
- /* */
- /* frame transforms */
- /* */
- /* x,y,z,yaw,pitch,roll */
- /* */
- /***************************************************************/
- tfs->sendEuler("base",
- "FRAMEID_ROBOT",
- 0,
- 0,
- 0,
- 0,
- 0,
- 0,
- timeMsg.rostime);
-
-
+ /***********************************************************************************************/
+ /* */
+ /* frame transforms for laser sensors */
+ /* */
+ /* FIXME mechanism transform should treat <sensor> in pr2.xml same way <link> is treated, */
+ /* i.e. publish frame transform for <sensor> as well. */
+ /* */
+ /***********************************************************************************************/
// base laser location
tfs->sendEuler("base_laser",
"base",
@@ -752,22 +715,7 @@
this->lock.unlock();
}
-
void
- TestActuators::CmdBaseVelReceived()
- {
- this->lock.lock();
- controller::Controller* cc = mc_.getControllerByName( "base_controller" );
- controller::BaseControllerNode* bc = dynamic_cast<controller::BaseControllerNode*>(cc);
- if (bc)
- bc->setCommand(velMsg.vx,0.0,velMsg.vw);
- this->lock.unlock();
- }
-
-
-
-
- void
TestActuators::CmdLeftarmcartesianReceived()
{
this->lock.lock();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-16 01:49:36
|
Revision: 4320
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4320&view=rev
Author: hsujohnhsu
Date: 2008-09-16 01:49:47 +0000 (Tue, 16 Sep 2008)
Log Message:
-----------
updated teleop_arm_keyboard and generic joint position controller to communicate over ROS topic.
Modified Paths:
--------------
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h
pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp
pkg/trunk/manip/teleop_arm_keyboard/manifest.xml
pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
Added Paths:
-----------
pkg/trunk/controllers/generic_controllers/msg/
pkg/trunk/controllers/generic_controllers/msg/SingleJointPosCmd.msg
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h 2008-09-16 01:38:16 UTC (rev 4319)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h 2008-09-16 01:49:47 UTC (rev 4320)
@@ -61,6 +61,8 @@
#include <generic_controllers/SetCommand.h>
#include <generic_controllers/GetActual.h>
+#include <generic_controllers/SingleJointPosCmd.h>
+
namespace controller
{
@@ -176,7 +178,13 @@
bool getActual(generic_controllers::GetActual::request &req,
generic_controllers::GetActual::response &resp);
+ /*!
+ * \brief ROS topic callback
+ */
+ void setJointPosSingle();
+
private:
+ generic_controllers::SingleJointPosCmd msg_; //The message used by the ROS callback
JointPositionController *c_;
};
}
Added: pkg/trunk/controllers/generic_controllers/msg/SingleJointPosCmd.msg
===================================================================
--- pkg/trunk/controllers/generic_controllers/msg/SingleJointPosCmd.msg (rev 0)
+++ pkg/trunk/controllers/generic_controllers/msg/SingleJointPosCmd.msg 2008-09-16 01:49:47 UTC (rev 4320)
@@ -0,0 +1,3 @@
+float64 position
+float64 margin
+float64 timeout
Modified: pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp 2008-09-16 01:38:16 UTC (rev 4319)
+++ pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp 2008-09-16 01:49:47 UTC (rev 4320)
@@ -225,8 +225,31 @@
if (!c_->initXml(robot, config))
return false;
+
node->advertise_service(prefix + "/set_command", &JointPositionControllerNode::setCommand, this);
node->advertise_service(prefix + "/get_actual", &JointPositionControllerNode::getActual, this);
+
+ TiXmlElement * ros_cb = config->FirstChildElement("listen_topic");
+ if(ros_cb)
+ {
+ const char * topic_name=ros_cb->Attribute("name");
+ if(!topic_name)
+ {
+ std::cout<<" A listen _topic is present in the xml file but no name is specified\n";
+ return false;
+ }
+ node->subscribe(topic_name, msg_, &JointPositionControllerNode::setJointPosSingle, this, 1);
+ std::cout<<"Listening to topic: "<<topic_name<<std::endl;
+ }
+
+
+
return true;
}
+void JointPositionControllerNode::setJointPosSingle()
+{
+ c_->setCommand(msg_.position);
+}
+
+
Modified: pkg/trunk/manip/teleop_arm_keyboard/manifest.xml
===================================================================
--- pkg/trunk/manip/teleop_arm_keyboard/manifest.xml 2008-09-16 01:38:16 UTC (rev 4319)
+++ pkg/trunk/manip/teleop_arm_keyboard/manifest.xml 2008-09-16 01:49:47 UTC (rev 4320)
@@ -4,6 +4,7 @@
<author>Advait Jain</author>
<license>BSD</license>
<depend package="roscpp"/>
- <depend package="std_msgs"/>
+ <depend package="generic_controllers"/>
+ <depend package="pr2_controllers"/>
<depend package="rosTF"/>
</package>
Modified: pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
===================================================================
--- pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-09-16 01:38:16 UTC (rev 4319)
+++ pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-09-16 01:49:47 UTC (rev 4320)
@@ -55,7 +55,7 @@
- None
Publishes to (name / type):
- - @b "cmd_leftarmconfig"/PR2Arm : configuration of the left arm (all the joint angles); sent on every keypress.
+ - @b "lArmCmd"/JointPosCmd : configuration of the left arm (all the joint angles); sent on every keypress.
<hr>
@@ -70,7 +70,8 @@
#include <math.h>
#include <ros/node.h>
-#include <std_msgs/PR2Arm.h>
+#include <pr2_controllers/JointPosCmd.h>
+#include <generic_controllers/SingleJointPosCmd.h>
// For transform support
#include <rosTF/rosTF.h>
@@ -125,8 +126,10 @@
class TArmK_Node : public ros::node
{
private:
- std_msgs::PR2Arm cmd_leftarmconfig;
- std_msgs::PR2Arm cmd_rightarmconfig;
+ pr2_controllers::JointPosCmd lArmCmd;
+ pr2_controllers::JointPosCmd rArmCmd;
+ generic_controllers::SingleJointPosCmd lGripperCmd;
+ generic_controllers::SingleJointPosCmd rGripperCmd;
public:
TArmK_Node() : ros::node("tarmk"), tf(*this, true)
@@ -134,58 +137,97 @@
// cmd_armconfig should probably be initialised
// with the current joint angles of the arm rather
// than zeros.
- this->cmd_leftarmconfig.turretAngle = 0;
- this->cmd_leftarmconfig.shoulderLiftAngle = 0;
- this->cmd_leftarmconfig.upperarmRollAngle = 0;
- this->cmd_leftarmconfig.elbowAngle = 0;
- this->cmd_leftarmconfig.forearmRollAngle = 0;
- this->cmd_leftarmconfig.wristPitchAngle = 0;
- this->cmd_leftarmconfig.wristRollAngle = 0;
- this->cmd_leftarmconfig.gripperForceCmd = 1000;
- this->cmd_leftarmconfig.gripperGapCmd = 0;
- this->cmd_rightarmconfig.turretAngle = 0;
- this->cmd_rightarmconfig.shoulderLiftAngle = 0;
- this->cmd_rightarmconfig.upperarmRollAngle = 0;
- this->cmd_rightarmconfig.elbowAngle = 0;
- this->cmd_rightarmconfig.forearmRollAngle = 0;
- this->cmd_rightarmconfig.wristPitchAngle = 0;
- this->cmd_rightarmconfig.wristRollAngle = 0;
- this->cmd_rightarmconfig.gripperForceCmd = 1000;
- this->cmd_rightarmconfig.gripperGapCmd = 0;
- advertise<std_msgs::PR2Arm>("cmd_leftarmconfig");
- advertise<std_msgs::PR2Arm>("cmd_rightarmconfig");
+ this->lArmCmd.set_names_size(7);
+ this->rArmCmd.set_names_size(7);
+ this->lArmCmd.set_positions_size(7);
+ this->rArmCmd.set_positions_size(7);
+ this->lArmCmd.set_margins_size(7);
+ this->rArmCmd.set_margins_size(7);
- _leftInit = false;
- _rightInit = false;
+ 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";
- //for getting positions
- subscribe("left_pr2arm_pos", leftArmPosMsg, &TArmK_Node::leftArmPosReceived);
- subscribe("right_pr2arm_pos", rightArmPosMsg, &TArmK_Node::rightArmPosReceived);
+ this->lArmCmd.positions[0] = 0;
+ this->lArmCmd.positions[1] = 0;
+ this->lArmCmd.positions[2] = 0;
+ this->lArmCmd.positions[3] = 0;
+ this->lArmCmd.positions[4] = 0;
+ this->lArmCmd.positions[5] = 0;
+ this->lArmCmd.positions[6] = 0;
+
+ this->lArmCmd.margins[0] = 0;
+ this->lArmCmd.margins[1] = 0;
+ this->lArmCmd.margins[2] = 0;
+ this->lArmCmd.margins[3] = 0;
+ this->lArmCmd.margins[4] = 0;
+ 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.positions[0] = 0;
+ this->rArmCmd.positions[1] = 0;
+ this->rArmCmd.positions[2] = 0;
+ this->rArmCmd.positions[3] = 0;
+ this->rArmCmd.positions[4] = 0;
+ this->rArmCmd.positions[5] = 0;
+ this->rArmCmd.positions[6] = 0;
+
+ this->rArmCmd.margins[0] = 0;
+ this->rArmCmd.margins[1] = 0;
+ this->rArmCmd.margins[2] = 0;
+ this->rArmCmd.margins[3] = 0;
+ this->rArmCmd.margins[4] = 0;
+ this->rArmCmd.margins[5] = 0;
+ this->rArmCmd.margins[6] = 0;
+
+ advertise<pr2_controllers::JointPosCmd>("left_arm_commands");
+ advertise<pr2_controllers::JointPosCmd>("right_arm_commands");
+ advertise<generic_controllers::SingleJointPosCmd>("left_gripper_commands");
+ advertise<generic_controllers::SingleJointPosCmd>("right_gripper_commands");
+
+ // deal with grippers separately
+ this->lGripperCmd.position = 0;
+ this->rGripperCmd.position = 0;
+
}
~TArmK_Node() { }
void printCurrentJointValues() {
- std::cout << "Left joint angles:" << std::endl;
- std::cout << "turretAngle " << leftArmPosMsg.turretAngle << std::endl;
- std::cout << "shoulderLiftAngle " << leftArmPosMsg.shoulderLiftAngle << std::endl;
- std::cout << "upperarmRollAngle " << leftArmPosMsg.upperarmRollAngle << std::endl;
- std::cout << "elbowAngle " << leftArmPosMsg.elbowAngle << std::endl;
- std::cout << "forearmRollAngle " << leftArmPosMsg.forearmRollAngle << std::endl;
- std::cout << "wristPitchAngle " << leftArmPosMsg.wristPitchAngle << std::endl;
- std::cout << "wristRollAngle " << leftArmPosMsg.wristRollAngle << std::endl;
- std::cout << "gripperForceCmd " << leftArmPosMsg.gripperForceCmd << std::endl;
- std::cout << "gripperGapCmd " << leftArmPosMsg.gripperGapCmd << std::endl;
+ std::cout << "left arm " << std::endl;
+ std::cout << " cmds: "
+ << " " << this->lArmCmd.positions[0]
+ << " " << this->lArmCmd.positions[1]
+ << " " << this->lArmCmd.positions[2]
+ << " " << this->lArmCmd.positions[3]
+ << " " << this->lArmCmd.positions[4]
+ << " " << this->lArmCmd.positions[5]
+ << " " << this->lArmCmd.positions[6]
+ << " " << this->lGripperCmd.position
+ << std::endl;
+ std::cout << "right arm " << std::endl;
+ std::cout << " cmds: "
+ << " " << this->rArmCmd.positions[0]
+ << " " << this->rArmCmd.positions[1]
+ << " " << this->rArmCmd.positions[2]
+ << " " << this->rArmCmd.positions[3]
+ << " " << this->rArmCmd.positions[4]
+ << " " << this->rArmCmd.positions[5]
+ << " " << this->rArmCmd.positions[6]
+ << " " << this->rGripperCmd.position
+ << std::endl;
- std::cout << "Right joint angles:" << std::endl;
- std::cout << "turretAngle " << rightArmPosMsg.turretAngle << std::endl;
- std::cout << "shoulderLiftAngle " << rightArmPosMsg.shoulderLiftAngle << std::endl;
- std::cout << "upperarmRollAngle " << rightArmPosMsg.upperarmRollAngle << std::endl;
- std::cout << "elbowAngle " << rightArmPosMsg.elbowAngle << std::endl;
- std::cout << "forearmRollAngle " << rightArmPosMsg.forearmRollAngle << std::endl;
- std::cout << "wristPitchAngle " << rightArmPosMsg.wristPitchAngle << std::endl;
- std::cout << "wristRollAngle " << rightArmPosMsg.wristRollAngle << std::endl;
- std::cout << "gripperForceCmd " << rightArmPosMsg.gripperForceCmd << std::endl;
- std::cout << "gripperGapCmd " << rightArmPosMsg.gripperGapCmd << std::endl;
}
void printCurrentEndEffectorWorldCoord() {
@@ -224,45 +266,11 @@
std::cout << "In shoulder frame z " << inOdomFrame.z << std::endl;
}
- void leftArmPosReceived() {
- if(_leftInit == false) {
- this->cmd_leftarmconfig.turretAngle = leftArmPosMsg.turretAngle;
- this->cmd_leftarmconfig.shoulderLiftAngle = leftArmPosMsg.shoulderLiftAngle;
- this->cmd_leftarmconfig.upperarmRollAngle = leftArmPosMsg.upperarmRollAngle;
- this->cmd_leftarmconfig.elbowAngle = leftArmPosMsg.elbowAngle;
- this->cmd_leftarmconfig.forearmRollAngle = leftArmPosMsg.forearmRollAngle;
- this->cmd_leftarmconfig.wristPitchAngle = leftArmPosMsg.wristPitchAngle;
- this->cmd_leftarmconfig.wristRollAngle = leftArmPosMsg.wristRollAngle;
- this->cmd_leftarmconfig.gripperForceCmd = leftArmPosMsg.gripperForceCmd;
- this->cmd_leftarmconfig.gripperGapCmd = leftArmPosMsg.gripperGapCmd;
- _leftInit = true;
- }
- }
-
- void rightArmPosReceived() {
- if(_rightInit == false) {
- this->cmd_rightarmconfig.turretAngle = rightArmPosMsg.turretAngle;
- this->cmd_rightarmconfig.shoulderLiftAngle = rightArmPosMsg.shoulderLiftAngle;
- this->cmd_rightarmconfig.upperarmRollAngle = rightArmPosMsg.upperarmRollAngle;
- this->cmd_rightarmconfig.elbowAngle = rightArmPosMsg.elbowAngle;
- this->cmd_rightarmconfig.forearmRollAngle = rightArmPosMsg.forearmRollAngle;
- this->cmd_rightarmconfig.wristPitchAngle = rightArmPosMsg.wristPitchAngle;
- this->cmd_rightarmconfig.wristRollAngle = rightArmPosMsg.wristRollAngle;
- this->cmd_rightarmconfig.gripperForceCmd = rightArmPosMsg.gripperForceCmd;
- this->cmd_rightarmconfig.gripperGapCmd = rightArmPosMsg.gripperGapCmd;
- _rightInit = true;
- }
- }
-
void keyboardLoop();
void changeJointAngle(PR2_JOINT_ID jointID, bool increment);
void openGripper(PR2_JOINT_ID jointID);
void closeGripper(PR2_JOINT_ID jointID);
-
- bool _leftInit;
- bool _rightInit;
- std_msgs::PR2Arm leftArmPosMsg, rightArmPosMsg;
rosTFClient tf;
};
@@ -295,103 +303,88 @@
void TArmK_Node::openGripper(PR2_JOINT_ID jointID) {
if(jointID != ARM_R_GRIPPER_GAP && jointID != ARM_L_GRIPPER_GAP) return;
- if(_leftInit == false || _rightInit == false) {
- printf("No init, so not sending command.\n");
- return;
- }
if(jointID == ARM_R_GRIPPER_GAP) {
- this->cmd_rightarmconfig.gripperForceCmd = 50;
- this->cmd_rightarmconfig.gripperGapCmd = .2;
+ this->rGripperCmd.position = .2;
printf("Opening right gripper\n");
} else {
- this->cmd_leftarmconfig.gripperForceCmd = 50;
- this->cmd_leftarmconfig.gripperGapCmd = .2;
+ this->lGripperCmd.position = .2;
printf("Opening left gripper\n");
}
}
void TArmK_Node::closeGripper(PR2_JOINT_ID jointID) {
if(jointID != ARM_R_GRIPPER_GAP && jointID != ARM_L_GRIPPER_GAP) return;
- if(_leftInit == false || _rightInit == false) {
- printf("No init, so not sending command.\n");
- return;
- }
if(jointID == ARM_R_GRIPPER_GAP) {
- this->cmd_rightarmconfig.gripperForceCmd = 50;
- this->cmd_rightarmconfig.gripperGapCmd = 0;
+ this->rGripperCmd.position = 0;
} else {
- this->cmd_leftarmconfig.gripperForceCmd = 50;
- this->cmd_leftarmconfig.gripperGapCmd = 0;
+ this->lGripperCmd.position = 0;
}
}
void TArmK_Node::changeJointAngle(PR2_JOINT_ID jointID, bool increment)
{
- if(_leftInit == false || _rightInit == false) {
- printf("No init, so not sending command.\n");
- return;
- }
float jointCmdStep = 5*M_PI/180;
- float gripperStep = 0.002;
+ float gripperStep = 1*M_PI/180;
if (increment == false)
{
jointCmdStep *= -1;
gripperStep *= -1;
}
- this->cmd_leftarmconfig.gripperForceCmd = 10; // FIXME: why is this getting reset to 0?
- this->cmd_rightarmconfig.gripperForceCmd = 10; // FIXME: why is this getting reset to 0?
+ //this->lArmCmd.gripperForceCmd = 10; // FIXME: why is this getting reset to 0?
+ //this->rArmCmd.gripperForceCmd = 10; // FIXME: why is this getting reset to 0?
switch(jointID)
{
case ARM_L_PAN:
- this->cmd_leftarmconfig.turretAngle += jointCmdStep;
+ this->lArmCmd.positions[0] += jointCmdStep;
+ std::cout << " lp " << this->lArmCmd.positions[0] << std::endl;
break;
case ARM_L_SHOULDER_PITCH:
- this->cmd_leftarmconfig.shoulderLiftAngle += jointCmdStep;
+ this->lArmCmd.positions[1] += jointCmdStep;
break;
case ARM_L_SHOULDER_ROLL:
- this->cmd_leftarmconfig.upperarmRollAngle += jointCmdStep;
+ this->lArmCmd.positions[2] += jointCmdStep;
break;
case ARM_L_ELBOW_PITCH:
- this->cmd_leftarmconfig.elbowAngle += jointCmdStep;
+ this->lArmCmd.positions[3] += jointCmdStep;
break;
case ARM_L_ELBOW_ROLL:
- this->cmd_leftarmconfig.forearmRollAngle += jointCmdStep;
+ this->lArmCmd.positions[4] += jointCmdStep;
break;
case ARM_L_WRIST_PITCH:
- this->cmd_leftarmconfig.wristPitchAngle += jointCmdStep;
+ this->lArmCmd.positions[5] += jointCmdStep;
break;
case ARM_L_WRIST_ROLL:
- this->cmd_leftarmconfig.wristRollAngle += jointCmdStep;
+ this->lArmCmd.positions[6] += jointCmdStep;
break;
case ARM_L_GRIPPER_GAP:
- this->cmd_leftarmconfig.gripperGapCmd += gripperStep;
+ this->lGripperCmd.position += gripperStep;
break;
case ARM_R_PAN:
- this->cmd_rightarmconfig.turretAngle += jointCmdStep;
+ this->rArmCmd.positions[0] += jointCmdStep;
break;
case ARM_R_SHOULDER_PITCH:
- this->cmd_rightarmconfig.shoulderLiftAngle += jointCmdStep;
+ this->rArmCmd.positions[1] += jointCmdStep;
break;
case ARM_R_SHOULDER_ROLL:
- this->cmd_rightarmconfig.upperarmRollAngle += jointCmdStep;
+ this->rArmCmd.positions[2] += jointCmdStep;
break;
case ARM_R_ELBOW_PITCH:
- this->cmd_rightarmconfig.elbowAngle += jointCmdStep;
+ this->rArmCmd.positions[3] += jointCmdStep;
break;
case ARM_R_ELBOW_ROLL:
- this->cmd_rightarmconfig.forearmRollAngle += jointCmdStep;
+ this->rArmCmd.positions[4] += jointCmdStep;
break;
case ARM_R_WRIST_PITCH:
- this->cmd_rightarmconfig.wristPitchAngle += jointCmdStep;
+ this->rArmCmd.positions[5] += jointCmdStep;
break;
case ARM_R_WRIST_ROLL:
- this->cmd_rightarmconfig.wristRollAngle += jointCmdStep;
+ this->rArmCmd.positions[6] += jointCmdStep;
break;
case ARM_R_GRIPPER_GAP:
- this->cmd_rightarmconfig.gripperGapCmd += gripperStep;
+ this->rGripperCmd.position += gripperStep;
break;
default:
printf("This joint is not handled.\n");
@@ -457,14 +450,10 @@
dirty=true;
break;
case '.':
- _rightInit = false;
- _leftInit = false;
openGripper(curr_jointID);
dirty = true;
break;
case '/':
- _rightInit = false;
- _leftInit = false;
sleep(1);
closeGripper(curr_jointID);
dirty = true;
@@ -519,7 +508,6 @@
printf("left gripper\n");
break;
case '9':
- _leftInit = false;
printf("Resetting left commands to current position.\n");
default:
break;
@@ -562,7 +550,6 @@
printf("right gripper\n");
break;
case '9':
- _rightInit = false;
printf("Resetting right commands to current position.\n");
default:
break;
@@ -572,9 +559,11 @@
if (dirty == true) {
dirty=false; // Sending the command only once for each key press.
if(!right_arm) {
- publish("cmd_leftarmconfig",cmd_leftarmconfig);
+ publish("left_arm_commands",lArmCmd);
+ publish("left_gripper_commands",lGripperCmd);
} else {
- publish("cmd_rightarmconfig",cmd_rightarmconfig);
+ publish("right_arm_commands",rArmCmd);
+ publish("right_gripper_commands",rGripperCmd);
}
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-16 02:06:46
|
Revision: 4322
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4322&view=rev
Author: hsujohnhsu
Date: 2008-09-16 02:06:56 +0000 (Tue, 16 Sep 2008)
Log Message:
-----------
update gazebo ground truth frame names by appending _gazebo_ground_truth, and updating fake localization correspondingly.
Modified Paths:
--------------
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test_headless.xml
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2008-09-16 01:51:38 UTC (rev 4321)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2008-09-16 02:06:56 UTC (rev 4322)
@@ -54,7 +54,7 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "base_pose"/Pose3DEulerFloat32 : robot's odometric pose. Only the position information is used (velocity is ignored).
+- @b "base_pose_gazebo_ground_truth"/Pose3DEulerFloat32 : 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):
@@ -99,7 +99,7 @@
param("max_publish_frequency", m_maxPublishFrequency, 0.5);
- subscribe("base_pose", m_basePosMsg, &FakeOdomNode::basePosReceived);
+ subscribe("base_pose_gazebo_ground_truth", m_basePosMsg, &FakeOdomNode::basePosReceived);
subscribe("initialpose", m_iniPos, &FakeOdomNode::initialPoseReceived);
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml 2008-09-16 01:51:38 UTC (rev 4321)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml 2008-09-16 02:06:56 UTC (rev 4322)
@@ -64,7 +64,7 @@
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
<bodyName>base</bodyName>
- <topicName>base_pose</topicName>
+ <topicName>base_pose_gazebo_ground_truth</topicName>
<frameName>base_frame</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml 2008-09-16 01:51:38 UTC (rev 4321)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml 2008-09-16 02:06:56 UTC (rev 4322)
@@ -71,9 +71,9 @@
<controller:P3D name="p3d_base_controller" plugin="libP3D.so">
<alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
+ <updateRate>1000.0</updateRate>
<bodyName>base</bodyName>
- <topicName>base_pose</topicName>
+ <topicName>base_pose_gazebo_ground_truth</topicName>
<frameName>base_frame</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test_headless.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test_headless.xml 2008-09-16 01:51:38 UTC (rev 4321)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test_headless.xml 2008-09-16 02:06:56 UTC (rev 4322)
@@ -71,9 +71,9 @@
<controller:P3D name="p3d_base_controller" plugin="libP3D.so">
<alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
+ <updateRate>1000.0</updateRate>
<bodyName>base</bodyName>
- <topicName>base_pose</topicName>
+ <topicName>base_pose_gazebo_ground_truth</topicName>
<frameName>base_frame</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mm...@us...> - 2008-09-16 21:02:03
|
Revision: 4347
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4347&view=rev
Author: mmwise
Date: 2008-09-16 21:02:13 +0000 (Tue, 16 Sep 2008)
Log Message:
-----------
updating test_controllers
Modified Paths:
--------------
pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/CMakeLists.txt
pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/manifest.xml
pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/src/motor_test1.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml
Modified: pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/CMakeLists.txt 2008-09-16 21:01:42 UTC (rev 4346)
+++ pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/CMakeLists.txt 2008-09-16 21:02:13 UTC (rev 4347)
@@ -3,4 +3,4 @@
rospack(motor_qualification_controllers)
genmsg()
gensrv()
-rospack_add_library(motor_qualification_controllers src/motor_test1.cpp)
+rospack_add_library(motor_qualification_controllers src/motor_test1.cpp src/motor_test2.cpp src/motor_test3.cpp)
Modified: pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/manifest.xml 2008-09-16 21:01:42 UTC (rev 4346)
+++ pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/manifest.xml 2008-09-16 21:02:13 UTC (rev 4347)
@@ -2,11 +2,12 @@
<description brief='Motor Qualification Controllers'>
Library for doing testing controllers and methods.
</description>
- <author> David Li, Melonee Wise</author>
+ <author> Melonee Wise</author>
<license>BSD</license>
<depend package="mechanism_model" />
<depend package="wg_robot_description_parser" />
<depend package="stl_utils" />
+ <depend package="newmat10" />
<depend package="misc_utils" />
<depend package="robot_msgs" />
<depend package="generic_controllers" />
Modified: pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/src/motor_test1.cpp
===================================================================
--- pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/src/motor_test1.cpp 2008-09-16 21:01:42 UTC (rev 4346)
+++ pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/src/motor_test1.cpp 2008-09-16 21:02:13 UTC (rev 4347)
@@ -140,9 +140,12 @@
{
double f_delta = fixture_joint_end_pos_-fixture_joint_start_pos_;
double t_delta = test_joint_end_pos_-test_joint_start_pos_;
+ double error = fabs(fabs(f_delta)-fabs(t_delta))/((fabs(f_delta)+fabs(t_delta))/2);
+ printf("f: %f , t: %f\n", f_delta, t_delta);
diagnostic_message_.set_status_size(1);
robot_msgs::DiagnosticStatus *status = diagnostic_message_.status;
status->name = "MotorTest";
+ printf("error: %f\n", error);
if (f_delta==0 && t_delta==0)
{
//the motor isn't moving
@@ -156,7 +159,7 @@
status->message = "ERROR: The motor encoder is not attached or not powered.";
}
- else if(f_delta+1<t_delta || f_delta-1>t_delta)
+ else if(error<0.005)
{
//the encoder is slipping
status->level = 2;
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml 2008-09-16 21:01:42 UTC (rev 4346)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml 2008-09-16 21:02:13 UTC (rev 4347)
@@ -9,6 +9,9 @@
<depend package="ethercat_hardware"/>
<depend package="hardware_interface"/>
<depend package="std_srvs"/>
+ <depend package="generic_controllers"/>
+ <depend package="pr2_controllers"/>
+ <depend package="motor_qualification_controllers"/>
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-16 22:55:45
|
Revision: 4360
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4360&view=rev
Author: hsujohnhsu
Date: 2008-09-16 22:55:55 +0000 (Tue, 16 Sep 2008)
Log Message:
-----------
updated gazebo_actuators to include physical damping.
changed base tag for controllers from <robot name="pr2"> to <controllers> due to requirement from mech.py definition for spawning from xml files.
updated test_actuators to reflect base tag change to <controllers> for loading controllers.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/controllers_arm_test.xml
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h 2008-09-16 22:50:42 UTC (rev 4359)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h 2008-09-16 22:55:55 UTC (rev 4360)
@@ -30,6 +30,7 @@
#define GAZEBO_ACTUATORS_H
#include <vector>
+#include <map>
#include <gazebo/Controller.hh>
#include <gazebo/Entity.hh>
#include <gazebo/Model.hh>
@@ -72,7 +73,21 @@
// actuator values.
// TODO mechanism::Robot fake_model_;
mechanism::RobotState *fake_state_;
- std::vector<gazebo::Joint*> joints_;
+ std::vector<gazebo::Joint*> joints_;
+
+ // added for joint damping coefficients
+ std::vector<double> joints_damping_;
+ std::map<std::string,double> joints_damping_map_;
+
+ /*
+ * \brief read pr2.xml for actuators, and pass tinyxml node to mechanism control node's initXml.
+ */
+ void ReadPr2Xml(XMLConfigNode *node);
+
+ /*
+ * \brief read gazebo_joints.xml for joint damping and additional simulation parameters for joints
+ */
+ void ReadGazeboPhysics(XMLConfigNode *node);
};
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp 2008-09-16 22:50:42 UTC (rev 4359)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp 2008-09-16 22:55:55 UTC (rev 4360)
@@ -44,6 +44,7 @@
#include <gazebo/GazeboError.hh>
#include <gazebo/ControllerFactory.hh>
#include <urdf/parser.h>
+#include <map>
namespace gazebo {
@@ -66,62 +67,45 @@
void GazeboActuators::LoadChild(XMLConfigNode *node)
{
- XMLConfigNode *robot = node->GetChild("robot");
- if (!robot)
- {
- fprintf(stderr, "Error loading gazebo_actuators config: no robot element\n");
- return;
- }
+ // read pr2.xml (pr2_gazebo_actuators.xml)
+ ReadPr2Xml(node);
- std::string filename = robot->GetFilename("filename", "", 1);
- printf("Loading %s\n", filename.c_str());
-
- TiXmlDocument doc(filename);
- if (!doc.LoadFile())
- {
- fprintf(stderr, "Error: Could not load the gazebo actuators plugin's configuration file: %s\n",
- filename.c_str());
- abort();
- }
- urdf::normalizeXml(doc.RootElement());
-
- // Pulls out the list of actuators used in the robot configuration.
- struct GetActuators : public TiXmlVisitor
- {
- std::set<std::string> actuators;
- virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *)
- {
- if (elt.ValueStr() == std::string("actuator") && elt.Attribute("name"))
- actuators.insert(elt.Attribute("name"));
- return true;
- }
- } get_actuators;
- doc.RootElement()->Accept(&get_actuators);
-
- // Places the found actuators into the hardware interface.
- std::set<std::string>::iterator it;
- for (it = get_actuators.actuators.begin(); it != get_actuators.actuators.end(); ++it)
- {
- hw_.actuators_.push_back(new Actuator(*it));
- }
-
- mcn_.initXml(doc.RootElement());
-
// Initializes the fake state (for running the transmissions backwards).
fake_state_ = new mechanism::RobotState(&mc_.model_, &hw_);
+ // Get gazebo joint properties such as explicit damping coefficient, simulation specific.
+ // Currently constructs a map of joint-name/damping-value pairs.
+ ReadGazeboPhysics(node);
+
// The gazebo joints and mechanism joints should match up.
for (unsigned int i = 0; i < mc_.model_.joints_.size(); ++i)
{
std::string joint_name = mc_.model_.joints_[i]->name_;
+
+ // fill in gazebo joints pointer
gazebo::Joint *joint = parent_model_->GetJoint(joint_name);
if (joint)
+ {
joints_.push_back(joint);
+ }
else
{
fprintf(stderr, "Gazebo does not know about a joint named \"%s\"\n", joint_name.c_str());
joints_.push_back(NULL);
}
+
+ // fill in gazebo joints / damping value pairs
+ std::map<std::string,double>::iterator jt = joints_damping_map_.find(joint_name);
+ if (jt!=joints_damping_map_.end())
+ {
+ joints_damping_.push_back(jt->second);
+ std::cout << "adding gazebo joint name (" << joint_name << ") with damping=" << jt->second << std::endl;
+ }
+ else
+ {
+ joints_damping_.push_back(0); // no damping
+ std::cout << "adding gazebo joint name (" << joint_name << ") with no damping." << std::endl;
+ }
}
hw_.current_time_ = Simulator::Instance()->GetSimTime();
@@ -135,6 +119,7 @@
void GazeboActuators::UpdateChild()
{
assert(joints_.size() == fake_state_->joint_states_.size());
+ assert(joints_.size() == joints_damping_.size());
//--------------------------------------------------
// Pushes out simulation state
@@ -189,14 +174,17 @@
if (!joints_[i])
continue;
+ double damping_force;
double effort = fake_state_->joint_states_[i].commanded_effort_;
switch (joints_[i]->GetType())
{
case Joint::HINGE:
- ((HingeJoint*)joints_[i])->SetTorque(effort);
+ damping_force = joints_damping_[i] * ((HingeJoint*)joints_[i])->GetAngleRate();
+ ((HingeJoint*)joints_[i])->SetTorque(effort - damping_force);
break;
case Joint::SLIDER:
- ((SliderJoint*)joints_[i])->SetSliderForce(effort);
+ damping_force = joints_damping_[i] * ((SliderJoint*)joints_[i])->GetPositionRate();
+ ((SliderJoint*)joints_[i])->SetSliderForce(effort - damping_force);
break;
default:
abort();
@@ -209,4 +197,100 @@
}
+void GazeboActuators::ReadPr2Xml(XMLConfigNode *node)
+{
+ XMLConfigNode *robot = node->GetChild("robot");
+ if (!robot)
+ {
+ fprintf(stderr, "Error loading gazebo_actuators config: no robot element\n");
+ return;
+ }
+
+ std::string filename = robot->GetFilename("filename", "", 1);
+ printf("Loading %s\n", filename.c_str());
+
+ TiXmlDocument doc(filename);
+ if (!doc.LoadFile())
+ {
+ fprintf(stderr, "Error: Could not load the gazebo actuators plugin's configuration file: %s\n",
+ filename.c_str());
+ abort();
+ }
+ urdf::normalizeXml(doc.RootElement());
+
+ // Pulls out the list of actuators used in the robot configuration.
+ struct GetActuators : public TiXmlVisitor
+ {
+ std::set<std::string> actuators;
+ virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *)
+ {
+ if (elt.ValueStr() == std::string("actuator") && elt.Attribute("name"))
+ actuators.insert(elt.Attribute("name"));
+ return true;
+ }
+ } get_actuators;
+ doc.RootElement()->Accept(&get_actuators);
+
+ // Places the found actuators into the hardware interface.
+ std::set<std::string>::iterator it;
+ for (it = get_actuators.actuators.begin(); it != get_actuators.actuators.end(); ++it)
+ {
+ hw_.actuators_.push_back(new Actuator(*it));
+ }
+
+ mcn_.initXml(doc.RootElement());
+}
+
+void GazeboActuators::ReadGazeboPhysics(XMLConfigNode *node)
+{
+ XMLConfigNode *robot = node->GetChild("gazebo_physics");
+ if (!robot)
+ {
+ fprintf(stderr, "Error loading gazebo_physics config: no robot element\n");
+ return;
+ }
+
+ std::string filename = robot->GetFilename("filename", "", 1);
+ printf("Loading %s\n", filename.c_str());
+
+ TiXmlDocument doc(filename);
+ if (!doc.LoadFile())
+ {
+ fprintf(stderr, "Error: Could not load the gazebo actuators plugin's configuration file for gazebo physics: %s\n",
+ filename.c_str());
+ abort();
+ }
+ // Pulls out the list of joints used in the gazebo physics configuration.
+ struct GetActuators : public TiXmlVisitor
+ {
+ std::map<const char*,double> joints;
+ virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *)
+ {
+ if (elt.ValueStr() == std::string("joint") && elt.Attribute("name"))
+ {
+ double damp;
+ //extract damping coefficient value
+ if (elt.FirstChildElement("explicitDampingCoefficient"))
+ //std::cout << "damp : " << elt.FirstChildElement("explicitDampingCoefficient")->GetText() << std::endl;
+ damp = atof(elt.FirstChildElement("explicitDampingCoefficient")->GetText());
+ else
+ damp = 0;
+
+ //std::cout << "inserting pair to map " << elt.Attribute("name") << " " << damp << std::endl;
+ joints.insert(make_pair(elt.Attribute("name"),damp));
+ }
+ return true;
+ }
+ } get_joints;
+ doc.RootElement()->Accept(&get_joints);
+
+ // Copy the found joint/damping pairs into the class variable
+ std::map<const char*,double>::iterator it;
+ for (it = get_joints.joints.begin(); it != get_joints.joints.end(); ++it)
+ {
+ std::string *jn = new std::string((it->first));
+ joints_damping_map_.insert(make_pair(*jn,it->second));
+ }
+
+}
} // namespace gazebo
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-09-16 22:50:42 UTC (rev 4359)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-09-16 22:55:55 UTC (rev 4360)
@@ -287,7 +287,7 @@
//-----------------------------------------------------------------------------------------
// make mc parse xml for controllers
std::cout << " Loading controllers : " << std::endl;
- for (TiXmlElement *xit = controller_xml->FirstChildElement("robot"); xit ; xit = xit->NextSiblingElement("robot") )
+ for (TiXmlElement *xit = controller_xml->FirstChildElement("controllers"); xit ; xit = xit->NextSiblingElement("robot") )
for (TiXmlElement *zit = xit->FirstChildElement("controller"); zit ; zit = zit->NextSiblingElement("controller") )
{
std::string* controller_name = new std::string(zit->Attribute("name"));
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-09-16 22:50:42 UTC (rev 4359)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-09-16 22:55:55 UTC (rev 4360)
@@ -1,8 +1,7 @@
<?xml version="1.0"?>
-<robot name="pr2"><!-- name of the robot-->
+<controllers>
-
<controller name="base_controller" topic="base_controller" type="BaseControllerNode">
<map name="velocity_control" flag="base_control">
<elem key="kp_speed">10.0</elem>
@@ -217,4 +216,4 @@
</position>
</controller>
-->
-</robot>
+</controllers>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/controllers_arm_test.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/controllers_arm_test.xml 2008-09-16 22:50:42 UTC (rev 4359)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/controllers_arm_test.xml 2008-09-16 22:55:55 UTC (rev 4360)
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
-<robot name="pr2"><!-- name of the robot-->
+<controllers>
<!-- ========================================= -->
@@ -56,4 +56,4 @@
<gripper_defaults effortLimit="100" velocityLimit="10" />
</joint>
</controller>
-</robot>
+</controllers>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-17 00:17:26
|
Revision: 4366
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4366&view=rev
Author: hsujohnhsu
Date: 2008-09-17 00:17:37 +0000 (Wed, 17 Sep 2008)
Log Message:
-----------
modify laser scanner controller to take a default value.
modify startup script for gazebo_actuators in simulation to initialize laser scanner controller.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_controllers/src/laser_scanner_controller.cpp
pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators_headless.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators_wg.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators_wg_headless.xml
Modified: pkg/trunk/controllers/pr2_controllers/src/laser_scanner_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/src/laser_scanner_controller.cpp 2008-09-17 00:13:55 UTC (rev 4365)
+++ pkg/trunk/controllers/pr2_controllers/src/laser_scanner_controller.cpp 2008-09-17 00:17:37 UTC (rev 4366)
@@ -427,6 +427,7 @@
else if(req.command==43)c_->setSawtoothProfile(2,0.5,100,0.5);
else if(req.command==44)c_->setSawtoothProfile(0.5,0.5,100,0);
else if(req.command==45)c_->setSawtoothProfile(1,0.5,0);
+ else if(req.command==46)c_->setSawtoothProfile(20,0.872,100,0.3475); // JMH- latest running numbers from David with proper range considered
else if(req.command==-41)c_->setSinewaveProfile(2,0.5,100,0.5);
else if(req.command==-42)c_->setSinewaveProfile(2,0.5,100,0);
else if(req.command==-43)c_->setSinewaveProfile(1,0.5,100,0);
@@ -493,6 +494,7 @@
c_->setSinewaveProfile(period, amplitude,offset);
break;
case LaserScannerController::SAWTOOTH:
+ std::cout << " setting sawtooth with these numbers " << period << " " << amplitude << " " << num_elements << " " << offset << std::endl;
c_->setSawtoothProfile(period, amplitude, num_elements, offset);
break;
case LaserScannerController::DYNAMIC_SAWTOOTH:
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators.xml 2008-09-17 00:13:55 UTC (rev 4365)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators.xml 2008-09-17 00:17:37 UTC (rev 4366)
@@ -8,6 +8,7 @@
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2/controllers.xml" respawn="false" output="screen" />
+ <node pkg="generic_controllers" type="controllers.py" args="set tilt_laser_controller 46" respawn="false" output="screen" />
</group>
</launch>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators_headless.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators_headless.xml 2008-09-17 00:13:55 UTC (rev 4365)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators_headless.xml 2008-09-17 00:17:37 UTC (rev 4366)
@@ -8,6 +8,7 @@
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2/controllers.xml" respawn="false" output="screen" />
+ <node pkg="generic_controllers" type="controllers.py" args="set tilt_laser_controller 46" respawn="false" output="screen" />
</group>
</launch>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators_wg.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators_wg.xml 2008-09-17 00:13:55 UTC (rev 4365)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators_wg.xml 2008-09-17 00:17:37 UTC (rev 4366)
@@ -8,6 +8,8 @@
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2/controllers.xml" respawn="false" output="screen" />
+ <node pkg="generic_controllers" type="controllers.py" args="set tilt_laser_controller 46" respawn="false" output="screen" />
</group>
</launch>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators_wg_headless.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators_wg_headless.xml 2008-09-17 00:13:55 UTC (rev 4365)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators_wg_headless.xml 2008-09-17 00:17:37 UTC (rev 4366)
@@ -8,6 +8,8 @@
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2/controllers.xml" respawn="false" output="screen" />
+ <node pkg="generic_controllers" type="controllers.py" args="set tilt_laser_controller 46" respawn="false" output="screen" />
</group>
</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-17 02:24:56
|
Revision: 4375
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4375&view=rev
Author: hsujohnhsu
Date: 2008-09-17 02:25:05 +0000 (Wed, 17 Sep 2008)
Log Message:
-----------
update controllers to use shortest_angular_distance for steering angle.
update gains in controllers.xml for slightly improved steering.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
Modified: pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-09-17 02:13:25 UTC (rev 4374)
+++ pkg/trunk/controllers/pr2_controllers/src/base_controller.cpp 2008-09-17 02:25:05 UTC (rev 4375)
@@ -298,11 +298,11 @@
{
result = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_);
steer_angle_desired = atan2(result.y,result.x);
-// steer_angle_desired = modNPiBy2(steer_angle_desired);//Clean steer Angle
+ // steer_angle_desired = modNPiBy2(steer_angle_desired);//Clean steer Angle
- error_steer = steer_angle_actual_[i] - steer_angle_desired;
+ error_steer = shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
steer_velocity_desired_[i] = -kp_speed_*error_steer;
-// std::cout << "setting steering velocity " << i << " : " << steer_velocity_desired_[i] << " kp: " << kp_speed_ << "error_steer" << error_steer << std::endl;
+ // std::cout << "setting steering velocity " << i << " : " << steer_velocity_desired_[i] << " kp: " << kp_speed_ << "error_steer" << error_steer << std::endl;
base_casters_[i].controller_.setCommand(steer_velocity_desired_[i]);
}
}
@@ -327,13 +327,13 @@
steer_angle_actual = base_wheels_[i].parent_->joint_state_->position_;
wheel_point_velocity = computePointVelocity2D(base_wheels_position_[i],cmd_vel_);
-// cout << "wheel_point_velocity" << wheel_point_velocity << ",pos::" << base_wheels_position_[i] << ",cmd::" << cmd_vel_ << endl;
+ // cout << "wheel_point_velocity" << wheel_point_velocity << ",pos::" << base_wheels_position_[i] << ",cmd::" << cmd_vel_ << endl;
wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
-// wheel_point_velocity_projected = rotate2D(wheel_point_velocity,-steer_angle_actual);
+ // wheel_point_velocity_projected = rotate2D(wheel_point_velocity,-steer_angle_actual);
wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
wheel_speed_cmd = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/wheel_radius_;
-// std::cout << "setting wheel speed " << i << " : " << wheel_speed_cmd << " r:" << wheel_radius_ << std::endl;
+ // std::cout << "setting wheel speed " << i << " : " << wheel_speed_cmd << " r:" << wheel_radius_ << std::endl;
base_wheels_[i].controller_.setCommand(base_wheels_[i].direction_multiplier_*wheel_speed_cmd);
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-09-17 02:13:25 UTC (rev 4374)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-09-17 02:25:05 UTC (rev 4375)
@@ -8,7 +8,7 @@
</map>
<controller name="caster_front_left_controller" topic="caster_front_left_controller" type="JointVelocityControllerNode">
<joint name="caster_front_left_joint" >
- <pid p="1" d="0" i="0" iClamp="0" />
+ <pid p="3" d="0" i="0.001" iClamp="0.01" />
</joint>
</controller>
<controller name="wheel_front_left_l_controller" topic="wheel_front_left_l_controller" type="JointVelocityControllerNode">
@@ -23,7 +23,7 @@
</controller>
<controller name="caster_front_right_controller" topic="caster_front_right_controller" type="JointVelocityControllerNode">
<joint name="caster_front_right_joint" >
- <pid p="1" d="0" i="0" iClamp="0" />
+ <pid p="3" d="0" i="0.001" iClamp="0.01" />
</joint>
</controller>
<controller name="wheel_front_right_l_controller" topic="wheel_front_right_l_controller" type="JointVelocityControllerNode">
@@ -38,7 +38,7 @@
</controller>
<controller name="caster_rear_left_controller" topic="caster_rear_left_controller" type="JointVelocityControllerNode">
<joint name="caster_rear_left_joint" >
- <pid p="1" d="0" i="0" iClamp="0" />
+ <pid p="3" d="0" i="0.001" iClamp="0.01" />
</joint>
</controller>
<controller name="wheel_rear_left_l_controller" topic="wheel_rear_left_l_controller" type="JointVelocityControllerNode">
@@ -53,7 +53,7 @@
</controller>
<controller name="caster_rear_right_controller" topic="caster_rear_right_controller" type="JointVelocityControllerNode">
<joint name="caster_rear_right_joint" >
- <pid p="1" d="0" i="0" iClamp="0" />
+ <pid p="3" d="0" i="0.001" iClamp="0.01" />
</joint>
</controller>
<controller name="wheel_rear_right_l_controller" topic="wheel_rear_right_l_controller" type="JointVelocityControllerNode">
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-09-17 21:26:53
|
Revision: 4398
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4398&view=rev
Author: rob_wheeler
Date: 2008-09-17 21:26:58 +0000 (Wed, 17 Sep 2008)
Log Message:
-----------
Change value_label to label
Modified Paths:
--------------
pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/src/motor_test2.cpp
pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/src/motor_test4.cpp
pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
pkg/trunk/drivers/imu/imu_node/imu_node.cc
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
pkg/trunk/hardware_test/qualification/src/qualification/base_test.py
pkg/trunk/hardware_test/runtime_monitor/monitor
pkg/trunk/hardware_test/runtime_monitor/wxmonitor
pkg/trunk/hardware_test/self_test/src/run_selftest.cpp
pkg/trunk/robot_msgs/msg/DiagnosticValue.msg
Modified: pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/src/motor_test2.cpp
===================================================================
--- pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/src/motor_test2.cpp 2008-09-17 21:26:22 UTC (rev 4397)
+++ pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/src/motor_test2.cpp 2008-09-17 21:26:58 UTC (rev 4398)
@@ -171,9 +171,9 @@
//the motor isn't moving
status->level = 2;
status->message = "ERROR: The motor is not correctly labeled. The speed constant is not correct.";
- status->values[0].value_label = "measured speed constant";
+ status->values[0].label = "measured speed constant";
status->values[0].value = speed_const_meas;
- status->values[1].value_label = "expected speed constant";
+ status->values[1].label = "expected speed constant";
status->values[1].value = speed_constant_;
}
else
@@ -181,9 +181,9 @@
//test passed
status->level = 0;
status->message = "OK: Passed.";
- status->values[0].value_label = "measured speed constant";
+ status->values[0].label = "measured speed constant";
status->values[0].value = speed_const_meas;
- status->values[1].value_label = "expected speed constant";
+ status->values[1].label = "expected speed constant";
status->values[1].value = speed_constant_;
}
publisher_.publish(diagnostic_message_);
Modified: pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/src/motor_test4.cpp
===================================================================
--- pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/src/motor_test4.cpp 2008-09-17 21:26:22 UTC (rev 4397)
+++ pkg/trunk/controllers/testing_controllers/motor_qualification_controllers/src/motor_test4.cpp 2008-09-17 21:26:58 UTC (rev 4398)
@@ -159,9 +159,9 @@
//the motor isn't moving
status->level = 2;
status->message = "ERROR: The motor is not correctly labeled. The speed constant is not correct.";
- status->values[0].value_label = "measured speed constant";
+ status->values[0].label = "measured speed constant";
status->values[0].value = speed_torque_const_meas;
- status->values[1].value_label = "expected speed constant";
+ status->values[1].label = "expected speed constant";
status->values[1].value = speed_torque_constant_;
}
else
@@ -169,9 +169,9 @@
//test passed
status->level = 0;
status->message = "OK: Passed.";
- status->values[0].value_label = "measured speed constant";
+ status->values[0].label = "measured speed constant";
status->values[0].value = speed_torque_const_meas;
- status->values[1].value_label = "expected speed constant";
+ status->values[1].label = "expected speed constant";
status->values[1].value = speed_torque_constant_;
}
publisher_.publish(diagnostic_message_);
Modified: pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp 2008-09-17 21:26:22 UTC (rev 4397)
+++ pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp 2008-09-17 21:26:58 UTC (rev 4398)
@@ -164,9 +164,9 @@
status.level = 0;
status.message = "Retrieved image successfully.";
status.set_values_size(2);
- status.values[0].value_label = "Width";
+ status.values[0].label = "Width";
status.values[0].value = image.width;
- status.values[1].value_label = "Height";
+ status.values[1].label = "Height";
status.values[1].value = image.height;
}
}
Modified: pkg/trunk/drivers/imu/imu_node/imu_node.cc
===================================================================
--- pkg/trunk/drivers/imu/imu_node/imu_node.cc 2008-09-17 21:26:22 UTC (rev 4397)
+++ pkg/trunk/drivers/imu/imu_node/imu_node.cc 2008-09-17 21:26:58 UTC (rev 4398)
@@ -328,12 +328,12 @@
status.message = "Successfully calculated gyro biases.";
status.set_values_size(3);
- status.values[0].value_label = "Bias_X";
- status.values[0].value = bias_x;
- status.values[1].value_label = "Bias_Y";
- status.values[1].value = bias_y;
- status.values[2].value_label = "Bias_Z";
- status.values[2].value = bias_z;
+ status.values[0].label = "Bias_X";
+ status.values[0].value = bias_x;
+ status.values[1].label = "Bias_Y";
+ status.values[1].value = bias_y;
+ status.values[2].label = "Bias_Z";
+ status.values[2].value = bias_z;
}
@@ -417,10 +417,10 @@
}
status.set_values_size(2);
- status.values[0].value_label = "Measured gravity";
- status.values[0].value = grav;
- status.values[1].value_label = "Gravity error";
- status.values[1].value = err;
+ status.values[0].label = "Measured gravity";
+ status.values[0].value = grav;
+ status.values[1].label = "Gravity error";
+ status.values[1].value = err;
}
}
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-09-17 21:26:22 UTC (rev 4397)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-09-17 21:26:58 UTC (rev 4398)
@@ -202,7 +202,7 @@
// Num devices
v.value = num_slaves_;
- v.value_label = "EtherCAT devices";
+ v.label = "EtherCAT devices";
values.push_back(v);
// Interface
@@ -218,11 +218,11 @@
diagnostics_.max_roundtrip_ = max(diagnostics_.max_roundtrip_, diagnostics_.iteration_[i].roundtrip_);
}
v.value = total / 1000.0;
- v.value_label = "Average roundtrip time";
+ v.label = "Average roundtrip time";
values.push_back(v);
v.value = diagnostics_.max_roundtrip_;
- v.value_label = "Maximum roundtrip time";
+ v.label = "Maximum roundtrip time";
values.push_back(v);
status.set_values_vec(values);
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-09-17 21:26:22 UTC (rev 4397)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-09-17 21:26:58 UTC (rev 4398)
@@ -688,7 +688,7 @@
s.value = (val); \
strings.push_back(s)
#define ADD_VALUE(lab, val) \
- v.value_label = (lab); \
+ v.label = (lab); \
v.value = (val); \
values.push_back(v)
void WG0X::diagnostics(robot_msgs::DiagnosticStatus &d)
Modified: pkg/trunk/hardware_test/qualification/src/qualification/base_test.py
===================================================================
--- pkg/trunk/hardware_test/qualification/src/qualification/base_test.py 2008-09-17 21:26:22 UTC (rev 4397)
+++ pkg/trunk/hardware_test/qualification/src/qualification/base_test.py 2008-09-17 21:26:58 UTC (rev 4398)
@@ -194,7 +194,7 @@
res += 'Test %2d) %s\n' % (i, stat.name)
res += ' [%s]: %s\n' % (statdict[stat.level], stat.message)
for val in stat.values:
- res += ' [%s] = %f\n' % (val.value_label, val.value)
+ res += ' [%s] = %f\n' % (val.label, val.value)
i += 1
head = '----------------------------------------\n'
Modified: pkg/trunk/hardware_test/runtime_monitor/monitor
===================================================================
--- pkg/trunk/hardware_test/runtime_monitor/monitor 2008-09-17 21:26:22 UTC (rev 4397)
+++ pkg/trunk/hardware_test/runtime_monitor/monitor 2008-09-17 21:26:58 UTC (rev 4398)
@@ -50,7 +50,7 @@
## @TODO process byte level
print "Name: %s \nMessage: %s"%(s.name, s.message)
for v in s.values:
- print " Value: %.2f Label: %s"%(v.value, v.value_label)
+ print " Value: %.2f Label: %s"%(v.value, v.label)
def listener():
rospy.TopicSub("/diagnostics", DiagnosticMessage, callback)
Modified: pkg/trunk/hardware_test/runtime_monitor/wxmonitor
===================================================================
--- pkg/trunk/hardware_test/runtime_monitor/wxmonitor 2008-09-17 21:26:22 UTC (rev 4397)
+++ pkg/trunk/hardware_test/runtime_monitor/wxmonitor 2008-09-17 21:26:58 UTC (rev 4398)
@@ -123,7 +123,7 @@
for v in self.components[self.button_id_dict[e.GetId()]].strings:
values_str = values_str + "%s: %s\n"%(v.label, v.value)
for v in self.components[self.button_id_dict[e.GetId()]].values:
- values_str = values_str + "%s: %f\n"%(v.value_label, v.value)
+ values_str = values_str + "%s: %f\n"%(v.label, v.value)
d = wx.MessageDialog(self, "%s\n%s\n%s"%(self.components[self.button_id_dict[e.GetId()]].name, self.components[self.button_id_dict[e.GetId()]].message, values_str),"%s"%self.components[self.button_id_dict[e.GetId()]].name, wx.OK)
Modified: pkg/trunk/hardware_test/self_test/src/run_selftest.cpp
===================================================================
--- pkg/trunk/hardware_test/self_test/src/run_selftest.cpp 2008-09-17 21:26:22 UTC (rev 4397)
+++ pkg/trunk/hardware_test/self_test/src/run_selftest.cpp 2008-09-17 21:26:58 UTC (rev 4398)
@@ -73,7 +73,7 @@
printf("%s\n", res.status[i].message.c_str());
for (size_t j = 0; j < res.status[i].get_values_size(); j++)
- printf(" [%s] %f\n", res.status[i].values[j].value_label.c_str(), res.status[i].values[j].value);
+ printf(" [%s] %f\n", res.status[i].values[j].label.c_str(), res.status[i].values[j].value);
printf("\n");
}
Modified: pkg/trunk/robot_msgs/msg/DiagnosticValue.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/DiagnosticValue.msg 2008-09-17 21:26:22 UTC (rev 4397)
+++ pkg/trunk/robot_msgs/msg/DiagnosticValue.msg 2008-09-17 21:26:58 UTC (rev 4398)
@@ -1,2 +1,2 @@
float32 value # a value to track over time
-string value_label # what to label this value when viewing
+string label # what to label this value when viewing
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2008-09-18 00:59:21
|
Revision: 4418
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4418&view=rev
Author: jfaustwg
Date: 2008-09-18 00:59:31 +0000 (Thu, 18 Sep 2008)
Log Message:
-----------
Change the camera panel to take a name instead of the 3 subscription topics
Also part of this change is updating the camera panel (mostly) to the coding standard
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless.xml
pkg/trunk/visualization/wx_camera_panel/camera_panels.fbp
pkg/trunk/visualization/wx_camera_panel/scripts/standalone_camera.py
pkg/trunk/visualization/wx_camera_panel/src/test/test.cpp
pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp
pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.h
pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanelsGenerated.cpp
pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanelsGenerated.h
pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraSetupDialog.cpp
pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraSetupDialog.h
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless.xml 2008-09-18 00:34:43 UTC (rev 4417)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless.xml 2008-09-18 00:59:31 UTC (rev 4418)
@@ -8,9 +8,9 @@
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
<!-- for visualization -->
- <node pkg="wx_camera_panel" type="standalone_camera.py" args="-i axis_left/image -s axis_left/ptz_state -c axis_left/ptz_cmd" respawn="true" output="screen" />
- <node pkg="wx_camera_panel" type="standalone_camera.py" args="-i axis_right/image -s axis_right/ptz_state -c axis_right/ptz_cmd" respawn="true" output="screen" />
- <node pkg="ogre_visualizer" type="visualizer_test" respawn="true" output="screen" />
+ <node pkg="wx_camera_panel" type="standalone_camera.py" args="axis_left --ptz" respawn="true" output="screen" />
+ <node pkg="wx_camera_panel" type="standalone_camera.py" args="axis_right --ptz" respawn="true" output="screen" />
+ <node pkg="ogre_visualizer" type="standalone_visualizer.py" respawn="true" output="screen" />
<node pkg="laser_view" type="laser_view" args="scan:=tilt_scan" respawn="true" output="screen" />
<!--node pkg="wxpy_ros" type="ros_panel" respawn="false" output="screen" /--><!-- does not start correctly-->
Modified: pkg/trunk/visualization/wx_camera_panel/camera_panels.fbp
===================================================================
--- pkg/trunk/visualization/wx_camera_panel/camera_panels.fbp 2008-09-18 00:34:43 UTC (rev 4417)
+++ pkg/trunk/visualization/wx_camera_panel/camera_panels.fbp 2008-09-18 00:59:31 UTC (rev 4418)
@@ -90,7 +90,7 @@
<property name="label">Enable</property>
<property name="maximum_size"></property>
<property name="minimum_size"></property>
- <property name="name">m_Enable</property>
+ <property name="name">enable_</property>
<property name="permission">protected</property>
<property name="pos"></property>
<property name="size"></property>
@@ -101,7 +101,7 @@
<property name="window_name"></property>
<property name="window_style"></property>
<event name="OnChar"></event>
- <event name="OnCheckBox">OnEnable</event>
+ <event name="OnCheckBox">onEnable</event>
<event name="OnEnterWindow"></event>
<event name="OnEraseBackground"></event>
<event name="OnKeyDown"></event>
@@ -151,7 +151,7 @@
<property name="label">Setup</property>
<property name="maximum_size"></property>
<property name="minimum_size"></property>
- <property name="name">m_Setup</property>
+ <property name="name">setup_</property>
<property name="permission">protected</property>
<property name="pos"></property>
<property name="size"></property>
@@ -161,7 +161,7 @@
<property name="window_extra_style"></property>
<property name="window_name"></property>
<property name="window_style"></property>
- <event name="OnButtonClick">OnSetup</event>
+ <event name="OnButtonClick">onSetup</event>
<event name="OnChar"></event>
<event name="OnEnterWindow"></event>
<event name="OnEraseBackground"></event>
@@ -217,7 +217,7 @@
<property name="id">wxID_ANY</property>
<property name="maximum_size"></property>
<property name="minimum_size"></property>
- <property name="name">m_ImagePanel</property>
+ <property name="name">image_panel_</property>
<property name="permission">protected</property>
<property name="pos"></property>
<property name="size"></property>
@@ -234,20 +234,20 @@
<event name="OnKillFocus"></event>
<event name="OnLeaveWindow"></event>
<event name="OnLeftDClick"></event>
- <event name="OnLeftDown">OnLeftMouseDown</event>
- <event name="OnLeftUp">OnLeftMouseUp</event>
+ <event name="OnLeftDown">onLeftMouseDown</event>
+ <event name="OnLeftUp">onLeftMouseUp</event>
<event name="OnMiddleDClick"></event>
- <event name="OnMiddleDown">OnMiddleMouseDown</event>
- <event name="OnMiddleUp">OnMiddleMouseUp</event>
- <event name="OnMotion">OnMouseMotion</event>
+ <event name="OnMiddleDown">onMiddleMouseDown</event>
+ <event name="OnMiddleUp">onMiddleMouseUp</event>
+ <event name="OnMotion">onMouseMotion</event>
<event name="OnMouseEvents"></event>
- <event name="OnMouseWheel">OnMouseWheel</event>
- <event name="OnPaint">OnImagePaint</event>
+ <event name="OnMouseWheel">onMouseWheel</event>
+ <event name="OnPaint">onImagePaint</event>
<event name="OnRightDClick"></event>
- <event name="OnRightDown">OnRightMouseDown</event>
- <event name="OnRightUp">OnRightMouseUp</event>
+ <event name="OnRightDown">onRightMouseDown</event>
+ <event name="OnRightUp">onRightMouseUp</event>
<event name="OnSetFocus"></event>
- <event name="OnSize">OnImageSize</event>
+ <event name="OnSize">onImageSize</event>
<event name="OnUpdateUI"></event>
</object>
</object>
@@ -269,7 +269,7 @@
<property name="minimum_size"></property>
<property name="name">CameraSetupDialogBase</property>
<property name="pos"></property>
- <property name="size">472,400</property>
+ <property name="size">472,333</property>
<property name="style">wxDEFAULT_DIALOG_STYLE</property>
<property name="subclass"></property>
<property name="title">Camera Setup</property>
@@ -318,7 +318,7 @@
<property name="proportion">0</property>
<object class="wxStaticBoxSizer" expanded="1">
<property name="id">wxID_ANY</property>
- <property name="label">Image</property>
+ <property name="label">Camera</property>
<property name="minimum_size"></property>
<property name="name">sbSizer4</property>
<property name="orient">wxVERTICAL</property>
@@ -345,7 +345,7 @@
<property name="font"></property>
<property name="hidden">0</property>
<property name="id">wxID_ANY</property>
- <property name="label">Subscription</property>
+ <property name="label">Name</property>
<property name="maximum_size"></property>
<property name="minimum_size"></property>
<property name="name">m_staticText711</property>
@@ -408,11 +408,11 @@
<property name="maximum_size"></property>
<property name="maxlength">0</property>
<property name="minimum_size"></property>
- <property name="name">m_ImageSubscriptionText</property>
+ <property name="name">camera_name_</property>
<property name="permission">protected</property>
<property name="pos"></property>
<property name="size"></property>
- <property name="style">wxTE_PROCESS_ENTER</property>
+ <property name="style"></property>
<property name="subclass"></property>
<property name="tooltip"></property>
<property name="value"></property>
@@ -448,58 +448,6 @@
<event name="OnUpdateUI"></event>
</object>
</object>
- <object class="sizeritem" expanded="1">
- <property name="border">5</property>
- <property name="flag">wxALL|wxALIGN_CENTER_VERTICAL</property>
- <property name="proportion">0</property>
- <object class="wxButton" expanded="1">
- <property name="bg"></property>
- <property name="context_help"></property>
- <property name="default">0</property>
- <property name="enabled">1</property>
- <property name="fg"></property>
- <property name="font"></property>
- <property name="hidden">0</property>
- <property name="id">wxID_ANY</property>
- <property name="label"> ... </property>
- <property name="maximum_size"></property>
- <property name="minimum_size"></property>
- <property name="name">m_ImageSubscriptionBrowse</property>
- <property name="permission">protected</property>
- <property name="pos"></property>
- <property name="size"></property>
- <property name="style">wxBU_EXACTFIT</property>
- <property name="subclass"></property>
- <property name="tooltip"></property>
- <property name="window_extra_style"></property>
- <property name="window_name"></property>
- <property name="window_style"></property>
- <event name="OnButtonClick">OnImageSubscriptionBrowse</event>
- <event name="OnChar"></event>
- <event name="OnEnterWindow"></event>
- <event name="OnEraseBackground"></event>
- <event name="OnKeyDown"></event>
- <event name="OnKeyUp"></event>
- <event name="OnKillFocus"></event>
- <event name="OnLeaveWindow"></event>
- <event name="OnLeftDClick"></event>
- <event name="OnLeftDown"></event>
- <event name="OnLeftUp"></event>
- <event name="OnMiddleDClick"></event>
- <event name="OnMiddleDown"></event>
- <event name="OnMiddleUp"></event>
- <event name="OnMotion"></event>
- <event name="OnMouseEvents"></event>
- <event name="OnMouseWheel"></event>
- <event name="OnPaint"></event>
- <event name="OnRightDClick"></event>
- <event name="OnRightDown"></event>
- <event name="OnRightUp"></event>
- <event name="OnSetFocus"></event>
- <event name="OnSize"></event>
- <event name="OnUpdateUI"></event>
- </object>
- </object>
</object>
</object>
</object>
@@ -534,7 +482,7 @@
<property name="label">Enable</property>
<property name="maximum_size"></property>
<property name="minimum_size"></property>
- <property name="name">m_EnablePTZCheck</property>
+ <property name="name">enable_ptz_</property>
<property name="permission">protected</property>
<property name="pos"></property>
<property name="size"></property>
@@ -545,7 +493,7 @@
<property name="window_name"></property>
<property name="window_style"></property>
<event name="OnChar"></event>
- <event name="OnCheckBox">OnPTZEnableChecked</event>
+ <event name="OnCheckBox"></event>
<event name="OnEnterWindow"></event>
<event name="OnEraseBackground"></event>
<event name="OnKeyDown"></event>
@@ -574,7 +522,7 @@
<property name="border">5</property>
<property name="flag">wxEXPAND</property>
<property name="proportion">1</property>
- <object class="wxBoxSizer" expanded="0">
+ <object class="wxBoxSizer" expanded="1">
<property name="minimum_size"></property>
<property name="name">bSizer12</property>
<property name="orient">wxHORIZONTAL</property>
@@ -698,7 +646,7 @@
<property name="maximum_size"></property>
<property name="min">-9999999</property>
<property name="minimum_size"></property>
- <property name="name">m_PanMinSpin</property>
+ <property name="name">pan_min_</property>
<property name="permission">protected</property>
<property name="pos"></property>
<property name="size"></property>
@@ -804,7 +752,7 @@
<property name="maximum_size"></property>
<property name="min">-9999999</property>
<property name="minimum_size"></property>
- <property name="name">m_PanMaxSpin</property>
+ <property name="name">pan_max_</property>
<property name="permission">protected</property>
<property name="pos"></property>
<property name="size"></property>
@@ -848,7 +796,7 @@
<property name="border">5</property>
<property name="flag">wxEXPAND</property>
<property name="proportion">1</property>
- <object class="wxBoxSizer" expanded="0">
+ <object class="wxBoxSizer" expanded="1">
<property name="minimum_size"></property>
<property name="name">bSizer1211</property>
<property name="orient">wxHORIZONTAL</property>
@@ -972,7 +920,7 @@
<property name="maximum_size"></property>
<property name="min">-9999999</property>
<property name="minimum_size"></property>
- <property name="name">m_TiltMinSpin</property>
+ <property name="name">tilt_min_</property>
<property name="permission">protected</property>
<property name="pos"></property>
<property name="size"></property>
@@ -1078,7 +1026,7 @@
<property name="maximum_size"></property>
<property name="min">-9999999</property>
<property name="minimum_size"></property>
- <property name="name">m_TiltMaxSpin</property>
+ <property name="name">tilt_max_</property>
<property name="permission">protected</property>
<property name="pos"></property>
<property name="size"></property>
@@ -1122,7 +1070,7 @@
<property name="border">5</property>
<property name="flag">wxEXPAND</property>
<property name="proportion">1</property>
- <object class="wxBoxSizer" expanded="0">
+ <object class="wxBoxSizer" expanded="1">
<property name="minimum_size"></property>
<property name="name">bSizer121</property>
<property name="orient">wxHORIZONTAL</property>
@@ -1246,7 +1194,7 @@
<property name="maximum_size"></property>
<property name="min">-9999999</property>
<property name="minimum_size"></property>
- <property name="name">m_ZoomMinSpin</property>
+ <property name="name">zoom_min_</property>
<property name="permission">protected</property>
<property name="pos"></property>
<property name="size"></property>
@@ -1352,7 +1300,7 @@
<property name="maximum_size"></property>
<property name="min">-9999999</property>
<property name="minimum_size"></property>
- <property name="name">m_ZoomMaxSpin</property>
+ <property name="name">zoom_max_</property>
<property name="permission">protected</property>
<property name="pos"></property>
<property name="size"></property>
@@ -1392,314 +1340,6 @@
</object>
</object>
</object>
- <object class="sizeritem" expanded="1">
- <property name="border">5</property>
- <property name="flag">wxEXPAND</property>
- <property name="proportion">1</property>
- <object class="wxBoxSizer" expanded="0">
- <property name="minimum_size"></property>
- <property name="name">bSizer141</property>
- <property name="orient">wxHORIZONTAL</property>
- <property name="permission">none</property>
- <object class="sizeritem" expanded="1">
- <property name="border">5</property>
- <property name="flag">wxALL|wxALIGN_CENTER_VERTICAL</property>
- <property name="proportion">1</property>
- <object class="wxStaticText" expanded="1">
- <property name="bg"></property>
- <property name="context_help"></property>
- <property name="enabled">1</property>
- <property name="fg"></property>
- <property name="font"></property>
- <property name="hidden">0</property>
- <property name="id">wxID_ANY</property>
- <property name="label">State Subscription</property>
- <property name="maximum_size"></property>
- <property name="minimum_size"></property>
- <property name="name">m_staticText71</property>
- <property name="permission">protected</property>
- <property name="pos"></property>
- <property name="size"></property>
- <property name="style"></property>
- <property name="subclass"></property>
- <property name="tooltip"></property>
- <property name="window_extra_style"></property>
- <property name="window_name"></property>
- <property name="window_style"></property>
- <property name="wrap">-1</property>
- <event name="OnChar"></event>
- <event name="OnEnterWindow"></event>
- <event name="OnEraseBackground"></event>
- <event name="OnKeyDown"></event>
- <event name="OnKeyUp"></event>
- <event name="OnKillFocus"></event>
- <event name="OnLeaveWindow"></event>
- <event name="OnLeftDClick"></event>
- <event name="OnLeftDown"></event>
- <event name="OnLeftUp"></event>
- <event name="OnMiddleDClick"></event>
- <event name="OnMiddleDown"></event>
- <event name="OnMiddleUp"></event>
- <event name="OnMotion"></event>
- <event name="OnMouseEvents"></event>
- <event name="OnMouseWheel"></event>
- <event name="OnPaint"></event>
- <event name="OnRightDClick"></event>
- <event name="OnRightDown"></event>
- <event name="OnRightUp"></event>
- <event name="OnSetFocus"></event>
- <event name="OnSize"></event>
- <event name="OnUpdateUI"></event>
- </object>
- </object>
- <object class="sizeritem" expanded="1">
- <property name="border">5</property>
- <property name="flag">wxEXPAND</property>
- <property name="proportion">1</property>
- <object class="wxBoxSizer" expanded="1">
- <property name="minimum_size"></property>
- <property name="name">bSizer37</property>
- <property name="orient">wxHORIZONTAL</property>
- <property name="permission">none</property>
- <object class="sizeritem" expanded="1">
- <property name="border">5</property>
- <property name="flag">wxALL|wxALIGN_CENTER_VERTICAL</property>
- <property name="proportion">1</property>
- <object class="wxTextCtrl" expanded="1">
- <property name="bg"></property>
- <property name="context_help"></property>
- <property name="enabled">1</property>
- <property name="fg"></property>
- <property name="font"></property>
- <property name="hidden">0</property>
- <property name="id">wxID_ANY</property>
- <property name="maximum_size"></property>
- <property name="maxlength">0</property>
- <property name="minimum_size"></property>
- <property name="name">m_PTZStateSubscriptionText</property>
- <property name="permission">protected</property>
- <property name="pos"></property>
- <property name="size"></property>
- <property name="style">wxTE_PROCESS_ENTER</property>
- ...
[truncated message content] |
|
From: <hsu...@us...> - 2008-09-18 03:07:46
|
Revision: 4422
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4422&view=rev
Author: hsujohnhsu
Date: 2008-09-18 03:07:56 +0000 (Thu, 18 Sep 2008)
Log Message:
-----------
update gazebo plugin to publish full_cloud from stereo. can be viewed from irrlicht viewer.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
Added Paths:
-----------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-09-18 01:48:34 UTC (rev 4421)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-09-18 03:07:56 UTC (rev 4422)
@@ -12,6 +12,7 @@
rospack_add_library(Ros_Stereo_Camera src/Ros_Stereo_Camera.cc)
rospack_add_library(Ros_Camera src/Ros_Camera.cc)
rospack_add_library(Ros_Laser src/Ros_Laser.cc)
+rospack_add_library(Ros_Block_Laser src/Ros_Block_Laser.cc)
rospack_add_library(Ros_Time src/Ros_Time.cc)
rospack_add_library(Ros_PTZ src/Ros_PTZ.cc)
rospack_add_library(P3D src/P3D.cc)
Added: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh 2008-09-18 03:07:56 UTC (rev 4422)
@@ -0,0 +1,139 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/*
+ * Desc: ros laser controller.
+ * Author: Nathan Koenig
+ * Date: 01 Feb 2007
+ * SVN: $Id: Ros_Block_Laser.hh 6656 2008-06-20 22:52:19Z natepak $
+ */
+
+#ifndef ROS_BLOCK_LASER_HH
+#define ROS_BLOCK_LASER_HH
+
+#include <gazebo/Controller.hh>
+
+#include <ros/node.h>
+#include <std_msgs/LaserScan.h>
+#include <std_msgs/PointCloudFloat32.h>
+
+namespace gazebo
+{
+ class LaserIface;
+ class FiducialIface;
+ class RaySensor;
+
+/// @addtogroup gazebo_controller
+/// @{
+/** \defgroup ros ros
+
+ \brief ros laser controller.
+
+ This is a controller that collects data from a ray sensor, and populates a libgazebo laser interface.
+
+ \verbatim
+ <model:physical name="laser_model">
+ <body:box name="laser_body">
+
+ <sensor:ray name="laser">
+ <controller:ros_laser name="controller-name">
+ <interface:laser name="iface-name"/>
+ </controller:ros_laser>
+ </sensor:ray>
+
+ </body:box>
+ </model:physical>
+ \endverbatim
+
+\{
+*/
+
+/// \brief ros laser controller.
+///
+/// This is a controller that simulates a ros laser
+class Ros_Block_Laser : public Controller
+{
+ /// \brief Constructor
+ /// \param parent The parent entity, must be a Model or a Sensor
+ public: Ros_Block_Laser(Entity *parent);
+
+ /// \brief Destructor
+ public: virtual ~Ros_Block_Laser();
+
+ /// \brief Load the controller
+ /// \param node XML config node
+ /// \return 0 on success
+ protected: virtual void LoadChild(XMLConfigNode *node);
+
+ /// \brief Init the controller
+ /// \return 0 on success
+ protected: virtual void InitChild();
+
+ /// \brief Update the controller
+ /// \return 0 on success
+ protected: virtual void UpdateChild();
+
+ /// \brief Finalize the controller
+ /// \return 0 on success
+ protected: virtual void FiniChild();
+
+ /// \brief Put laser data to the iface
+ private: void PutLaserData();
+
+ /// \brief Put fiducial data to the iface
+ private: void PutFiducialData();
+
+ /// The laser interface
+ private: LaserIface *laserIface;
+
+ private: FiducialIface *fiducialIface;
+
+ /// The parent sensor
+ private: RayBlockSensor *myParent;
+
+ // pointer to ros node
+ private: ros::node *rosnode;
+
+ // ros message
+ private: std_msgs::PointCloudFloat32 cloudMsg;
+
+ // topic name
+ private: std::string topicName;
+
+ // frame transform name, should match link name
+ // FIXME: extract link name directly?
+ private: std::string frameName;
+
+ private: double gaussianNoise;
+
+ private: double GaussianKernel(double mu,double sigma);
+
+ // A mutex to lock access to fields that are used in message callbacks
+ private: ros::thread::mutex lock;
+
+};
+
+/** /} */
+/// @}
+
+}
+
+#endif
+
Added: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc 2008-09-18 03:07:56 UTC (rev 4422)
@@ -0,0 +1,450 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/*
+ * Desc: Ros Block Laser controller.
+ * Author: Nathan Koenig
+ * Date: 01 Feb 2007
+ * SVN info: $Id: Ros_Block_Laser.cc 6683 2008-06-25 19:12:30Z natepak $
+ */
+
+#include <algorithm>
+#include <assert.h>
+
+#include <gazebo/Sensor.hh>
+#include <gazebo/Global.hh>
+#include <gazebo/XMLConfig.hh>
+#include <gazebo/HingeJoint.hh>
+#include <gazebo/World.hh>
+#include <gazebo/Simulator.hh>
+#include <gazebo/gazebo.h>
+#include <gazebo/GazeboError.hh>
+#include <gazebo/ControllerFactory.hh>
+#include <RayBlockSensor.hh>
+#include <gazebo_plugin/Ros_Block_Laser.hh>
+
+using namespace gazebo;
+
+GZ_REGISTER_DYNAMIC_CONTROLLER("ros_block_laser", Ros_Block_Laser);
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+Ros_Block_Laser::Ros_Block_Laser(Entity *parent)
+ : Controller(parent)
+{
+ this->myParent = dynamic_cast<RayBlockSensor*>(this->parent);
+
+ if (!this->myParent)
+ gzthrow("Ros_Block_Laser controller requires a Ray Block Sensor as its parent");
+
+ // set parent sensor to active automatically
+ this->myParent->SetActive(true);
+
+ this->laserIface = NULL;
+ this->fiducialIface = NULL;
+
+ rosnode = ros::g_node; // comes from where? common.h exports as global variable
+ int argc = 0;
+ char** argv = NULL;
+ if (rosnode == NULL)
+ {
+ // start a ros node if none exist
+ ros::init(argc,argv);
+ rosnode = new ros::node("ros_gazebo",ros::node::DONT_HANDLE_SIGINT);
+ printf("-------------------- starting node in laser \n");
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Destructor
+Ros_Block_Laser::~Ros_Block_Laser()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Load the controller
+void Ros_Block_Laser::LoadChild(XMLConfigNode *node)
+{
+ std::vector<Iface*>::iterator iter;
+
+ for (iter = this->ifaces.begin(); iter != this->ifaces.end(); iter++)
+ {
+ if ((*iter)->GetType() == "laser")
+ this->laserIface = dynamic_cast<LaserIface*>(*iter);
+ else if ((*iter)->GetType() == "fiducial")
+ this->fiducialIface = dynamic_cast<FiducialIface*>(*iter);
+ }
+
+ if (!this->laserIface) gzthrow("Ros_Block_Laser controller requires a LaserIface");
+
+ this->topicName = node->GetString("topicName","default_ros_laser",0); //read from xml file
+ std::cout << "================= " << this->topicName << std::endl;
+ rosnode->advertise<std_msgs::PointCloudFloat32>(this->topicName);
+ this->frameName = node->GetString("frameName","default_ros_laser",0); //read from xml file
+ this->gaussianNoise = node->GetDouble("gaussianNoise",0.0,0); //read from xml file
+
+
+ // set size of cloud message, starts at 0!! FIXME: not necessary
+ Angle maxAngle = this->myParent->GetMaxAngle();
+ Angle minAngle = this->myParent->GetMinAngle();
+ int rangeCount = this->myParent->GetRangeCount();
+ int verticalRangeCount = this->myParent->GetVerticalRangeCount();
+ Angle verticalMaxAngle = this->myParent->GetVerticalMaxAngle();
+ Angle verticalMinAngle = this->myParent->GetVerticalMinAngle();
+
+ int r_size = rangeCount * verticalRangeCount;
+ this->cloudMsg.set_pts_size(r_size);
+ this->cloudMsg.set_chan_size(r_size);
+ for (int i=0; i< r_size; i++)
+ this->cloudMsg.chan[i].set_vals_size(1);
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Initialize the controller
+void Ros_Block_Laser::InitChild()
+{
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Update the controller
+void Ros_Block_Laser::UpdateChild()
+{
+#if 0
+ bool laserOpened = false;
+ bool fidOpened = false;
+
+ if (this->laserIface->Lock(1))
+ {
+ laserOpened = this->laserIface->GetOpenCount() > 0;
+ this->laserIface->Unlock();
+ }
+
+ if (this->fiducialIface && this->fiducialIface->Lock(1))
+ {
+ fidOpened = this->fiducialIface->GetOpenCount() > 0;
+ this->fiducialIface->Unlock();
+ }
+
+ if (laserOpened)
+ {
+ this->myParent->SetActive(true);
+ this->PutLaserData();
+ }
+
+ if (fidOpened)
+ {
+ this->myParent->SetActive(true);
+ this->PutFiducialData();
+ }
+
+ if (!laserOpened && !fidOpened)
+ {
+ this->myParent->SetActive(false);
+ }
+#endif
+ this->PutLaserData();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Finalize the controller
+void Ros_Block_Laser::FiniChild()
+{
+ // TODO: will be replaced by global ros node eventually
+ if (rosnode != NULL)
+ {
+ std::cout << "shutdown rosnode in Ros_Block_Laser" << std::endl;
+ //ros::fini();
+ rosnode->shutdown();
+ //delete rosnode;
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Put laser data to the interface
+void Ros_Block_Laser::PutLaserData()
+{
+ int i, hja, hjb;
+ int j, vja, vjb;
+ double vb, hb;
+ int j1, j2, j3, j4; // four corners indices
+ double r1, r2, r3, r4, r; // four corner values + interpolated range
+ int v;
+
+
+ Angle maxAngle = this->myParent->GetMaxAngle();
+ Angle minAngle = this->myParent->GetMinAngle();
+
+ double maxRange = this->myParent->GetMaxRange();
+ double minRange = this->myParent->GetMinRange();
+ int rayCount = this->myParent->GetRayCount();
+ int rangeCount = this->myParent->GetRangeCount();
+
+ int verticalRayCount = this->myParent->GetVerticalRayCount();
+ int verticalRangeCount = this->myParent->GetVerticalRangeCount();
+ Angle verticalMaxAngle = this->myParent->GetVerticalMaxAngle();
+ Angle verticalMinAngle = this->myParent->GetVerticalMinAngle();
+
+ double yDiff = maxAngle.GetAsRadian() - minAngle.GetAsRadian();
+ double pDiff = verticalMaxAngle.GetAsRadian() - verticalMinAngle.GetAsRadian();
+
+
+ // set size of cloud message everytime!
+ int r_size = rangeCount * verticalRangeCount;
+ this->cloudMsg.set_pts_size(r_size);
+ this->cloudMsg.set_chan_size(r_size);
+ for (int i=0; i< r_size; i++)
+ this->cloudMsg.chan[i].set_vals_size(1);
+
+
+ if (this->laserIface->Lock(1))
+ {
+ // Add Frame Name
+
+
+ // Data timestamp
+ this->laserIface->data->head.time = Simulator::Instance()->GetSimTime();
+
+ // Read out the laser range data
+ this->laserIface->data->min_angle = minAngle.GetAsRadian();
+ this->laserIface->data->max_angle = maxAngle.GetAsRadian();
+ this->laserIface->data->res_angle = (maxAngle.GetAsRadian() - minAngle.GetAsRadian()) / (rangeCount - 1);
+ this->laserIface->data->res_range = 0.1;
+ this->laserIface->data->max_range = maxRange;
+ this->laserIface->data->range_count = rangeCount;
+
+ assert(this->laserIface->data->range_count < GZ_LASER_MAX_RANGES );
+
+ /***************************************************************/
+ /* */
+ /* point scan from laser */
+ /* */
+ /***************************************************************/
+ this->lock.lock();
+ this->cloudMsg.header.frame_id = this->frameName;
+ this->cloudMsg.header.stamp.sec = (unsigned long)floor(this->laserIface->data->head.time);
+ this->cloudMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->laserIface->data->head.time - this->cloudMsg.header.stamp.sec) );
+
+ for (j = 0; j<verticalRangeCount; j++)
+ {
+ // interpolating in vertical direction
+ vb = (double) j * (verticalRayCount - 1) / (verticalRangeCount - 1);
+ vja = (int) floor(vb);
+ vjb = std::min(vja + 1, verticalRayCount - 1);
+ vb = vb - floor(vb); // fraction from min
+
+ assert(vja >= 0 && vja < verticalRayCount);
+ assert(vjb >= 0 && vjb < verticalRayCount);
+
+ for (i = 0; i<rangeCount; i++)
+ {
+ // Interpolate the range readings from the rays in horizontal direction
+ hb = (double) i * (rayCount - 1) / (rangeCount - 1);
+ hja = (int) floor(hb);
+ hjb = std::min(hja + 1, rayCount - 1);
+ hb = hb - floor(hb); // fraction from min
+
+ assert(hja >= 0 && hja < rayCount);
+ assert(hjb >= 0 && hjb < rayCount);
+
+ // indices of 4 corners
+ j1 = hja + vja * rayCount;
+ j2 = hjb + vja * rayCount;
+ j3 = hja + vjb * rayCount;
+ j4 = hjb + vjb * rayCount;
+ // range readings of 4 corners
+ r1 = std::min(this->myParent->GetRange(j1) , maxRange);
+ r2 = std::min(this->myParent->GetRange(j2) , maxRange);
+ r3 = std::min(this->myParent->GetRange(j3) , maxRange);
+ r4 = std::min(this->myParent->GetRange(j4) , maxRange);
+
+ // Range is linear interpolation if values are close,
+ // and min if they are very different
+ r = (1-vb)*((1 - hb) * r1 + hb * r2)
+ + vb *((1 - hb) * r3 + hb * r4);
+
+ // Intensity is either-or
+ v = (int) this->myParent->GetRetro(j1) || (int) this->myParent->GetRetro(j2) ||
+ (int) this->myParent->GetRetro(j3) || (int) this->myParent->GetRetro(j4);
+
+ this->laserIface->data->ranges[i] = r + minRange;
+ this->laserIface->data->intensity[i] = v;
+
+ // get angles of ray to get xyz for point
+ double yAngle = 0.5*(hja+hjb) * yDiff / (rayCount -1) + minAngle.GetAsRadian();
+ double pAngle = 0.5*(vja+vjb) * pDiff / (verticalRayCount -1) + verticalMinAngle.GetAsRadian();
+
+ int n = i + j*rayCount;
+ /***************************************************************/
+ /* */
+ /* point scan from laser */
+ /* */
+ /***************************************************************/
+ if (r == maxRange)
+ {
+ // no noise if at max range
+ this->cloudMsg.pts[n].x = r * cos(pAngle)*cos(yAngle);
+ this->cloudMsg.pts[n].y = r * sin(yAngle);
+ this->cloudMsg.pts[n].z = r * sin(pAngle)*cos(yAngle);
+ }
+ else
+ {
+ this->cloudMsg.pts[n].x = r * cos(pAngle)*cos(yAngle) + this->GaussianKernel(0,this->gaussianNoise) ;
+ this->cloudMsg.pts[n].y = r * sin(yAngle) + this->GaussianKernel(0,this->gaussianNoise) ;
+ this->cloudMsg.pts[n].z = r * sin(pAngle)*cos(yAngle) + this->GaussianKernel(0,this->gaussianNoise) ;
+ }
+ this->cloudMsg.chan[n].vals[0] = v + this->GaussianKernel(0,this->gaussianNoise) ;
+ }
+ }
+
+ // iface writing can be skipped if iface is not used.
+ // send data out via ros message
+ rosnode->publish(this->topicName,this->cloudMsg);
+ this->lock.unlock();
+
+ this->laserIface->Unlock();
+
+ // New data is available
+ this->laserIface->Post();
+ }
+
+
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Update the data in the interface
+void Ros_Block_Laser::PutFiducialData()
+{
+ int i, j, count;
+ FiducialFid *fid;
+ double r, b;
+ double ax, ay, bx, by, cx, cy;
+
+ Angle maxAngle = this->myParent->GetMaxAngle();
+ Angle minAngle = this->myParent->GetMinAngle();
+
+ double maxRange = this->myParent->GetMaxRange();
+ double minRange = this->myParent->GetMinRange();
+ int rayCount = this->myParent->GetRayCount();
+ int rangeCount = this->myParent->GetRangeCount();
+
+ if (this->fiducialIface->Lock(1))
+ {
+ // Data timestamp
+ this->fiducialIface->data->head.time = Simulator::Instance()->GetSimTime();
+ this->fiducialIface->data->count = 0;
+
+ // TODO: clean this up
+ count = 0;
+ for (i = 0; i < rayCount; i++)
+ {
+ if (this->myParent->GetFiducial(i) < 0)
+ continue;
+
+ // Find the end of the fiducial
+ for (j = i + 1; j < rayCount; j++)
+ {
+ if (this->myParent->GetFiducial(j) != this->myParent->GetFiducial(i))
+ break;
+ }
+ j--;
+
+ // Need at least three points to get orientation
+ if (j - i + 1 >= 3)
+ {
+ r = minRange + this->myParent->GetRange(i);
+ b = minAngle.GetAsRadian() + i * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
+ ax = r * cos(b);
+ ay = r * sin(b);
+
+ r = minRange + this->myParent->GetRange(j);
+ b = minAngle.GetAsRadian() + j * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
+ bx = r * cos(b);
+ by = r * sin(b);
+
+ cx = (ax + bx) / 2;
+ cy = (ay + by) / 2;
+
+ assert(count < GZ_FIDUCIAL_MAX_FIDS);
+ fid = this->fiducialIface->data->fids + count++;
+
+ fid->id = this->myParent->GetFiducial(j);
+ fid->pose.pos.x = cx;
+ fid->pose.pos.y = cy;
+ fid->pose.yaw = atan2(by - ay, bx - ax) + M_PI / 2;
+ }
+
+ // Fewer points get no orientation
+ else
+ {
+ r = minRange + this->myParent->GetRange(i);
+ b = minAngle.GetAsRadian() + i * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
+ ax = r * cos(b);
+ ay = r * sin(b);
+
+ r = minRange + this->myParent->GetRange(j);
+ b = minAngle.GetAsRadian() + j * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
+ bx = r * cos(b);
+ by = r * sin(b);
+
+ cx = (ax + bx) / 2;
+ cy = (ay + by) / 2;
+
+ assert(count < GZ_FIDUCIAL_MAX_FIDS);
+ fid = this->fiducialIface->data->fids + count++;
+
+ fid->id = this->myParent->GetFiducial(j);
+ fid->pose.pos.x = cx;
+ fid->pose.pos.y = cy;
+ fid->pose.yaw = atan2(cy, cx) + M_PI;
+ }
+
+ /*printf("fiducial %d i[%d] j[%d] %.2f %.2f %.2f\n",
+ fid->id, i,j,fid->pos[0], fid->pos[1], fid->rot[2]);
+ */
+ i = j;
+ }
+
+ this->fiducialIface->data->count = count;
+
+ this->fiducialIface->Unlock();
+ this->fiducialIface->Post();
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Utility for adding noise
+double Ros_Block_Laser::GaussianKernel(double mu,double sigma)
+{
+ // using Box-Muller transform to generate two independent standard normally disbributed normal variables
+ // see wikipedia
+ double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
+ double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
+ double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
+ //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
+ // we'll just use X
+ // scale to our mu and sigma
+ X = sigma * X + mu;
+ return X;
+}
+
+
+
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml 2008-09-18 01:48:34 UTC (rev 4421)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml 2008-09-18 03:07:56 UTC (rev 4422)
@@ -2340,39 +2340,40 @@
</sensor:stereocamera>
</verbatim>
</map>
+ -->
<map name="sensor" flag="gazebo">
<verbatim key="sensor_ray_block">
- <sensor:ray name="stereo_laser">
+ <sensor:ray_block name="stereo_laser">
<gaussianNoise>0.05</gaussianNoise>
- <rayCount>320</rayCount>
- <rangeCount>320</rangeCount>
+ <rayCount>20</rayCount>
+ <rangeCount>20</rangeCount>
<laserCount>1</laserCount>
<origin>0.0 0.0 0.05</origin>
<displayRays>false</displayRays>
- <minAngle>-70</minAngle>
- <maxAngle> 70</maxAngle>
+ <minAngle>-30</minAngle>
+ <maxAngle> 30</maxAngle>
<minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
+ <maxRange>100.0</maxRange>
<updateRate>10.0</updateRate>
- <verticalCount>240</verticalCount>
- <verticalMinAngle>-70</verticalMinAngle>
- <verticalMaxAngle> 70</verticalMaxAngle>
+ <verticalRayCount>30</verticalRayCount>
+ <verticalRangeCount>30</verticalRangeCount>
+ <verticalMinAngle>-30</verticalMinAngle>
+ <verticalMaxAngle> 0</verticalMaxAngle>
- <controller:ros_laser name="stereo_laser_controller" plugin="libRos_Laser.so">
+ <controller:ros_block_laser name="stereo_laser_controller" plugin="libRos_Block_Laser.so">
<alwaysOn>true</alwaysOn>
<updateRate>15.0</updateRate>
- <topicName>stereo_scan</topicName>
- <frameName>stereo_laser</frameName>
+ <topicName>full_cloud</topicName>
+ <frameName>stereo_cloud</frameName>
<interface:laser name="stereo_laser_iface" />
- </controller:ros_laser>
- </sensor:ray>
+ </controller:ros_block_laser>
+ </sensor:ray_block>
</verbatim>
</map>
- -->
</sensor>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-09-18 01:48:34 UTC (rev 4421)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-09-18 03:07:56 UTC (rev 4422)
@@ -2340,39 +2340,40 @@
</sensor:stereocamera>
</verbatim>
</map>
+ -->
<map name="sensor" flag="gazebo">
<verbatim key="sensor_ray_block">
- <sensor:ray name="stereo_laser">
+ <sensor:ray_block name="stereo_laser">
<gaussianNoise>0.05</gaussianNoise>
- <rayCount>320</rayCount>
- <rangeCount>320</rangeCount>
+ <rayCount>20</rayCount>
+ <rangeCount>20</rangeCount>
<laserCount>1</laserCount>
<origin>0.0 0.0 0.05</origin>
<displayRays>false</displayRays>
- <minAngle>-70</minAngle>
- <maxAngle> 70</maxAngle>
+ <minAngle>-30</minAngle>
+ <maxAngle> 30</maxAngle>
<minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
+ <maxRange>100.0</maxRange>
<updateRate>10.0</updateRate>
- <verticalCount>240</verticalCount>
- <verticalMinAngle>-70</verticalMinAngle>
- <verticalMaxAngle> 70</verticalMaxAngle>
+ <verticalRayCount>30</verticalRayCount>
+ <verticalRangeCount>30</verticalRangeCount>
+ <verticalMinAngle>-30</verticalMinAngle>
+ <verticalMaxAngle> 0</verticalMaxAngle>
- <controller:ros_laser name="stereo_laser_controller" plugin="libRos_Laser.so">
+ <controller:ros_block_laser name="stereo_laser_controller" plugin="libRos_Block_Laser.so">
<alwaysOn>true</alwaysOn>
<updateRate>15.0</updateRate>
- <topicName>stereo_scan</topicName>
- <frameName>stereo_laser</frameName>
+ <topicName>full_cloud</topicName>
+ <frameName>stereo_cloud</frameName>
<interface:laser name="stereo_laser_iface" />
- </controller:ros_laser>
- </sensor:ray>
+ </controller:ros_block_laser>
+ </sensor:ray_block>
</verbatim>
</map>
- -->
</sensor>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-18 15:27:23
|
Revision: 4441
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4441&view=rev
Author: hsujohnhsu
Date: 2008-09-18 22:27:30 +0000 (Thu, 18 Sep 2008)
Log Message:
-----------
added wait_for_service for controller and mechanism control python scripts.
Modified Paths:
--------------
pkg/trunk/controllers/generic_controllers/src/generic_controllers/controllers.py
pkg/trunk/mechanism/mechanism_control/src/mechanism_control/mechanism.py
Modified: pkg/trunk/controllers/generic_controllers/src/generic_controllers/controllers.py
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/generic_controllers/controllers.py 2008-09-18 22:21:16 UTC (rev 4440)
+++ pkg/trunk/controllers/generic_controllers/src/generic_controllers/controllers.py 2008-09-18 22:27:30 UTC (rev 4441)
@@ -8,36 +8,43 @@
from generic_controllers.srv import *
def list_controllers():
+ rospy.wait_for_service('list_controllers')
s = rospy.ServiceProxy('list_controllers', ListControllers)
resp = s.call(ListControllersRequest())
for t in resp.controllers:
print t
def set_controller(controller, command):
+ rospy.wait_for_service(controller + '/set_command')
s = rospy.ServiceProxy(controller + '/set_command', SetCommand)
resp = s.call(SetCommandRequest(command))
print resp.command
def set_controller_vector(controller, command):
+ rospy.wait_for_service(controller + '/set_command')
s = rospy.ServiceProxy(controller + '/set_command', SetVectorCommand)
resp = s(*command)
def get_controller(controller):
+ rospy.wait_for_service(controller + '/get_actual')
s = rospy.ServiceProxy(controller + '/get_actual', GetActual)
resp = s.call(GetActualRequest())
print str(resp.time) + ": " + str(resp.command)
def set_position(controller, command):
+ rospy.wait_for_service(controller + '/set_position')
s = rospy.ServiceProxy(controller + '/set_position', SetPosition)
resp = s.call(SetPositionRequest(command))
print resp.command
def get_position(controller):
+ rospy.wait_for_service(controller + '/get_position')
s = rospy.ServiceProxy(controller + '/get_position', GetPosition)
resp = s.call(GetPositionRequest())
print str(resp.time) + ": " + str(resp.command)
def set_profile(controller, upper_turnaround, lower_turnaround, upper_decel_buffer, lower_decel_buffer):
+ rospy.wait_for_service(controller + '/set_profile')
s = rospy.ServiceProxy(controller + '/set_profile', SetProfile)
resp = s.call(SetProfileRequest(upper_turnaround, lower_turnaround, upper_decel_buffer, lower_decel_buffer))
print str(resp.time)
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control/mechanism.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control/mechanism.py 2008-09-18 22:21:16 UTC (rev 4440)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control/mechanism.py 2008-09-18 22:27:30 UTC (rev 4441)
@@ -16,6 +16,7 @@
print t
def list_controllers():
+ rospy.wait_for_service('list_controllers')
s = rospy.ServiceProxy('list_controllers', ListControllers)
resp = s.call(ListControllersRequest())
for c in resp.controllers:
@@ -31,6 +32,7 @@
print "Error when spawning", resp.ok
def kill_controller(name):
+ rospy.wait_for_service('kill_controller')
s = rospy.ServiceProxy('kill_controller', KillController)
resp = s.call(KillControllerRequest(name))
if resp.ok == 1:
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-03-02 17:56:33
|
Revision: 11986
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11986&view=rev
Author: isucan
Date: 2009-03-02 17:56:22 +0000 (Mon, 02 Mar 2009)
Log Message:
-----------
moving packages
Added Paths:
-----------
pkg/trunk/motion_planning/planning_models/
pkg/trunk/world_models/collision_space/
Removed Paths:
-------------
pkg/trunk/world_models/robot_models/
Property changes on: pkg/trunk/motion_planning/planning_models
___________________________________________________________________
Added: svn:mergeinfo
+
Property changes on: pkg/trunk/world_models/collision_space
___________________________________________________________________
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X