Menu
▾
▴
personalrobots-commit
|
From: <ge...@us...> - 2008-05-14 20:58:27
|
Revision: 414
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=414&view=rev
Author: gerkey
Date: 2008-05-14 13:58:34 -0700 (Wed, 14 May 2008)
Log Message:
-----------
fixed up license summaries
Modified Paths:
--------------
pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc
pkg/trunk/drivers/laser/hokuyourg_player/manifest.xml
pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosstage/rosstage.cc
Modified: pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc
===================================================================
--- pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc 2008-05-14 20:54:48 UTC (rev 413)
+++ pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc 2008-05-14 20:58:34 UTC (rev 414)
@@ -1,25 +1,34 @@
-#include <assert.h>
/*
- * Software License Agreement (GNU LGPL)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * This library is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public
- * License as published by the Free Software Foundation; either
- * version 2.1 of the License, or (at your option) any later version.
- *
- * This library 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
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this library; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * hokuyourg_player
+ * 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 <ORGANIZATION> 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 <assert.h>
#include <libstandalone_drivers/urg_laser.h>
#include <ros/node.h>
Modified: pkg/trunk/drivers/laser/hokuyourg_player/manifest.xml
===================================================================
--- pkg/trunk/drivers/laser/hokuyourg_player/manifest.xml 2008-05-14 20:54:48 UTC (rev 413)
+++ pkg/trunk/drivers/laser/hokuyourg_player/manifest.xml 2008-05-14 20:58:34 UTC (rev 414)
@@ -1,7 +1,7 @@
<package>
<description>A ROS node that wraps up the Player urglaser driver, which provides access to a wide range of Hokuyo URG lasers.</description>
<author>Brian P. Gerkey</author>
- <license>LGPL</license>
+ <license>BSD</license>
<depend package="roscpp" />
<depend package="player" />
<depend package="std_msgs" />
Modified: pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2008-05-14 20:54:48 UTC (rev 413)
+++ pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2008-05-14 20:58:34 UTC (rev 414)
@@ -1,35 +1,33 @@
-//Software License Agreement (BSD License)
+/*
+ * erratic_player
+ * 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 <ORGANIZATION> 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.
+ */
-//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 <assert.h>
// For core Player stuff (message queues, config file objects, etc.)
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-14 20:54:48 UTC (rev 413)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-14 20:58:34 UTC (rev 414)
@@ -1,35 +1,33 @@
-//Software License Agreement (BSD License)
+/*
+ * amcl_player
+ * 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 <ORGANIZATION> 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.
+ */
-//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 <assert.h>
// For core Player stuff (message queues, config file objects, etc.)
Modified: pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc
===================================================================
--- pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc 2008-05-14 20:54:48 UTC (rev 413)
+++ pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc 2008-05-14 20:58:34 UTC (rev 414)
@@ -1,35 +1,33 @@
-//Software License Agreement (BSD License)
+/*
+ * teleop_base_keyboard
+ * 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 <ORGANIZATION> 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.
+ */
-//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 <termios.h>
#include <signal.h>
#include <math.h>
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-14 20:54:48 UTC (rev 413)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-14 20:58:34 UTC (rev 414)
@@ -1,35 +1,33 @@
-//Software License Agreement (BSD License)
+/*
+ * wavefront_player
+ * 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 <ORGANIZATION> 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.
+ */
-//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 <stdio.h>
#include <assert.h>
#include <stdlib.h>
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-14 20:54:48 UTC (rev 413)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-14 20:58:34 UTC (rev 414)
@@ -1,8 +1,6 @@
/*
- * Software License Agreement (GNU GPL)
- *
+ * rosstage
* Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
*
* 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
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-05-15 03:04:43
|
Revision: 431
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=431&view=rev
Author: gerkey
Date: 2008-05-14 20:04:50 -0700 (Wed, 14 May 2008)
Log Message:
-----------
fixed shutdown behavior
Modified Paths:
--------------
pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc
pkg/trunk/simulators/rosstage/rosstage.cc
Modified: pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc
===================================================================
--- pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc 2008-05-15 03:04:15 UTC (rev 430)
+++ pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc 2008-05-15 03:04:50 UTC (rev 431)
@@ -90,7 +90,7 @@
quit(int sig)
{
tbk->stopRobot();
- ros::msg_destruct();
+ ros::fini();
exit(0);
}
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-15 03:04:15 UTC (rev 430)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-15 03:04:50 UTC (rev 431)
@@ -223,9 +223,7 @@
sn.Update();
}
- // have to call this explicitly for some reason. probably interference
- // from signal handling in Stage / FLTK?
- ros::msg_destruct();
+ ros::fini();
exit(0);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-05-15 23:00:41
|
Revision: 442
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=442&view=rev
Author: gerkey
Date: 2008-05-15 16:00:48 -0700 (Thu, 15 May 2008)
Log Message:
-----------
working on dynamic obstacle modeling
Modified Paths:
--------------
pkg/trunk/nav/nav_view/nav_view.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosstage/willow-erratic.world
Modified: pkg/trunk/nav/nav_view/nav_view.cpp
===================================================================
--- pkg/trunk/nav/nav_view/nav_view.cpp 2008-05-15 21:16:15 UTC (rev 441)
+++ pkg/trunk/nav/nav_view/nav_view.cpp 2008-05-15 23:00:48 UTC (rev 442)
@@ -7,6 +7,7 @@
#include "std_msgs/MsgRobotBase2DOdom.h"
#include "std_msgs/MsgParticleCloud2D.h"
#include "std_msgs/MsgPlanner2DGoal.h"
+#include "std_msgs/MsgPolyline2D.h"
#include "sdlgl/sdlgl.h"
class NavView : public ros::node, public ros::SDLGL
@@ -15,6 +16,8 @@
MsgRobotBase2DOdom odom;
MsgParticleCloud2D cloud;
MsgPlanner2DGoal goal;
+ MsgPolyline2D pathline;
+ MsgPolyline2D laserscan;
float view_scale, view_x, view_y;
SDL_Surface* map_surface;
GLuint map_texture;
@@ -27,6 +30,8 @@
advertise<MsgPlanner2DGoal>("goal");
subscribe("odom", odom, &NavView::odom_cb);
subscribe("particlecloud", cloud, &NavView::odom_cb);
+ subscribe("gui_path", pathline, &NavView::odom_cb);
+ subscribe("gui_laser", laserscan, &NavView::odom_cb);
gwidth = 1024;
gheight = 768;
init_gui(gwidth, gheight, "nav view");
@@ -113,6 +118,23 @@
glPopMatrix();
}
cloud.unlock();
+
+ pathline.lock();
+ glColor3f(pathline.color.r,pathline.color.g,pathline.color.b);
+ glBegin(GL_LINES);
+ for(unsigned int i=0;i<pathline.get_points_size();i++)
+ glVertex2f(pathline.points[i].x,pathline.points[i].y);
+ glEnd();
+ pathline.unlock();
+
+ pathline.lock();
+ glColor3f(laserscan.color.r,laserscan.color.g,laserscan.color.b);
+ glBegin(GL_POINTS);
+ for(unsigned int i=0;i<laserscan.get_points_size();i++)
+ glVertex2f(laserscan.points[i].x,laserscan.points[i].y);
+ glEnd();
+ pathline.unlock();
+
SDL_GL_SwapBuffers();
}
void set_view_params(int width, int height)
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-15 21:16:15 UTC (rev 441)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-15 23:00:48 UTC (rev 442)
@@ -34,6 +34,8 @@
#include <math.h>
#include <sys/time.h>
+#include <list>
+
// The Player lib we're using
#include <libstandalone_drivers/plan.h>
@@ -46,7 +48,11 @@
#include <std_msgs/MsgBaseVel.h>
#include <std_msgs/MsgRobotBase2DOdom.h>
#include <std_msgs/MsgLaserScan.h>
+#include <std_msgs/MsgPose2DFloat32.h>
+// For GUI debug
+#include <std_msgs/MsgPolyline2D.h>
+
// For transform support
#include <rosTF/rosTF.h>
@@ -60,7 +66,8 @@
#include <gdk-pixbuf/gdk-pixbuf.h>
// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
-double get_time(void);
+// computes the signed minimum difference between the two angles.
+static double angle_diff(double a, double b);
int read_map_from_image(int* size_x, int* size_y, char** mapdata,
const char* fname, int negate);
@@ -73,6 +80,7 @@
enum
{
NO_GOAL,
+ NEW_GOAL,
PURSUING_GOAL,
REACHED_GOAL
} planner_state;
@@ -103,7 +111,10 @@
// Map update paramters (for adding obstacles)
double laser_maxrange;
- int laser_maxscans;
+ double laser_buffer_time;
+ std::list<std::pair<MsgPose2DFloat32, MsgLaserScan*> > laser_scans;
+ double* laser_hitpts;
+ size_t laser_hitpts_len, laser_hitpts_size;
// Controller paramters
double lookahead_maxdist;
@@ -114,7 +125,10 @@
MsgPlanner2DGoal goalMsg;
MsgRobotBase2DOdom odomMsg;
MsgLaserScan laserMsg;
+ MsgPolyline2D polylineMsg;
+ MsgPolyline2D pointcloudMsg;
std::vector<MsgLaserScan> laserScans;
+ MsgRobotBase2DOdom* odomStack[2];
// Lock for access to class members in callbacks
ros::thread::mutex lock;
@@ -176,15 +190,15 @@
printed_warning(false),
stopped(false),
robot_radius(0.2),
- safety_dist(0.05),
- max_radius(0.25),
+ safety_dist(0.1),
+ max_radius(1.0),
dist_penalty(1.0),
plan_halfwidth(5.0),
dist_eps(1.0),
ang_eps(DTOR(10.0)),
cycletime(0.1),
laser_maxrange(4.0),
- laser_maxscans(40),
+ laser_buffer_time(2.0),
lookahead_maxdist(2.0),
lookahead_distweight(10.0),
tvmin(0.1),
@@ -216,12 +230,15 @@
{
for(int i=0;i<sx;i++)
{
+ this->plan->cells[i+j*sx].occ_state = mapdata[i+j*sx];
+ /*
if(mapdata[i+j*sx] < 0.1*255)
this->plan->cells[i+j*sx].occ_state = -1;
else if(mapdata[i+j*sx] > 0.5*255)
this->plan->cells[i+j*sx].occ_state = 1;
else
this->plan->cells[i+j*sx].occ_state = 0;
+ */
}
}
free(mapdata);
@@ -240,7 +257,15 @@
this->tf = new rosTFClient(*this);
+ this->laser_hitpts_size = this->laser_hitpts_len = 0;
+ this->laser_hitpts = NULL;
+
+ this->odomStack[0] = NULL;
+ this->odomStack[1] = NULL;
+
advertise<MsgPlanner2DState>("state");
+ advertise<MsgPolyline2D>("gui_path");
+ advertise<MsgPolyline2D>("gui_laser");
advertise<MsgBaseVel>("cmd_vel");
subscribe("goal", goalMsg, &WavefrontNode::goalReceived);
//subscribe("odom", odomMsg, &WavefrontNode::odomReceived);
@@ -271,7 +296,7 @@
this->goal[0] = goalMsg.goal.x;
this->goal[1] = goalMsg.goal.y;
this->goal[2] = goalMsg.goal.th;
- this->planner_state = PURSUING_GOAL;
+ this->planner_state = NEW_GOAL;
}
this->lock.unlock();
}
@@ -321,14 +346,150 @@
this->pose[1],
RTOD(this->pose[2]));
+ if(!this->odomStack[0])
+ this->odomStack[0] = new MsgRobotBase2DOdom(odomMsg);
+ else if(!this->odomStack[1])
+ this->odomStack[1] = new MsgRobotBase2DOdom(odomMsg);
+ else
+ {
+ assert(this->odomStack[0]);
+ delete this->odomStack[0];
+ this->odomStack[0] = this->odomStack[1];
+ this->odomStack[1] = new MsgRobotBase2DOdom(odomMsg);
+ }
+
this->lock.unlock();
}
void
WavefrontNode::laserReceived()
{
+ // Add the new message, interpolating its pose.
+ // TODO: use libTF's interpolation
+ if(this->odomStack[0] && this->odomStack[1])
+ {
+ double dx = this->odomStack[1]->pos.x - this->odomStack[0]->pos.x;
+ double dy = this->odomStack[1]->pos.y - this->odomStack[0]->pos.y;
+ double da = angle_diff(this->odomStack[1]->pos.th,
+ this->odomStack[0]->pos.th);
+ double t0 = this->odomStack[0]->header.stamp_secs +
+ this->odomStack[0]->header.stamp_nsecs / 1e9;
+ double t1 = this->odomStack[1]->header.stamp_secs +
+ this->odomStack[1]->header.stamp_nsecs / 1e9;
+ double dt = t1 - t0;
+ double tl = this->laserMsg.header.stamp_secs +
+ this->laserMsg.header.stamp_nsecs / 1e9;
+ double dtl = tl - t0;
+
+ printf("0: %.3f %.3f %.3f %.6f\n",
+ this->odomStack[0]->pos.x,
+ this->odomStack[0]->pos.y,
+ this->odomStack[0]->pos.th, t0);
+ printf("1: %.3f %.3f %.3f %.6f\n",
+ this->odomStack[1]->pos.x,
+ this->odomStack[1]->pos.y,
+ this->odomStack[1]->pos.th, t1);
+
+ MsgPose2DFloat32 p;
+ p.x = this->odomStack[1]->pos.x - (dx / dt) * dtl;
+ p.y = this->odomStack[1]->pos.y - (dy / dt) * dtl;
+ p.th = ANG_NORM(this->odomStack[1]->pos.th - (da / dt) * dtl);
+
+ printf("I: %.3f %.3f %.3f %.6f\n",
+ p.x, p.y, p.th, tl);
+
+ MsgLaserScan* scan = new MsgLaserScan(this->laserMsg);
+
+ std::pair<MsgPose2DFloat32,MsgLaserScan*> item(p,scan);
+
+ this->laser_scans.push_back(item);
+ }
+
+ // Remove anything that's too old
+ double currtime = this->laserMsg.header.stamp_secs +
+ this->laserMsg.header.stamp_nsecs / 1e9;
+ // Also count how many points we have
+ unsigned int hitpt_cnt=0;
+ for(std::list<std::pair<MsgPose2DFloat32,MsgLaserScan*> >::iterator it = this->laser_scans.begin();
+ it != this->laser_scans.end();
+ it++)
+ {
+ double msgtime = it->second->header.stamp_secs + it->second->header.stamp_nsecs / 1e9;
+ if((currtime - msgtime) > this->laser_buffer_time)
+ {
+ delete it->second;
+ it = this->laser_scans.erase(it);
+ it--;
+ }
+ else
+ {
+ hitpt_cnt += it->second->get_ranges_size()*2;
+ }
+ }
+
+ // allocate more space as necessary
+ if(this->laser_hitpts_size < hitpt_cnt)
+ {
+ this->laser_hitpts_size = hitpt_cnt;
+ this->laser_hitpts =
+ (double*)realloc(this->laser_hitpts,
+ this->laser_hitpts_size*sizeof(double));
+ assert(this->laser_hitpts);
+ }
+
+ // project hit points from each scan
+ double* pts = this->laser_hitpts;
+ this->laser_hitpts_len = 0;
+ for(std::list<std::pair<MsgPose2DFloat32, MsgLaserScan*> >::iterator it = this->laser_scans.begin();
+ it != this->laser_scans.end();
+ it++)
+ {
+ float b=it->second->angle_min;
+ float* r=it->second->ranges;
+ for(unsigned int j=0;
+ j<it->second->get_ranges_size();
+ j++,r++,b+=it->second->angle_increment)
+ {
+ if(((*r) >= this->laser_maxrange) || ((*r) >= it->second->range_max))
+ continue;
+ //double rx, ry;
+ //rx = (*r)*cos(b);
+ //ry = (*r)*sin(b);
+
+ double cs,sn;
+ cs = cos(it->first.th+b);
+ sn = sin(it->first.th+b);
+
+ double lx,ly;
+ lx = it->first.x + (*r)*cs;
+ ly = it->first.y + (*r)*sn;
+
+ assert(this->laser_hitpts_len*2 < this->laser_hitpts_size);
+ *(pts++) = lx;
+ *(pts++) = ly;
+ this->laser_hitpts_len++;
+ }
+ }
+
+ //printf("setting %d hit points\n", this->scan_points_count);
this->lock.lock();
+ plan_set_obstacles(this->plan, this->laser_hitpts, this->laser_hitpts_len);
this->lock.unlock();
+
+ // Draw the points
+
+ this->pointcloudMsg.set_points_size(hitpt_cnt/2);
+ this->pointcloudMsg.color.a = 0.0;
+ this->pointcloudMsg.color.r = 0.0;
+ this->pointcloudMsg.color.b = 1.0;
+ this->pointcloudMsg.color.g = 0.0;
+ for(unsigned int i=0;i<hitpt_cnt/2;i++)
+ {
+ this->pointcloudMsg.points[i].x = this->laser_hitpts[2*i];
+ this->pointcloudMsg.points[i].y = this->laser_hitpts[2*i+1];
+ }
+ publish("gui_laser",this->pointcloudMsg);
+ printf("published %d laser hits\n", hitpt_cnt/2);
}
void
@@ -380,6 +541,7 @@
puts("still done");
this->stopRobot();
break;
+ case NEW_GOAL:
case PURSUING_GOAL:
{
// Are we done?
@@ -395,8 +557,9 @@
}
// Try a local plan
- if(plan_do_local(this->plan, this->pose[0], this->pose[1],
- this->plan_halfwidth) < 0)
+ if((this->planner_state == NEW_GOAL) ||
+ (plan_do_local(this->plan, this->pose[0], this->pose[1],
+ this->plan_halfwidth) < 0))
{
// Fallback on global plan
if(plan_do_global(this->plan, this->pose[0], this->pose[1],
@@ -426,6 +589,9 @@
}
}
+ if(this->planner_state == NEW_GOAL)
+ this->planner_state = PURSUING_GOAL;
+
// We have a valid local plan. Now compute controls
double vx, va;
if(plan_compute_diffdrive_cmds(this->plan, &vx, &va,
@@ -446,6 +612,22 @@
break;
}
+ assert(this->plan->path_count);
+
+ this->polylineMsg.set_points_size(this->plan->path_count);
+ this->polylineMsg.color.r = 0;
+ this->polylineMsg.color.g = 1.0;
+ this->polylineMsg.color.b = 0;
+ this->polylineMsg.color.a = 0;
+ for(int i=0;i<this->plan->path_count;i++)
+ {
+ this->polylineMsg.points[i].x =
+ PLAN_WXGX(this->plan,this->plan->path[i]->ci);
+ this->polylineMsg.points[i].y =
+ PLAN_WYGY(this->plan,this->plan->path[i]->cj);
+ }
+ publish("gui_path", polylineMsg);
+
printf("computed velocities: %.3f %.3f\n", vx, RTOD(va));
this->sendVelCmd(vx, 0.0, va);
@@ -526,7 +708,7 @@
occ = color_avg / 255.0;
else
occ = (255 - color_avg) / 255.0;
- if(occ > 0.95)
+ if(occ > 0.5)
(*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = +1;
else if(occ < 0.1)
(*mapdata)[MAP_IDX(*size_x,i,*size_y - j - 1)] = -1;
@@ -542,10 +724,19 @@
return(0);
}
-double
-get_time(void)
+// computes the signed minimum difference between the two angles.
+static double
+angle_diff(double a, double b)
{
- struct timeval curr;
- gettimeofday(&curr,NULL);
- return(curr.tv_sec + curr.tv_usec / 1e6);
+ double d1, d2;
+ a = ANG_NORM(a);
+ b = ANG_NORM(b);
+ d1 = a-b;
+ d2 = 2*M_PI - fabs(d1);
+ if(d1 > 0)
+ d2 *= -1.0;
+ if(fabs(d1) < fabs(d2))
+ return(d1);
+ else
+ return(d2);
}
Modified: pkg/trunk/simulators/rosstage/willow-erratic.world
===================================================================
--- pkg/trunk/simulators/rosstage/willow-erratic.world 2008-05-15 21:16:15 UTC (rev 441)
+++ pkg/trunk/simulators/rosstage/willow-erratic.world 2008-05-15 23:00:48 UTC (rev 442)
@@ -1,3 +1,9 @@
+define block model
+(
+ size3 [0.5 0.5 0.5]
+ gui_nose 0
+)
+
define topurg laser
(
range_min 0.0
@@ -16,7 +22,6 @@
gui_nose 1
drive "diff"
topurg(pose [0.050 0.000 0.000])
- color "blue"
)
define floorplan model
@@ -60,4 +65,5 @@
)
# throw in a robot
-erratic( pose [-11.277 23.266 180.000] name "era")
+erratic( pose [-11.277 23.266 180.000] name "era" color "blue")
+block( pose [-13.924 25.020 180.000] color "red")
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-05-16 01:10:13
|
Revision: 444
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=444&view=rev
Author: gerkey
Date: 2008-05-15 18:10:19 -0700 (Thu, 15 May 2008)
Log Message:
-----------
disabled laser obstacle/u/gerkey/code/personalrobots/nav/wavefront_player/wavefront_player.cc
Modified Paths:
--------------
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/simulators/rosstage/rosstage.cc
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-16 00:10:09 UTC (rev 443)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-16 01:10:19 UTC (rev 444)
@@ -317,6 +317,11 @@
localizedOdomMsg.pos.x = pdata->pos.px;
localizedOdomMsg.pos.y = pdata->pos.py;
localizedOdomMsg.pos.th = pdata->pos.pa;
+ localizedOdomMsg.header.stamp_secs = (unsigned long)floor(hdr->timestamp);
+ localizedOdomMsg.header.stamp_nsecs =
+ (unsigned long)rint(1e9 * (hdr->timestamp -
+ localizedOdomMsg.header.stamp_secs));
+ localizedOdomMsg.__timestamp_override = true;
publish("localizedpose", localizedOdomMsg);
// Also request and publish the particle cloud
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-16 00:10:09 UTC (rev 443)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-16 01:10:19 UTC (rev 444)
@@ -181,6 +181,12 @@
this->laserMsg.intensities[i] = (uint8_t)samples[i].reflectance;
}
+ this->laserMsg.header.stamp_secs =
+ (unsigned long)floor(world->SimTimeNow() / 1e6);
+ this->laserMsg.header.stamp_nsecs =
+ (unsigned long)rint(1e3 * (world->SimTimeNow() -
+ this->laserMsg.header.stamp_secs * 1e6));
+ this->laserMsg.__timestamp_override = true;
publish("scan",this->laserMsg);
}
@@ -197,6 +203,12 @@
// TODO: get the frame ID from somewhere
this->odomMsg.header.frame_id = 2;
+ this->odomMsg.header.stamp_secs =
+ (unsigned long)floor(world->SimTimeNow() / 1e6);
+ this->odomMsg.header.stamp_nsecs =
+ (unsigned long)rint(1e3 * (world->SimTimeNow() -
+ this->odomMsg.header.stamp_secs * 1e6));
+ this->odomMsg.__timestamp_override = true;
publish("odom",this->odomMsg);
this->lock.unlock();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-05-16 03:04:38
|
Revision: 445
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=445&view=rev
Author: gerkey
Date: 2008-05-15 20:04:45 -0700 (Thu, 15 May 2008)
Log Message:
-----------
working on dynamic obstacles
Modified Paths:
--------------
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosstage/willow-erratic.world
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-16 01:10:19 UTC (rev 444)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-16 03:04:45 UTC (rev 445)
@@ -200,7 +200,7 @@
ang_eps(DTOR(10.0)),
cycletime(0.1),
laser_maxrange(4.0),
- laser_buffer_time(2.0),
+ laser_buffer_time(0.5),
lookahead_maxdist(2.0),
lookahead_distweight(10.0),
tvmin(0.1),
@@ -257,7 +257,7 @@
// Compute cspace over static map
plan_compute_cspace(this->plan);
- this->tf = new rosTFClient(*this);
+ //this->tf = new rosTFClient(*this);
this->laser_hitpts_size = this->laser_hitpts_len = 0;
this->laser_hitpts = NULL;
@@ -356,7 +356,8 @@
{
// Interpolate poses for all buffered scans
// TODO: use libTF's interpolation
- for(std::list<MsgLaserScan*>::iterator it = this->buffered_laser_scans.begin();
+ for(std::list<MsgLaserScan*>::iterator it =
+ this->buffered_laser_scans.begin();
it != this->buffered_laser_scans.end();
it++)
{
@@ -393,6 +394,19 @@
p->y = prevOdom.pos.y + (dy / dt) * dtl;
p->th = ANG_NORM(prevOdom.pos.th + (da / dt) * dtl);
+ printf("0: %.3f %.3f %.3f\t%.6f\n",
+ odomMsg.pos.x,
+ odomMsg.pos.y,
+ odomMsg.pos.th,
+ t0);
+ printf("I: %.3f %.3f %.3f\t%.6f\n",
+ p->x,p->y,p->th, tl);
+ printf("1: %.3f %.3f %.3f\t%.6f\n",
+ prevOdom.pos.x,
+ prevOdom.pos.y,
+ prevOdom.pos.th,
+ t1);
+
MsgLaserScan* scan = new MsgLaserScan(this->laserMsg);
std::pair<MsgPose2DFloat32*,MsgLaserScan*> item(p,scan);
@@ -403,25 +417,25 @@
}
// Remove anything that's too old
+ // Also count how many points we have
double currtime = this->laserMsg.header.stamp_secs +
this->laserMsg.header.stamp_nsecs / 1e9;
- // Also count how many points we have
unsigned int hitpt_cnt=0;
for(std::list<std::pair<MsgPose2DFloat32*,MsgLaserScan*> >::iterator it = this->laser_scans.begin();
it != this->laser_scans.end();
it++)
{
- double msgtime = it->second->header.stamp_secs + it->second->header.stamp_nsecs / 1e9;
- if((currtime - msgtime) > this->laser_buffer_time)
- {
- delete it->first;
- delete it->second;
- it = this->laser_scans.erase(it);
- it--;
+ double msgtime = it->second->header.stamp_secs + it->second->header.stamp_nsecs / 1e9;
+ if((currtime - msgtime) > this->laser_buffer_time)
+ {
+ delete it->first;
+ delete it->second;
+ it = this->laser_scans.erase(it);
+ it--;
+ }
+ else
+ hitpt_cnt += it->second->get_ranges_size()*2;
}
- else
- hitpt_cnt += it->second->get_ranges_size()*2;
- }
// allocate more space as necessary
if(this->laser_hitpts_size < hitpt_cnt)
@@ -480,7 +494,6 @@
this->pointcloudMsg.points[i].y = this->laser_hitpts[2*i+1];
}
publish("gui_laser",this->pointcloudMsg);
- printf("published %d laser hits\n", hitpt_cnt/2);
prevOdom = odomMsg;
}
@@ -493,7 +506,9 @@
WavefrontNode::laserReceived()
{
// Buffer the scan. It will get processed in odomReceived()
+ this->lock.lock();
this->buffered_laser_scans.push_back(new MsgLaserScan(laserMsg));
+ this->lock.unlock();
}
void
Modified: pkg/trunk/simulators/rosstage/willow-erratic.world
===================================================================
--- pkg/trunk/simulators/rosstage/willow-erratic.world 2008-05-16 01:10:19 UTC (rev 444)
+++ pkg/trunk/simulators/rosstage/willow-erratic.world 2008-05-16 03:04:45 UTC (rev 445)
@@ -8,8 +8,8 @@
(
range_min 0.0
range_max 30.0
- fov 282.0
- samples 1128
+ fov 270.25
+ samples 1081
# generic model properties
color "black"
size [ 0.05 0.05 0.1 ]
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-05-16 17:49:33
|
Revision: 449
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=449&view=rev
Author: gerkey
Date: 2008-05-16 10:49:36 -0700 (Fri, 16 May 2008)
Log Message:
-----------
added documentation
Modified Paths:
--------------
pkg/trunk/3rdparty/player/manifest.xml
pkg/trunk/3rdparty/stage/manifest.xml
pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc
pkg/trunk/drivers/laser/hokuyourg_player/manifest.xml
pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/nav_view/nav_view.cpp
pkg/trunk/nav/teleop_base_keyboard/manifest.xml
pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc
pkg/trunk/nav/wavefront_player/manifest.xml
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosstage/rosstage.cc
pkg/trunk/unported/laser_viewer/src/laser_viewer/laser_viewer.cpp
Modified: pkg/trunk/3rdparty/player/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/player/manifest.xml 2008-05-16 07:15:57 UTC (rev 448)
+++ pkg/trunk/3rdparty/player/manifest.xml 2008-05-16 17:49:36 UTC (rev 449)
@@ -13,7 +13,7 @@
<url>http://playerstage.sf.net</url>
<export>
<cpp lflags="-Xlinker -rpath ${prefix}/player/lib -L${prefix}/player/lib -lplayercore -lplayerxdr -lplayerutils -lplayererror" cflags="-I${prefix}/player/include/player-2.2"/>
- <doxymaker external="http://playerstage.sourceforge.net/doc/Player-2.0.0/player/" />
+ <doxymaker external="http://playerstage.sourceforge.net/doc/Player-cvs/player"/>
</export>
</package>
Modified: pkg/trunk/3rdparty/stage/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/stage/manifest.xml 2008-05-16 07:15:57 UTC (rev 448)
+++ pkg/trunk/3rdparty/stage/manifest.xml 2008-05-16 17:49:36 UTC (rev 449)
@@ -13,6 +13,7 @@
<url>http://playerstage.sf.net</url>
<export>
<cpp lflags="-Wl,-rpath,${prefix}/stage/lib `PKG_CONFIG_PATH=${prefix}/stage/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --libs stage`" cflags="`PKG_CONFIG_PATH=${prefix}/stage/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --cflags stage`"/>
+ <doxymaker external="http://playerstage.sourceforge.net/doc/stage-cvs"/>
</export>
</package>
Modified: pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc
===================================================================
--- pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc 2008-05-16 07:15:57 UTC (rev 448)
+++ pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc 2008-05-16 17:49:36 UTC (rev 449)
@@ -28,6 +28,56 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+@b hokuyourg_player is a driver for the Hokuyo URG line of laser range-finders.
+
+This node uses part of the the Player @b urglaser driver. For detailed documentation,
+consult <a href="http://playerstage.sourceforge.net/doc/Player-cvs/player/group__driver__urglaser.html">Player urglaser documentation</a>.
+Note that this node does not actually wrap the @b
+urglaser driver, but rather calls into the underlying library, @b
+liburglaser_standalone.
+
+This node should be capable to controlling any of Hokuyo's URG laser.
+However, to date is has only been tested on the TOP-URG.
+
+<hr>
+
+@section usage Usage
+@verbatim
+$ hokuyourg_player [standard ROS args]
+@endverbatim
+
+@par Example
+
+@verbatim
+$ hokuyourg_player
+@endverbatim
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- None
+
+Publishes to (name / type):
+- @b "scan"/LaserScan : scan data from the laser.
+
+<hr>
+
+@section parameters ROS parameters
+
+- None
+
+@todo Expose the various urglaser parameters via ROS.
+
+ **/
+
#include <assert.h>
#include <libstandalone_drivers/urg_laser.h>
Modified: pkg/trunk/drivers/laser/hokuyourg_player/manifest.xml
===================================================================
--- pkg/trunk/drivers/laser/hokuyourg_player/manifest.xml 2008-05-16 07:15:57 UTC (rev 448)
+++ pkg/trunk/drivers/laser/hokuyourg_player/manifest.xml 2008-05-16 17:49:36 UTC (rev 449)
@@ -1,6 +1,6 @@
<package>
- <description>A ROS node that wraps up the Player urglaser driver, which provides access to a wide range of Hokuyo URG lasers.</description>
- <author>Brian P. Gerkey</author>
+ <description>A ROS node that uses the Player liburglaser_standalone library, which provides access to a wide range of Hokuyo URG lasers.</description>
+ <author>Brian P. Gerkey, Jeremy Leibs</author>
<license>BSD</license>
<depend package="roscpp" />
<depend package="player" />
Modified: pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2008-05-16 07:15:57 UTC (rev 448)
+++ pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2008-05-16 17:49:36 UTC (rev 449)
@@ -28,6 +28,51 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+@b erratic_player is a driver for the Erratic mobile robot, available from
+<a href="http://www.videredesign.com">Videre Design</a>.
+
+This node wraps up the Player @b erratic driver. For detailed documentation,
+consult <a href="http://playerstage.sourceforge.net/doc/Player-cvs/player/group__driver__erratic.html">Player erratic documentation</a>.
+
+<hr>
+
+@section usage Usage
+@verbatim
+$ erratic_player [standard ROS args]
+@endverbatim
+
+@par Example
+
+@verbatim
+$ erratic_player
+@endverbatim
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- @b "cmd_vel"/BaseVel : velocity commands to differentially drive the robot.
+
+Publishes to (name / type):
+- @b "odom"/RobotBase2DOdom : odometry data from the robot.
+
+<hr>
+
+@section parameters ROS parameters
+
+- None
+
+@todo Expose the various erratic parameters via ROS.
+
+ **/
+
#include <assert.h>
// For core Player stuff (message queues, config file objects, etc.)
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-16 07:15:57 UTC (rev 448)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-16 17:49:36 UTC (rev 449)
@@ -28,6 +28,66 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+@b amcl_player is a probabilistic localization system for a robot moving in
+2D. It implements the adaptive Monte Carlo localization algorithm (as
+described by Dieter Fox), which uses a particle filter to track the pose of
+a robot against a known map.
+
+This node wraps up the Player @b amcl driver. For detailed documentation,
+consult <a href="http://playerstage.sourceforge.net/doc/Player-cvs/player/group__driver__amcl.html">Player amcl documentation</a>.
+
+<hr>
+
+@section usage Usage
+@verbatim
+$ amcl_player <map> <res> <x> <y> <th> [standard ROS args]
+@endverbatim
+
+@param map An image file to load as an occupancy grid map. The robot will be localized against this map using laser scans. The lower-left pixel of the map is assigned the pose (0,0,0).
+@param res The resolution of the map, in meters, pixel.
+@param (x,y,th) Approximate pose of robot, used to initialize particle filter. Units are (meters,meters,degrees).
+
+@todo Remove the map and res arguments in favor map retrieval via ROSRPC.
+@todo Remove the x,y,th arguments and expose the particle filter initialization via a ROS topic.
+
+@par Example
+
+@verbatim
+$ amcl_player mymap.png 0.1 10.4 31.2 90.0
+@endverbatim
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- @b "odom"/RobotBase2DOdom : robot's odometric pose. Only the position information is used (velocity is ignored).
+- @b "scan"/LaserScan : laser scans.
+
+Publishes to (name / type):
+- @b "localizedpose"/RobotBase2DOdom : robot's localized map pose. Only the position information is set (no velocity).
+- @b "particlecloud"/ParticleCloud2D : the set of particles being maintained by the filter.
+
+@todo Start using libTF for transform management:
+ - remove the manual interpolation of poses to attach to laser scans
+ - add the ability to set the laser's pose relative to the robot (currently assumed to be 0,0,0).
+ - publish map->odom transform, instead of map pose
+
+<hr>
+
+@section parameters ROS parameters
+
+- None
+
+@todo Expose the various amcl parameters via ROS.
+
+ **/
#include <assert.h>
// For core Player stuff (message queues, config file objects, etc.)
Modified: pkg/trunk/nav/nav_view/nav_view.cpp
===================================================================
--- pkg/trunk/nav/nav_view/nav_view.cpp 2008-05-16 07:15:57 UTC (rev 448)
+++ pkg/trunk/nav/nav_view/nav_view.cpp 2008-05-16 17:49:36 UTC (rev 449)
@@ -29,18 +29,61 @@
*/
/**
- *
- * @mainpage
- *
- * @htmlinclude manifest.html
- *
- *
- * @b nav_view is a GUI for 2-D navigation. It can:
- * - display a map
- * - display a robot's pose
- * - display a cloud of particles (e.g., from a localization system)
- * - display a path (e.g., from a path planner)
- *
+
+@mainpage
+
+@htmlinclude manifest.html
+
+@b nav_view is a GUI for 2-D navigation. It can:
+ - display a map
+ - display a robot's pose
+ - display a cloud of particles (e.g., from a localization system)
+ - display a path (e.g., from a path planner)
+ - send a goal to a planner
+
+<hr>
+
+@section usage Usage
+@verbatim
+$ nav_view [map res] [standard ROS args]
+@endverbatim
+
+@param map An image file to load as an occupancy grid map. The lower-left pixel of the map is assigned the pose (0,0,0).
+@param res The resolution of the map, in meters, pixel.
+
+@todo Remove the map and res arguments in favor map retrieval via ROSRPC.
+
+@par Example
+
+@verbatim
+$ nav_view mymap.png 0.1 odom:=localizedpose
+@endverbatim
+
+@par GUI controls
+Mouse bindings:
+- drag left button: pan view
+- drag right button: zoom view
+- click middle button: send goal
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- @b "odom"/RobotBase2DOdom : the robot's 2D pose. Rendered as a circle with a heading line.
+- @b "particlecloud"/ParticleCloud2D : a set particles from a probabilistic localization system. Rendered is a set of small arrows.
+- @b "gui_path"/Polyline2D : a path from a planner. Rendered as a dashed line.
+- @b "gui_laser"/Polyline2D : re-projected laser scan from a planner. Rendered as a set of points.
+
+Publishes to (name / type):
+- @b "goal"/Planner2DGoal : goal for planner. Sent on middle button click.
+
+<hr>
+
+@section parameters ROS parameters
+
+- None
+
**/
#include <unistd.h>
Modified: pkg/trunk/nav/teleop_base_keyboard/manifest.xml
===================================================================
--- pkg/trunk/nav/teleop_base_keyboard/manifest.xml 2008-05-16 07:15:57 UTC (rev 448)
+++ pkg/trunk/nav/teleop_base_keyboard/manifest.xml 2008-05-16 17:49:36 UTC (rev 449)
@@ -1,5 +1,5 @@
<package>
-<description>A ROS that supports velocity-mode teleoperation of a
+<description>A ROS node that supports velocity-mode teleoperation of a
differential drive robot base from keyboard input</description>
<author>Brian P. Gerkey</author>
<license>BSD</license>
Modified: pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc
===================================================================
--- pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc 2008-05-16 07:15:57 UTC (rev 448)
+++ pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cc 2008-05-16 17:49:36 UTC (rev 449)
@@ -28,6 +28,43 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+@b teleop_base_keyboard teleoperates a differential-drive robot by mapping
+key presses into velocity commands. Consider it a poor man's joystick.
+
+<hr>
+
+@section usage Usage
+@verbatim
+$ teleop_base_keyboard [standard ROS args]
+@endverbatim
+
+Key mappings are printed to screen on startup. Press any unmapped key to
+stop the robot.
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- None
+
+Publishes to (name / type):
+- @b "cmd_vel"/BaseVel : velocity to the robot; sent on every keypress.
+
+<hr>
+
+@section parameters ROS parameters
+
+- None
+
+ **/
+
#include <termios.h>
#include <signal.h>
#include <math.h>
Modified: pkg/trunk/nav/wavefront_player/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront_player/manifest.xml 2008-05-16 07:15:57 UTC (rev 448)
+++ pkg/trunk/nav/wavefront_player/manifest.xml 2008-05-16 17:49:36 UTC (rev 449)
@@ -1,5 +1,5 @@
<package>
- <description>A ROS node that wraps up the Player wavefront driver, which implements wavefront path-planning for a planar robot.</description>
+ <description>A ROS node that uses the Player libwavefront_standalone library, which implements wavefront path-planning for a planar robot.</description>
<author>Brian P. Gerkey</author>
<license>BSD</license>
<depend package="roscpp" />
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-16 07:15:57 UTC (rev 448)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-16 17:49:36 UTC (rev 449)
@@ -28,6 +28,87 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+@b wavefront_player is a path-planning system for a robot moving in 2D. It
+implements the wavefront algorithm (described in various places; Latombe's
+book is a good reference), which uses dynamic programming over an occupancy
+grid map to find the lowest-cost path from the robot's pose to the goal.
+
+This node uses part of the Player @b wavefront driver. For detailed
+documentation, consult <a
+href="http://playerstage.sourceforge.net/doc/Player-cvs/player/group__driver__wavefront.html">Player
+wavefront documentation</a>. Note that this node does not actually wrap the @b
+wavefront driver, but rather calls into the underlying library, @b
+libwavefront_standalone.
+
+The planning algorithm assumes that the robot is circular and holonomic.
+Under these assumptions, it efficiently dilates obstacles and then plans
+over the map as if the robot occupied a single grid cell.
+
+If provided, laser scans are used to temporarily modify the map. In this
+way, the planner can avoid obstacles that are not in the static map.
+
+The node also contains a controller that generates velocities for a
+differential drive robot. The intended usage is to run the node at a
+modest rate, e.g., 20Hz, allowing it to replan and generate new controls
+every cycle. The controller ignores dynamics (i.e., assumes infinite
+acceleration); this becomes a problem with robots that take non-trivial
+time to slow down.
+
+<hr>
+
+@section usage Usage
+@verbatim
+$ wavefront_player <map> <res> [standard ROS args]
+@endverbatim
+
+@param map An image file to load as an occupancy grid map. The robot will be localized against this map using laser scans. The lower-left pixel of the map is assigned the pose (0,0,0).
+@param res The resolution of the map, in meters, pixel.
+
+@todo Remove the map and res arguments in favor map retrieval via ROSRPC.
+
+@par Example
+
+@verbatim
+$ wavefront_player mymap.png 0.1
+@endverbatim
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- @b "localizedpose"/RobotBase2DOdom : robot's map pose. Only the position information is used (velocity is ignored).
+- @b "goal"/Planner2DGoal : goal for the robot.
+- @b "scan"/LaserScan : laser scans. Used to temporarily modify the map for dynamic obstacles.
+
+Publishes to (name / type):
+- @b "cmd_vel"/BaseVel : velocity commands to robot
+- @b "state"/Planner2DState : current planner state (e.g., goal reached, no
+path)
+- @b "gui_path"/Polyline2D : current global path (for visualization)
+- @b "gui_laser"/Polyline2D : re-projected laser scans (for visualization)
+
+@todo Start using libTF for transform management:
+ - subscribe to odometry and use transform to recover map pose.
+
+<hr>
+
+@section parameters ROS parameters
+
+- None
+
+@todo There are an enormous number of parameters, values for which are all
+currently hardcoded. These should be exposed as ROS parameters. In
+particular, robot radius and safety distance must be changed for each
+robot.
+
+ **/
#include <stdio.h>
#include <assert.h>
#include <stdlib.h>
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-16 07:15:57 UTC (rev 448)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-16 17:49:36 UTC (rev 449)
@@ -17,6 +17,63 @@
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+@b rosstage wraps the Stage 2-D multi-robot simulator, via @b libstage.
+
+For detailed documentation,
+consult the <a href="http://playerstage.sourceforge.net/doc/stage-cvs">Stage manual</a>.
+
+This node finds the first Stage model of type @b laser, and the first model
+of type @b position, and maps these models to the ROS topics given below.
+If a laser and a position model are not found, rosstage exits.
+
+@todo Define a more general method for mapping Stage models onto ROS topics
+/ services. Something like the Player/Stage model, in which a Player .cfg
+file is used to map named Stage models onto Player devices, is probably the
+way to go. The same technique can be used for rosgazebo.
+
+<hr>
+
+@section usage Usage
+@verbatim
+$ rosstage <world> [standard ROS args]
+@endverbatim
+
+@param world The Stage .world file to load.
+
+@par Example
+
+@verbatim
+$ rosstage willow-erratic.world
+@endverbatim
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- @b "cmd_vel"/BaseVel : velocity commands to differentially drive the
+position model.
+
+Publishes to (name / type):
+- @b "odom"/RobotBase2DOdom : odometry data from the position model.
+- @b "scan"/LaserScan : scans from the laser model.
+
+
+<hr>
+
+@section parameters ROS parameters
+
+- None
+
+
+ **/
+
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
Modified: pkg/trunk/unported/laser_viewer/src/laser_viewer/laser_viewer.cpp
===================================================================
--- pkg/trunk/unported/laser_viewer/src/laser_viewer/laser_viewer.cpp 2008-05-16 07:15:57 UTC (rev 448)
+++ pkg/trunk/unported/laser_viewer/src/laser_viewer/laser_viewer.cpp 2008-05-16 17:49:36 UTC (rev 449)
@@ -32,6 +32,50 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+@b laser_viewer is a GUI for displaying laser scans, in 3-D.
+
+<hr>
+
+@section usage Usage
+@verbatim
+$ laser_viewer [standard ROS args]
+@endverbatim
+
+@par Example
+
+@verbatim
+$ laser_viewer
+@endverbatim
+
+@par GUI controls
+
+GL-style mouse-based manipulation.
+
+@todo Actually document the controls.
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- @b "scan"/LaserScan : scans to display
+
+Publishes to (name / type):
+- None
+
+<hr>
+
+@section parameters ROS parameters
+
+- None
+
+ **/
#include "ros/node.h"
#include "std_msgs/MsgLaserScan.h"
#include "cloud_viewer/cloud_viewer.h"
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2008-05-18 06:38:45
|
Revision: 459
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=459&view=rev
Author: morgan_quigley
Date: 2008-05-17 23:38:44 -0700 (Sat, 17 May 2008)
Log Message:
-----------
minor tweaks to default settings for flea2, and resurrecting the cv_cam driver from 'unported'
Modified Paths:
--------------
pkg/trunk/drivers/cam/ptgrey/flea2/src/libflea2/flea2.cpp
pkg/trunk/drivers/cam/ptgrey/flea2/test/view/view.cpp
Added Paths:
-----------
pkg/trunk/drivers/cam/cv_cam/
Removed Paths:
-------------
pkg/trunk/unported/cv_cam/
Copied: pkg/trunk/drivers/cam/cv_cam (from rev 456, pkg/trunk/unported/cv_cam)
Modified: pkg/trunk/drivers/cam/ptgrey/flea2/src/libflea2/flea2.cpp
===================================================================
--- pkg/trunk/drivers/cam/ptgrey/flea2/src/libflea2/flea2.cpp 2008-05-18 06:30:38 UTC (rev 458)
+++ pkg/trunk/drivers/cam/ptgrey/flea2/src/libflea2/flea2.cpp 2008-05-18 06:38:44 UTC (rev 459)
@@ -86,6 +86,9 @@
fprintf(stderr, "Unable to start camera iso transmission\n" );
throw std::runtime_error("asdf");
}
+ set_shutter(0.8);
+ set_gamma(0.24);
+ set_gain(0);
}
Flea2::~Flea2()
Modified: pkg/trunk/drivers/cam/ptgrey/flea2/test/view/view.cpp
===================================================================
--- pkg/trunk/drivers/cam/ptgrey/flea2/test/view/view.cpp 2008-05-18 06:30:38 UTC (rev 458)
+++ pkg/trunk/drivers/cam/ptgrey/flea2/test/view/view.cpp 2008-05-18 06:38:44 UTC (rev 459)
@@ -48,11 +48,11 @@
uint8_t *frame;
uint32_t w, h;
- double shutter = 0.2;
+ double shutter = 0.8;
flea2.set_shutter(shutter);
- double gamma = 0.3;
+ double gamma = 0.24;
flea2.set_gamma(gamma);
- double gain = 0.5;
+ double gain = 0;
flea2.set_gain(gain);
bool done = false;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ehb...@us...> - 2008-05-18 23:07:44
|
Revision: 461
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=461&view=rev
Author: ehberger
Date: 2008-05-18 16:07:42 -0700 (Sun, 18 May 2008)
Log Message:
-----------
xenomai linker/compile flags and xenomai test node
Added Paths:
-----------
pkg/trunk/realtime/
pkg/trunk/realtime/xenomai/
pkg/trunk/realtime/xenomai/manifest.xml
pkg/trunk/realtime/xenomai_demo/
pkg/trunk/realtime/xenomai_demo/Makefile
pkg/trunk/realtime/xenomai_demo/README
pkg/trunk/realtime/xenomai_demo/manifest.xml
pkg/trunk/realtime/xenomai_demo/xenomai_demo.cpp
Added: pkg/trunk/realtime/xenomai/manifest.xml
===================================================================
--- pkg/trunk/realtime/xenomai/manifest.xml (rev 0)
+++ pkg/trunk/realtime/xenomai/manifest.xml 2008-05-18 23:07:42 UTC (rev 461)
@@ -0,0 +1,15 @@
+<package>
+ <description brief="Xenomai dependencies and build flags"> This package ensures that Xenomai is
+properly installed on the system and provides the build flags to create realtime ROS nodes that
+depend on Xenomai. </description>
+ <author>Eric Berger be...@wi...></author>
+ <license>BSD</license>
+ <export>
+ <cpp
+ cflags="-I/usr/xenomai/include -I/usr/xenomai/src/include -D_GNU_SOURCE -D_REENTRANT -D__XENO__"
+ lflags="-L/usr/xenomai/lib -lnative -lrtdk -Xlinker -rpath -Xlinker /usr/xenomai/lib"/>
+ <!--cpp
+ cflags="-I${prefix}/xenomai/include -I{$prefix}/xenomai/src/include -D_GNU_SOURCE -D_REENTRANT -D__XENO__"
+ lflags="-L${prefix}/xenomai/lib -lnative -lrtdk -Xlinker -rpath -Xlinker {$prefix}/xenomai/lib"/-->
+ </export>
+</package>
Added: pkg/trunk/realtime/xenomai_demo/Makefile
===================================================================
--- pkg/trunk/realtime/xenomai_demo/Makefile (rev 0)
+++ pkg/trunk/realtime/xenomai_demo/Makefile 2008-05-18 23:07:42 UTC (rev 461)
@@ -0,0 +1,4 @@
+SRC = xenomai_demo.cpp
+OUT = xenomai_demo
+PKG = xenomai_demo
+include $(shell rospack find mk)/node.mk
Added: pkg/trunk/realtime/xenomai_demo/README
===================================================================
--- pkg/trunk/realtime/xenomai_demo/README (rev 0)
+++ pkg/trunk/realtime/xenomai_demo/README 2008-05-18 23:07:42 UTC (rev 461)
@@ -0,0 +1,4 @@
+This is a test of the realtime execution system.
+Xenomai must be properly installed for this code to compile and run.
+All this code does is instantiate a realtime loop and print statistics about the actual execution time. All printed values are in
+ms.
Added: pkg/trunk/realtime/xenomai_demo/manifest.xml
===================================================================
--- pkg/trunk/realtime/xenomai_demo/manifest.xml (rev 0)
+++ pkg/trunk/realtime/xenomai_demo/manifest.xml 2008-05-18 23:07:42 UTC (rev 461)
@@ -0,0 +1,9 @@
+<package>
+ <description brief = "Package to test Xenomai">
+ Xenomai test program. Designed to test that Xenomai build system and runtime system are both configured correctly and quantify
+timing jitter.
+ </description>
+ <depend package="roscpp"/>
+ <depend package="std_msgs"/>
+ <depend package="xenomai"/>
+</package>
Added: pkg/trunk/realtime/xenomai_demo/xenomai_demo.cpp
===================================================================
--- pkg/trunk/realtime/xenomai_demo/xenomai_demo.cpp (rev 0)
+++ pkg/trunk/realtime/xenomai_demo/xenomai_demo.cpp 2008-05-18 23:07:42 UTC (rev 461)
@@ -0,0 +1,117 @@
+#include <stdio.h>
+#include <signal.h>
+#include <unistd.h>
+#include <sys/mman.h>
+
+#include <native/task.h>
+#include <native/timer.h>
+
+RT_TASK demo_task;
+
+#include "ros/node.h"
+#include "std_msgs/MsgString.h"
+
+/* NOTE: error handling omitted. */
+
+float period = 1; //period in milliseconds
+
+float elapsed = 0.0;
+long elapsed_ticks = 0;
+double max_elapsed = 0.0;
+double min_elapsed = period * 2;
+double total_elapsed = 0.0;
+long int num_overruns = 0;
+long int num_ticks = 0;
+
+void demo(void *arg)
+{
+ RTIME now, previous;
+
+ /*
+ * Arguments: &task (NULL=self),
+ * start time,
+ * period)
+ */
+ rt_task_set_periodic(NULL, TM_NOW, period * 1000000);
+ previous = rt_timer_read();
+
+ while (1) {
+ rt_task_wait_period(NULL);
+ now = rt_timer_read();
+ elapsed_ticks = (long)(now - previous);
+ previous = now;
+
+ elapsed = elapsed_ticks / 1000000.0;
+ if(elapsed > max_elapsed)
+ max_elapsed = elapsed;
+ if(elapsed < min_elapsed)
+ min_elapsed = elapsed;
+ if(elapsed_ticks > 1.1 * period * 1000000)
+ num_overruns++;
+ num_ticks++;
+ total_elapsed += elapsed;
+ }
+}
+
+void cleanup(int sig)
+{
+ printf("Goodbye.\n");
+ rt_task_delete(&demo_task);
+ //exit(0);
+}
+
+class XenomaiTest : public ros::node
+{
+public:
+ MsgString msg;
+
+ XenomaiTest() : ros::node("XenomaiTest")
+ {
+ advertise<MsgString>("status");
+ }
+
+ void update(char *s)
+ {
+ msg.data = string(s);
+ publish("status", msg);
+ }
+};
+
+int main(int argc, char* argv[])
+{
+ char msg[1024];
+ //signal(SIGTERM, cleanup);
+ //signal(SIGINT, cleanup);
+
+ /* Avoids memory swapping for this program */
+ mlockall(MCL_CURRENT|MCL_FUTURE);
+
+ rt_timer_set_mode(TM_ONESHOT);
+ //rt_timer_set_mode(900000);
+
+ /*
+ * Arguments: &task,
+ * name,
+ * stack size (0=default),
+ * priority,
+ * mode (FPU, start suspended, ...)
+ */
+ rt_task_create(&demo_task, "trivial", 0, 99, XNFPU);
+
+ /*
+ * Arguments: &task,
+ * task function,
+ * function argument
+ */
+ rt_task_start(&demo_task, &demo, NULL);
+
+ ros::init(argc, argv);
+ XenomaiTest x;
+ while(x.ok()){
+ usleep(100000);
+ snprintf(msg, sizeof(msg), "(milliseconds) min offset: %f \t max offset: %f \t average offset:%f \t overruns: %ld/%ld\n", min_elapsed - period, max_elapsed - period, total_elapsed / num_ticks - period, num_overruns, num_ticks);
+ printf(msg);
+ x.update(msg);
+ }
+ cleanup(0);
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-05-22 00:24:45
|
Revision: 495
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=495&view=rev
Author: jleibs
Date: 2008-05-21 17:24:52 -0700 (Wed, 21 May 2008)
Log Message:
-----------
Assembly and viewing of 3d point clouds with intensity data.
Modified Paths:
--------------
pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc
pkg/trunk/drivers/laser/urg_driver/urg_laser.cc
pkg/trunk/drivers/laser/urg_driver/urg_laser.h
pkg/trunk/drivers/motor/etherdrive/src/etherdrive_node/etherdrive_node.cpp
pkg/trunk/drivers/robot/pr2/tilting_laser/launch_tl
pkg/trunk/drivers/robot/pr2/tilting_laser/manifest.xml
pkg/trunk/drivers/robot/pr2/tilting_laser/params.xml
pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp
pkg/trunk/unported/vision/cloud_viewer/src/Makefile
pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/Makefile
pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/cloud_node.cpp
Added Paths:
-----------
pkg/trunk/unported/vision/cloud_viewer/bin/
Modified: pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc
===================================================================
--- pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/laser/hokuyourg_player/hokuyourg_player.cc 2008-05-22 00:24:52 UTC (rev 495)
@@ -110,27 +110,27 @@
MsgLaserScan scan_msg;
double min_ang;
double max_ang;
+
+ int cluster;
+ int skip;
+
string port;
HokuyoNode() : ros::node("urglaser"), count(0), next_time(0)
{
advertise<MsgLaserScan>("scan");
- // TODO: add min/max angle support
- /*
- if (!get_double_param(".min_ang", &min_ang))
- min_ang = -90;
- printf("Setting min_ang to: %g\n",min_ang);
+ param("min_ang", min_ang, -90.0);
+ min_ang *= M_PI/180;
- if (!get_double_param(".max_ang", &max_ang))
- max_ang = 90;
- printf("Setting max_ang to: %g\n",max_ang);
- */
+ param("max_ang", max_ang, -90.0);
+ max_ang *= M_PI/180;
- if (!has_param("port") || !get_param("port", port))
- port = "/dev/ttyACM0";
- printf("Setting port to: %s\n",port.c_str());
+ param("cluster", cluster, 1);
+ param("skip", skip, 1);
+ param("port", port, string("/dev/ttyACM0"));
+
scan = new urg_laser_scan_t;
assert(scan);
running = false;
@@ -157,9 +157,8 @@
urg.urg_cmd("BM");
+ int status = urg.request_scans(true, min_ang, max_ang, cluster, skip);
- int status = urg.request_scans(true, -M_PI/2.0, M_PI/2.0);
-
if (status != 0) {
printf("Failed to request scans %d.\n", status);
return -1;
@@ -180,8 +179,8 @@
int publish_scan()
{
- // TODO: add support for pushing readings out
+
// int status = urg.poll_scan(scan, -M_PI/2.0, M_PI/2.0);
int status = urg.service_scan(scan);
if(status != 0)
@@ -198,23 +197,16 @@
next_time += 1000000000;
}
- // printf("%d readings\n", readings->num_readings);
-
scan_msg.angle_min = scan->config.min_angle;
scan_msg.angle_max = scan->config.max_angle;
scan_msg.angle_increment = scan->config.resolution;
-
- // printf("%g %g %g\n", scan.angle_min * 180.0/M_PI, scan.angle_max*180.0/M_PI, scan.angle_increment*180.0/M_PI);
-
scan_msg.range_max = cfg.max_range;
scan_msg.set_ranges_size(scan->num_readings);
scan_msg.set_intensities_size(scan->num_readings);
for(int i = 0; i < scan->num_readings; ++i)
{
- // scan.ranges[i] = readings->ranges[i] < 20 ? (scan.range_max) : (readings->ranges[i] / 1000.0);
scan_msg.ranges[i] = scan->ranges[i];
- // TODO: add intensity support
scan_msg.intensities[i] = scan->intensities[i];
}
publish("scan", scan_msg);
Modified: pkg/trunk/drivers/laser/urg_driver/urg_laser.cc
===================================================================
--- pkg/trunk/drivers/laser/urg_driver/urg_laser.cc 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/laser/urg_driver/urg_laser.cc 2008-05-22 00:24:52 UTC (rev 495)
@@ -151,6 +151,8 @@
///////////////////////////////////////////////////////////////////////////////
int urg_laser::urg_cmd(const char* cmd, int timeout)
{
+ printf("Sending CMD: %s\n",cmd);
+
char buf[100];
if (urg_write(cmd) < 0)
@@ -615,8 +617,6 @@
sprintf(cmdbuf,"M%c%.4d%.4d%.2d%.1d%.2d", intensity_char, min_i, max_i, cluster, skip, count);
- printf("Sending command: %s\n", cmdbuf);
-
status = urg_cmd(cmdbuf, timeout);
return status;
Modified: pkg/trunk/drivers/laser/urg_driver/urg_laser.h
===================================================================
--- pkg/trunk/drivers/laser/urg_driver/urg_laser.h 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/laser/urg_driver/urg_laser.h 2008-05-22 00:24:52 UTC (rev 495)
@@ -60,6 +60,8 @@
float intensities[MAX_READINGS];
//! Number of readings
int num_readings;
+ //! Self reported time
+ int time_stamp;
//! Configuration of scan
urg_laser_config_t config;
} urg_laser_scan_t;
Modified: pkg/trunk/drivers/motor/etherdrive/src/etherdrive_node/etherdrive_node.cpp
===================================================================
--- pkg/trunk/drivers/motor/etherdrive/src/etherdrive_node/etherdrive_node.cpp 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/motor/etherdrive/src/etherdrive_node/etherdrive_node.cpp 2008-05-22 00:24:52 UTC (rev 495)
@@ -69,9 +69,10 @@
for (int i = 0;i < 6;i++) {
ostringstream oss;
- oss << ".pulse_per_rad" << i;
+ oss << "pulse_per_rad" << i;
if (!get_param(oss.str().c_str(), pulse_per_rad[i]))
pulse_per_rad[i] = 1591.54943;
+ printf("Using %g pulse_per_rad for motor %d\n", pulse_per_rad[i], i);
}
ed = new EtherDrive();
@@ -115,6 +116,8 @@
return false;
}
+ // printf("%d %d %d\n",ed->get_enc(0), ed->get_cur(0), ed->get_pwm(0));
+
for (int i = 0; i < 6; i++) {
mot[i].val = val[i] / pulse_per_rad[i];
mot[i].rel = false;
@@ -135,11 +138,14 @@
ros::init(argc, argv);
EtherDrive_Node a;
while (a.ok()) {
+
+ usleep(1000);
if (!a.do_tick())
{
a.log(ros::ERROR,"Etherdrive tick failed.");
break;
}
}
+ ros::fini();
return 0;
}
Modified: pkg/trunk/drivers/robot/pr2/tilting_laser/launch_tl
===================================================================
--- pkg/trunk/drivers/robot/pr2/tilting_laser/launch_tl 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/robot/pr2/tilting_laser/launch_tl 2008-05-22 00:24:52 UTC (rev 495)
@@ -1,6 +1,10 @@
#!/bin/bash
`rospack find xmlparam`/xmlparam `rospack find tilting_laser`/params.xml
-xterm -e `rospack find hokuyourg_player`/hokuyourg_player&
-xterm -e `rospack find etherdrive`/bin/etherdrive&
+export ROS_NAMESPACE=HK
+xterm -e `rospack find hokuyourg_player`/hokuyourg_player scan:=/scan&
+export ROS_NAMESPACE=ED
+xterm -e `rospack find etherdrive`/bin/etherdrive mot0:=/mot0 mot0_cmd:=/mot0_cmd&
+export ROS_NAMESPACE=CV
+xterm -e `rospack find cloud_viewer`/bin/cloud_node shutter:=/shutter cloud:=/cloud&
export ROS_NAMESPACE=TL
-xterm -e `rospack find tilting_laser`/bin/tilting_laser mot:=/mot2 mot_cmd:=/mot2_cmd scan:=/scan&
+xterm -e `rospack find tilting_laser`/bin/tilting_laser mot:=/mot0 mot_cmd:=/mot0_cmd scan:=/scan cloud:=/cloud shutter:=/shutter&
Modified: pkg/trunk/drivers/robot/pr2/tilting_laser/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/tilting_laser/manifest.xml 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/robot/pr2/tilting_laser/manifest.xml 2008-05-22 00:24:52 UTC (rev 495)
@@ -13,6 +13,8 @@
<depend package="std_msgs"/>
<depend package="unstable_msgs"/>
<depend package="hokuyourg_player"/>
+<depend package="etherdrive"/>
+<depend package="cloud_viewer"/>
<depend package="libTF"/>
</package>
Modified: pkg/trunk/drivers/robot/pr2/tilting_laser/params.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/tilting_laser/params.xml 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/robot/pr2/tilting_laser/params.xml 2008-05-22 00:24:52 UTC (rev 495)
@@ -1,5 +1,10 @@
<params>
- <double name="TL/period">5.0</double>
- <double name="TL/min_ang">-1.3</double>
+ <double name="HK/min_ang">-70</double>
+ <double name="HK/max_ang">70</double>
+ <int name="HK/cluster">1</int>
+ <int name="HK/skip">0</int>
+ <double name="ED/pulse_per_rad0">9549.29659</double>
+ <double name="TL/period">10.0</double>
+ <double name="TL/min_ang">-1</double>
<double name="TL/max_ang">0.6</double>
</params>
Modified: pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp 2008-05-22 00:24:52 UTC (rev 495)
@@ -61,7 +61,7 @@
double min_ang;
double max_ang;
- TransformReference TR;
+ libTF::TransformReference TR;
ros::time::clock clock;
@@ -100,10 +100,12 @@
void scans_callback() {
encoder.lock();
- cloud.set_x_size(scans.get_ranges_size());
- cloud.set_y_size(scans.get_ranges_size());
- cloud.set_z_size(scans.get_ranges_size());
+ cloud.set_pts_size(scans.get_ranges_size());
+ cloud.set_chan_size(1);
+ cloud.chan[0].name = "intensities";
+ cloud.chan[0].set_vals_size(scans.get_ranges_size());
+
/*
for (int i = 0; i < scans.get_ranges_size(); i++) {
cloud.x[i] = cos(encoder.val)*cos(scans.angle_min + i*scans.angle_increment) * scans.ranges[i];
@@ -130,17 +132,20 @@
NEWMAT::Matrix rot_points = TR.getMatrix(1,3,clock.ulltime()) * points;
for (uint32_t i = 0; i < scans.get_ranges_size(); i++) {
- cloud.x[i] = rot_points(1,i+1);
- cloud.y[i] = rot_points(2,i+1);
- cloud.z[i] = rot_points(3,i+1);
+ cloud.pts[i].x = rot_points(1,i+1);
+ cloud.pts[i].y = rot_points(2,i+1);
+ cloud.pts[i].z = rot_points(3,i+1);
+ cloud.chan[0].vals[i] = scans.intensities[i];
}
encoder.unlock();
+
publish("cloud", cloud);
}
void encoder_callback() {
unsigned long long time = clock.ulltime();
+
TR.setWithEulers(3, 2,
.02, 0, .02,
0.0, 0.0, 0.0,
@@ -151,7 +156,6 @@
time);
motor_control(); // Control on encoder reads sounds reasonable
- printf("I got some encoder values: %g!\n", encoder.val);
}
void motor_control() {
@@ -169,7 +173,7 @@
next_shutter += 0.5;
publish("shutter", shutter);
}
- publish("mot2_cmd",cmd);
+ publish("mot_cmd",cmd);
}
};
@@ -181,6 +185,7 @@
while (tl.ok()) {
usleep(10000);
}
+ ros::fini();
return 0;
}
Modified: pkg/trunk/unported/vision/cloud_viewer/src/Makefile
===================================================================
--- pkg/trunk/unported/vision/cloud_viewer/src/Makefile 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/unported/vision/cloud_viewer/src/Makefile 2008-05-22 00:24:52 UTC (rev 495)
@@ -1,3 +1,3 @@
-SUBDIRS = libcloud_viewer standalone
+SUBDIRS = libcloud_viewer standalone cloud_node
include $(shell rospack find mk)/recurse_subdirs.mk
Modified: pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/Makefile
===================================================================
--- pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/Makefile 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/Makefile 2008-05-22 00:24:52 UTC (rev 495)
@@ -1,4 +1,4 @@
SRC = cloud_node.cpp
-OUT = ../../nodes/cloud_node
+OUT = ../../bin/cloud_node
PKG = cloud_viewer
-include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
+include $(shell rospack find mk)/node.mk
Modified: pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/cloud_node.cpp
===================================================================
--- pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/cloud_node.cpp 2008-05-21 21:56:50 UTC (rev 494)
+++ pkg/trunk/unported/vision/cloud_viewer/src/cloud_node/cloud_node.cpp 2008-05-22 00:24:52 UTC (rev 495)
@@ -32,143 +32,322 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#include "ros/ros_slave.h"
-#include "common_flows/FlowPointCloudFloat32.h"
-#include "common_flows/FlowEmpty.h"
+#include "ros/node.h"
+#include "std_msgs/MsgPointCloudFloat32.h"
+#include "std_msgs/MsgEmpty.h"
#include "cloud_viewer/cloud_viewer.h"
#include <SDL/SDL.h>
#include "math.h"
+#include "sdlgl/sdlgl.h"
+#include "rosthread/mutex.h"
+#include <fstream>
+#include <sstream>
+#include <sys/stat.h>
-class Cloud_Node : public ROS_Slave
+#include <vector>
+
+struct cloud_point {
+public:
+ float x;
+ float y;
+ float z;
+ float i;
+
+ cloud_point(float _x, float _y, float _z, float _i) : x(_x), y(_y), z(_z), i(_i) {}
+};
+
+class Cloud_Node : public ros::node, public ros::SDLGL
{
public:
- FlowPointCloudFloat32 *cloud;
- FlowEmpty *shutter;
- CloudViewer *cloud_viewer;
+ MsgPointCloudFloat32 cloud;
+ MsgEmpty shutter;
+ CloudViewer cloud_viewer;
+
+ int level;
+ int spread;
+
+ vector<cloud_point> buf[2];
+ int buf_read_ind;
+ int buf_use_ind;
+
+ char dir_name[256];
+ int cloud_cnt;
+
+ bool made_dir;
+ ros::thread::mutex cloud_mutex;
- int count;
-
- Cloud_Node() : ROS_Slave(), cloud_viewer(NULL)
+ Cloud_Node() : ros::node("cloud_viewer"), level(20), spread(400), buf_read_ind(0), buf_use_ind(1), cloud_cnt(0), made_dir(false)
{
- register_sink(cloud = new FlowPointCloudFloat32("cloud"), ROS_CALLBACK(Cloud_Node, cloud_callback));
- register_sink(shutter = new FlowEmpty("shutter"), ROS_CALLBACK(Cloud_Node, shutter_callback));
+ subscribe("cloud", cloud, &Cloud_Node::cloud_callback);
+ subscribe("shutter", shutter, &Cloud_Node::shutter_callback);
- cloud_viewer = new CloudViewer;
+ if (!init_gui(1024, 768))
+ self_destruct();
- count = 0;
+ cloud_viewer.set_opengl_params(1024,768);
- register_with_master();
+ time_t rawtime;
+ struct tm* timeinfo;
+ time(&rawtime);
+ timeinfo = localtime(&rawtime);
+
+ sprintf(dir_name, "clouds_%.2d%.2d%.2d_%.2d%.2d%.2d", timeinfo->tm_mon + 1, timeinfo->tm_mday,timeinfo->tm_year - 100,timeinfo->tm_hour, timeinfo->tm_min, timeinfo->tm_sec);
+
}
virtual ~Cloud_Node()
{
- if (cloud_viewer)
- delete cloud_viewer;
}
-
- void refresh() {
- // SDL (and Xlib) don't handle threads well, so we do both of these
- check_keyboard();
- display_image();
+ virtual void render()
+ {
+ cloud_mutex.lock(); // I've commandeered the cloud mutex for my own nefarious purposes
+ cloud_viewer.render();
+ cloud_mutex.unlock();
+ SDL_GL_SwapBuffers();
}
- void check_keyboard() {
- SDL_Event event;
- while(SDL_PollEvent(&event))
- {
- switch(event.type)
- {
- case SDL_MOUSEMOTION:
- cloud_viewer->mouse_motion(event.motion.x, event.motion.y, event.motion.xrel, event.motion.yrel);
- break;
- case SDL_MOUSEBUTTONDOWN:
- case SDL_MOUSEBUTTONUP:
- {
- int button = -1;
- switch(event.button.button)
- {
- case SDL_BUTTON_LEFT: button = 0; break;
- case SDL_BUTTON_MIDDLE: button = 1; break;
- case SDL_BUTTON_RIGHT: button = 2; break;
- }
- cloud_viewer->mouse_button(event.button.x, event.button.y,
- button, (event.type == SDL_MOUSEBUTTONDOWN ? true : false));
- break;
- }
- case SDL_KEYDOWN:
- cloud_viewer->keypress(event.key.keysym.sym);
- break;
- }
- }
+ virtual void mouse_motion(int x, int y, int dx, int dy, int buttons)
+ {
+ cloud_mutex.lock();
+ cloud_viewer.mouse_motion(x, y, dx, dy);
+ cloud_mutex.unlock();
+ request_render();
}
- void display_image() {
- cloud->lock_atom(); // I've commandeered the cloud mutex for my own nefarious purposes
- cloud_viewer->render();
- SDL_GL_SwapBuffers();
- cloud->unlock_atom();
+ virtual void mouse_button(int x, int y, int button, bool is_down)
+ {
+ switch(button) {
+ case SDL_BUTTON_LEFT: button = 0; break;
+ case SDL_BUTTON_MIDDLE: button = 1; break;
+ case SDL_BUTTON_RIGHT: button = 2; break;
+ }
+ cloud_mutex.lock();
+ cloud_viewer.mouse_button(x, y, button, is_down);
+ cloud_mutex.unlock();
+ request_render();
}
+ virtual void keypress(char c, uint16_t u, SDLMod mod)
+ {
- bool sdl_init()
- {
- SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE, 24);
- SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
- const int w = 640, h = 480;
- if (SDL_SetVideoMode(w, h, 32, SDL_OPENGL | SDL_HWSURFACE) == 0) {
- fprintf(stderr, "setvideomode failed: %s\n", SDL_GetError());
- return false;
+ if (c == 91 || c == 93)
+ {
+ if (c == 91 && mod == 0)
+ level -= 5;
+ else if (c == 93 && mod == 0)
+ level += 5;
+ else if (c == 91 && mod == 1)
+ spread -= 5;
+ else if (c == 93 && mod == 1)
+ spread += 5;
+
+ printf("Level: %d, Spread: %d\n",level,spread);
+ refill_cloud();
}
- cloud_viewer->set_opengl_params(w,h);
+ if (c == SDLK_RETURN) {
+ save();
+ }
- SDL_EnableKeyRepeat(SDL_DEFAULT_REPEAT_DELAY, SDL_DEFAULT_REPEAT_INTERVAL);//People expect Key Repeat
-
- return true;
+ cloud_mutex.lock();
+ cloud_viewer.keypress(c);
+ cloud_mutex.unlock();
+ request_render();
}
void cloud_callback()
{
- for (int i = 0; i < cloud->get_x_size(); i++) {
- cloud_viewer->add_point(cloud->x[i], cloud->y[i], cloud->z[i],
- 255,255,255);
+ int intensity_ind = -1;
+
+ for (int i = 0; i < cloud.get_chan_size(); i++)
+ if (cloud.chan[i].name == string("intensities"))
+ intensity_ind = i;
+
+ double intensity;
+
+ for (int i = 0; i < cloud.get_pts_size(); i++)
+ {
+ intensity = 4000;
+ if (intensity_ind != -1)
+ intensity = cloud.chan[intensity_ind].vals[i];
+
+ buf[buf_read_ind].push_back(cloud_point(cloud.pts[i].x, cloud.pts[i].y, cloud.pts[i].z, intensity));
+
}
+
+ /*
+ int intensity;
+
+ for (int i = 0; i < cloud.get_pts_size(); i++)
+ {
+ if (intensity_ind < 0)
+ {
+ intensity = 0;
+ } else {
+ intensity = level + cloud.chan[intensity_ind].vals[i] / spread;
+ if (intensity > 255)
+ intensity = 255;
+ else if (intensity < 0)
+ intensity = 0;
+ }
+
+ cloud_viewer.add_point(-cloud.pts[i].y, -cloud.pts[i].z, cloud.pts[i].x,
+ intensity, intensity, intensity);
+ }
+ */
+
}
void shutter_callback()
{
- cloud->lock_atom();
- cloud_viewer->clear_cloud();
- cloud->unlock_atom();
+
+ buf_use_ind = buf_read_ind;
+ buf_read_ind = !buf_read_ind;
+
+ buf[buf_read_ind].clear();
+
+ refill_cloud();
}
+ void refill_cloud()
+ {
+ cloud_mutex.lock();
+ cloud_viewer.clear_cloud();
+
+ double val = 0.0;
+ double valsq = 0.0;
+ int cnt = 0;
+ for (int i = 0; i < buf[buf_use_ind].size(); i++) {
+ val += buf[buf_use_ind][i].i;
+ valsq += pow(buf[buf_use_ind][i].i, 2.0);
+ cnt++;
+ }
+ double mean = val / cnt;
+ double std = sqrt( (valsq - cnt*pow(mean, 2.0))/(cnt - 1));
+
+ for (int i = 0; i < buf[buf_use_ind].size(); i++) {
+ float intensity = level + spread * (buf[buf_use_ind][i].i - mean) / (2*std);
+
+ if (intensity > 255)
+ intensity = 255;
+ else if (intensity < 0)
+ intensity = 0;
+
+ cloud_viewer.add_point(-buf[buf_use_ind][i].y, -buf[buf_use_ind][i].z, buf[buf_use_ind][i].x,
+ intensity, intensity, intensity);
+ }
+
+ cloud_mutex.unlock();
+
+ request_render();
+ }
+
+ void save() {
+
+ printf("Trying to save..\n");
+
+ if (!made_dir) {
+ if (mkdir(dir_name, 0755)) {
+ printf("Failed to make directory: %s\n", dir_name);
+ return;
+ } else {
+ made_dir = true;
+ }
+ }
+
+ std::ostringstream oss;
+ oss << dir_name << "/Cloud" << cloud_cnt++ << ".wrl";
+ ofstream out(oss.str().c_str());
+
+ out.setf(ios::fixed, ios::floatfield);
+ out.setf(ios::showpoint);
+ out.precision(4);
+
+ double val = 0.0;
+ double valsq = 0.0;
+ int cnt = 0;
+
+ for (int i = 0; i < buf[buf_use_ind].size(); i++) {
+ val += buf[buf_use_ind][i].i;
+ valsq += pow(buf[buf_use_ind][i].i, 2.0);
+ cnt++;
+ }
+ double mean = val / cnt;
+ double std = sqrt( (valsq - cnt*pow(mean, 2.0))/(cnt - 1));
+
+ out << "#VRML V2.0 utf8" << endl
+ << "DEF InitView Viewpoint" << endl
+ << "{" << endl
+ << "position -2 0 0" << endl
+ << "orientation 0 1 0 -1.57079633" << endl
+ << "" << endl
+ << "}DEF World Transform " << endl
+ << "{" << endl
+ << "translation 0 0 0" << endl
+ << "children [" << endl
+ << "DEF Scene Transform" << endl
+ << "{" << endl
+ << "translation 0 0 0" << endl
+ << "children [" << endl
+ << "Shape" << endl
+ << "{" << endl
+ << "geometry PointSet" << endl
+ << "{" << endl
+ << "coord Coordinate" << endl
+ << "{" << endl
+ << "point[" << endl;
+
+ for (int i = 0; i < buf[buf_use_ind].size(); i++) {
+ out << buf[buf_use_ind][i].x << " " << buf[buf_use_ind][i].z << " " << -buf[buf_use_ind][i].y << ", " << endl;
+ }
+
+ out << "]"
+ << "}" << endl
+ << "color Color" << endl
+ << "{" << endl
+ << "color[" << endl;
+
+ for (int i = 0; i < buf[buf_use_ind].size(); i++) {
+ float intensity = (level + spread * (buf[buf_use_ind][i].i - mean) / (2*std)) / 255;
+
+ if (intensity > 255)
+ intensity = 255;
+ else if (intensity < 0)
+ intensity = 0;
+
+ out << intensity << " " << intensity << " " << intensity << "," << endl;
+ }
+
+ out << "]"
+ << "}" << endl
+ << "}" << endl
+ << "}" << endl
+ << "" << endl
+ << "]" << endl
+ << "} # end of scene transform" << endl
+ << "]" << endl
+ << "} # end of world transform" << endl;
+
+ }
+
};
+
int main(int argc, char **argv)
{
- if (SDL_Init(SDL_INIT_VIDEO) < 0)
- {
- fprintf(stderr, "SDL initialization error: %s\n", SDL_GetError());
- exit(1);
- }
+ ros::init(argc, argv);
+
atexit(SDL_Quit);
Cloud_Node cn;
- if (!cn.sdl_init())
- {
- fprintf(stderr, "SDL video startup error: %s\n", SDL_GetError());
- exit(1);
- }
+ cn.main_loop();
- while (cn.happy()) {
- cn.refresh();
- usleep(1000);
- }
+ ros::fini();
return 0;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-05-22 00:33:40
|
Revision: 497
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=497&view=rev
Author: jleibs
Date: 2008-05-21 17:33:48 -0700 (Wed, 21 May 2008)
Log Message:
-----------
Cloud_viewer is actually ported now.
Added Paths:
-----------
pkg/trunk/visualization/cloud_viewer/
pkg/trunk/visualization/cloud_viewer/Makefile
pkg/trunk/visualization/cloud_viewer/bin/
pkg/trunk/visualization/cloud_viewer/include/
pkg/trunk/visualization/cloud_viewer/lib/
pkg/trunk/visualization/cloud_viewer/manifest.xml
pkg/trunk/visualization/cloud_viewer/src/
Removed Paths:
-------------
pkg/trunk/unported/vision/cloud_viewer/
pkg/trunk/visualization/cloud_viewer/Makefile
pkg/trunk/visualization/cloud_viewer/include/
pkg/trunk/visualization/cloud_viewer/lib/
pkg/trunk/visualization/cloud_viewer/manifest.xml
pkg/trunk/visualization/cloud_viewer/src/
Copied: pkg/trunk/visualization/cloud_viewer (from rev 491, pkg/trunk/unported/vision/cloud_viewer)
Deleted: pkg/trunk/visualization/cloud_viewer/Makefile
===================================================================
--- pkg/trunk/unported/vision/cloud_viewer/Makefile 2008-05-21 07:03:52 UTC (rev 491)
+++ pkg/trunk/visualization/cloud_viewer/Makefile 2008-05-22 00:33:48 UTC (rev 497)
@@ -1,3 +0,0 @@
-SUBDIRS = src
-include $(shell rospack find mk)/recurse_subdirs.mk
-
Copied: pkg/trunk/visualization/cloud_viewer/Makefile (from rev 496, pkg/trunk/unported/vision/cloud_viewer/Makefile)
===================================================================
--- pkg/trunk/visualization/cloud_viewer/Makefile (rev 0)
+++ pkg/trunk/visualization/cloud_viewer/Makefile 2008-05-22 00:33:48 UTC (rev 497)
@@ -0,0 +1,3 @@
+SUBDIRS = src
+include $(shell rospack find mk)/recurse_subdirs.mk
+
Copied: pkg/trunk/visualization/cloud_viewer/bin (from rev 496, pkg/trunk/unported/vision/cloud_viewer/bin)
Copied: pkg/trunk/visualization/cloud_viewer/include (from rev 496, pkg/trunk/unported/vision/cloud_viewer/include)
Copied: pkg/trunk/visualization/cloud_viewer/lib (from rev 496, pkg/trunk/unported/vision/cloud_viewer/lib)
Deleted: pkg/trunk/visualization/cloud_viewer/manifest.xml
===================================================================
--- pkg/trunk/unported/vision/cloud_viewer/manifest.xml 2008-05-21 07:03:52 UTC (rev 491)
+++ pkg/trunk/visualization/cloud_viewer/manifest.xml 2008-05-22 00:33:48 UTC (rev 497)
@@ -1,15 +0,0 @@
-<package>
-<description brief="SDL-based point cloud">
-A simple point cloud viewer written using SDL.
-</description>
-<author>Morgan Quigley mqu...@cs...</author>
-<license>BSD</license>
-<url>http://stair.stanford.edu</url>
-<depend package="roscpp"/>
-<depend package="std_msgs"/>
-<depend package="sdlgl"/>
-<export>
- <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lcloud_viewer -lGL -lGLU"/>
-</export>
-</package>
-
Copied: pkg/trunk/visualization/cloud_viewer/manifest.xml (from rev 496, pkg/trunk/unported/vision/cloud_viewer/manifest.xml)
===================================================================
--- pkg/trunk/visualization/cloud_viewer/manifest.xml (rev 0)
+++ pkg/trunk/visualization/cloud_viewer/manifest.xml 2008-05-22 00:33:48 UTC (rev 497)
@@ -0,0 +1,15 @@
+<package>
+<description brief="SDL-based point cloud">
+A simple point cloud viewer written using SDL.
+</description>
+<author>Morgan Quigley mqu...@cs...</author>
+<license>BSD</license>
+<url>http://stair.stanford.edu</url>
+<depend package="roscpp"/>
+<depend package="std_msgs"/>
+<depend package="sdlgl"/>
+<export>
+ <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lcloud_viewer -lGL -lGLU"/>
+</export>
+</package>
+
Copied: pkg/trunk/visualization/cloud_viewer/src (from rev 496, pkg/trunk/unported/vision/cloud_viewer/src)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-05-28 02:25:34
|
Revision: 527
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=527&view=rev
Author: gerkey
Date: 2008-05-27 19:25:41 -0700 (Tue, 27 May 2008)
Log Message:
-----------
fixes on the robot
Modified Paths:
--------------
pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/wavefront_player/Makefile
pkg/trunk/nav/wavefront_player/wavefront_player.cc
Modified: pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2008-05-28 00:06:52 UTC (rev 526)
+++ pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2008-05-28 02:25:41 UTC (rev 527)
@@ -309,6 +309,8 @@
// Stop the robot
en.stop();
+ ros::fini();
+
// To quote Morgan, Hooray!
return(0);
}
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-28 00:06:52 UTC (rev 526)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-28 02:25:41 UTC (rev 527)
@@ -225,11 +225,14 @@
// TODO: remove XDR dependency
playerxdr_ftable_init();
+ puts("advertising");
advertise<MsgRobotBase2DOdom>("localizedpose");
advertise<MsgParticleCloud2D>("particlecloud");
+ puts("subscribing");
subscribe("odom", odomMsg, &AmclNode::odomReceived);
subscribe("scan", laserMsg, &AmclNode::laserReceived);
subscribe("initialpose", initialPoseMsg, &AmclNode::initialPoseReceived);
+ puts("done");
// TODO: get map via RPC
assert(read_map_from_image(&this->sx, &this->sy, &this->mapdata, fname, 0)
@@ -566,9 +569,9 @@
p.mean.py = y;
p.mean.pa = a;
- p.cov[0] = 0.5*0.5;
- p.cov[1] = 0.5*0.5;
- p.cov[2] = (M_PI/6.0)*(M_PI/6.0);
+ p.cov[0] = 0.25*0.25;
+ p.cov[1] = 0.25*0.25;
+ p.cov[2] = (M_PI/12.0)*(M_PI/12.0);
this->ldevice->PutMsg(this->Driver::InQueue,
PLAYER_MSGTYPE_REQ,
@@ -585,7 +588,11 @@
pdata.min_angle = this->laserMsg.angle_min;
pdata.max_angle = this->laserMsg.angle_max;
pdata.resolution = this->laserMsg.angle_increment;
- pdata.max_range = this->laserMsg.range_max;
+ // HACK, until the hokuyourg_player node is fixed
+ if(this->laserMsg.range_max > 0.1)
+ pdata.max_range = this->laserMsg.range_max;
+ else
+ pdata.max_range = 30.0;
pdata.ranges_count = this->laserMsg.get_ranges_size();
pdata.ranges = new float[pdata.ranges_count];
assert(pdata.ranges);
Modified: pkg/trunk/nav/wavefront_player/Makefile
===================================================================
--- pkg/trunk/nav/wavefront_player/Makefile 2008-05-28 00:06:52 UTC (rev 526)
+++ pkg/trunk/nav/wavefront_player/Makefile 2008-05-28 02:25:41 UTC (rev 527)
@@ -2,7 +2,7 @@
CXX = g++
all: $(PKG)
-CFLAGS = -g -Wall `rospack export/cpp/cflags $(PKG)` `pkg-config --cflags gdk-pixbuf-2.0`
+CFLAGS = -O2 -Wall `rospack export/cpp/cflags $(PKG)` `pkg-config --cflags gdk-pixbuf-2.0`
LFLAGS = `rospack export/cpp/lflags $(PKG)` -lwavefront_standalone `pkg-config --libs gdk-pixbuf-2.0`
wavefront_player: wavefront_player.cc
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-28 00:06:52 UTC (rev 526)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-28 02:25:41 UTC (rev 527)
@@ -265,6 +265,7 @@
wn.sleep(curr.tv_sec+curr.tv_usec/1e6);
}
ros::fini();
+ //exit(0);
return(0);
}
@@ -550,10 +551,11 @@
it++)
{
/*
- printf("%.3f %.3f %.3f\n",
+ printf("%.3f %.3f %.3f: %d\n",
it->first->x,
it->first->y,
- it->first->th);
+ it->first->th,
+ it->second->get_ranges_size());
*/
float b=it->second->angle_min;
float* r=it->second->ranges;
@@ -561,7 +563,11 @@
j<it->second->get_ranges_size();
j++,r++,b+=it->second->angle_increment)
{
- if(((*r) >= this->laser_maxrange) || ((*r) >= it->second->range_max))
+ // TODO: take out the bogus epsilon range_max check, after the
+ // hokuyourg_player node is fixed
+ if(((*r) >= this->laser_maxrange) ||
+ ((it->second->range_max > 0.1) && ((*r) >= it->second->range_max)) ||
+ ((*r) <= 0.01))
continue;
// Project into laser frame
@@ -585,13 +591,15 @@
wy = it->first->y + sn*rx + cs*ry;
assert(this->laser_hitpts_len*2 < this->laser_hitpts_size);
- *(pts++) = wx;
- *(pts++) = wy;
+ *pts = wx;
+ pts++;
+ *pts = wy;
+ pts++;
this->laser_hitpts_len++;
}
}
- //printf("setting %d hit points\n", this->scan_points_count);
+ //printf("setting %d hit points\n", this->laser_hitpts_len);
plan_set_obstacles(this->plan, this->laser_hitpts, this->laser_hitpts_len);
// Draw the points
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-05-30 00:53:09
|
Revision: 556
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=556&view=rev
Author: tfoote
Date: 2008-05-29 17:51:25 -0700 (Thu, 29 May 2008)
Log Message:
-----------
switching 2d nav stack to use non caching transforms.
Modified Paths:
--------------
pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
pkg/trunk/drivers/robot/erratic_player/manifest.xml
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/nav_view/manifest.xml
pkg/trunk/nav/nav_view/nav_view.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosstage/manifest.xml
pkg/trunk/simulators/rosstage/rosstage.cc
pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp
Modified: pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2008-05-30 00:51:25 UTC (rev 556)
@@ -82,6 +82,8 @@
// roscpp
#include <ros/node.h>
+//rosTF
+#include "rosTF/rosTF.h"
// Messages that I need
#include <std_msgs/MsgRobotBase2DOdom.h>
//#include <std_msgs/MsgRobotBase2DCmdVel.h>
@@ -102,7 +104,10 @@
//MsgRobotBase2DCmdVel cmdvel;
MsgBaseVel cmdvel;
- ErraticNode() : ros::node("erratic_player")
+ rosTF::rosTFServer tf;
+
+ ErraticNode() : ros::node("erratic_player"),
+ tf(*this)
{
// libplayercore boiler plate
player_globals_init();
@@ -283,11 +288,29 @@
en.odom.vel.th = pdata->vel.pa;
en.odom.stall = pdata->stall;
- en.odom.header.frame_id = 2;
+ en.odom.header.frame_id = FRAMEID_ODOM;
+
+ en.odom.header.stamp.sec = (long long unsigned int)floor(hdr->timestamp);
+ en.odom.header.stamp.sec = (long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL);
+
// Publish the new data
en.publish("odom", en.odom);
+ tf.sendInverseEuler(FRAMEID_ODOM,
+ FRAMEID_ROBOT,
+ pdata->pos.px,
+ pdata->pos.py,
+ 0.0,
+ pdata->pos.pa,
+ 0.0,
+ 0.0,
+ (long long unsigned int)floor(hdr->timestamp),
+ (long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL));
+
+ std::cout <<"Sent 32" <<std::endl;
+
+
//printf("Published new odom: (%.3f,%.3f,%.3f)\n",
//en.odom.pos.x, en.odom.pos.y, en.odom.pos.th);
}
Modified: pkg/trunk/drivers/robot/erratic_player/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/manifest.xml 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/drivers/robot/erratic_player/manifest.xml 2008-05-30 00:51:25 UTC (rev 556)
@@ -5,4 +5,5 @@
<depend package="roscpp" />
<depend package="player" />
<depend package="std_msgs" />
+ <depend package="rosTF" />
</package>
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-05-30 00:51:25 UTC (rev 556)
@@ -351,23 +351,28 @@
player_position2d_data_t* pdata =
(player_position2d_data_t*)data;
- /*
+
// publish new transform map->odom
printf("lpose: %.3f %.3f %.3f\n",
odomMsg.pos.x,
odomMsg.pos.y,
RTOD(odomMsg.pos.th));
- this->tf->sendEuler(ROSTF_FRAME_ODOM,
- ROSTF_FRAME_MAP,
- pdata->pos.px-this->odomMsg.pos.x,
- pdata->pos.py-this->odomMsg.pos.y,
+ this->tf->sendEuler(FRAMEID_ROBOT,
+ FRAMEID_MAP,
+ pdata->pos.px,
+ pdata->pos.py,
0.0,
- pdata->pos.pa-this->odomMsg.pos.th,
+ pdata->pos.pa,
0.0,
0.0,
- (long long unsigned int)floor(hdr->timestamp),
- (long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL));
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+ //(long long unsigned int)floor(hdr->timestamp),
+ //(long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL));
+
+ std::cout <<"Sent 21" <<std::endl;
+
printf("pose: (%.3f %.3f %.3f) @ (%llu:%llu)\n",
pdata->pos.px,
pdata->pos.py,
@@ -375,7 +380,7 @@
(long long unsigned int)floor(hdr->timestamp),
(long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) *
1000000000ULL));
- */
+
localizedOdomMsg.pos.x = pdata->pos.px;
localizedOdomMsg.pos.y = pdata->pos.py;
localizedOdomMsg.pos.th = pdata->pos.pa;
Modified: pkg/trunk/nav/nav_view/manifest.xml
===================================================================
--- pkg/trunk/nav/nav_view/manifest.xml 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/nav/nav_view/manifest.xml 2008-05-30 00:51:25 UTC (rev 556)
@@ -6,4 +6,6 @@
<depend package="std_msgs"/>
<depend package="sdlgl"/>
<depend package="sdl_image"/>
+ <depend package="rosTF"/>
+ <depend package="rostime"/>
</package>
Modified: pkg/trunk/nav/nav_view/nav_view.cpp
===================================================================
--- pkg/trunk/nav/nav_view/nav_view.cpp 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/nav/nav_view/nav_view.cpp 2008-05-30 00:51:25 UTC (rev 556)
@@ -106,6 +106,9 @@
#include "std_msgs/MsgPose2DFloat32.h"
#include "sdlgl/sdlgl.h"
+#include "rosTF/rosTF.h"
+#include "rostime/clock.h"
+
class NavView : public ros::node, public ros::SDLGL
{
public:
@@ -124,13 +127,17 @@
bool setting_theta;
double gx,gy;
+ rosTFClient tf;
+ ros::time::clock myClock;
+
NavView() : ros::node("nav_view",false),
- view_scale(10), view_x(0), view_y(0)
+ view_scale(10), view_x(0), view_y(0),
+ tf(*this,false)
{
advertise<MsgPlanner2DGoal>("goal");
advertise<MsgPose2DFloat32>("initialpose");
//subscribe("state", pstate, &NavView::pstate_cb);
- subscribe("odom", odom, &NavView::odom_cb);
+ // subscribe("odomm", odom, &NavView::odom_cb); //replaced with rosTF
subscribe("particlecloud", cloud, &NavView::odom_cb);
subscribe("gui_path", pathline, &NavView::odom_cb);
subscribe("gui_laser", laserscan, &NavView::odom_cb);
@@ -201,10 +208,24 @@
}
glPushMatrix();
- odom.lock();
- glTranslatef(odom.pos.x, odom.pos.y, 0);
- glRotatef(odom.pos.th * 180 / M_PI, 0, 0, 1);
- odom.unlock();
+ libTF::TFPose2D robotPose;
+ robotPose.x = 0;
+ robotPose.y = 0;
+ robotPose.yaw = 0;
+ robotPose.frame = FRAMEID_ROBOT;
+ robotPose.time = 0;//myClock.ulltime();
+
+ libTF::TFPose2D mapPose = tf.transformPose2D(FRAMEID_MAP, robotPose);
+
+ glTranslatef(mapPose.x, mapPose.y, 0);
+ glRotatef(mapPose.yaw * 180 / M_PI, 0, 0, 1);
+
+ /* printf("lpose: %.3f %.3f %.3f\n",
+ mapPose.x,
+ mapPose.y,
+ mapPose.yaw);
+ */
+
glColor3f(0.2, 1.0, 0.4);
glBegin(GL_LINE_LOOP);
const float robot_rad = 0.3;
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-30 00:51:25 UTC (rev 556)
@@ -229,7 +229,7 @@
public:
// Transform client
- rosTFClient* tf;
+ rosTFClient tf;
WavefrontNode(char* fname, double res);
~WavefrontNode();
@@ -293,7 +293,7 @@
avmax(DTOR(80.0)),
amin(DTOR(10.0)),
amax(DTOR(40.0)),
- tf(NULL)
+ tf(*this, false)
{
// TODO: get map via RPC
char* mapdata;
@@ -331,8 +331,6 @@
// Compute cspace over static map
plan_compute_cspace(this->plan);
- //this->tf = new rosTFClient(*this);
-
this->laser_hitpts_size = this->laser_hitpts_len = 0;
this->laser_hitpts = NULL;
@@ -348,16 +346,16 @@
advertise<MsgPolyline2D>("gui_laser");
advertise<MsgBaseVel>("cmd_vel");
subscribe("goal", goalMsg, &WavefrontNode::goalReceived);
- //subscribe("odom", odomMsg, &WavefrontNode::odomReceived);
- subscribe("localizedpose", odomMsg, &WavefrontNode::odomReceived);
+ subscribe("odom", odomMsg, &WavefrontNode::odomReceived);
+ //subscribe("localizedpose", odomMsg, &WavefrontNode::odomReceived);
subscribe("scan", laserMsg, &WavefrontNode::laserReceived);
}
WavefrontNode::~WavefrontNode()
{
plan_free(this->plan);
- if(this->tf)
- delete this->tf;
+ // if(this->tf)
+ // delete this->tf;
}
void
@@ -385,19 +383,31 @@
WavefrontNode::odomReceived()
{
this->lock.lock();
- /*
- libTF::TFPose2D odom_pose;
+
+ libTF::TFPose2D robotPose;
+ robotPose.x = 0;
+ robotPose.y = 0;
+ robotPose.yaw = 0;
+ robotPose.frame = FRAMEID_ROBOT;
+ // robotPose.time = myClock.ulltime();
+ robotPose.time = odomMsg.header.stamp.sec * 1000000000ULL +
+ odomMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else
+
+ // std::cout <<"robotTime" << robotPose.time << std::endl;
+ /* libTF::TFPose2D pose;
odom_pose.x = odomMsg.pos.x;
odom_pose.y = odomMsg.pos.y;
odom_pose.yaw = odomMsg.pos.th;
odom_pose.time = odomMsg.header.stamp.sec * 1000000000ULL +
odomMsg.header.stamp.nsec;
odom_pose.frame = odomMsg.header.frame_id;
-
+ */
try
{
libTF::TFPose2D global_pose =
- this->tf->transformPose2D(ROSTF_FRAME_MAP, odom_pose);
+ this->tf.transformPose2D(FRAMEID_MAP, robotPose);
+ cout << tf.getMatrix(FRAMEID_MAP, FRAMEID_ROBOT, robotPose.time);
+
this->pose[0] = global_pose.x;
this->pose[1] = global_pose.y;
this->pose[2] = global_pose.yaw;
@@ -416,12 +426,13 @@
{
puts("no global->local Tx yet");
}
- */
+
+ /*
this->pose[0] = odomMsg.pos.x;
this->pose[1] = odomMsg.pos.y;
this->pose[2] = odomMsg.pos.th;
- /*
+
printf("gpose: %.3f %.3f %.3f\n",
this->pose[0],
this->pose[1],
Modified: pkg/trunk/simulators/rosstage/manifest.xml
===================================================================
--- pkg/trunk/simulators/rosstage/manifest.xml 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/simulators/rosstage/manifest.xml 2008-05-30 00:51:25 UTC (rev 556)
@@ -6,4 +6,5 @@
<depend package="rosthread" />
<depend package="stage" />
<depend package="std_msgs" />
+ <depend package="rosTF" />
</package>
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-05-30 00:51:25 UTC (rev 556)
@@ -87,6 +87,8 @@
#include <std_msgs/MsgRobotBase2DOdom.h>
#include <std_msgs/MsgBaseVel.h>
+#include "rosTF/rosTF.h"
+
#define USAGE "rosstage <worldfile>"
// Our node
@@ -109,6 +111,8 @@
// to search for models of interest.
static void ghfunc(gpointer key, Stg::StgModel* mod, StageNode* node);
+ rosTFServer tf;
+
public:
// Constructor; stage itself needs argc/argv. fname is the .world file
// that stage should load.
@@ -158,7 +162,8 @@
}
StageNode::StageNode(int argc, char** argv, const char* fname) :
- ros::node("rosstage")
+ ros::node("rosstage"),
+ tf(*this)
{
this->lasermodel = NULL;
this->positionmodel = NULL;
@@ -255,16 +260,30 @@
this->odomMsg.vel.th = v.a;
this->odomMsg.stall = this->positionmodel->Stall();
// TODO: get the frame ID from somewhere
- this->odomMsg.header.frame_id = 2;
+ this->odomMsg.header.frame_id = FRAMEID_ODOM;
this->odomMsg.header.stamp.sec =
- (unsigned long)floor(world->SimTimeNow() / 1e3);
+ (unsigned long)floor(world->SimTimeNow() / 1e6);
this->odomMsg.header.stamp.nsec =
- (unsigned long)rint(1e6 * (world->SimTimeNow() -
- this->odomMsg.header.stamp.sec * 1e3));
+ (unsigned long)rint(1e3 * (world->SimTimeNow() -
+ this->odomMsg.header.stamp.sec * 1e6));
this->odomMsg.__timestamp_override = true;
publish("odom",this->odomMsg);
+ // printf("%u \n",world->SimTimeNow());
+ printf("time: %u, %u \n",odomMsg.header.stamp.sec, odomMsg.header.stamp.nsec);
+
+ tf.sendInverseEuler(FRAMEID_ODOM,
+ FRAMEID_ROBOT,
+ odomMsg.pos.x,
+ odomMsg.pos.y,
+ 0.0,
+ odomMsg.pos.th,
+ 0.0,
+ 0.0,
+ odomMsg.header.stamp.sec,
+ odomMsg.header.stamp.nsec);
+
this->lock.unlock();
}
Modified: pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp
===================================================================
--- pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp 2008-05-29 23:48:27 UTC (rev 555)
+++ pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp 2008-05-30 00:51:25 UTC (rev 556)
@@ -47,7 +47,7 @@
void rosTFClient::receiveEuler()
{
setWithEulers(eulerIn.frame, eulerIn.parent, eulerIn.x, eulerIn.y, eulerIn.z, eulerIn.yaw, eulerIn.pitch, eulerIn.roll, eulerIn.header.stamp.sec * 1000000000ULL + eulerIn.header.stamp.nsec);
- std::cout << "recieved euler frame: " << eulerIn.frame << " with parent:" << eulerIn.parent << std::endl;
+ std::cout << "received euler frame: " << eulerIn.frame << " with parent:" << eulerIn.parent << "time " << eulerIn.header.stamp.sec * 1000000000ULL + eulerIn.header.stamp.nsec << std::endl;
};
void rosTFClient::receiveDH()
@@ -86,6 +86,7 @@
eulerOut.roll = roll;
eulerOut.header.stamp.sec = secs;
eulerOut.header.stamp.nsec = nsecs;
+ eulerOut.__timestamp_override = true;
myNode.publish("TransformEuler", eulerOut);
@@ -107,6 +108,7 @@
eulerOut.roll = odomeuler.roll;
eulerOut.header.stamp.sec = secs;
eulerOut.header.stamp.nsec = nsecs;
+ eulerOut.__timestamp_override = true;
myNode.publish("TransformEuler", eulerOut);
@@ -138,6 +140,7 @@
dhOut.angle = angle;
dhOut.header.stamp.sec = secs;
dhOut.header.stamp.nsec = nsecs;
+ dhOut.__timestamp_override = true;
myNode.publish("TransformDH", dhOut);
@@ -156,6 +159,7 @@
quaternionOut.w = w;
quaternionOut.header.stamp.sec = secs;
quaternionOut.header.stamp.nsec = nsecs;
+ quaternionOut.__timestamp_override = true;
myNode.publish("TransformQuaternion", quaternionOut);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mc...@us...> - 2008-05-30 15:34:37
|
Revision: 568
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=568&view=rev
Author: mcgann
Date: 2008-05-30 08:34:43 -0700 (Fri, 30 May 2008)
Log Message:
-----------
Baseline of demo
Added Paths:
-----------
pkg/trunk/exec_trex/
pkg/trunk/exec_trex/.config
pkg/trunk/exec_trex/Components.cc
pkg/trunk/exec_trex/Components.hh
pkg/trunk/exec_trex/Debug.cfg
pkg/trunk/exec_trex/ExecDefs.cc
pkg/trunk/exec_trex/ExecDefs.hh
pkg/trunk/exec_trex/ExecListener.cc
pkg/trunk/exec_trex/Executive.cc
pkg/trunk/exec_trex/Executive.hh
pkg/trunk/exec_trex/Jamfile
pkg/trunk/exec_trex/Jamrules
pkg/trunk/exec_trex/MessageLogger.cc
pkg/trunk/exec_trex/Monitor.cc
pkg/trunk/exec_trex/NDDL.cfg
pkg/trunk/exec_trex/RCS.cc
pkg/trunk/exec_trex/RCSAdapter.cc
pkg/trunk/exec_trex/RCSAdapter.hh
pkg/trunk/exec_trex/Startup.ROS
pkg/trunk/exec_trex/exec.cfg
pkg/trunk/exec_trex/exec.sim.cfg
pkg/trunk/exec_trex/exec.solver.cfg
pkg/trunk/exec_trex/logs/
pkg/trunk/exec_trex/monitor.cfg
pkg/trunk/exec_trex/pr2.model.nddl
pkg/trunk/exec_trex/rcs.cfg
pkg/trunk/exec_trex/ros.exec.nddl
pkg/trunk/exec_trex/ros.sim.nddl
pkg/trunk/exec_trex/sim.solver.cfg
Added: pkg/trunk/exec_trex/.config
===================================================================
--- pkg/trunk/exec_trex/.config (rev 0)
+++ pkg/trunk/exec_trex/.config 2008-05-30 15:34:43 UTC (rev 568)
@@ -0,0 +1 @@
+export TREX_LOG_DIR=`pwd`/logs
\ No newline at end of file
Added: pkg/trunk/exec_trex/Components.cc
===================================================================
--- pkg/trunk/exec_trex/Components.cc (rev 0)
+++ pkg/trunk/exec_trex/Components.cc 2008-05-30 15:34:43 UTC (rev 568)
@@ -0,0 +1,539 @@
+#include "Components.hh"
+#include "PlanDatabase.hh"
+#include "Token.hh"
+#include "TokenVariable.hh"
+#include "TemporalAdvisor.hh"
+#include "DbCore.hh"
+#include "Constraints.hh"
+#include "ConstraintLibrary.hh"
+#include "Timeline.hh"
+#include "Agent.hh"
+
+
+#include <gdk-pixbuf/gdk-pixbuf.h>
+#include <libstandalone_drivers/plan.h>
+
+#include <math.h>
+
+namespace TREX{
+
+
+ void initROSExecutive(){
+
+ // Initialize glib
+ g_type_init();
+
+ initTREX();
+
+ // Constraint Registration
+ REGISTER_CONSTRAINT(SubsetOfConstraint, "in", "Default");
+ REGISTER_CONSTRAINT(CalcDistanceConstraint, "calcDistance", "Default");
+ REGISTER_CONSTRAINT(FloorFunction, "calcFloor", "Default");
+
+ // Solver Components
+ REGISTER_FLAW_FILTER(TREX::GoalsOnlyFilter, GoalsOnly);
+ REGISTER_FLAW_FILTER(TREX::NoGoalsFilter, NoGoals);
+ REGISTER_FLAW_MANAGER(TREX::GoalManager, GoalManager);
+ }
+
+ FloorFunction::FloorFunction(const LabelStr& name,
+ const LabelStr& propagatorName,
+ const ConstraintEngineId& constraintEngine,
+ const std::vector<ConstrainedVariableId>& variables)
+ : Constraint(name, propagatorName, constraintEngine, variables),
+ m_target(getCurrentDomain(m_variables[0])),
+ m_source(getCurrentDomain(m_variables[1])){
+ checkError(m_variables.size() == 2, "Exactly 2 parameters required. ");
+ }
+
+ void FloorFunction::handleExecute(){
+ if(m_source.isSingleton()){
+ int f = (int) m_source.getSingletonValue();
+ m_target.set(f);
+ }
+ }
+
+ GoalsOnlyFilter::GoalsOnlyFilter(const TiXmlElement& configData): FlawFilter(configData, true) {}
+
+ bool GoalsOnlyFilter::test(const EntityId& entity){
+ checkError(TokenId::convertable(entity), "Invalid configuration for " << entity->toString());
+
+ TokenId token(entity);
+ return !token->getState()->baseDomain().isMember(Token::REJECTED);
+ }
+
+ NoGoalsFilter::NoGoalsFilter(const TiXmlElement& configData): FlawFilter(configData, true) {}
+
+ bool NoGoalsFilter::test(const EntityId& entity){
+ checkError(TokenId::convertable(entity), "Invalid configuration for " << entity->toString());
+
+ TokenId token(entity);
+ return token->getState()->baseDomain().isMember(Token::REJECTED);
+ }
+
+
+ /**
+ * @brief Constructor will read xml configuration data as needed. Can extend to include a map input source for example.
+ * @todo Include a map input file as a configuration argument
+ */
+ GoalManager::GoalManager(const TiXmlElement& configData)
+ : OpenConditionManager(configData),
+ m_maxIterations(1000),
+ m_maxPlateau(5),
+ m_positionSourceCfg(""),
+ m_cycleCount(0),
+ m_lastCycle(0) {
+ // See if there are configuration inputs to over-ride the defaults
+ // TODO: Add error checking
+ const char * positionSrc = configData.Attribute(CFG_POSITION_SOURCE().c_str());
+ if(positionSrc != NULL)
+ m_positionSourceCfg = LabelStr(positionSrc);
+ }
+
+ /**
+ * @brief When a new flaw is added we will re-evaluate all the options. This is accomplished
+ * by incrementing a cycle count which makes existing solution stale.
+ */
+ void GoalManager::addFlaw(const TokenId& token){
+ OpenConditionManager::addFlaw(token);
+
+ if(token->getState()->baseDomain().isMember(Token::REJECTED)){
+ debugMsg("GoalManager:addFlaw", token->toString());
+ m_cycleCount++;
+ }
+ }
+
+ void GoalManager::removeFlaw(const TokenId& token){
+ OpenConditionManager::removeFlaw(token);
+
+ // If the flaw was rejected, come up with a revised solution to get better utility
+ if(token->isRejected())
+ m_cycleCount++;
+
+ // If the token is active, remove it from the current solution if present
+ if(token->isActive()) {
+ SOLUTION::iterator it = m_currentSolution.begin();
+ while(it != m_currentSolution.end()){
+ TokenId t = *it;
+ if(t == token){
+ m_currentSolution.erase(it);
+ break;
+ }
+ ++it;
+ }
+ }
+
+ // Clear from ommittedTokens
+ m_ommissions.erase(token);
+ }
+
+ void GoalManager::handleInitialize(){
+ static const LabelStr sl_nullLabel("");
+ OpenConditionManager::handleInitialize();
+ m_cycleCount++;
+ if(m_positionSourceCfg != sl_nullLabel){
+ debugMsg("GoalManager:handleInitialize", "Looking to source position information from '" << m_positionSourceCfg.toString() << "'");
+ m_positionSource = getPlanDatabase()->getObject(m_positionSourceCfg);
+ }
+ }
+
+ /**
+ * @brief Generates a simple seed solution
+ */
+ void GoalManager::generateInitialSolution(){
+ debugMsg("GoalManager:generateInitialSolution", "Resetting solution");
+
+ m_currentSolution.clear();
+ m_ommissions.clear();
+
+ // Set the initial conditions since the problem may have moved on
+ setInitialConditions();
+
+ // The empty solution is the default solution
+ IteratorId it = OpenConditionManager::createIterator();
+ while(!it->done()){
+ TokenId goal = (TokenId) it->next();
+ checkError(goal->getMaster().isNoId(), goal->toString());
+ m_currentSolution.push_back(goal);
+ }
+
+ delete (FlawIterator*) it;
+ }
+
+ /**
+ * @brief For now we use euclidean distance. This is where map integration comes in.
+ */
+ double GoalManager::computeDistance(const Position& p1, const Position& p2){
+ return sqrt(pow(p1.x-p2.x, 2) + pow(p1.y-p2.y, 2));
+ }
+
+ /**
+ * @brief For now, this is going to be all about speed, distance and time
+ */
+ bool GoalManager::evaluate(const SOLUTION& s, double& cost, double& utility){
+ static const int MAX_PRIORITY = 5;
+ cost = 0;
+ utility = 0;
+ TokenId predecessor;
+ unsigned int numConflicts(0);
+
+ static const LabelStr INACTIVE("Navigator.Inactive");
+ double pathLength = 0;
+ Position currentPosition = getCurrentPosition();
+
+
+ for(SOLUTION::const_iterator it = s.begin(); it != s.end(); ++it){
+ TokenId candidate = *it;
+ checkError(candidate.isId() && candidate->getMaster().isNoId(), candidate->toString());
+
+ // Utility is 10 to the power of the priority
+ utility += pow(10.0, (MAX_PRIORITY - getPriority(candidate)));
+
+ // Check if there is a conflict
+ if(predecessor.isId() && !getPlanDatabase()->getTemporalAdvisor()->canPrecede(predecessor, candidate))
+ numConflicts++;
+
+ // TODO: Validate goal structure
+ Position nextPosition = getPosition(candidate);
+
+ pathLength += computeDistance(currentPosition, nextPosition);
+ predecessor = candidate;
+ }
+
+ // Priority is to remove conflicts so a much higher weight is given to that
+ cost = (pathLength / getSpeed()) + (numConflicts * pow(10.0, MAX_PRIORITY));
+
+ // Finally, feasibility is based on cost being within available budget for time.
+ // That budget is defined by the look ahead window of the solver
+ return cost <= m_timeBudget;
+ }
+
+ std::string GoalManager::toString(const SOLUTION& s){
+ double cost, utility;
+ evaluate(s, cost, utility);
+
+ std::stringstream ss;
+ for(SOLUTION::const_iterator it = s.begin(); it != s.end(); ++it){
+ TokenId t = *it;
+ ss << t->getKey() << ":";
+ }
+ ss << "(" << cost << "/" << utility << ")";
+ return ss.str();
+ }
+
+ /**
+ * @brief Get the best neighbor for the current solution. The neighborhood is the set of solutions
+ * within a single operation from the current solution. The operators are:
+ * 1. insert
+ * 2. swap
+ * 3. remove
+ * There are O(n^2) neigbors
+ *
+ * @note There is alot more we can do to exploit temporal constraints and evaluate feasibility. We are not including
+ * deadlines and we are not factoring in the possibility that insertion in the solution imposes implied temporal constraints.
+ */
+ void GoalManager::selectNeighbor(GoalManager::SOLUTION& s, TokenId& delta){
+ checkError(!m_currentSolution.empty() || !m_ommissions.empty(), "There must be something to do");
+ s = m_currentSolution;
+ delta = TokenId::noId();
+
+ // Try insertions - could skip if current solution is infeasible.
+ for(TokenSet::const_iterator it = m_ommissions.begin(); it != m_ommissions.end(); ++it){
+ TokenId t = *it;
+ for (unsigned int i = 0; i <= m_currentSolution.size(); i++){
+ SOLUTION c = m_currentSolution;
+ insert(c, t, i);
+ update(s, delta, c, t);
+ }
+ }
+
+ // Finally, swap
+ for(unsigned int i=0; i< m_currentSolution.size(); i++){
+ for(unsigned int j=i+1; j<m_currentSolution.size(); j++){
+ if(i != j){
+ SOLUTION c = m_currentSolution;
+ swap(c, i, j);
+ update(s, delta, c, TokenId::noId());
+ }
+ }
+ }
+
+ // Try removals
+ for(SOLUTION::const_iterator it = m_currentSolution.begin(); it != m_currentSolution.end(); ++it){
+ TokenId t = *it;
+
+ if(t->isActive())
+ continue;
+
+ SOLUTION c = m_currentSolution;
+ remove(c, t);
+ update(s, delta, c, t);
+ }
+ }
+
+ void GoalManager::insert(SOLUTION& s, const TokenId& t, unsigned int pos){
+ checkError(pos <= s.size(), pos << " > " << m_currentSolution.size());
+
+ debugMsg("GoalManager:insert", "Inserting " << t->toString() << " at [" << pos << "] in " << toString(s));
+
+ SOLUTION::iterator it = s.begin();
+ for(unsigned int i=0;i<=pos;i++){
+ // If we have the insertion point, do it and quit
+ if (i == pos){
+ s.insert(it, t);
+ break;
+ }
+
+ // Otherwise advance
+ ++it;
+ }
+ }
+
+ void GoalManager::swap(SOLUTION& s, unsigned int a, unsigned int b){
+ debugMsg("GoalManager:swap", "Swapping [" << a << "] and [" << b << "] in " << toString(s));
+ SOLUTION::iterator it = s.begin();
+ SOLUTION::iterator it_a = s.begin();
+ SOLUTION::iterator it_b = s.begin();
+ TokenId t_a, t_b;
+ unsigned int i = 0;
+ unsigned int max_i = std::max(a, b);
+
+ // Compute swap data
+ while (i <= max_i){
+ TokenId t = *it;
+ if (i == a){
+ it_a = it;
+ t_a = t;
+ }
+ else if(i == b){
+ it_b = it;
+ t_b = t;
+ }
+
+ i++;
+ ++it;
+ }
+
+ checkError(a != b && it_a != it_b && t_a != t_b, "Should be different");
+
+ // Execute swap
+ it_a = s.erase(it_a);
+ s.insert(it_a, t_b);
+ it_b = s.erase(it_b);
+ s.insert(it_b, t_a);
+ }
+
+ void GoalManager::remove(SOLUTION& s, const TokenId& t){
+ debugMsg("GoalManager:remove", "Removing " << t->toString() << " from " << toString(s));
+ SOLUTION::iterator it = s.begin();
+ while((*it) != t && it != s.end()) ++it;
+ checkError(it != s.end(), "Not found");
+ s.erase(it);
+ }
+
+ /**
+ * @brief Will promote a solution of equal or better score. This allows a move in a plateau
+ */
+ void GoalManager::update(SOLUTION& s, TokenId& delta, const SOLUTION& c, TokenId t){
+ debugMsg("GoalManager:update", "Evaluating " << toString(c));
+ if(compare(c, s) >= 0){
+ s = c;
+ delta = t;
+ debugMsg("GoalManager:update", "Promote " << toString(c));
+ }
+ }
+
+ int GoalManager::getPriority(const TokenId& token){
+ // Slaves take top priority. Cannot be rejected.
+ if(token->getMaster().isId())
+ return 0;
+
+ // Goals with no priority specified are assumed to be priority 0
+ ConstrainedVariableId p = token->getVariable(PRIORITY());
+ if(p.isNoId())
+ return 0;
+
+ // Now we get the actual priority value
+ checkError(p->lastDomain().isSingleton(), p->toString() << " must be set for " << token->toString());
+
+ return (int) p->lastDomain().getSingletonValue();
+ }
+
+ void GoalManager::setInitialConditions(){
+ // Start time
+ const IntervalIntDomain& horizon = DeliberationFilter::getHorizon();
+ m_startTime = (int) horizon.getLowerBound();
+ m_timeBudget = (int) (horizon.getUpperBound() - horizon.getLowerBound());
+ }
+
+ IteratorId GoalManager::createIterator(){
+ if(m_lastCycle < m_cycleCount){
+ generateInitialSolution();
+
+ search();
+
+ m_lastCycle = m_cycleCount;
+
+ // Now we allocate a trivial iterator with just a single goal - the first available inactive token
+ debugMsg("GoalManager:search", "Solution: " << toString(m_currentSolution));
+ }
+
+ TokenId nextGoal;
+ for(SOLUTION::const_iterator it = m_currentSolution.begin(); it != m_currentSolution.end(); ++it){
+ TokenId t = *it;
+ if(t->isInactive()){
+ nextGoal = t;
+ debugMsg("GoalManager:search", "Selecting " << nextGoal->toString() << " in " << toString(m_currentSolution));
+ break;
+ }
+ }
+
+ return (new GoalManager::Iterator(*this, nextGoal))->getId();
+ }
+
+ void GoalManager::search(){
+ // If nothing to be done, quit
+ if(m_currentSolution.empty() && m_ommissions.empty())
+ return;
+
+ // Local Search for a max number of iterations, or until we have plateued
+ unsigned int watchDog(0);
+ unsigned int i(0);
+ while(i < m_maxIterations && watchDog < m_maxPlateau){
+ // Update counters to handle termination
+ i++;
+ watchDog++;
+
+ // Get best neighbor
+ TokenId delta;
+ GoalManager::SOLUTION candidate;
+ selectNeighbor(candidate, delta);
+
+ // In the event that the candidate is the same solution, we have hit the end of exploration unless
+ // we escape somehow. The algorithm does not include such random walks to explore beyond the immediate
+ // neighborhood
+ if(candidate == m_currentSolution)
+ break;
+
+ // Promote if not worse. Allos for some exploration
+ int result = compare(candidate, m_currentSolution);
+ if(result >= 0){
+ // If a token was removed, insert into ommitted token list, and opposite if appended
+ if(m_currentSolution.size() > candidate.size())
+ m_ommissions.insert(delta);
+ else if(m_currentSolution.size() < candidate.size())
+ m_ommissions.erase(delta);
+
+ // Update current solution Values
+ m_currentSolution = candidate;
+
+ // Only pet the watchdog if we have improved the score. This allows us to terminate when
+ // we have plateaued
+ if(result == 1)
+ watchDog = 0;
+
+ debugMsg("GoalManager:search", "Switching to new solution: " << toString(m_currentSolution));
+ }
+ else
+ break;
+ }
+
+ debugMsg("GoalManager:search", "Completed after " << i << " iterations.");
+ }
+
+ /**
+ * @brief is s1 better than s2
+ * @return WORSE if s1 < s2. EQUAL if s1 == s2. BETTER if s1 > s2
+ */
+ int GoalManager::compare(const SOLUTION& s1, const SOLUTION& s2){
+ bool f1, f2;
+ double c1, c2, u1, u2;
+ f1 = evaluate(s1, c1, u1);
+ f2 = evaluate(s2, c2, u2);
+
+ // Feasibility is dominant.
+ if(f1 && !f2)
+ return BETTER;
+ if(f2 && !f1)
+ return WORSE;
+
+ // If both infeasible, cost dominates
+ if(!f1 && !f2 && c1 < c2)
+ return BETTER;
+ if(!f1 && !f2 && c1 > c2)
+ return WORSE;
+
+ // If both feasible, utility dominates
+ if(f1 && f2 && u1 > u2)
+ return BETTER;
+ if(f1 && f2 && u1 < u2)
+ return WORSE;
+
+ // Finally, a straight comparison
+ if((u1/c1) > (u2/c2))
+ return BETTER;
+ if((u1/c1) < (u2/c2))
+ return WORSE;
+ else
+ return EQUAL;
+ }
+
+ GoalManager::Iterator::Iterator(GoalManager& manager, const TokenId& nextGoal)
+ : FlawIterator(manager), m_nextGoal(nextGoal){ advance();}
+
+ const EntityId GoalManager::Iterator::nextCandidate(){
+ EntityId nextGoal = (EntityId) m_nextGoal;
+ m_nextGoal = EntityId::noId();
+ return nextGoal;
+ }
+
+ Position GoalManager::getPosition(const TokenId& token){
+ // Retrieve Variables by parameter name
+ ConstrainedVariableId x = token->getVariable(X());
+ ConstrainedVariableId y = token->getVariable(Y());
+
+ // Validate token structure for valid position data
+ checkError(x.isId(), "No variable for X");
+ checkError(y.isId(), "No variable for Y");
+ checkError(x->lastDomain().isSingleton(), x->lastDomain().toString());
+ checkError(y->lastDomain().isSingleton(), y->lastDomain().toString());
+
+ Position p;
+ p.x = x->lastDomain().getSingletonValue();
+ p.y = y->lastDomain().getSingletonValue();
+ return p;
+ }
+
+ /**
+ * @todo Implement this to bind to the current position in the designated timeline where that can be sourced.
+ */
+ Position GoalManager::getCurrentPosition() const {
+ static Position sl_pos;
+ sl_pos.x = 0.0;
+ sl_pos.y = 0.0;
+
+ TICK tick = Agent::instance()->getCurrentTick();
+
+ if(m_positionSource.isId()){
+ const std::list<TokenId>& tokens = m_positionSource->getTokenSequence();
+ for(std::list<TokenId>::const_iterator it = tokens.begin(); it != tokens.end(); ++it){
+ TokenId token = *it;
+ if(token->getStart()->lastDomain().getUpperBound() <= tick && token->getEnd()->lastDomain().getLowerBound() > tick)
+ return getPosition(token);
+
+ // Break if past the current time
+ if(token->getStart()->lastDomain().getLowerBound() > tick)
+ break;
+ }
+ }
+ return sl_pos;
+ }
+
+ /**
+ * @todo Implement an accessor for speed to compute time bounds. Should be a parameter, ultimately, of the goal
+ * but we can assume a nominal speed.
+ */
+ double GoalManager::getSpeed() const {
+ return 1.0;
+ }
+}
Added: pkg/trunk/exec_trex/Components.hh
===================================================================
--- pkg/trunk/exec_trex/Components.hh (rev 0)
+++ pkg/trunk/exec_trex/Components.hh 2008-05-30 15:34:43 UTC (rev 568)
@@ -0,0 +1,193 @@
+/**
+ * @brief Collection of various components to use
+ */
+#include "ExecDefs.hh"
+#include "OpenConditionManager.hh"
+#include "FlawFilter.hh"
+#include <list>
+
+using namespace EUROPA::SOLVERS;
+
+namespace TREX {
+
+ class FloorFunction: public Constraint{
+ public:
+ FloorFunction(const LabelStr& name,
+ const LabelStr& propagatorName,
+ const ConstraintEngineId& constraintEngine,
+ const std::vector<ConstrainedVariableId>& variables);
+
+ private:
+ void handleExecute();
+ AbstractDomain& m_target;
+ const AbstractDomain& m_source;
+ };
+
+
+ class GoalsOnlyFilter: public FlawFilter {
+ public:
+ GoalsOnlyFilter(const TiXmlElement& configData);
+ bool test(const EntityId& entity);
+ };
+
+ class NoGoalsFilter: public FlawFilter {
+ public:
+ NoGoalsFilter(const TiXmlElement& configData);
+ bool test(const EntityId& entity);
+ };
+
+ struct Position {
+ double x;
+ double y;
+ };
+
+ /**
+ * @brief Goal Manager to manage goal flaw selection. Will involve solving an orienteering problem
+ * over the relaxed problem to provide a good ordering. We can imagine this manager maintaining
+ * a priority q and releasing goals 1 at a time.
+ */
+ class GoalManager: public OpenConditionManager {
+ public:
+
+ /**
+ * @brief Solution is a sequence of tokens. None can be rejected. Each has a 2d position.
+ */
+ typedef std::list<TokenId> SOLUTION;
+
+ /**
+ * @brief Uses standard constructor
+ */
+ GoalManager(const TiXmlElement& configData);
+
+ /**
+ * @brief Handles the iteration. Will always impose a limit of the next goal
+ */
+ IteratorId createIterator();
+
+ // Assumptions about the fields in a goal
+ DECLARE_STATIC_CLASS_CONST(LabelStr, X, "x");
+ DECLARE_STATIC_CLASS_CONST(LabelStr, Y, "y");
+ DECLARE_STATIC_CLASS_CONST(LabelStr, PRIORITY, "Priority");
+
+ // Parameters used for configuration
+ DECLARE_STATIC_CLASS_CONST(LabelStr, CFG_POSITION_SOURCE, "positionSource");
+ DECLARE_STATIC_CLASS_CONST(LabelStr, CFG_MAP_SOURCE, "mapSource");
+ DECLARE_STATIC_CLASS_CONST(LabelStr, CFG_MAX_ITERATIONS, "maxIterations");
+ DECLARE_STATIC_CLASS_CONST(LabelStr, CFG_MAX_PLATEAU, "maxPlateau");
+
+ private:
+ /**
+ * @brief A Trivial iterator - one goal only. Used to return a goal to the solver
+ */
+ class Iterator : public FlawIterator {
+ public:
+ Iterator(GoalManager& manager, const TokenId& nextGoal);
+
+ private:
+ const EntityId nextCandidate();
+ TokenId m_nextGoal;
+ };
+
+ /**
+ * @brief Used to synch mark current goal value as dirty
+ * @see OpenConditionManager::addFlaw
+ */
+ void addFlaw(const TokenId& token);
+
+ /**
+ * @brief Used to synch mark current goal value as dirty
+ * @see OpenConditionManager::removeFlaw
+ */
+ void removeFlaw(const TokenId& token);
+
+ /**
+ * @brief Used to synch mark current goal value as dirty
+ */
+ void handleInitialize();
+
+ /**
+ * @brief Generate an initial solution. May not be feasible.
+ */
+ void generateInitialSolution();
+
+ /**
+ * @brief Search for best solution
+ */
+ void search();
+
+ /**
+ * @brief Helper method
+ */
+ int getPriority(const TokenId& token);
+
+ /**
+ * @brief Evaluate the solution s. This will compute both the cost and the utility.
+ * @return true if feasible, false if infeasible
+ */
+ bool evaluate(const SOLUTION& s, double& cost, double& utility);
+
+ /**
+ * @brief Compute a neighboring solution for s
+ */
+ void selectNeighbor(GoalManager::SOLUTION& s, TokenId& delta);
+
+ /**
+ * @brief Set initial conditions in terms of position, time and energy
+ */
+ void setInitialConditions();
+
+ /**
+ * @brief Get a distance estimate between points.
+ */
+ double computeDistance(const Position& p1, const Position& p2);
+
+ /**
+ * @brief Accessor to get a Position. For convenience
+ */
+ static Position getPosition(const TokenId& token);
+
+ /**
+ * @brief Accessor to get current position (at current tick)
+ */
+ Position getCurrentPosition() const;
+
+ /**
+ * @brief Accessor for robot speed
+ */
+ double getSpeed() const;
+
+ /**
+ * @brief Comparator
+ */
+ int compare(const SOLUTION& s1, const SOLUTION& s2);
+
+ void insert(SOLUTION&s, const TokenId& t, unsigned int pos);
+ void swap(SOLUTION& s, unsigned int a, unsigned int b);
+ void remove(SOLUTION& s, const TokenId& t);
+ void update(SOLUTION& s, TokenId& delta, const SOLUTION& c, TokenId t);
+
+ std::string toString(const SOLUTION& s);
+
+ // Configuration derived members
+ unsigned int m_maxIterations;
+ unsigned int m_maxPlateau;
+ LabelStr m_positionSourceCfg;
+ TimelineId m_positionSource;
+
+ unsigned int m_cycleCount;
+ unsigned int m_lastCycle;
+ SOLUTION m_currentSolution;
+ TokenSet m_ommissions;
+
+ /*!< INITIAL CONDITIONS */
+ int m_startTime;
+ double m_timeBudget;
+
+ // Integration with wavefront planner
+ //plan_t* wv_plan;
+
+ static const int WORSE = -1;
+ static const int EQUAL = 0;
+ static const int BETTER = 1;
+ };
+}
Added: pkg/trunk/exec_trex/Debug.cfg
===================================================================
--- pkg/trunk/exec_trex/Debug.cfg (rev 0)
+++ pkg/trunk/exec_trex/Debug.cfg 2008-05-30 15:34:43 UTC (rev 568)
@@ -0,0 +1,10 @@
+:Executive
+#:Monitor
+:DbCore
+#:Synchronizer
+:GoalManager
+#:ConstraintEngine
+#:PlanDatabase
+#:Constraint
+:RCSAdapter
+#:Solver
Added: pkg/trunk/exec_trex/ExecDefs.cc
===================================================================
--- pkg/trunk/exec_trex/ExecDefs.cc (rev 0)
+++ pkg/trunk/exec_trex/ExecDefs.cc 2008-05-30 15:34:43 UTC (rev 568)
@@ -0,0 +1,17 @@
+#include "ExecDefs.hh"
+
+namespace TREX {
+
+ std::string toString(const MsgPlanner2DState& s){
+ std::stringstream ss;
+ ss << "At(" << s.pos.x << ", " << s.pos.y << ") Going(" << s.goal.x << ", " << s.goal.y << ") Planner is:" << (s.done == 1 ? "INACTIVE" : "ACTIVE");
+ return ss.str();
+ }
+
+ std::string toString(const MsgPlanner2DGoal& g){
+ std::stringstream ss;
+ ss << "Going(" << g.goal.x << ", " << g.goal.y << ")";
+ return ss.str();
+ }
+
+}
Added: pkg/trunk/exec_trex/ExecDefs.hh
===================================================================
--- pkg/trunk/exec_trex/ExecDefs.hh (rev 0)
+++ pkg/trunk/exec_trex/ExecDefs.hh 2008-05-30 15:34:43 UTC (rev 568)
@@ -0,0 +1,35 @@
+#ifndef H_ExecDefs
+#define H_ExecDefs
+
+#include "Id.hh"
+#include "ros/node.h"
+#include "std_msgs/MsgPlanner2DState.h"
+#include "std_msgs/MsgPlanner2DGoal.h"
+#include "std_msgs/MsgToken.h"
+
+
+using namespace EUROPA;
+using namespace std;
+
+namespace TREX {
+
+ enum PlannerState {
+ UNDEFINED = 0,
+ ACTIVE,
+ INACTIVE
+ };
+
+ class Executive;
+ typedef Id<Executive> ExecutiveId;
+ class Monitor;
+ typedef Id<Monitor> MonitorId;
+ class GoalManager;
+
+ std::string toString(const MsgPlanner2DState& s);
+ std::string toString(const MsgPlanner2DGoal& g);
+
+ void initROSExecutive();
+}
+
+using namespace TREX;
+#endif
Added: pkg/trunk/exec_trex/ExecListener.cc
===================================================================
--- pkg/trunk/exec_trex/ExecListener.cc (rev 0)
+++ pkg/trunk/exec_trex/ExecListener.cc 2008-05-30 15:34:43 UTC (rev 568)
@@ -0,0 +1,28 @@
+#include "ros/node.h"
+#include "std_msgs/MsgToken.h"
+
+
+class ExecListener : public ros::node
+{
+public:
+
+ ExecListener() : ros::node("execListener"){
+ subscribe("navigator", msg, &ExecListener::navigator_cb);
+ }
+
+ void navigator_cb(){
+ printf("Monitor received status update: [%s]\n", msg.predicate.c_str());
+ }
+
+private:
+ MsgToken msg;
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ ExecListener l;
+ l.spin();
+ l.shutdown();
+ return 0;
+}
Added: pkg/trunk/exec_trex/Executive.cc
===================================================================
--- pkg/trunk/exec_trex/Executive.cc (rev 0)
+++ pkg/trunk/exec_trex/Executive.cc 2008-05-30 15:34:43 UTC (rev 568)
@@ -0,0 +1,241 @@
+///////////////////////////////////////////////////////////////////////////////
+// The trex demo node is a simple encapsulation of a Teleo-Reactive Executive
+//
+// Copyright (C) 2008, Willow Garage
+//////////////////////////////////////////////////////////////////////////////
+
+#include "Nddl.hh"
+#include "Executive.hh"
+#include "RCSAdapter.hh"
+#include "Token.hh"
+#include <signal.h>
+
+TiXmlElement* root = NULL;
+Clock* agentClock = NULL;
+std::ofstream dbgFile("Debug.log");
+
+namespace TREX {
+
+ /**
+ * Executive class implementation.
+ */
+
+ /**
+ * @brief Singleton accessor
+ */
+ ExecutiveId Executive::instance(){
+ return s_id;
+ }
+
+ /**
+ * @brief Sets up publish and subscribe topics for this node to integrate it
+ * at the top to publish Execution Status updates and at the bottom to dispatch
+ * commands to the RCS and receive updates
+ */
+ Executive::Executive() : ros::node("trex"), m_id(this), m_initialized(1), m_state(UNDEFINED)
+ {
+ s_id = m_id;
+ pthread_mutex_init(&m_lock, NULL);
+ m_rcs_obs.pos.x = 0.0;
+ m_rcs_obs.pos.y = 0.0;
+ m_rcs_obs.pos.th = 0.0;
+ m_rcs_obs.done = 1;
+ m_rcs_obs.valid = 1;
+ }
+
+ Executive::~Executive(){
+ m_id.remove();
+ s_id = ExecutiveId::noId();
+ }
+
+ /**
+ * @brief Nothing to do here. The msg object will already have been updated and that is what we use
+ * directly in synchronization
+ */
+ void Executive::rcs_cb(){
+ m_initialized = true;
+ std::cout << "Received Update:" << toString(m_rcs_obs) << std::endl;
+ }
+
+ /**
+ * @brief Called when an update is received in the Monitor during synchronization
+ */
+ void Executive::publish(const Observation& obs)
+ {
+ debugMsg("Executive:publish", obs.toString());
+ MsgToken msg;
+ msg.predicate = obs.toString();
+ ros::node::publish("navigator", msg);
+ }
+
+ /**
+ * @brief Dispatch a token
+ */
+ void Executive::dispatch(const TokenId& goal){
+ static const LabelStr ACTIVE("WaypointController.Active");
+ static const LabelStr X2("x");
+ static const LabelStr Y2("y");
+
+ if(goal->getPredicateName() == ACTIVE){
+ MsgPlanner2DGoal msg;
+ const IntervalDomain& x = goal->getVariable(X2)->lastDomain();
+ const IntervalDomain& y = goal->getVariable(Y2)->lastDomain();
+
+ checkError(x.isSingleton(), x.toString());
+ checkError(y.isSingleton(), y.toString());
+
+ // Fill outbound msg. Don't care about orientation
+ msg.goal.x = x.getSingleton...
[truncated message content] |
|
From: <tf...@us...> - 2008-05-30 21:04:16
|
Revision: 571
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=571&view=rev
Author: tfoote
Date: 2008-05-30 14:04:24 -0700 (Fri, 30 May 2008)
Log Message:
-----------
compiling test program for irrlicht renderer
Modified Paths:
--------------
pkg/trunk/3rdparty/irrlicht/manifest.xml
Added Paths:
-----------
pkg/trunk/visualization/irrlicht_viewer/
pkg/trunk/visualization/irrlicht_viewer/CustomNodes/
pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILGrid.cc
pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILGrid.hh
pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILPointCloud.cc
pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILPointCloud.hh
pkg/trunk/visualization/irrlicht_viewer/ILClient.cc
pkg/trunk/visualization/irrlicht_viewer/ILClient.hh
pkg/trunk/visualization/irrlicht_viewer/ILRender.cc
pkg/trunk/visualization/irrlicht_viewer/ILRender.hh
pkg/trunk/visualization/irrlicht_viewer/ILRenderServer.hh
pkg/trunk/visualization/irrlicht_viewer/ILRenderTest.cc
pkg/trunk/visualization/irrlicht_viewer/Makefile
pkg/trunk/visualization/irrlicht_viewer/PPILRender.hh
pkg/trunk/visualization/irrlicht_viewer/manifest.xml
pkg/trunk/visualization/irrlicht_viewer/media/
pkg/trunk/visualization/irrlicht_viewer/media/aibo_lo.x3d
pkg/trunk/visualization/irrlicht_viewer/media/board.x3d
pkg/trunk/visualization/irrlicht_viewer/media/circle.x3d
pkg/trunk/visualization/irrlicht_viewer/media/dirt.jpg
pkg/trunk/visualization/irrlicht_viewer/media/grid.jpg
pkg/trunk/visualization/irrlicht_viewer/media/ground.x3d
pkg/trunk/visualization/irrlicht_viewer/media/prius.x3d
pkg/trunk/visualization/irrlicht_viewer/media/priusS.jpg
pkg/trunk/visualization/irrlicht_viewer/media/square.x3d
pkg/trunk/visualization/irrlicht_viewer/media/star.x3d
pkg/trunk/visualization/irrlicht_viewer/media/wedge.x3d
Modified: pkg/trunk/3rdparty/irrlicht/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/irrlicht/manifest.xml 2008-05-30 19:10:34 UTC (rev 570)
+++ pkg/trunk/3rdparty/irrlicht/manifest.xml 2008-05-30 21:04:24 UTC (rev 571)
@@ -11,7 +11,7 @@
<license>zlib</license>
<url>http://irrlicht.sourceforge.net/index.html</url>
<export>
- <cpp lflags="-L${prefix}/irrlicht-1.4/lib/Linux -lIrrlicht -L/usr/X111R6/lib -lGL -lXxf86vm -lXext -lX11" cflags="-I${prefix}/irrlicht-1.4/include /usr/X11R6/include -ffast-math"/>
+ <cpp lflags="-L${prefix}/irrlicht-1.4/lib/Linux -lIrrlicht -L/usr/X111R6/lib -lGL -lXxf86vm -lXext -lX11" cflags="-I${prefix}/irrlicht-1.4/include -ffast-math"/>
<doxymaker external="http://irrlicht.sourceforge.net/docu/index.html" />
</export>
</package>
Added: pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILGrid.cc
===================================================================
--- pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILGrid.cc (rev 0)
+++ pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILGrid.cc 2008-05-30 21:04:24 UTC (rev 571)
@@ -0,0 +1,101 @@
+///////////////////////////////////////////////////////////////////////////////
+//
+// Copyright (C) 2008, Willow Garage Inc.
+//
+// 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 "ILGrid.hh"
+
+#define MIN(a,b) (((a)<(b))?(a):(b))
+
+using namespace irr;
+
+ILGrid::ILGrid(irr::scene::ISceneNode* parent,
+ irr::scene::ISceneManager* mgr,
+ irr::s32 id)
+: scene::ISceneNode(parent, mgr, id)
+{
+ m_material.Lighting = false;
+ m_material.Wireframe = true;
+ m_material.BackfaceCulling = false;
+ m_material.Thickness = 1;
+}
+
+ILGrid::~ILGrid() {
+ deallocatePoints();
+}
+
+// Manipulators
+void ILGrid::makegrid(const size_t gridSize, const double cellLength, int r, int g, int b) {
+ m_gridSize = gridSize;
+ m_cellLength = cellLength;
+ m_extent = (cellLength*((double)gridSize))/2;
+ m_r = r;
+ m_g = g;
+ m_b = b;
+}
+
+void ILGrid::deallocatePoints() {
+}
+
+void ILGrid::OnRegisterSceneNode() {
+ if (IsVisible) {
+ SceneManager->registerNodeForRendering(this);
+ }
+
+ ISceneNode::OnRegisterSceneNode();
+}
+
+void ILGrid::render() {
+ video::IVideoDriver* driver = SceneManager->getVideoDriver();
+
+ driver->setMaterial(m_material);
+ driver->setTransform(video::ETS_WORLD, AbsoluteTransformation);
+
+ double inc;
+
+ for(size_t i=0; i<=m_gridSize; i++) {
+ inc = m_extent-i*m_cellLength;
+
+ driver->draw3DLine(core::vector3df(inc,0,-1*m_extent),
+ core::vector3df(inc,0,m_extent),
+ video::SColor(255,m_r,m_g,m_b));
+ driver->draw3DLine(core::vector3df(-1*m_extent,0,inc),
+ core::vector3df(m_extent,0,inc),
+ video::SColor(255,m_r,m_g,m_b));
+ }
+}
+
+const irr::core::aabbox3d<irr::f32>& ILGrid::getBoundingBox() const {
+ return m_box;
+}
+
+irr::u32 ILGrid::getMaterialCount() {
+ return 1;
+}
+
+irr::video::SMaterial& ILGrid::getMaterial(irr::u32 i) {
+ return m_material;
+}
Property changes on: pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILGrid.cc
___________________________________________________________________
Name: svn:mime-type
+ text/cpp
Added: pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILGrid.hh
===================================================================
--- pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILGrid.hh (rev 0)
+++ pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILGrid.hh 2008-05-30 21:04:24 UTC (rev 571)
@@ -0,0 +1,66 @@
+#ifndef __IL_GRID_HH
+#define __IL_GRID_HH
+///////////////////////////////////////////////////////////////////////////////
+//
+// Copyright (C) 2008, Willow Garage Inc.
+//
+// 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 <irrlicht.h>
+#include <iostream>
+#include <math.h>
+
+class ILGrid : public irr::scene::ISceneNode {
+public:
+ static const size_t MAX_RENDERABLE = 65535;
+
+ ILGrid(irr::scene::ISceneNode* parent, irr::scene::ISceneManager* mgr, irr::s32 id);
+ ~ILGrid();
+
+ // Manipulators
+ void makegrid(const size_t gidsize, const double cellLength, int r, int g, int b);
+ void deallocatePoints();
+
+ // Methods implemented for engine
+ virtual void OnRegisterSceneNode();
+ virtual void render();
+
+ virtual const irr::core::aabbox3d<irr::f32>& getBoundingBox() const;
+ virtual irr::u32 getMaterialCount();
+ virtual irr::video::SMaterial& getMaterial(irr::u32 i);
+
+private:
+
+ size_t m_gridSize;
+ double m_cellLength;
+ double m_extent;
+ int m_r,m_g,m_b;
+
+ irr::video::SMaterial m_material;
+ irr::core::aabbox3d<irr::f32> m_box;
+};
+
+#endif // ifndef __IL_GRID_HH
Added: pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILPointCloud.cc
===================================================================
--- pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILPointCloud.cc (rev 0)
+++ pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILPointCloud.cc 2008-05-30 21:04:24 UTC (rev 571)
@@ -0,0 +1,137 @@
+///////////////////////////////////////////////////////////////////////////////
+//
+// Copyright (C) 2008, Willow Garage Inc.
+//
+// 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 "ILPointCloud.hh"
+
+#define MIN(a,b) (((a)<(b))?(a):(b))
+
+using namespace irr;
+
+ILPointCloud::ILPointCloud(irr::scene::ISceneNode* parent,
+ irr::scene::ISceneManager* mgr,
+ irr::s32 id)
+: scene::ISceneNode(parent, mgr, id),
+ m_numPoints (0),
+ m_numAllocPoints (0),
+ m_points (NULL)
+{
+ m_material.Lighting = false;
+ m_material.Wireframe = false;
+ m_material.PointCloud = true;
+ m_material.BackfaceCulling = false;
+ m_material.Thickness = 2;
+}
+
+ILPointCloud::~ILPointCloud() {
+ deallocatePoints();
+}
+
+// Memory management
+void ILPointCloud::preallocatePoints(const size_t numPoints) {
+ if(m_numAllocPoints == 0) {
+ m_points = (video::S3DVertex*)malloc(numPoints*sizeof(video::S3DVertex));
+ m_numAllocPoints = numPoints;
+ }
+}
+
+void ILPointCloud::deallocatePoints() {
+ if(m_numAllocPoints > 0) {
+ free(m_points);
+ m_points = NULL;
+ m_numAllocPoints = 0;
+ m_numPoints = 0;
+ }
+}
+
+// Manipulators
+void ILPointCloud::addPoint(const double x, const double y, const double z,
+ const int r, const int g, const int b)
+{
+ if(m_numPoints == m_numAllocPoints) {
+ m_points = (video::S3DVertex*)realloc(m_points,2*m_numAllocPoints*sizeof(video::S3DVertex));
+ }
+
+ m_points[m_numPoints].Pos.set(x,y,z);
+ m_points[m_numPoints].Color.set(255,r,g,b);
+
+ m_numPoints++;
+}
+
+void ILPointCloud::addPoints(double *rgX, double *rgY, double *rgZ,
+ int *rgR, int *rgG, int *rgB,
+ const size_t numPoints)
+{
+ if(m_numPoints+numPoints >= m_numAllocPoints) {
+ m_points = (video::S3DVertex*)realloc(m_points,numPoints*sizeof(video::S3DVertex));
+ }
+
+ for(int i=0; i<numPoints; i++) {
+ m_points[m_numPoints].Pos.set(rgX[i],rgY[i],rgZ[i]);
+ m_points[m_numPoints].Color.set(255,rgR[i],rgG[i],rgB[i]);
+ }
+
+ m_numPoints = numPoints;
+}
+
+void ILPointCloud::resetCount() {
+ m_numPoints = 0;
+}
+
+void ILPointCloud::OnRegisterSceneNode() {
+ if (IsVisible) {
+ SceneManager->registerNodeForRendering(this);
+ }
+
+ ISceneNode::OnRegisterSceneNode();
+}
+
+void ILPointCloud::render() {
+ video::IVideoDriver* driver = SceneManager->getVideoDriver();
+
+ driver->setMaterial(m_material);
+ driver->setTransform(video::ETS_WORLD, AbsoluteTransformation);
+
+ for(size_t i=0; i<m_numPoints; i+=MAX_RENDERABLE) {
+ driver->drawVertexPrimitiveList(m_points,MIN(m_numPoints-i, MAX_RENDERABLE),
+ NULL, MIN(m_numPoints-i,MAX_RENDERABLE),
+ video::EVT_STANDARD,
+ scene::EPT_POINTS);
+ }
+}
+
+const irr::core::aabbox3d<irr::f32>& ILPointCloud::getBoundingBox() const {
+ return m_box;
+}
+
+irr::u32 ILPointCloud::getMaterialCount() {
+ return 1;
+}
+
+irr::video::SMaterial& ILPointCloud::getMaterial(irr::u32 i) {
+ return m_material;
+}
Property changes on: pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILPointCloud.cc
___________________________________________________________________
Name: svn:mime-type
+ text/cpp
Added: pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILPointCloud.hh
===================================================================
--- pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILPointCloud.hh (rev 0)
+++ pkg/trunk/visualization/irrlicht_viewer/CustomNodes/ILPointCloud.hh 2008-05-30 21:04:24 UTC (rev 571)
@@ -0,0 +1,69 @@
+#ifndef __IL_POINT_CLOUD_HH
+#define __IL_POINT_CLOUD_HH
+///////////////////////////////////////////////////////////////////////////////
+//
+// Copyright (C) 2008, Willow Garage Inc.
+//
+// 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 <irrlicht.h>
+#include <iostream>
+#include <math.h>
+
+class ILPointCloud : public irr::scene::ISceneNode {
+public:
+ static const size_t MAX_RENDERABLE = 65535;
+
+ ILPointCloud(irr::scene::ISceneNode* parent, irr::scene::ISceneManager* mgr, irr::s32 id);
+ ~ILPointCloud();
+
+ // Memory management
+ void preallocatePoints(const size_t numPoints);
+ void deallocatePoints();
+
+ // Manipulators
+ void addPoint(const double x, const double y, const double z, const int r, const int g, const int b);
+ void addPoints(double *rgX, double *rgY, double *rgZ,
+ int *rgR, int *rgG, int *rgB,
+ const size_t numPoints);
+ void resetCount();
+
+ // Methods implemented for engine
+ virtual void OnRegisterSceneNode();
+ virtual void render();
+
+ virtual const irr::core::aabbox3d<irr::f32>& getBoundingBox() const;
+ virtual irr::u32 getMaterialCount();
+ virtual irr::video::SMaterial& getMaterial(irr::u32 i);
+
+private:
+ size_t m_numPoints, m_numAllocPoints;
+ irr::video::S3DVertex *m_points;
+
+ irr::video::SMaterial m_material;
+ irr::core::aabbox3d<irr::f32> m_box;
+};
+
+#endif // #ifndef __IL_POINT_CLOUD_HH
Added: pkg/trunk/visualization/irrlicht_viewer/ILClient.cc
===================================================================
--- pkg/trunk/visualization/irrlicht_viewer/ILClient.cc (rev 0)
+++ pkg/trunk/visualization/irrlicht_viewer/ILClient.cc 2008-05-30 21:04:24 UTC (rev 571)
@@ -0,0 +1,87 @@
+///////////////////////////////////////////////////////////////////////////////
+//
+// Copyright (C) 2008, Willow Garage Inc.
+//
+// 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 "ILClient.hh"
+
+// Initialize singletona members
+ILRender *ILClient::s_pRenderer = NULL;
+uint32_t ILClient::s_numClients = 0;
+pthread_mutex_t *ILClient::s_pSingletonMutex = NULL;
+
+ILClient::ILClient() {
+ // If it is uninialized, initialize mutex for singleton access
+ if(s_pSingletonMutex == NULL) {
+ s_pSingletonMutex = new pthread_mutex_t;
+ pthread_mutex_init(s_pSingletonMutex,0L);
+ }
+
+ pthread_mutex_lock(s_pSingletonMutex);
+
+ // Increment client counter
+ s_numClients++;
+
+ pthread_mutex_unlock(s_pSingletonMutex);
+}
+
+ILClient::~ILClient() {
+ // Acquire singleton lock
+ pthread_mutex_lock(s_pSingletonMutex);
+
+ // Decrement client counter
+ s_numClients--;
+
+ // If this is the last client
+ if(s_numClients == 0) {
+ // Destruct the singleton members
+ delete s_pRenderer;
+ s_pRenderer = NULL;
+ pthread_mutex_destroy(s_pSingletonMutex);
+ delete s_pSingletonMutex;
+ } else {
+ // Release the lock
+ pthread_mutex_unlock(s_pSingletonMutex);
+ }
+}
+
+ILRender* ILClient::getSingleton() {
+ pthread_mutex_lock(s_pSingletonMutex);
+
+ // Check for renderer instantiation
+ if(s_pRenderer == NULL) {
+ // Create a default ILRender
+ s_pRenderer = new ILRender();
+
+ while(s_pRenderer->uninitialized()) {
+ usleep(10);
+ }
+ }
+
+ pthread_mutex_unlock(s_pSingletonMutex);
+
+ return s_pRenderer;
+}
Property changes on: pkg/trunk/visualization/irrlicht_viewer/ILClient.cc
___________________________________________________________________
Name: svn:mime-type
+ text/cpp
Added: pkg/trunk/visualization/irrlicht_viewer/ILClient.hh
===================================================================
--- pkg/trunk/visualization/irrlicht_viewer/ILClient.hh (rev 0)
+++ pkg/trunk/visualization/irrlicht_viewer/ILClient.hh 2008-05-30 21:04:24 UTC (rev 571)
@@ -0,0 +1,52 @@
+#ifndef __IL_CLIENT_HH
+#define __IL_CLIENT_HH
+
+///////////////////////////////////////////////////////////////////////////////
+//
+// Copyright (C) 2008, Willow Garage Inc.
+//
+// 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 <pthread.h>
+#include <irrlicht.h>
+
+#include "ILRender.hh"
+
+class ILClient {
+public:
+ // Constructor
+ ILClient();
+ ~ILClient();
+
+ static ILRender* getSingleton();
+
+private:
+ static uint32_t s_numClients;
+ static ILRender *s_pRenderer;
+ static pthread_mutex_t *s_pSingletonMutex;
+};
+
+#endif // ifndef __IL_CLIENT_HH
Added: pkg/trunk/visualization/irrlicht_viewer/ILRender.cc
===================================================================
--- pkg/trunk/visualization/irrlicht_viewer/ILRender.cc (rev 0)
+++ pkg/trunk/visualization/irrlicht_viewer/ILRender.cc 2008-05-30 21:04:24 UTC (rev 571)
@@ -0,0 +1,237 @@
+///////////////////////////////////////////////////////////////////////////////
+//
+// Copyright (C) 2008, Willow Garage Inc.
+//
+// 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 "ILRender.hh"
+
+using namespace irr;
+
+// Construction / Destruction
+ILRender::ILRender(core::dimension2d<s32> resolution, video::E_DRIVER_TYPE driverType)
+: m_enabled (false),
+ m_initialized (false),
+ m_pRenderThread (NULL)
+{
+ pthread_mutex_init(&m_renderMutex,0L);
+ init(resolution,driverType);
+}
+
+ILRender::~ILRender() {
+ cleanup();
+ pthread_mutex_destroy(&m_renderMutex);
+}
+
+bool ILRender::uninitialized() {
+ return !m_initialized;
+}
+
+void ILRender::init(core::dimension2d<s32> resolution, video::E_DRIVER_TYPE driverTyped) {
+ using namespace ros::thread::member_thread;
+ lock();
+ if(!m_enabled) {
+ // Start render thread
+ m_enabled = true;
+ m_pRenderThread = startMemberFunctionThread(this, &ILRender::renderLoop);
+ }
+ unlock();
+}
+
+void ILRender::cleanup() {
+ lock();
+ if(m_enabled && m_pRenderThread != NULL) {
+ m_enabled = false;
+ pthread_join(*m_pRenderThread, 0L);
+ m_pRenderThread = NULL;
+ m_pDevice->drop();
+ }
+ unlock();
+}
+
+// Renderer Management
+void ILRender::lock() {
+ if(pthread_mutex_lock(&m_renderMutex) == -1) {
+ //TODO: throw exception here
+ }
+}
+
+void ILRender::unlock() {
+ if(pthread_mutex_unlock(&m_renderMutex) == -1) {
+ //TODO: throw exception here
+ }
+}
+
+// Node Management
+void ILRender::addNode(scene::ISceneNode *pNode) {
+ lock();
+
+ // Check for node first, to ensure only one copy
+ if(m_nodes.find(pNode) == m_nodes.end()) {
+ // Initialize nodeInfo structure
+ NodeInfo *tempinfo = new NodeInfo();
+
+ // Lock mutex before insertion
+ int ret = pthread_mutex_lock(&(tempinfo->lock));
+ if(ret != 0) {
+ //TODO: throw exception here
+ }
+ // Insert
+ m_nodes.insert(std::make_pair(pNode,tempinfo));
+ // Unlock mutex
+ unlock(pNode);
+ }
+
+ unlock();
+}
+
+void ILRender::remNode(scene::ISceneNode *pNode) {
+ lock();
+
+ lock(pNode);
+ delete m_nodes[pNode];
+ m_nodes.erase(pNode);
+
+ unlock();
+}
+
+void ILRender::pushToDraw(scene::ISceneNode *pNode) {
+ m_drawPipe.push(pNode);
+}
+
+void ILRender::enable(scene::ISceneNode *pNode) {
+ lock(pNode);
+ m_nodes[pNode]->enabled = true;
+ unlock(pNode);
+}
+
+void ILRender::disable(scene::ISceneNode *pNode) {
+ lock(pNode);
+ m_nodes[pNode]->enabled = false;
+ unlock(pNode);
+}
+
+// Functions for locking individual nodes
+bool ILRender::trylock(scene::ISceneNode *pNode) {
+ int ret = pthread_mutex_trylock(&(m_nodes[pNode]->lock));
+ if(ret != 0 && errno == EBUSY) {
+ return false;
+ }
+ return true;
+}
+
+void ILRender::lock(scene::ISceneNode *pNode) {
+ int ret = pthread_mutex_lock(&(m_nodes[pNode]->lock));
+ if(ret != 0) {
+ //TODO: throw exception here
+ }
+}
+
+void ILRender::unlock(scene::ISceneNode *pNode) {
+ int ret = pthread_mutex_unlock(&(m_nodes[pNode]->lock));
+ if(ret != 0) {
+ //TODO: throw exception here
+ }
+}
+
+// Render Loop
+void ILRender::renderLoop() {
+
+ std::cerr<<"RENDERLOOP!"<<std::endl;
+
+ lock();
+ m_pDevice = createDevice(video::EDT_OPENGL, core::dimension2d<s32>(640, 480));
+
+ if (m_pDevice == 0) {
+ // TODO: REPLACE THIS WITH AN EXCEPTION
+ exit(-1);
+ }
+
+ m_pDriver = m_pDevice->getVideoDriver();
+ m_pManager = m_pDevice->getSceneManager();
+#if 1
+ m_pManager->addCameraSceneNodeMaya(NULL,-1500.0f,200.0f,1500.0f);
+#else
+ SKeyMap keyMap[8];
+ keyMap[0].Action = EKA_MOVE_FORWARD;
+ keyMap[0].Key...
[truncated message content] |
|
From: <ge...@us...> - 2008-05-30 21:33:39
|
Revision: 575
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=575&view=rev
Author: gerkey
Date: 2008-05-30 14:33:41 -0700 (Fri, 30 May 2008)
Log Message:
-----------
added extrapolate exception
Modified Paths:
--------------
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h
pkg/trunk/util/transforms/libTF/include/libTF/libTF.h
pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp
pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp
pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h
pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-30 21:27:11 UTC (rev 574)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-05-30 21:33:41 UTC (rev 575)
@@ -201,7 +201,7 @@
// Map update paramters (for adding obstacles)
double laser_maxrange;
ros::Duration laser_buffer_time;
- //std::list<MsgLaserScan*> buffered_laser_scans;
+ std::list<MsgLaserScan*> buffered_laser_scans;
std::list<laser_pts_t> laser_scans;
double* laser_hitpts;
size_t laser_hitpts_len, laser_hitpts_size;
@@ -298,7 +298,7 @@
avmax(DTOR(80.0)),
amin(DTOR(10.0)),
amax(DTOR(40.0)),
- tf(*this, true)
+ tf(*this, true, false)
{
// TODO: get map via RPC
char* mapdata;
@@ -386,54 +386,81 @@
void
WavefrontNode::laserReceived()
{
- this->lock.lock();
+ // Copy and push this scan into the list of buffered scans
+ MsgLaserScan* newscan = new MsgLaserScan(laserMsg);
+ assert(newscan);
+ // Do a deep copy of the range data
+ newscan->set_ranges_size(laserMsg.get_ranges_size());
+ memcpy(newscan->ranges,laserMsg.ranges,laserMsg.get_ranges_size()*sizeof(float));
+ newscan->set_intensities_size(0);
+ this->buffered_laser_scans.push_back(newscan);
- // For each beam, convert to cartesian in the laser's frame, then convert
- // to the map frame and store the result in the the laser_scans list
+ // Iterate through the buffered scans, trying to interpolate each one
+ for(std::list<MsgLaserScan*>::iterator it = this->buffered_laser_scans.begin();
+ it != this->buffered_laser_scans.end();
+ it++)
+ {
+ // For each beam, convert to cartesian in the laser's frame, then convert
+ // to the map frame and store the result in the the laser_scans list
- laser_pts_t pts;
- pts.pts_num = laserMsg.get_ranges_size();
- pts.pts = new double[pts.pts_num*2];
- assert(pts.pts);
- pts.ts = laserMsg.header.stamp;
+ laser_pts_t pts;
+ pts.pts_num = (*it)->get_ranges_size();
+ pts.pts = new double[pts.pts_num*2];
+ assert(pts.pts);
+ pts.ts = (*it)->header.stamp;
- libTF::TFPose2D local,global;
- //local.frame = FRAMEID_LASER;
- local.frame = FRAMEID_ROBOT;
- local.time = laserMsg.header.stamp.sec * 1000000000ULL +
- laserMsg.header.stamp.nsec;
- float b=laserMsg.angle_min;
- float* r=laserMsg.ranges;
- for(unsigned int i=0;i<laserMsg.get_ranges_size();
- i++,r++,b+=laserMsg.angle_increment)
- {
- // TODO: take out the bogus epsilon range_max check, after the
- // hokuyourg_player node is fixed
- if(((*r) >= this->laser_maxrange) ||
- ((laserMsg.range_max > 0.1) && ((*r) >= laserMsg.range_max)) ||
- ((*r) <= 0.01))
- continue;
+ libTF::TFPose2D local,global;
+ local.frame = FRAMEID_LASER;
+ //local.frame = FRAMEID_ROBOT;
+ local.time = (*it)->header.stamp.sec * 1000000000ULL +
+ (*it)->header.stamp.nsec;
+ float b=(*it)->angle_min;
+ float* r=(*it)->ranges;
+ unsigned int i;
+ for(i=0;i<(*it)->get_ranges_size();
+ i++,r++,b+=(*it)->angle_increment)
+ {
+ // TODO: take out the bogus epsilon range_max check, after the
+ // hokuyourg_player node is fixed
+ if(((*r) >= this->laser_maxrange) ||
+ (((*it)->range_max > 0.1) && ((*r) >= (*it)->range_max)) ||
+ ((*r) <= 0.01))
+ continue;
- local.x = (*r)*cos(b);
- local.y = (*r)*sin(b);
- local.yaw = 0;
- try
- {
- global = this->tf.transformPose2D(FRAMEID_MAP, local);
+ local.x = (*r)*cos(b);
+ local.y = (*r)*sin(b);
+ local.yaw = 0;
+ try
+ {
+ global = this->tf.transformPose2D(FRAMEID_MAP, local);
+ }
+ catch(libTF::TransformReference::LookupException& ex)
+ {
+ puts("no global->local Tx yet");
+ delete[] pts.pts;
+ return;
+ }
+ catch(libTF::Quaternion3D::ExtrapolateException& ex)
+ {
+ //puts("extrapolation required");
+ delete[] pts.pts;
+ break;
+ }
+
+ // Copy in the result
+ pts.pts[2*i] = global.x;
+ pts.pts[2*i+1] = global.y;
}
- catch(libTF::TransformReference::LookupException& ex)
+ // Did we break early?
+ if(i < (*it)->get_ranges_size())
+ continue;
+ else
{
- puts("no global->local Tx yet");
- delete[] pts.pts;
- this->lock.unlock();
- return;
+ this->laser_scans.push_back(pts);
+ it = this->buffered_laser_scans.erase(it);
+ it--;
}
-
- // Copy in the result
- pts.pts[2*i] = global.x;
- pts.pts[2*i+1] = global.y;
}
- this->laser_scans.push_back(pts);
// Remove anything that's too old from the laser_scans list
// Also count how many points we have
@@ -452,6 +479,9 @@
hitpt_cnt += it->pts_num;
}
+ // Lock here because we're operating on the laser_hitpts array, which is
+ // used in another thread.
+ this->lock.lock();
// allocate more space as necessary
if(this->laser_hitpts_size < hitpt_cnt)
{
@@ -474,6 +504,7 @@
it->pts, it->pts_num * 2 * sizeof(double));
this->laser_hitpts_len += it->pts_num;
}
+ this->lock.unlock();
// Draw the points
@@ -488,8 +519,6 @@
this->pointcloudMsg.points[i].y = this->laser_hitpts[2*i+1];
}
publish("gui_laser",this->pointcloudMsg);
-
- this->lock.unlock();
}
void
@@ -547,6 +576,10 @@
this->stopRobot();
return;
}
+ catch(libTF::Quaternion3D::ExtrapolateException& ex)
+ {
+ // no big deal.
+ }
this->lock.lock();
switch(this->planner_state)
Modified: pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h
===================================================================
--- pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h 2008-05-30 21:27:11 UTC (rev 574)
+++ pkg/trunk/util/transforms/libTF/include/libTF/Quaternion3D.h 2008-05-30 21:33:41 UTC (rev 575)
@@ -43,7 +43,6 @@
namespace libTF{
-
struct PoseYPR
{
double x,y,z,yaw, pitch, roll;
@@ -150,10 +149,21 @@
Quaternion3DStorage & operator=(const Quaternion3DStorage & input);
unsigned long long time; //nanoseconds since 1970
};
+
+ /** \brief An exception class to notify that the requested value would have required extrapolation, and extrapolation is not allowed.
+ *
+ */
+ class ExtrapolateException : public std::exception
+ {
+ public:
+ virtual const char* what() const throw() { return "Disallowed extrapolation required."; }
+ };
/** Constructors **/
// Standard constructor max_cache_time is how long to cache transform data
- Quaternion3D(bool caching = true, unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME);
+ Quaternion3D(bool caching = true,
+ unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME,
+ bool extrapolate = true);
~Quaternion3D();
/** Mutators **/
@@ -198,6 +208,7 @@
void add_value(const Quaternion3DStorage&);//todo fixme finish implementing this
+ ExtrapolateException NoExtrapolation;
// insert a node into the sorted linked list
void insertNode(const Quaternion3DStorage & );
@@ -220,6 +231,8 @@
unsigned long long max_storage_time;
//Max length of linked list
unsigned long long max_length_linked_list;
+ //Whether to allow extrapolation
+ bool extrapolate;
//A mutex to prevent linked list collisions
pthread_mutex_t linked_list_mutex;
Modified: pkg/trunk/util/transforms/libTF/include/libTF/libTF.h
===================================================================
--- pkg/trunk/util/transforms/libTF/include/libTF/libTF.h 2008-05-30 21:27:11 UTC (rev 574)
+++ pkg/trunk/util/transforms/libTF/include/libTF/libTF.h 2008-05-30 21:33:41 UTC (rev 575)
@@ -202,7 +202,9 @@
/** Constructor
* \param How long to keep a history of transforms in nanoseconds
*/
- TransformReference(bool caching = true, ULLtime cache_time = DEFAULT_CACHE_TIME);
+ TransformReference(bool caching = true,
+ ULLtime cache_time = DEFAULT_CACHE_TIME,
+ bool extrapolate = true);
~TransformReference();
/********** Mutators **************/
@@ -264,7 +266,6 @@
*/
static ULLtime gettime(void);
-
/************ Possible Exceptions ****************************/
/** \brief An exception class to notify of bad frame number
@@ -307,6 +308,7 @@
private:
} MaxSearchDepth;
+
private:
/** RefFrame *******
@@ -323,7 +325,9 @@
public:
/** Constructor */
- RefFrame(bool caching = true, unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME);
+ RefFrame(bool caching = true,
+ unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME,
+ bool extrapolate = true);
/** \brief Get the parent nodeID */
inline unsigned int getParent(){return parent;};
@@ -339,7 +343,7 @@
/** Internal storage of the parent */
unsigned int parent;
-
+
};
/******************** Internal Storage ****************/
@@ -353,6 +357,9 @@
/// whether or not to cache
bool caching;
+
+ /// whether or not to allow extrapolation
+ bool extrapolate;
public:
/** \brief An internal representation of transform chains
Modified: pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp
===================================================================
--- pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp 2008-05-30 21:27:11 UTC (rev 574)
+++ pkg/trunk/util/transforms/libTF/src/simple/Quaternion3D.cpp 2008-05-30 21:33:41 UTC (rev 575)
@@ -45,9 +45,12 @@
};
-Quaternion3D::Quaternion3D(bool caching, unsigned long long max_cache_time):
+Quaternion3D::Quaternion3D(bool caching,
+ unsigned long long max_cache_time,
+ bool _extrapolate):
max_storage_time(max_cache_time),
max_length_linked_list(MAX_LENGTH_LINKED_LIST),
+ extrapolate(_extrapolate),
first(NULL),
last(NULL),
list_length(0)
@@ -677,6 +680,17 @@
else
{
//Two or more elements
+
+ // Are we allowed to extrapolate?
+ if(!extrapolate)
+ {
+ if(target_time > first->data.time)
+ {
+ pthread_mutex_unlock(&linked_list_mutex);
+ throw NoExtrapolation;
+ }
+ }
+
//Find the one that just exceeds the time or hits the end
//and then take the previous one
p_current = first->next; //Start on the 2nd element so if we fail we fall back to the first one
Modified: pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp
===================================================================
--- pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp 2008-05-30 21:27:11 UTC (rev 574)
+++ pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp 2008-05-30 21:33:41 UTC (rev 575)
@@ -35,17 +35,22 @@
using namespace libTF;
-TransformReference::RefFrame::RefFrame(bool caching, unsigned long long max_cache_time) :
- Quaternion3D(caching, max_cache_time),
+TransformReference::RefFrame::RefFrame(bool caching,
+ unsigned long long max_cache_time,
+ bool extrapolate) :
+ Quaternion3D(caching, max_cache_time, extrapolate),
parent(TransformReference::NO_PARENT)
{
return;
}
-TransformReference::TransformReference(bool caching, ULLtime cache_time):
+TransformReference::TransformReference(bool caching,
+ ULLtime cache_time,
+ bool extrapolate):
cache_time(cache_time),
- caching (caching)
+ caching (caching),
+ extrapolate(extrapolate)
{
frames = new RefFrame*[MAX_NUM_FRAMES];
@@ -78,7 +83,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching);
+ frames[frameID] = new RefFrame(caching, cache_time, extrapolate);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromEuler(a,b,c,d,e,f,time);
@@ -90,7 +95,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching);
+ frames[frameID] = new RefFrame(caching, cache_time, extrapolate);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromDH(a,b,c,d,time);
@@ -104,7 +109,7 @@
//TODO check and throw exception if matrix wrong size
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching);
+ frames[frameID] = new RefFrame(caching, cache_time, extrapolate);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromMatrix(matrix_in,time);
@@ -117,7 +122,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching);
+ frames[frameID] = new RefFrame(caching, cache_time, extrapolate);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromQuaternion(xt, yt, zt, xr, yr, zr, w,time);
Modified: pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h
===================================================================
--- pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h 2008-05-30 21:27:11 UTC (rev 574)
+++ pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h 2008-05-30 21:33:41 UTC (rev 575)
@@ -61,7 +61,7 @@
{
public:
//Constructor
- rosTFClient(ros::node & rosnode, bool caching = true);
+ rosTFClient(ros::node & rosnode, bool caching = true, bool extrapolate = true);
//Call back functions
void receiveEuler();
Modified: pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp
===================================================================
--- pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp 2008-05-30 21:27:11 UTC (rev 574)
+++ pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp 2008-05-30 21:33:41 UTC (rev 575)
@@ -32,8 +32,12 @@
#include "rosTF/rosTF.h"
-rosTFClient::rosTFClient(ros::node & rosnode, bool caching):
- TransformReference(caching),
+rosTFClient::rosTFClient(ros::node & rosnode,
+ bool caching,
+ bool extrapolate):
+ TransformReference(caching,
+ TransformReference::DEFAULT_CACHE_TIME,
+ extrapolate),
myNode(rosnode)
{
myNode.subscribe("TransformEuler", eulerIn, &rosTFClient::receiveEuler, this,100);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-06-03 19:48:21
|
Revision: 608
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=608&view=rev
Author: hsujohnhsu
Date: 2008-06-03 12:48:17 -0700 (Tue, 03 Jun 2008)
Log Message:
-----------
fix rosmake settings for rosgazebo.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/manifest.xml
pkg/trunk/simulators/rosgazebo/Makefile
pkg/trunk/simulators/rosgazebo/manifest.xml
Modified: pkg/trunk/3rdparty/gazebo/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/gazebo/manifest.xml 2008-06-03 18:56:39 UTC (rev 607)
+++ pkg/trunk/3rdparty/gazebo/manifest.xml 2008-06-03 19:48:17 UTC (rev 608)
@@ -12,7 +12,7 @@
<license>GPL</license>
<url>http://playerstage.sf.net</url>
<export>
- <!--<cpp lflags="-Xlinker -rpath ${prefix}/player/lib -L${prefix}/player/lib -lplayercore -lplayerxdr -lplayerutils -lplayererror" cflags="-I${prefix}/player/include/player-2.2"/> -->
+ <cpp lflags="-Wl,-rpath,${prefix}/gazebo-git/root/lib `PKG_CONFIG_PATH=${prefix}/gazebo-git/root/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --libs libgazebo` `PKG_CONFIG_PATH=${prefix}/gazebo-git/root/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --libs libgazeboServer` `PKG_CONFIG_PATH=${prefix}/gazebo-git/root/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --libs libpr2API`" cflags="`PKG_CONFIG_PATH=${prefix}/gazebo-git/root/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --cflags libgazebo` `PKG_CONFIG_PATH=${prefix}/gazebo-git/root/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --cflags libgazeboServer` `PKG_CONFIG_PATH=${prefix}/gazebo-git/root/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --cflags libpr2API`"/>
<doxymaker external="http://playerstage.sourceforge.net/doc/Gazebo-manual-svn-html/"/>
</export>
</package>
Modified: pkg/trunk/simulators/rosgazebo/Makefile
===================================================================
--- pkg/trunk/simulators/rosgazebo/Makefile 2008-06-03 18:56:39 UTC (rev 607)
+++ pkg/trunk/simulators/rosgazebo/Makefile 2008-06-03 19:48:17 UTC (rev 608)
@@ -4,9 +4,11 @@
CXX = g++
all: $(PKG)
-CFLAGS = -g -Wall $(shell rospack export/cpp/cflags $(PKG)) $(shell pkg-config --cflags libpr2API) $(shell pkg-config --cflags libgazebo) $(shell pkg-config --cflags libgazeboServer)
-LFLAGS = $(shell rospack export/cpp/lflags $(PKG)) $(shell pkg-config --libs libpr2API) $(shell pkg-config --libs libgazebo) $(shell pkg-config --libs libgazeboServer)
+CFLAGS = -g -Wall $(shell rospack export/cpp/cflags $(PKG))
+
+LFLAGS = $(shell rospack export/cpp/lflags $(PKG))
+
rosgazebo: rosgazebo.cc
$(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
Modified: pkg/trunk/simulators/rosgazebo/manifest.xml
===================================================================
--- pkg/trunk/simulators/rosgazebo/manifest.xml 2008-06-03 18:56:39 UTC (rev 607)
+++ pkg/trunk/simulators/rosgazebo/manifest.xml 2008-06-03 19:48:17 UTC (rev 608)
@@ -3,6 +3,7 @@
<author>John M. Hsu</author>
<license>TBD</license>
<depend package="roscpp" />
+ <depend package="gazebo" />
<depend package="std_msgs" />
<depend package="rosthread" />
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-06-04 01:14:18
|
Revision: 614
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=614&view=rev
Author: jleibs
Date: 2008-06-03 18:14:22 -0700 (Tue, 03 Jun 2008)
Log Message:
-----------
Changes to tilting laser for pseudo-image generation and flea2 for running at slower mode by default.
Modified Paths:
--------------
pkg/trunk/drivers/cam/ptgrey/flea2/src/flea2_node/flea2_node.cpp
pkg/trunk/drivers/cam/ptgrey/flea2/src/libflea2/flea2.cpp
pkg/trunk/drivers/motor/etherdrive/manifest.xml
pkg/trunk/drivers/motor/etherdrive/src/etherdrive_node/etherdrive_node.cpp
pkg/trunk/drivers/robot/pr2/tilting_laser/params.xml
pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp
Removed Paths:
-------------
pkg/trunk/unstable_msgs/msg/Actuator.msg
Modified: pkg/trunk/drivers/cam/ptgrey/flea2/src/flea2_node/flea2_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/ptgrey/flea2/src/flea2_node/flea2_node.cpp 2008-06-04 01:13:43 UTC (rev 613)
+++ pkg/trunk/drivers/cam/ptgrey/flea2/src/flea2_node/flea2_node.cpp 2008-06-04 01:14:22 UTC (rev 614)
@@ -38,6 +38,10 @@
MsgImage img;
Flea2 flea2;
+ ros::Time next_time;
+
+ int count;
+
Flea2_Node() : ros::node("flea2")
{
advertise<MsgImage>("image");
@@ -45,7 +49,39 @@
flea2.set_shutter(0.8);
flea2.set_gamma(0.24);
flea2.set_gain(0);
+
+ next_time = ros::Time::now();
+ count = 0;
}
+
+ bool get_and_send_raw()
+ {
+ uint8_t *buf;
+ uint32_t buf_size;
+ uint32_t width;
+ uint32_t height;
+
+ flea2.get_frame(&buf, &width, &height);
+
+ buf_size = width*height;
+ img.width = width;
+ img.height = height;
+ img.compression = "raw";
+ img.colorspace = "mono8";
+ img.set_data_size(buf_size);
+ memcpy(img.data, buf, buf_size);
+
+ count++;
+ ros::Time now_time = ros::Time::now();
+ if (now_time > next_time) {
+ std::cout << count << " imgs/sec at " << now_time << std::endl;
+ count = 0;
+ next_time = next_time + ros::Duration(1,0);
+ }
+
+ publish("image", img);
+ return true;
+ }
bool get_and_send_jpeg()
{
@@ -60,8 +96,15 @@
img.set_data_size(buf_size);
memcpy(img.data, buf, buf_size);
- printf("Got %d bytes\n", buf_size);
+ count++;
+ ros::Time now_time = ros::Time::now();
+ if (now_time > next_time) {
+ std::cout << count << " imgs/sec at " << now_time << std::endl;
+ count = 0;
+ next_time = next_time + ros::Duration(1,0);
+ }
+
publish("image", img);
return true;
}
@@ -75,6 +118,7 @@
while (fn.ok()) {
fn.get_and_send_jpeg();
+ //fn.get_and_send_raw();
}
ros::fini();
Modified: pkg/trunk/drivers/cam/ptgrey/flea2/src/libflea2/flea2.cpp
===================================================================
--- pkg/trunk/drivers/cam/ptgrey/flea2/src/libflea2/flea2.cpp 2008-06-04 01:13:43 UTC (rev 613)
+++ pkg/trunk/drivers/cam/ptgrey/flea2/src/libflea2/flea2.cpp 2008-06-04 01:14:22 UTC (rev 614)
@@ -64,11 +64,12 @@
int e;
if (video_mode == FLEA2_MONO)
e = dc1394_dma_setup_capture(raw, cam_node, channel,
- FORMAT_VGA_NONCOMPRESSED, MODE_640x480_MONO, SPEED_800,
+ FORMAT_VGA_NONCOMPRESSED, MODE_640x480_MONO, SPEED_400,
+ // FORMAT_SVGA_NONCOMPRESSED_2, MODE_1280x960_MONO, SPEED_400,
FRAMERATE_30, 8, 1, host_dev, &cam);
else
e = dc1394_dma_setup_capture(raw, cam_node, channel,
- FORMAT_VGA_NONCOMPRESSED, MODE_640x480_RGB, SPEED_800,
+ FORMAT_VGA_NONCOMPRESSED, MODE_640x480_RGB, SPEED_400,
FRAMERATE_30, 8, 1, host_dev, &cam);
if ( e != DC1394_SUCCESS )
{
Modified: pkg/trunk/drivers/motor/etherdrive/manifest.xml
===================================================================
--- pkg/trunk/drivers/motor/etherdrive/manifest.xml 2008-06-04 01:13:43 UTC (rev 613)
+++ pkg/trunk/drivers/motor/etherdrive/manifest.xml 2008-06-04 01:14:22 UTC (rev 614)
@@ -11,7 +11,6 @@
<url>http://pr.willowgarage.com</url>
<depend package='roscpp'/>
<depend package='std_msgs'/>
-<depend package='unstable_msgs'/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -letherdrive"/>
</export>
Modified: pkg/trunk/drivers/motor/etherdrive/src/etherdrive_node/etherdrive_node.cpp
===================================================================
--- pkg/trunk/drivers/motor/etherdrive/src/etherdrive_node/etherdrive_node.cpp 2008-06-04 01:13:43 UTC (rev 613)
+++ pkg/trunk/drivers/motor/etherdrive/src/etherdrive_node/etherdrive_node.cpp 2008-06-04 01:14:22 UTC (rev 614)
@@ -28,7 +28,7 @@
// POSSIBILITY OF SUCH DAMAGE.
#include "ros/node.h"
-#include "unstable_msgs/MsgActuator.h"
+#include "std_msgs/MsgActuator.h"
#include "etherdrive/etherdrive.h"
#include <sstream>
@@ -138,7 +138,6 @@
ros::init(argc, argv);
EtherDrive_Node a;
while (a.ok()) {
-
usleep(1000);
if (!a.do_tick())
{
Modified: pkg/trunk/drivers/robot/pr2/tilting_laser/params.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/tilting_laser/params.xml 2008-06-04 01:13:43 UTC (rev 613)
+++ pkg/trunk/drivers/robot/pr2/tilting_laser/params.xml 2008-06-04 01:14:22 UTC (rev 614)
@@ -4,7 +4,7 @@
<int name="HK/cluster">1</int>
<int name="HK/skip">0</int>
<double name="ED/pulse_per_rad0">9549.29659</double>
- <double name="TL/period">20.0</double>
+ <int name="TL/num_scans">400</int>
<double name="TL/min_ang">-1.1</double>
<double name="TL/max_ang">0.645</double>
</params>
Modified: pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp 2008-06-04 01:13:43 UTC (rev 613)
+++ pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp 2008-06-04 01:14:22 UTC (rev 614)
@@ -37,9 +37,8 @@
#include "std_msgs/MsgLaserScan.h"
#include "std_msgs/MsgPointCloudFloat32.h"
#include "std_msgs/MsgImage.h"
-#include "unstable_msgs/MsgActuator.h"
+#include "std_msgs/MsgActuator.h"
#include "libTF/libTF.h"
-#include "rostime/clock.h"
#include "math.h"
class Tilting_Laser : public ros::node
@@ -54,21 +53,25 @@
MsgActuator encoder;
MsgImage image;
- double starttime;
+ int num_scans;
- double next_shutter;
- double period;
-
double min_ang;
double max_ang;
libTF::TransformReference TR;
- ros::time::clock clock;
+ bool img_ready;
+ int img_ind;
+ int img_dir;
- int lines;
+ double last_ang;
+ double rate_err_up;
+ double rate_err_down;
+ double accum_angle;
- Tilting_Laser() : ros::node("tilting_laser"), next_shutter(0.0), lines(0)
+ ros::Time last_motor_time;
+
+ Tilting_Laser() : ros::node("tilting_laser"), img_ready(false), img_ind(-1), last_ang(1000000), img_dir(1), rate_err_down(0.0), rate_err_up(0.0), accum_angle(0)
{
advertise<MsgPointCloudFloat32>("cloud");
@@ -76,14 +79,15 @@
advertise<MsgActuator>("mot_cmd");
advertise<MsgImage>("image");
- subscribe("scan", scans, &Tilting_Laser::scans_callback);
- subscribe("mot", encoder, &Tilting_Laser::encoder_callback);
+ subscribe("scan", scans, &Tilting_Laser::scans_callback,10);
+ subscribe("mot", encoder, &Tilting_Laser::encoder_callback,10);
- param("period", period, 5.0);
+ param("num_scans", num_scans, 400);
param("min_ang", min_ang, -1.3);
param("max_ang", max_ang, 0.6);
unsigned long long time = clock.ulltime();
+
TR.setWithEulers(3, 2,
0, 0, 0,
0, 0, 0,
@@ -94,16 +98,6 @@
0, 0, 0,
time);
-
-
- image.set_data_size(140*4*400*2);
- image.height = 400;
- image.width = 140*4;
-
- image.colorspace = "mono16";
- image.compression = "raw";
-
- starttime = clock.time();
}
virtual ~Tilting_Laser()
@@ -112,21 +106,25 @@
}
void scans_callback() {
- encoder.lock();
+
+ //TODO: Add check for hokuyo changing width!
+ if (!img_ready) {
+ image.height = num_scans;
+ image.width = scans.get_ranges_size();
+ image.set_data_size(2*image.height*image.width);
+
+ image.colorspace = "mono16";
+ image.compression = "raw";
+
+ img_ready = true;
+ }
+
cloud.set_pts_size(scans.get_ranges_size());
cloud.set_chan_size(1);
cloud.chan[0].name = "intensities";
cloud.chan[0].set_vals_size(scans.get_ranges_size());
- /*
- for (int i = 0; i < scans.get_ranges_size(); i++) {
- cloud.x[i] = cos(encoder.val)*cos(scans.angle_min + i*scans.angle_increment) * scans.ranges[i];
- cloud.y[i] = sin(scans.angle_min + i*scans.angle_increment) * scans.ranges[i];
- cloud.z[i] = sin(encoder.val)*cos(scans.angle_min + i*scans.angle_increment) * scans.ranges[i];
- }
- */
-
NEWMAT::Matrix points(4,scans.get_ranges_size());
for (uint32_t i = 0; i < scans.get_ranges_size(); i++) {
if (scans.ranges[i] < 20.0) {
@@ -141,66 +139,121 @@
points(4,i+1) = 1.0;
}
}
+
+ unsigned long long t = scans.header.stamp.to_ull() - 30000000;
- NEWMAT::Matrix rot_points = TR.getMatrix(1,3,clock.ulltime()) * points;
+ NEWMAT::Matrix rot_points = TR.getMatrix(1,3,t) * points;
+
+ libTF::TFVector u;
+ u.x = 1;
+ u.y = 0;
+ u.z = 0;
+ u.time = t;
+ u.frame = 3;
+
+ libTF::TFVector v = TR.transformVector(1,u);
+
+ double ang = atan2(v.z,v.x);
+
+ if (ang < max_ang && last_ang > max_ang)
+ {
+ img_dir = 1;
+ img_ind = 0;
+ }
+
+ if (ang > min_ang && last_ang < min_ang)
+ {
+ img_dir = -1;
+ img_ind = 0;
+ }
+
+ last_ang = ang;
+
for (uint32_t i = 0; i < scans.get_ranges_size(); i++) {
cloud.pts[i].x = rot_points(1,i+1);
cloud.pts[i].y = rot_points(2,i+1);
cloud.pts[i].z = rot_points(3,i+1);
cloud.chan[0].vals[i] = scans.intensities[i];
- if (lines < 400 && i < 4*140)
- {
- if (fmod(next_shutter, 1.0) > 0.49)
- {
- *(unsigned short*)(&(image.data[lines * 4*140 * 2 + 4*140*2 - 2 - 2*i])) = scans.intensities[i];
- } else {
- *(unsigned short*)(&(image.data[(400 - 1 - lines) * 4*140 * 2 + 4*140*2 - 2 - 2*i])) = scans.intensities[i];
- }
- }
+ if (img_ind >= 0)
+ {
+ if (img_dir == 1)
+ *(unsigned short*)(&(image.data[img_ind * 2 * image.width + image.width * 2 - 2 - 2*i])) = scans.intensities[i];
+ else
+ *(unsigned short*)(&(image.data[(num_scans - img_ind - 1) * 2 * image.width + image.width * 2 - 2 - 2*i])) = scans.intensities[i];
+ }
}
- encoder.unlock();
+ if (img_ind >= 0)
+ img_ind++;
- lines++;
publish("cloud", cloud);
+
+ if (img_ind == num_scans) {
+ printf("Sending image and shutter\n");
+
+ double ang_err;
+
+ if (img_dir == 1)
+ {
+ ang_err = min_ang - ang;
+ rate_err_down -= ang_err / (0.025 * num_scans);
+ }
+ else
+ {
+ ang_err = max_ang - ang;
+ rate_err_up += ang_err / (0.025 * num_scans);
+ }
+
+ printf("Final scan off by: %g\n", ang_err);
+
+ publish("image", image);
+ publish("shutter",shutter);
+ img_ind = -1;
+ }
}
void encoder_callback() {
- unsigned long long time = clock.ulltime();
TR.setWithEulers(3, 2,
.02, 0, .02,
0.0, 0.0, 0.0,
- time);
+ encoder.header.stamp.to_ull());
TR.setWithEulers(2, 1,
0, 0, 0,
0, -encoder.val, 0,
- time);
+ encoder.header.stamp.to_ull());
motor_control(); // Control on encoder reads sounds reasonable
}
+
void motor_control() {
+ cmd.rel = false;
+ cmd.valid = true;
- double nowtime = clock.time();
+ ros::Time t = ros::Time::now();
+
+ //This will not go at right rate if hokuyo is skipping scans
+ double rate = (max_ang - min_ang) / (0.025 * num_scans);
+ if (img_dir == 1)
+ rate += rate_err_down;
+ else
+ rate += rate_err_up;
- double elapsed_cycles = (nowtime - starttime) / (period);
- double index = fabs(fmod(elapsed_cycles, 1) * 2.0 - 1.0);
+ double ext_max_ang = max_ang + .025;
+ double ext_min_ang = min_ang - .025;
- cmd.val = index * (max_ang - min_ang) + min_ang;
- cmd.rel = false;
- cmd.valid = true;
+ accum_angle += (t - last_motor_time).to_double() * rate;
- if (elapsed_cycles > next_shutter) {
- next_shutter += 0.5;
- printf("Scanned %d lines\n", lines);
- lines = 0;
- publish("shutter", shutter);
- publish("image", image);
- }
+ double index = fabs(fmod( (accum_angle) / (2.0 * (ext_max_ang - ext_min_ang)), 1) * 2.0 - 1.0);
+
+ cmd.val = index * (ext_max_ang - ext_min_ang) + ext_min_ang;
publish("mot_cmd",cmd);
+
+ last_motor_time = t;
+
}
};
@@ -209,9 +262,7 @@
{
ros::init(argc, argv);
Tilting_Laser tl;
- while (tl.ok()) {
- usleep(10000);
- }
+ tl.spin();
ros::fini();
return 0;
}
Deleted: pkg/trunk/unstable_msgs/msg/Actuator.msg
===================================================================
--- pkg/trunk/unstable_msgs/msg/Actuator.msg 2008-06-04 01:13:43 UTC (rev 613)
+++ pkg/trunk/unstable_msgs/msg/Actuator.msg 2008-06-04 01:14:22 UTC (rev 614)
@@ -1,3 +0,0 @@
-float32 val
-byte rel
-byte valid
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-06-04 22:54:12
|
Revision: 629
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=629&view=rev
Author: tfoote
Date: 2008-06-04 15:54:17 -0700 (Wed, 04 Jun 2008)
Log Message:
-----------
added extrapolation limits. for ticket:86
Modified Paths:
--------------
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/util/transforms/libTF/include/libTF/libTF.h
pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp
pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h
pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-06-04 22:36:33 UTC (rev 628)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-06-04 22:54:17 UTC (rev 629)
@@ -298,7 +298,7 @@
avmax(DTOR(80.0)),
amin(DTOR(10.0)),
amax(DTOR(40.0)),
- tf(*this, true, true)
+ tf(*this, true, 200000000ULL) //nanoseconds
{
// TODO: get map via RPC
char* mapdata;
Modified: pkg/trunk/util/transforms/libTF/include/libTF/libTF.h
===================================================================
--- pkg/trunk/util/transforms/libTF/include/libTF/libTF.h 2008-06-04 22:36:33 UTC (rev 628)
+++ pkg/trunk/util/transforms/libTF/include/libTF/libTF.h 2008-06-04 22:54:17 UTC (rev 629)
@@ -197,6 +197,7 @@
/** The default length of time to cache information
* 10 seconds in nanoseconds */
static const ULLtime DEFAULT_CACHE_TIME = 10 * 1000000000ULL;
+ static const ULLtime DEFAULT_MAX_EXTRAPOLATION_DISTANCE = 10 * 1000000000ULL;
/** Constructor
@@ -204,7 +205,7 @@
*/
TransformReference(bool caching = true,
ULLtime cache_time = DEFAULT_CACHE_TIME,
- bool extrapolate = true);
+ unsigned long long max_extrapolation_distance = DEFAULT_MAX_EXTRAPOLATION_DISTANCE);
~TransformReference();
/********** Mutators **************/
@@ -320,7 +321,7 @@
/** Constructor */
RefFrame(bool caching = true,
unsigned long long max_cache_time = DEFAULT_MAX_STORAGE_TIME,
- bool extrapolate = true);
+ unsigned long long max_extrapolation_time = DEFAULT_MAX_EXTRAPOLATION_TIME);
/** \brief Get the parent nodeID */
inline unsigned int getParent(){return parent;};
@@ -352,7 +353,7 @@
bool caching;
/// whether or not to allow extrapolation
- bool extrapolate;
+ unsigned long long max_extrapolation_distance;
public:
/** \brief An internal representation of transform chains
Modified: pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp
===================================================================
--- pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp 2008-06-04 22:36:33 UTC (rev 628)
+++ pkg/trunk/util/transforms/libTF/src/simple/libTF.cpp 2008-06-04 22:54:17 UTC (rev 629)
@@ -37,8 +37,8 @@
TransformReference::RefFrame::RefFrame(bool caching,
unsigned long long max_cache_time,
- bool extrapolate) :
- Quaternion3D(caching, max_cache_time, extrapolate),
+ unsigned long long max_extrapolation_distance) :
+ Quaternion3D(caching, max_cache_time, max_extrapolation_distance),
parent(TransformReference::NO_PARENT)
{
return;
@@ -47,10 +47,10 @@
TransformReference::TransformReference(bool caching,
ULLtime cache_time,
- bool extrapolate):
+ unsigned long long max_extrapolation_distance):
cache_time(cache_time),
caching (caching),
- extrapolate(extrapolate)
+ max_extrapolation_distance(max_extrapolation_distance)
{
frames = new RefFrame*[MAX_NUM_FRAMES];
@@ -83,7 +83,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching, cache_time, extrapolate);
+ frames[frameID] = new RefFrame(caching, cache_time, max_extrapolation_distance);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromEuler(a,b,c,d,e,f,time);
@@ -95,7 +95,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching, cache_time, extrapolate);
+ frames[frameID] = new RefFrame(caching, cache_time, max_extrapolation_distance);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromDH(a,b,c,d,time);
@@ -109,7 +109,7 @@
//TODO check and throw exception if matrix wrong size
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching, cache_time, extrapolate);
+ frames[frameID] = new RefFrame(caching, cache_time, max_extrapolation_distance);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromMatrix(matrix_in,time);
@@ -122,7 +122,7 @@
throw InvalidFrame;
if (frames[frameID] == NULL)
- frames[frameID] = new RefFrame(caching, cache_time, extrapolate);
+ frames[frameID] = new RefFrame(caching, cache_time, max_extrapolation_distance);
getFrame(frameID)->setParent(parentID);
getFrame(frameID)->addFromQuaternion(xt, yt, zt, xr, yr, zr, w,time);
Modified: pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h
===================================================================
--- pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h 2008-06-04 22:36:33 UTC (rev 628)
+++ pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h 2008-06-04 22:54:17 UTC (rev 629)
@@ -62,7 +62,7 @@
{
public:
//Constructor
- rosTFClient(ros::node & rosnode, bool caching = true, bool extrapolate = true);
+ rosTFClient(ros::node & rosnode, bool caching = true, unsigned long long max_extrapolation_distance = libTF::TransformReference::DEFAULT_MAX_EXTRAPOLATION_DISTANCE);
// MsgPointCloudFloat32 transformPointCloud(unsigned int target_frame, const MsgPointCloudFloat32 & cloudIn); // todo switch after ticket:232
MsgPointCloudFloat32 transformPointCloud(unsigned int target_frame, MsgPointCloudFloat32 & cloudIn);
Modified: pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp
===================================================================
--- pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp 2008-06-04 22:36:33 UTC (rev 628)
+++ pkg/trunk/util/transforms/rosTF/src/rosTF/rosTF.cpp 2008-06-04 22:54:17 UTC (rev 629)
@@ -34,10 +34,10 @@
rosTFClient::rosTFClient(ros::node & rosnode,
bool caching,
- bool extrapolate):
+ unsigned long long max_extrapolation_distance):
TransformReference(caching,
TransformReference::DEFAULT_CACHE_TIME,
- extrapolate),
+ max_extrapolation_distance),
myNode(rosnode)
{
myNode.subscribe("TransformEuler", eulerIn, &rosTFClient::receiveEuler, this,100);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-06-05 22:57:58
|
Revision: 654
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=654&view=rev
Author: gerkey
Date: 2008-06-05 11:17:44 -0700 (Thu, 05 Jun 2008)
Log Message:
-----------
added demos directory
Added Paths:
-----------
pkg/trunk/demos/
pkg/trunk/demos/2dnav-stage/
pkg/trunk/demos/2dnav-stage/2dnav-stage.xml
pkg/trunk/demos/2dnav-stage/willow-erratic.world
pkg/trunk/demos/2dnav-stage/willow-full.png
pkg/trunk/demos/README
Added: pkg/trunk/demos/2dnav-stage/2dnav-stage.xml
===================================================================
--- pkg/trunk/demos/2dnav-stage/2dnav-stage.xml (rev 0)
+++ pkg/trunk/demos/2dnav-stage/2dnav-stage.xml 2008-06-05 18:17:44 UTC (rev 654)
@@ -0,0 +1,10 @@
+<launch>
+
+ <ns name="wg">
+ <node pkg="rosstage" type="rosstage" args="willow-erratic.world" respawn="true" />
+ <node pkg="amcl_player" type="amcl_player" args="willow-full.png 0.1" respawn="true" />
+ <node pkg="wavefront_player" type="wavefront_player" args="willow-full.png 0.1" respawn="true" />
+ <node pkg="nav_view" type="nav_view" args="willow-full.png 0.1" respawn="true" />
+ </ns>
+</launch>
+
Added: pkg/trunk/demos/2dnav-stage/willow-erratic.world
===================================================================
--- pkg/trunk/demos/2dnav-stage/willow-erratic.world (rev 0)
+++ pkg/trunk/demos/2dnav-stage/willow-erratic.world 2008-06-05 18:17:44 UTC (rev 654)
@@ -0,0 +1,70 @@
+define block model
+(
+ size3 [0.5 0.5 0.5]
+ gui_nose 0
+)
+
+define topurg laser
+(
+ range_min 0.0
+ range_max 30.0
+ fov 270.25
+ samples 1081
+ # generic model properties
+ color "black"
+ size [ 0.05 0.05 0.1 ]
+)
+
+define erratic position
+(
+ #size3 [0.415 0.392 0.25]
+ size3 [0.35 0.35 0.25]
+ origin3 [-0.05 0 0 0]
+ gui_nose 1
+ drive "diff"
+ topurg(pose [0.050 0.000 0.000])
+)
+
+define floorplan model
+(
+ # sombre, sensible, artistic
+ color "gray30"
+
+ # most maps will need a bounding box
+ boundary 1
+
+ gui_nose 0
+ gui_grid 0
+ gui_movemask 0
+ gui_outline 0
+ gripper_return 0
+ fiducial_return 0
+ laser_return 1
+)
+
+# set the resolution of the underlying raytrace model in meters
+resolution 0.02
+
+interval_sim 100 # simulation timestep in milliseconds
+interval_real 100 # real-time interval between simulation updates in milliseconds
+
+window
+(
+ size [ 745.000 448.000 ]
+ center [678.990 293.960]
+ rotate [ 0.000 -1.560 ]
+ scale 28.806
+)
+
+# load an environment bitmap
+floorplan
+(
+ name "willow"
+ bitmap "willow-full.png"
+ size3 [54.0 58.7 0.5]
+ pose [-29.350 27.000 90.000]
+)
+
+# throw in a robot
+erratic( pose [-11.277 23.266 180.000] name "era" color "blue")
+block( pose [-13.924 25.020 180.000] color "red")
Added: pkg/trunk/demos/2dnav-stage/willow-full.png
===================================================================
(Binary files differ)
Property changes on: pkg/trunk/demos/2dnav-stage/willow-full.png
___________________________________________________________________
Name: svn:mime-type
+ application/octet-stream
Added: pkg/trunk/demos/README
===================================================================
--- pkg/trunk/demos/README (rev 0)
+++ pkg/trunk/demos/README 2008-06-05 18:17:44 UTC (rev 654)
@@ -0,0 +1,7 @@
+This directory contains "demos," which are combinations of nodes that
+perform some useful or interesting function. A demo may be specified in a
+number of ways, including:
+ * .xml file that is meant to be passed to the roslaunch tool
+ * bash script that launches the nodes
+ * Text file that describes what to launch
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-06-06 00:28:26
|
Revision: 661
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=661&view=rev
Author: hsujohnhsu
Date: 2008-06-05 17:28:26 -0700 (Thu, 05 Jun 2008)
Log Message:
-----------
added pr2Core definitions. added kinematics and inverse kinematics libraries.
Modified Paths:
--------------
pkg/trunk/simulators/rosgazebo/world/pr2.model
pkg/trunk/simulators/rosgazebo/world/robot.world
Added Paths:
-----------
pkg/trunk/drivers/robot/pr2/pr2Core/
pkg/trunk/drivers/robot/pr2/pr2Core/Makefile
pkg/trunk/drivers/robot/pr2/pr2Core/include/
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/drivers/robot/pr2/pr2Core/manifest.xml
pkg/trunk/util/kinematics/
pkg/trunk/util/kinematics/libKinematics/
pkg/trunk/util/kinematics/libKinematics/Makefile
pkg/trunk/util/kinematics/libKinematics/doc/
pkg/trunk/util/kinematics/libKinematics/include/
pkg/trunk/util/kinematics/libKinematics/include/libKinematics/
pkg/trunk/util/kinematics/libKinematics/include/libKinematics/ik.h
pkg/trunk/util/kinematics/libKinematics/include/libKinematics/kinematics.h
pkg/trunk/util/kinematics/libKinematics/lib/
pkg/trunk/util/kinematics/libKinematics/manifest.xml
pkg/trunk/util/kinematics/libKinematics/src/
pkg/trunk/util/kinematics/libKinematics/src/ik.cpp
pkg/trunk/util/kinematics/libKinematics/src/kinematics.cpp
pkg/trunk/util/kinematics/libKinematics/src/test/
pkg/trunk/util/kinematics/libKinematics/src/test/test_PR2ik.cpp
Added: pkg/trunk/drivers/robot/pr2/pr2Core/Makefile
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/Makefile (rev 0)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/Makefile 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,13 @@
+PKG =
+
+CFLAGS = -g -Wall -Iinclude `rospack export/cpp/cflags $(PKG)`
+
+LDFLAGS = `rospack export/cpp/lflags $(PKG)`
+
+LIB =
+
+all: $(LIB)
+
+clean:
+ #rm -f *.o *.a lib/*.a
+
Added: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h (rev 0)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,213 @@
+// TODO: put enum in some Pr2 accessible file e.g. pr2.h and include it here
+#define MAX_JOINT_IDS 256
+
+namespace PR2
+{
+
+ enum PR2_JOINT_CONTROL_MODE{
+ TORQUE_CONTROL,
+ PD_CONTROL,
+ SPEED_CONTROL,
+ MAX_CONTROL_MODES
+ };
+
+ enum PR2_JOINT_TYPE{
+ PRISMATIC, ROTARY,
+ ROTARY_CONTINUOUS,
+ MAX_JOINT_TYPES
+ };
+
+ enum PR2_JOINT_ID{
+ CASTER_FL_STEER ,
+ CASTER_FL_DRIVE_L ,
+ CASTER_FL_DRIVE_R ,
+ CASTER_FR_STEER ,
+ CASTER_FR_DRIVE_L ,
+ CASTER_FR_DRIVE_R ,
+ CASTER_RL_STEER ,
+ CASTER_RL_DRIVE_L ,
+ CASTER_RL_DRIVE_R ,
+ CASTER_RR_STEER ,
+ CASTER_RR_DRIVE_L ,
+ CASTER_RR_DRIVE_R ,
+ SPINE_ELEVATOR ,
+ ARM_L_PAN ,
+ ARM_L_SHOULDER_PITCH,
+ ARM_L_SHOULDER_ROLL,
+ ARM_L_ELBOW_PITCH ,
+ ARM_L_ELBOW_ROLL ,
+ ARM_L_WRIST_PITCH ,
+ ARM_L_WRIST_ROLL ,
+ ARM_R_PAN ,
+ ARM_R_SHOULDER_PITCH,
+ ARM_R_SHOULDER_ROLL,
+ ARM_R_ELBOW_PITCH ,
+ ARM_R_ELBOW_ROLL ,
+ ARM_R_WRIST_PITCH ,
+ ARM_R_WRIST_ROLL ,
+ ARM_L_GRIPPER ,
+ ARM_R_GRIPPER ,
+ HEAD_YAW ,
+ HEAD_PITCH ,
+ HEAD_LASER_PITCH ,
+ HEAD_PTZ_L_PAN ,
+ HEAD_PTZ_L_TILT ,
+ HEAD_PTZ_R_PAN ,
+ HEAD_PTZ_R_TILT ,
+ MAX_JOINTS
+ };
+
+ enum PR2_SENSOR_ID{
+ CAMERA_GLOBAL ,
+ CAMERA_STEREO_LEFT ,
+ CAMERA_STEREO_RIGHT ,
+ CAMERA_HEAD_LEFT ,
+ CAMERA_HEAD_RIGHT ,
+ CAMERA_ARM_LEFT ,
+ CAMERA_ARM_RIGHT ,
+ LASER_HEAD ,
+ LASER_BASE ,
+ TACTILE_FINGER_LEFT ,
+ TACTILE_FINGER_RIGHT ,
+ MAX_SENSORS
+ };
+
+ enum PR2_BODY_ID{
+ CASTER_FL_WHEEL_L ,
+ CASTER_FL_WHEEL_R ,
+ CASTER_FL_BODY ,
+ CASTER_FR_WHEEL_L ,
+ CASTER_FR_WHEEL_R ,
+ CASTER_FR_BODY ,
+ CASTER_RL_WHEEL_L ,
+ CASTER_RL_WHEEL_R ,
+ CASTER_RL_BODY ,
+ CASTER_RR_WHEEL_L ,
+ CASTER_RR_WHEEL_R ,
+ CASTER_RR_BODY ,
+ BASE ,
+ TORSO ,
+ ARM_L_TURRET ,
+ ARM_L_SHOULDER ,
+ ARM_L_UPPERARM ,
+ ARM_L_ELBOW ,
+ ARM_L_FOREARM ,
+ ARM_L_WRIST ,
+ ARM_L_GRIPPER_TMP ,
+ ARM_L_FINGER_1 ,
+ ARM_L_FINGER_2 ,
+ ARM_R_TURRET ,
+ ARM_R_SHOULDER ,
+ ARM_R_UPPERARM ,
+ ARM_R_ELBOW ,
+ ARM_R_FOREARM ,
+ ARM_R_WRIST ,
+ ARM_R_GRIPPER_TMP ,
+ ARM_R_FINGER_1 ,
+ ARM_R_FINGER_2 ,
+ HEAD_BASE ,
+ LASER_BLOCK ,
+ STEREO_BLOCK ,
+ LASERBLOCK ,
+ MAX_BODY_IDS
+ };
+
+ enum PR2_JOINT_PARAM_ID {
+ P_GAIN,
+ I_GAIN,
+ D_GAIN,
+ I_CLAMP,
+ MAX_TORQUE
+ };
+
+ typedef struct
+ {
+ double timestamp;
+ double actualPosition;
+ double actualSpeed;
+ double actualEffectorForce;
+
+ int controlMode;
+ int jointType;
+
+ double cmdPosition;
+ double cmdSpeed;
+ double cmdEffectorForce;
+
+ int cmdEnableMotor;
+
+ double pGain;
+ double iGain;
+ double dGain;
+ double iClamp;
+ double saturationTorque;
+ } JointData;
+
+ enum PR2_ERROR_CODE {
+ PR2_ERROR,
+ PR2_ALL_OK,
+ PR2_MAX_ERROR_CODE
+ };
+
+ enum PR2_CONTROL_MODE {
+ PR2_CARTESIAN_CONTROL,
+ PR2_JOINT_CONTROL,
+ PR2_MAX_CONTROL_MODE
+ };
+
+ enum PR2_ROBOT_STATE {
+ STATE_INITIALIZED,
+ STATE_CALIBRATED,
+ STATE_RUNNING,
+ STATE_MOVING,
+ STATE_STOPPED,
+ MAX_ROBOT_STATES
+ };
+
+ typedef struct{
+ double x;
+ double y;
+ }point;
+
+ typedef struct{
+ double x;
+ double y;
+ double z;
+ }point3;
+
+ // JointStart/JointEnd corresponds to the PR2_MODEL_ID, start and end id for each model
+ enum PR2_MODEL_ID {PR2_BASE ,PR2_SPINE ,PR2_LEFT_ARM ,PR2_RIGHT_ARM ,PR2_LEFT_GRIPPER ,PR2_RIGHT_GRIPPER ,HEAD , MAX_MODELS };
+ const int JointStart[MAX_MODELS] = {CASTER_FL_STEER ,SPINE_ELEVATOR ,ARM_L_PAN ,ARM_R_PAN ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_YAW };
+ const int JointEnd[MAX_MODELS] = {CASTER_RR_DRIVE_R ,SPINE_ELEVATOR ,ARM_L_WRIST_ROLL ,ARM_R_WRIST_ROLL ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_LASER_PITCH };
+
+// Geometric description for the base
+ const float CASTER_FL_X_OFFSET = 0.25;
+ const float CASTER_FL_Y_OFFSET = 0.25;
+
+ const float CASTER_FR_X_OFFSET = 0.25;
+ const float CASTER_FR_Y_OFFSET = -0.25;
+
+ const float CASTER_RL_X_OFFSET = -0.25;
+ const float CASTER_RL_Y_OFFSET = 0.25;
+
+ const float CASTER_RR_X_OFFSET = -0.25;
+ const float CASTER_RR_Y_OFFSET = -0.25;
+
+ const float AXLE_WIDTH = 0.1;
+ const float WHEEL_RADIUS = 0.08;
+
+ const int NUM_CASTERS = 4;
+ const int NUM_WHEELS = 8;
+
+ const point BASE_CASTER_OFFSET[NUM_CASTERS] = {{0.25, 0.25}, {0.25, -0.25}, {-0.25,0.25}, {-0.25,-0.25}};
+ const point CASTER_DRIVE_OFFSET[NUM_WHEELS] = {{0,AXLE_WIDTH}, {0,-AXLE_WIDTH}, {0,AXLE_WIDTH}, {0,-AXLE_WIDTH},{0,AXLE_WIDTH}, {0,-AXLE_WIDTH},{0,AXLE_WIDTH}, {0,-AXLE_WIDTH}};
+
+ const point3 SPINE_ARM_PAN_OFFSET = {0,0,0};
+ const point3 ARM_PAN_SHOULDER_PITCH_OFFSET = {0.1,0,0};
+ const point3 ARM_SHOULDER_PITCH_ROLL_OFFSET = {0,0,0};
+ const point3 ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET = {0.4,0,0};
+ const point3 ELBOW_PITCH_ELBOW_ROLL_OFFSET = {0.09085,0,0};
+ const point3 ELBOW_ROLL_WRIST_PITCH_OFFSET = {0.2237,0,0};
+ const point3 WRIST_PITCH_WRIST_ROLL_OFFSET = {0,0,0};
+ const point3 WRIST_ROLL_GRIPPER_OFFSET = {0,0,0};
+}
Added: pkg/trunk/drivers/robot/pr2/pr2Core/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/manifest.xml (rev 0)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/manifest.xml 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,13 @@
+<package>
+<description brief='PR2 Core Definitions for Joints, Bodies, Models'>
+
+Header file for PR2 core definitions of joints, bodies, models
+
+</description>
+<author>Sachin Chitta</author>
+<license>BSD</license>
+<url>http://pr.willowgarage.com</url>
+<export>
+<cpp cflags="-I${prefix}/include" lflags=""/>
+</export>
+</package>
Modified: pkg/trunk/simulators/rosgazebo/world/pr2.model
===================================================================
--- pkg/trunk/simulators/rosgazebo/world/pr2.model 2008-06-05 22:18:25 UTC (rev 660)
+++ pkg/trunk/simulators/rosgazebo/world/pr2.model 2008-06-06 00:28:26 UTC (rev 661)
@@ -374,7 +374,7 @@
<body2>dummytorso_body</body2>
<anchor>dummytorso_body</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -383,7 +383,7 @@
<body2>dummytorso_body</body2>
<anchor>dummytorso_body</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -582,7 +582,7 @@
<body2>torso_body</body2>
<anchor>shoulder_pan_body_right</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -591,7 +591,7 @@
<body2>shoulder_pan_body_right</body2>
<anchor>upperarm_joint_right</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -600,7 +600,7 @@
<body2>upperarm_right</body2>
<anchor>upperarm_joint_right</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -609,7 +609,7 @@
<body2>upperarm_right</body2>
<anchor>elbow_right</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -618,7 +618,7 @@
<body2>elbow_right</body2>
<anchor>elbow_right</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -627,7 +627,7 @@
<body2>forearm_right</body2>
<anchor>wrist_right</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -636,7 +636,7 @@
<body2>wrist_right</body2>
<anchor>wrist_right</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -796,7 +796,7 @@
<body2>torso_body</body2>
<anchor>shoulder_pan_body_left</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -805,7 +805,7 @@
<body2>shoulder_pan_body_left</body2>
<anchor>upperarm_joint_left</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -814,7 +814,7 @@
<body2>upperarm_left</body2>
<anchor>upperarm_joint_left</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -823,7 +823,7 @@
<body2>upperarm_left</body2>
<anchor>elbow_left</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -832,7 +832,7 @@
<body2>elbow_left</body2>
<anchor>elbow_left</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -841,7 +841,7 @@
<body2>forearm_left</body2>
<anchor>wrist_left</anchor>
<axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -850,7 +850,7 @@
<body2>wrist_left</body2>
<anchor>wrist_left</anchor>
<axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.6</erp>
</joint:hinge>
@@ -1079,8 +1079,6 @@
<body2>front_left_wheel_l</body2>
<anchor>front_left_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="front_left_wheel_r_drive">
@@ -1088,8 +1086,6 @@
<body2>front_left_wheel_r</body2>
<anchor>front_left_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="front_right_wheel_l_drive">
@@ -1097,8 +1093,6 @@
<body2>front_right_wheel_l</body2>
<anchor>front_right_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="front_right_wheel_r_drive">
@@ -1106,8 +1100,6 @@
<body2>front_right_wheel_r</body2>
<anchor>front_right_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_left_wheel_l_drive">
@@ -1115,8 +1107,6 @@
<body2>rear_left_wheel_l</body2>
<anchor>rear_left_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_left_wheel_r_drive">
@@ -1124,8 +1114,6 @@
<body2>rear_left_wheel_r</body2>
<anchor>rear_left_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_right_wheel_l_drive">
@@ -1133,8 +1121,6 @@
<body2>rear_right_wheel_l</body2>
<anchor>rear_right_wheel_l</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_right_wheel_r_drive">
@@ -1142,8 +1128,6 @@
<body2>rear_right_wheel_r</body2>
<anchor>rear_right_wheel_r</anchor>
<axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
@@ -1154,8 +1138,6 @@
<body2>base_body</body2>
<anchor>front_left_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="front_right_caster_steer">
@@ -1163,8 +1145,6 @@
<body2>base_body</body2>
<anchor>front_right_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_left_caster_steer">
@@ -1172,8 +1152,6 @@
<body2>base_body</body2>
<anchor>rear_left_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
<joint:hinge name="rear_right_caster_steer">
@@ -1181,8 +1159,6 @@
<body2>base_body</body2>
<anchor>rear_right_caster_body</anchor>
<axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
</joint:hinge>
Modified: pkg/trunk/simulators/rosgazebo/world/robot.world
===================================================================
--- pkg/trunk/simulators/rosgazebo/world/robot.world 2008-06-05 22:18:25 UTC (rev 660)
+++ pkg/trunk/simulators/rosgazebo/world/robot.world 2008-06-06 00:28:26 UTC (rev 661)
@@ -23,9 +23,9 @@
<!-- cfm is 1e-5 for single precision -->
<!-- erp is typically .1-.8 -->
<physics:ode>
- <stepTime>0.01</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
- <cfm>0.001</cfm>
+ <cfm>0.0001</cfm>
<erp>0.8</erp>
</physics:ode>
Added: pkg/trunk/util/kinematics/libKinematics/Makefile
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/Makefile (rev 0)
+++ pkg/trunk/util/kinematics/libKinematics/Makefile 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,31 @@
+PKG = libKinematics
+
+CFLAGS = -g -Wall -Iinclude `rospack export/cpp/cflags $(PKG)`
+
+LDFLAGS = `rospack export/cpp/lflags $(PKG)`
+
+LIBK = lib/libKinematics.a
+LIBIK = lib/libIk.a
+
+all: $(LIBK) $(LIBIK) test_PR2ik
+
+
+ik.o: src/ik.cpp include/libKinematics/ik.h include/libKinematics/kinematics.h
+ g++ $(CFLAGS) -o $@ -c $<
+
+kinematics.o: src/kinematics.cpp include/libKinematics/kinematics.h
+ g++ $(CFLAGS) -o $@ -c $<
+
+$(LIBK): kinematics.o
+ ar -rs $@ $^
+
+$(LIBIK): ik.o kinematics.o
+ ar -rs $@ $^
+
+test_PR2ik: src/test/test_PR2ik.cpp $(LIBK) $(LIBIK)
+ g++ $(CFLAGS) -o $@ $^ $(LDFLAGS)
+
+clean:
+ rm -rf test_PR2ik
+ rm -f *.o *.a lib/*.a
+
Added: pkg/trunk/util/kinematics/libKinematics/include/libKinematics/ik.h
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/include/libKinematics/ik.h (rev 0)
+++ pkg/trunk/util/kinematics/libKinematics/include/libKinematics/ik.h 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,57 @@
+//Software License Agreement (BSD License)
+
+//Copyright (c) 2008, Willow Garage, Inc.
+//All rights reserved.
+
+//Redistribution and use in source and binary forms, with or without
+//modification, are permitted provided that the following conditions
+//are met:
+
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following
+// disclaimer in the documentation and/or other materials provided
+// with the distribution.
+// * Neither the name of the Willow Garage nor the names of its
+// contributors may be used to endorse or promote products derived
+// from this software without specific prior written permission.
+
+//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+//"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+//LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+//FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+//COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+//INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+//BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+//CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+//LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+//ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+//POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef PR2_KINEMATICS_HH
+#define PR2_KINEMATICS_HH
+
+#include <iostream>
+#include <newmat10/newmat.h>
+#include <newmat10/newmatio.h>
+#include <libKinematics/kinematics.h>
+
+using namespace kinematics;
+
+namespace PR2
+{
+ class PR2Arm : public kinematics::SerialRobot
+ {
+ public:
+
+ PR2Arm();
+
+ NEWMAT::Matrix ComputeIK(NEWMAT::Matrix g, double theta1);
+
+ SerialRobot Wrist;
+ };
+}
+
+#endif
Added: pkg/trunk/util/kinematics/libKinematics/include/libKinematics/kinematics.h
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/include/libKinematics/kinematics.h (rev 0)
+++ pkg/trunk/util/kinematics/libKinematics/include/libKinematics/kinematics.h 2008-06-06 00:28:26 UTC (rev 661)
@@ -0,0 +1,357 @@
+//Software License Agreement (BSD License)
+
+//Copyright (c) 2008, Willow Garage, Inc.
+//All rights reserved.
+
+//Redistribution and use in source and binary forms, with or without
+//modification, are permitted provided that the following conditions
+//are met:
+
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following
+// disclaimer in the documentation and/or other materials provided
+// with the distribution.
+// * Neither the name of the Willow Garage nor the names of its
+// contributors may be used to endorse or promote products derived
+// from this software without specific prior written permission.
+
+//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+//"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+//LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+//FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+//COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+//INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+//BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+//LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+//CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+//LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+//ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+//POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef KINEMATICS_HH
+#define KINEMATICS_HH
+
+#define MAX_NUM_JOINTS 64
+
+#include <iostream>
+#include <newmat10/newmat.h>
+#include <newmat10/newmatio.h>
+
+namespace kinematics
+{
+ /*! \fn
+ \brief Transform a twist from one coordinate frame to another
+ \param g - NEWMAT matrix representing transformation,
+ \param xi - twist vector that needs to be transformed,
+ \return NEWMAT matrix representing the transformed twist vector.
+ */
+ NEWMAT::Matrix TransformTwist(const NEWMAT::Matrix& g, const NEWMAT::Matrix& xi);
+
+ /*! \fn
+ \brief Return the twist matrix corresponding to a twist vector
+ \param xi - input twist vector (as a NEWMAT matrix)
+ \return NEWMAT matrix representing the transformed twist vector.
+ */
+ NEWMAT::Matrix TwistVectorToMatrix(const NEWMAT::Matrix& xi);
+
+ /*! \fn
+ \brief Convert twist matrix to vector representation
+ \param xiHat - input twist in matrix form (as a NEWMAT matrix)
+ \return NEWMAT vector representing the transformed twist matrix
+ */
+ NEWMAT::Matrix TwistMatrixToVector(const NEWMAT::Matrix& xiHat);
+
+ /*! \fn
+ \brief Compute the transform corresponding to the exponential of a twist
+ \param xi - input twist vector (as a NEWMAT matrix)
+ \param theta - rotation angle/translational distance
+ \return NEWMAT matrix corresponding to the matrix exponential
+ */
+ NEWMAT::Matrix ExpTwist(NEWMAT::Matrix xi, double theta);
+
+ /*! \fn
+ \brief Compute the rotation matrix corresponding to rotation about an axis by a desired angle
+ \param omega - axis of rotation
+ \param theta - angle of rotation
+ \return Output NEWMAT rotation matrix
+ */
+ NEWMAT::Matrix ExpRot(NEWMAT::Matrix omega, double theta);
+
+ /*! \fn
+ \brief Get the (3 x 1) axis corresponding to a twist vector
+ \param xi - input twist vector (as a NEWMAT matrix)
+ \return Output NEWMAT axis vector (3 x 1)
+ */
+ NEWMAT::Matrix GetAxis(NEWMAT::Matrix xi);
+
+ /*! \fn
+ \brief Get the (3 x 1) speed part of a twist vector
+ \param xi - input twist vector (as a NEWMAT matrix)
+ \return Output NEWMAT speed vector (3 x 1)
+ */
+ NEWMAT::Matrix GetSpeed(NEWMAT::Matrix xi);
+
+ /*! \fn
+ \brief Generate the homogeneous transformation matrix corresponding to a translation
+ \param p - translation vector (3 x 1)
+ \return Output homogeneous transformation matrix corresponding to a translation
+ */
+ NEWMAT::Matrix Translate(double p[]);
+
+ /*! \fn
+ \brief Get the position/translation part of a homogeneous transformation matrix
+ \param p - homogeneous transformation matrix
+ \return Output position vector
+ */
+ NEWMAT::Matrix GetPosition(const NEWMAT::Matrix& p);
+
+ /*! \fn
+ \brief Get the rotation matrix part of a homogeneous transformation matrix
+ \param p - homogeneous transformation matrix
+ \return Output rotation matrix
+ */
+ NEWMAT::Matrix GetRotationMatrix(const NEWMAT::Matrix& p);
+
+ /*! \fn
+ \brief Get the skew-symmetric matrix corresponding to a rotation axis
+ \param omega - (3 x 1) vector representing a rotation axis
+ \return Output skew-symmetric rotation matrix
+ */
+ NEWMAT::Matrix GetHatMatrix(NEWMAT::Matrix omega);
+
+ /*! \fn
+ \brief Compute the cross product of two vectors
+ \param p1 - NEWMAT matrix representation of the first vector
+ \param p2 - NEWMAT matrix representation of the second vector
+ \return Output skew-symmetric rotation matrix
+ */
+ NEWMAT::Matrix cross(NEWMAT::Matrix p1, NEWMAT::Matrix p2);
+
+ /*! \fn
+ \brief Compute the cross product of two vectors
+ \param p1 - array representation of the first vector
+ \param p2 - array representation of the second vector
+ \param p3 - array representation of the resultant vector from the cross-product
+ */
+ void cross(const double p1[], const double p2[], double p3[]);
+
+ /*! \fn
+ \brief Compute the magnitude of a vector
+ \param omega - input vector (as a NEWMAT matrix)
+ */
+ double GetNorm(NEWMAT::Matrix omega);
+
+ /*! \fn
+\brief Solution of the first form of the Paden-Kahan subproblems. This formulation solves for theta in exp(xi theta) p = q
+\param p NEWMAT matrix representation of a point on which the exponential operates
+\param q NEWMAT matrix representation of the resultant point after rotation through thets
+\param r point on the axis about which the rotation is happening
+\param omega direction vector for the axis about which the rotation is happening. The twist vector xi is a combination of this vector and a linear velocity term v.
+\return theta solution to the above expression
+ */
+ double SubProblem1(NEWMAT::Matrix p, NEWMAT::Matrix q, NEWMAT::Matrix r, NEWMAT::Matrix omega);
+
+
+ /*! \fn
+\brief Solution of the first form of the Paden-Kahan subproblems. This formulation solves for theta in exp(xi1 theta1) exp(xi2 theta2) p = q
+\param p NEWMAT matrix representation of a point on which the exponential operates
+\param q NEWMAT matrix representation of the resultant point after rotation through thets
+\param r point on the axis about which the rotation is happening
+\param omega direction vector for the axis about which the rotation is happening. The twist vectors xi1 and xi2 are a combination of this vector and linear velocity terms v1 and v2.
+\param theta[] a 4x1 vector containing the two solutions to this problem.
+ */
+ void SubProblem2(NEWMAT::Matrix pin, NEWMAT::Matrix qin, NEWMAT::Matrix rin, NEWMAT::Matrix omega1, NEWMAT::Matrix omega2, double theta[]);
+
+
+/*! \fn
+\brief Print the matrix in a nice form
+\param m Matrix to be printed
+\param c string representing matrix specification
+*/
+ void PrintMatrix(NEWMAT::Matrix m, std::string c);
+
+
+/*! \enum PR2_JOINT_TYPE
+ * Joint types
+*/
+ enum PR2_JOINT_TYPE{
+ PRISMATIC,
+ ROTARY,
+ ROTARY_CONTINUOUS,
+ MAX_JOINT_TYPES
+ };
+
+ class Joint
+ {
+ public:
+
+/*! \fn
+ Default constructor
+*/
+ Joint(){};
+
+/*! \fn
+ Default destructor
+*/
+ virtual ~Joint(){};
+
+ int jointId; /**< joint ID */
+
+ NEWMAT::Matrix linkPose; /**< link pose */
+
+ NEWMAT::Matrix twist;; /**< link twist */
+ };
+
+ /*! \class
+ \brief The serial robot class allows the construction of a serial manipulator using a series of links and joints
+ */
+ class SerialRobot
+ {
+
+ public:
+
+ /*! \fn
+ \brief Construct a serial robot with n joints
+ \param nJoints - number of joints
+ */
+ SerialRobot(int nJoints);
+
+ SerialRobot();
+
+ /*! \fn
+ \brief Default destructor
+ */
+ virtual ~SerialRobot();
+
+ /*! \fn
+ \brief Initialize the joint structure of the serial robot
+ \param nJoints - number of joints
+ */
+ void Initialize(int nJoints);
+
+ Joint *joints; /**< pointer to set of joints in the robot */
+
+ /*! \fn
+ \brief Add a new joint to the serial robot,
+ \param joint - twist vector corresponding to the joint
+ */
+ void AddJoint(NEWMAT::Matrix joint, double p[]);
+
+ /*! \fn
+ \brief Add a new joint to the serial robot,
+ \param joint - twist vector corresponding to the joint
+ */
+ void AddJoint(double p[], double axis[], int jointType);
+
+ /*! \fn
+ \brief Compute the inverse kinematics for a serial manipulator (NOT IMPLEMENTED YET)
+ \param p - homogeneous transformation matrix representing an end-effector pose
+ \param jointangle - return array of computed joint angles
+ */
+ void ComputeIK(NEWMAT::Matrix p, double jointAngles[]);
+
+ /*! \fn
+ \brief Get the COG pose for a particular body (NOT IMPLEMENTED YET)
+ */
+ void GetCOGPose(int id);
+
+ /*! \fn
+ \brief Get the pose for a particular link in the serial manipulator
+ \param id - body/link ID
+ \param jointangle - input array of joint angles
+ */
+ NEWMAT::Matrix GetPose(int id, double jointAngles[]);
+
+ /*! \fn
+ \brief Get the joint twist for a particular joint in the serial manipulator
+ \param id - body/link ID
+ \return twist vector for the joint
+ */
+ NEWMAT::Matrix GetTwist(int id);
+
+ /*! \fn
+ \brief Get the joint axis for a particular joint in the serial manipulator
+ \param id - body/link ID
+ \return axis for the joint
+ */
+ NEWMAT::Matrix GetJointAxis(int id);
+
+ /*! \fn
+ \brief Get the location of a point on the joint axis for a particular joint in the serial manipulator IN THE HOME POSITION
+ \param id - body/link ID
+ \r...
[truncated message content] |
|
From: <hsu...@us...> - 2008-06-06 16:29:01
|
Revision: 676
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=676&view=rev
Author: hsujohnhsu
Date: 2008-06-06 09:29:06 -0700 (Fri, 06 Jun 2008)
Log Message:
-----------
update to makefiles for gazebo plugins and rosgazebo dependencies.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/Makefile
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
pkg/trunk/simulators/gazebo_plugin/Makefile
pkg/trunk/simulators/rosgazebo/manifest.xml
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/Makefile
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/Makefile 2008-06-06 16:09:29 UTC (rev 675)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/Makefile 2008-06-06 16:29:06 UTC (rev 676)
@@ -14,10 +14,10 @@
$(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
$(LIBP): pr2API.o
- ar -rsv $@ $^
+ ar -rv $@ $^
-$(LIBS): $(LIBP)
- $(CXX) -shared -o $@ $^
+$(LIBS): src/pr2API.cpp
+ $(CXX) $(CFLAGS) -shared -o $@ $^ $(LFLAGS)
test_pr2API.o: src/test/test_pr2API.cpp
$(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-06 16:09:29 UTC (rev 675)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-06 16:29:06 UTC (rev 676)
@@ -33,11 +33,11 @@
#ifndef PR2_API_HH
#define PR2_API_HH
#include <newmat10/newmat.h>
+#include <libKinematics/ik.h>
#include <pr2Core/pr2Core.h>
#include <sys/types.h>
#include <stdint.h>
#include <string>
-#include <libKinematics/ik.h>
namespace PR2
{
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-06-06 16:09:29 UTC (rev 675)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-06-06 16:29:06 UTC (rev 676)
@@ -7,12 +7,12 @@
<author>Sachin Chitta</author>
<license>BSD</license>
<url>http://pr.willowgarage.com</url>
-<depend package="newmat10"/>
<depend package="gazebo"/>
<depend package="pr2Core"/>
<depend package="libKinematics"/>
+<depend package="newmat10"/>
<export>
-<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib"/>
+<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lpr2API"/>
</export>
</package>
Modified: pkg/trunk/simulators/gazebo_plugin/Makefile
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/Makefile 2008-06-06 16:09:29 UTC (rev 675)
+++ pkg/trunk/simulators/gazebo_plugin/Makefile 2008-06-06 16:29:06 UTC (rev 676)
@@ -24,11 +24,11 @@
$(LIB_GRP): libPr2_Gripper.o
ar -rs $@ $^
-$(LIB_ACT_S): libPr2_Actarray.o
- g++ -shared -o $@ $^
+$(LIB_ACT_S): src/Pr2_Actarray.cc
+ g++ $(CFLAGS) -shared -o $@ $^ $(LDFLAGS)
-$(LIB_GRP_S): libPr2_Gripper.o
- g++ -shared -o $@ $^
+$(LIB_GRP_S): src/Pr2_Gripper.cc
+ g++ $(CFLAGS) -shared -o $@ $^ $(LDFLAGS)
player_pr2: src/player/Pr2_Player.cc
g++ $(CFLAGS) -o $@ $^ $(LDFLAGS)
Modified: pkg/trunk/simulators/rosgazebo/manifest.xml
===================================================================
--- pkg/trunk/simulators/rosgazebo/manifest.xml 2008-06-06 16:09:29 UTC (rev 675)
+++ pkg/trunk/simulators/rosgazebo/manifest.xml 2008-06-06 16:29:06 UTC (rev 676)
@@ -6,6 +6,7 @@
<depend package="gazebo" />
<depend package="gazebo_plugin" />
<depend package="libpr2API" />
+ <depend package="pr2Core" />
<depend package="std_msgs" />
<depend package="rosthread" />
</package>
Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-06 16:09:29 UTC (rev 675)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-06 16:29:06 UTC (rev 676)
@@ -25,7 +25,7 @@
// gazebo
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
-#include "pr2API.hh"
+#include <libpr2API/pr2API.h>
// roscpp
#include <ros/node.h>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sac...@us...> - 2008-06-06 21:10:10
|
Revision: 687
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=687&view=rev
Author: sachinchitta
Date: 2008-06-06 14:10:09 -0700 (Fri, 06 Jun 2008)
Log Message:
-----------
Fixed nan problem with test_pr2API, Cartesian control (IK-based) now works (see test_pr2AP
I.cpp for an example of how to use it)
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/Makefile
pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_pr2API.cpp
pkg/trunk/util/kinematics/libKinematics/Makefile
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/Makefile
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/Makefile 2008-06-06 18:31:23 UTC (rev 686)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/Makefile 2008-06-06 21:10:09 UTC (rev 687)
@@ -1,11 +1,12 @@
PKG = libpr2API
CXX = g++
-CFLAGS = -g -Wall $(shell rospack export/cpp/cflags $(PKG))
+CFLAGS = -ggdb -g -Wall $(shell rospack export/cpp/cflags $(PKG))
LDFLAGS = $(shell rospack export/cpp/lflags gazebo) \
$(shell rospack export/cpp/lflags pr2Core) \
- $(shell rospack export/cpp/lflags libKinematics)
+ $(shell rospack export/cpp/lflags libKinematics) \
+ $(shell rospack export/cpp/lflags newmat10)
LFLAGS = $(shell rospack export/cpp/lflags $(PKG))
@@ -23,11 +24,8 @@
$(LIBS): src/pr2API.cpp
$(CXX) $(CFLAGS) -shared -o $@ $^ $(LDFLAGS)
-test_pr2API.o: src/test/test_pr2API.cpp
- $(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
-
test_libpr2API: src/test/test_pr2API.cpp
- $(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
+ $(CXX) $(CFLAGS) -o $@ $< $(LFLAGS) $(LDFLAGS)
clean:
rm -f test_pr2API $(LIBP) $(LIBS)
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-06-06 18:31:23 UTC (rev 686)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-06-06 21:10:09 UTC (rev 687)
@@ -7,10 +7,10 @@
<author>Sachin Chitta</author>
<license>BSD</license>
<url>http://pr.willowgarage.com</url>
+<depend package="newmat10"/>
<depend package="gazebo"/>
<depend package="pr2Core"/>
<depend package="libKinematics"/>
-<depend package="newmat10"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lpr2API"/>
</export>
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-06 18:31:23 UTC (rev 686)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-06 21:10:09 UTC (rev 687)
@@ -564,8 +564,7 @@
g(3,4) = g(3,4) - SPINE_LEFT_ARM_OFFSET.z;
}
theta = 0;
- theta = myArm.ComputeIK(g,0);
-
+ theta = myArm.ComputeIK(g,0.1);
for(int jj = 1; jj <= 8; jj++)
{
if (theta(8,jj) > -1)
@@ -574,15 +573,14 @@
break;
}
}
-
if(validSolution <= 8)
{
for(int ii = 0; ii < 7; ii++)
{
angles[ii] = theta(ii+1,validSolution);
speeds[ii] = 0;
+ this->SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),angles[ii],0);
}
- SetArmJointPosition(id,angles,speeds);
}
return PR2_ALL_OK;
};
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_pr2API.cpp 2008-06-06 18:31:23 UTC (rev 686)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_pr2API.cpp 2008-06-06 21:10:09 UTC (rev 687)
@@ -3,7 +3,7 @@
#include <pr2Core/pr2Core.h>
#include <math.h>
-//using namespace kinematics;
+using namespace kinematics;
using namespace PR2;
using namespace std;
@@ -19,16 +19,13 @@
myPR2.SetBaseControlMode(PR2_CARTESIAN_CONTROL);
myPR2.SetBaseCartesianSpeedCmd(0.0,-0.5,1*M_PI/8);
-
-
-
// create a end-effector position and orientation for inverse kinematics test
NEWMAT::Matrix g(4,4);
g = 0;
PR2Arm myArm;
- /* Joint angles (radians) and speeds for testing */
+ // Joint angles (radians) and speeds for testing
double angles[7] = {0,0,0,0,0,0,0};
angles[0] = 0.1; // shoulder pan angle
@@ -39,25 +36,27 @@
angles[5] = 0.5; // wrist pitch angle
angles[6] = 0.0; // wrist roll
- NEWMAT::Matrix pose = myArm.ComputeFK(angles);
+ NEWMAT::Matrix pose = myArm.ComputeFK(angles);
g = pose;
// offset from the base of the arm,
// subtracted for PR2 to cancel out effect of offset
// watchout whether left of right arm is used
- g(1,4) = g(1,4) + PR2::SPINE_RIGHT_ARM_OFFSET.x;
- g(2,4) = g(2,4) + PR2::SPINE_RIGHT_ARM_OFFSET.y;
- g(3,4) = g(3,4) + PR2::SPINE_RIGHT_ARM_OFFSET.z;
- cout << "Main::End-effector Pose:" << endl << g << endl ;
+
- // send command to robot
- myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,g);
-
- // compute analytical solution
+ // compute analytical solution
NEWMAT::Matrix theta(8,8);
theta = 0;
theta = myArm.ComputeIK(g,0.1);
PrintMatrix(theta,"exact solution for pos/orien of end effector");
+ cout << "Main::End-effector Pose:" << endl << g << endl ;
+ g(1,4) = g(1,4) + PR2::SPINE_RIGHT_ARM_OFFSET.x;
+ g(2,4) = g(2,4) + PR2::SPINE_RIGHT_ARM_OFFSET.y;
+ g(3,4) = g(3,4) + PR2::SPINE_RIGHT_ARM_OFFSET.z;
+ // send command to robot
+ myPR2.SetArmCartesianPosition(PR2::PR2_RIGHT_ARM,g);
+
+
return 0;
};
Modified: pkg/trunk/util/kinematics/libKinematics/Makefile
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/Makefile 2008-06-06 18:31:23 UTC (rev 686)
+++ pkg/trunk/util/kinematics/libKinematics/Makefile 2008-06-06 21:10:09 UTC (rev 687)
@@ -23,7 +23,7 @@
ar -rs $@ $^
test_PR2ik: src/test/test_PR2ik.cpp
- g++ $(CFLAGS) -o $@ $^ $(LDFLAGS)
+ g++ $(CFLAGS) -o $@ $^ -lnewmat
clean:
rm -rf test_PR2ik
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-06-10 16:06:23
|
Revision: 718
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=718&view=rev
Author: hsujohnhsu
Date: 2008-06-10 09:06:27 -0700 (Tue, 10 Jun 2008)
Log Message:
-----------
added base laser.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
pkg/trunk/simulators/rosgazebo/world/pr2.model
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-10 06:05:09 UTC (rev 717)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-10 16:06:27 UTC (rev 718)
@@ -547,10 +547,11 @@
/*! \fn
\brief - Get laser range data
*/
- public: PR2_ERROR_CODE GetLaserRanges(float* angle_min, float* angle_max, float* angle_increment,
- float* range_max,uint32_t* ranges_size ,uint32_t* ranges_alloc_size,
- uint32_t* intensities_size,uint32_t* intensities_alloc_size,
- float* ranges ,uint8_t* intensities);
+ public: PR2_ERROR_CODE GetLaserRanges(PR2_SENSOR_ID id,
+ float* angle_min, float* angle_max, float* angle_increment,
+ float* range_max,uint32_t* ranges_size ,uint32_t* ranges_alloc_size,
+ uint32_t* intensities_size,uint32_t* intensities_alloc_size,
+ float* ranges ,uint8_t* intensities);
/*! \fn
\brief - Open gripper
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-10 06:05:09 UTC (rev 717)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-10 16:06:27 UTC (rev 718)
@@ -15,6 +15,7 @@
static gazebo::PR2GripperIface *pr2GripperLeftIface;
static gazebo::PR2GripperIface *pr2GripperRightIface;
static gazebo::LaserIface *pr2LaserIface;
+static gazebo::LaserIface *pr2BaseLaserIface;
static gazebo::CameraIface *pr2CameraIface;
static gazebo::CameraIface *pr2CameraGlobalIface;
static gazebo::CameraIface *pr2CameraHeadLeftIface;
@@ -103,6 +104,7 @@
pr2GripperLeftIface = new gazebo::PR2GripperIface();
pr2GripperRightIface = new gazebo::PR2GripperIface();
pr2LaserIface = new gazebo::LaserIface();
+ pr2BaseLaserIface = new gazebo::LaserIface();
pr2CameraGlobalIface = new gazebo::CameraIface();
pr2CameraHeadLeftIface = new gazebo::CameraIface();
pr2CameraHeadRightIface = new gazebo::CameraIface();
@@ -193,6 +195,20 @@
}
+ /// Open the base laser interface for hokuyo
+ try
+ {
+ pr2BaseLaserIface->Open(client, "base_laser_iface_1");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the pr2 base laser interface\n"
+ << e << "\n";
+ pr2BaseLaserIface = NULL;
+ //return PR2_ERROR;
+ }
+
+
/// Open the camera interface for hokuyo
try
{
@@ -1177,56 +1193,70 @@
return PR2_ALL_OK;
};
-PR2_ERROR_CODE PR2Robot::GetLaserRanges(float* angle_min, float* angle_max, float* angle_increment,
+PR2_ERROR_CODE PR2Robot::GetLaserRanges(PR2_SENSOR_ID id,
+ float* angle_min, float* angle_max, float* angle_increment,
float* range_max,uint32_t* ranges_size ,uint32_t* ranges_alloc_size,
uint32_t* intensities_size,uint32_t* intensities_alloc_size,
float* ranges ,uint8_t* intensities)
{
- if (pr2LaserIface == NULL)
+
+ gazebo::LaserIface *tmpLaserIface;
+ switch (id)
{
+ case LASER_HEAD:
+ tmpLaserIface = pr2LaserIface;
+ break;
+ case LASER_BASE:
+ tmpLaserIface = pr2BaseLaserIface;
+ break;
+ default:
+ tmpLaserIface = NULL;
+ break;
+ }
+ if (tmpLaserIface == NULL)
+ {
return PR2_ERROR;
}
else
{
- *angle_min = (float)pr2LaserIface->data->min_angle;
- *angle_max = (float)pr2LaserIface->data->max_angle;
+ *angle_min = (float)tmpLaserIface->data->min_angle;
+ *angle_max = (float)tmpLaserIface->data->max_angle;
- *range_max = (float)pr2LaserIface->data->max_range;
+ *range_max = (float)tmpLaserIface->data->max_range;
- *ranges_size = (uint32_t)pr2LaserIface->data->range_count;
+ *ranges_size = (uint32_t)tmpLaserIface->data->range_count;
*ranges_alloc_size = (uint32_t)GZ_LASER_MAX_RANGES;
- *intensities_size = (uint32_t)pr2LaserIface->data->range_count;
+ *intensities_size = (uint32_t)tmpLaserIface->data->range_count;
*intensities_alloc_size = (uint32_t)GZ_LASER_MAX_RANGES;
- //*angle_increment = (float)pr2LaserIface->data->res_angle;
+ //*angle_increment = (float)tmpLaserIface->data->res_angle;
*angle_increment = (*angle_max - *angle_min)/((double)(*ranges_size -1));
// std::cout << "max " << *angle_max << std::endl;
// std::cout << "min " << *angle_min << std::endl;
// std::cout << "siz " << *ranges_size << std::endl;
// std::cout << "inc " << *angle_increment << std::endl;
- // std::cout << "in2 " << (float)pr2LaserIface->data->res_angle << std::endl;
+ // std::cout << "in2 " << (float)tmpLaserIface->data->res_angle << std::endl;
// std::cout << "getting laser ranges" << std::endl;
- //ranges = cast(float*)pr2LaserIface->data->ranges;
- //intensities = cast(float*)pr2LaserIface->data->intensity;
+ //ranges = cast(float*)tmpLaserIface->data->ranges;
+ //intensities = cast(float*)tmpLaserIface->data->intensity;
//std::cout << "range count = " << count << std::endl;
for (int i=0; i<(int)*ranges_size ; i++)
{
//std::cout << ranges[i] << " ";
// fill in the needed messages
- ranges[i] = (float)pr2LaserIface->data->ranges[i];
- intensities[i] = (uint8_t)pr2LaserIface->data->intensity[i];
+ ranges[i] = (float)tmpLaserIface->data->ranges[i];
+ intensities[i] = (uint8_t)tmpLaserIface->data->intensity[i];
}
//std::cout << std::endl;
return PR2_ALL_OK;
}
};
-
PR2_ERROR_CODE PR2Robot::OpenGripper(PR2_MODEL_ID id,double gap,double force)
{
switch(id)
Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-10 06:05:09 UTC (rev 717)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-10 16:06:27 UTC (rev 718)
@@ -262,42 +262,27 @@
/***************************************************************/
/* */
- /* laser */
+ /* laser - pitching */
/* */
/***************************************************************/
- if (this->myPR2->GetLaserRanges(&angle_min, &angle_max, &angle_increment,
- &range_max, &ranges_size , &ranges_alloc_size,
+ if (this->myPR2->GetLaserRanges(PR2::LASER_HEAD,
+ &angle_min, &angle_max, &angle_increment,
+ &range_max, &ranges_size , &ranges_alloc_size,
&intensities_size, &intensities_alloc_size,
this->ranges , this->intensities) == PR2::PR2_ALL_OK)
{
- //std::cout << "vx : " << vx << " vy: " << vy << " vw: " << vw << std::endl;
-
- // Get latest laser data
- // Stg::stg_laser_sample_t* samples = this->lasermodel->GetSamples();
- // if(samples)
- // {
- // // Translate into ROS message format and publish
- // Stg::stg_laser_cfg_t cfg = this->lasermodel->GetConfig();
- this->laserMsg.angle_min = angle_min;
- this->laserMsg.angle_max = angle_max;
- this->laserMsg.angle_increment = angle_increment;
- this->laserMsg.range_max = range_max;
- this->laserMsg.set_ranges_size(ranges_size);
- this->laserMsg.set_intensities_size(intensities_size);
for(unsigned int i=0;i<ranges_size;i++)
{
- this->laserMsg.ranges[i] = this->ranges[i];
- this->laserMsg.intensities[i] = this->intensities[i];
-
- // populating cloud data
// get laser pitch angle
double tmp_range, laser_yaw, laser_pitch, laser_pitch_rate;
this->myPR2->GetJointServoActual(PR2::HEAD_LASER_PITCH , &laser_pitch, &laser_pitch_rate);
// get laser yaw angle
laser_yaw = angle_min + (double)i * angle_increment;
- //std::cout << " pit " << laser_pitch << "yaw " << laser_yaw << " amin " << angle_min << " inc " << angle_increment << std::endl;
+ //std::cout << " pit " << laser_pitch << "yaw " << laser_yaw
+ // << " amin " << angle_min << " inc " << angle_increment << std::endl;
+ // populating cloud data by range
+ tmp_range = (this->ranges[i] >= range_max) ? 0 : this->ranges[i];
// transform from range to x,y,z
- tmp_range = (this->ranges[i] >= range_max) ? 0 : this->ranges[i];
tmp_cloud_pt.x = tmp_range * cos(laser_yaw) * cos(laser_pitch);
tmp_cloud_pt.y = tmp_range * sin(laser_yaw) ; //* cos(laser_pitch);
tmp_cloud_pt.z = tmp_range * cos(laser_yaw) * sin(laser_pitch);
@@ -305,9 +290,6 @@
this->cloud_ch1->add(this->intensities[i]);
}
-
- publish("laser",this->laserMsg);
-
/***************************************************************/
/* */
/* point cloud from laser image */
@@ -331,7 +313,35 @@
//publish("shutter",this->shutterMsg);
}
+ /***************************************************************/
+ /* */
+ /* laser - base */
+ /* */
+ /***************************************************************/
+ if (this->myPR2->GetLaserRanges(PR2::LASER_BASE,
+ &angle_min, &angle_max, &angle_increment,
+ &range_max, &ranges_size , &ranges_alloc_size,
+ &intensities_size, &intensities_alloc_size,
+ this->ranges , this->intensities) == PR2::PR2_ALL_OK)
+ {
+ // Get latest laser data
+ this->laserMsg.angle_min = angle_min;
+ this->laserMsg.angle_max = angle_max;
+ this->laserMsg.angle_increment = angle_increment;
+ this->laserMsg.range_max = range_max;
+ this->laserMsg.set_ranges_size(ranges_size);
+ this->laserMsg.set_intensities_size(intensities_size);
+ for(unsigned int i=0;i<ranges_size;i++)
+ {
+ this->laserMsg.ranges[i] = this->ranges[i];
+ this->laserMsg.intensities[i] = this->intensities[i];
+ }
+ publish("laser",this->laserMsg);
+ }
+
+
+
/***************************************************************/
/* */
/* odometry */
@@ -391,13 +401,14 @@
/***************************************************************/
/* */
- /* pitch hokuyo */
+ /* pitching Hokuyo joint */
/* */
/***************************************************************/
static double dAngle = -1;
double simTime;
- double simPitchAngle,simPitchRate,simPitchTimeScale,simPitchAmp,simPitchOffset;
- simPitchTimeScale = 1.0;
+ double simPitchFreq,simPitchAngle,simPitchRate,simPitchTimeScale,simPitchAmp,simPitchOffset;
+ simPitchFreq = 1.0;
+ simPitchTimeScale = 2.0*M_PI*simPitchFreq;
simPitchAmp = M_PI / 8.0;
simPitchOffset = -M_PI / 8.0;
simPitchAngle = simPitchOffset + simPitchAmp * sin(simTime * simPitchTimeScale);
Modified: pkg/trunk/simulators/rosgazebo/world/pr2.model
===================================================================
--- pkg/trunk/simulators/rosgazebo/world/pr2.model 2008-06-10 06:05:09 UTC (rev 717)
+++ pkg/trunk/simulators/rosgazebo/world/pr2.model 2008-06-10 16:06:27 UTC (rev 718)
@@ -52,6 +52,133 @@
</body:box>
+
+ <!-- Hokuyo laser body -->
+ <canonicalBody>base_laser_body</canonicalBody>
+ <body:box name="base_laser_body">
+ <xyz>0.5 0.0 0.00</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+
+ <geom:box name="base_laser_box">
+ <xyz>0.0 0.0 0.02</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.05 0.05 0.041</size>
+ <mass>0.12</mass>
+
+ <visual>
+ <scale>0.05 0.05 0.041</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Blue</material>
+ </visual>
+
+ </geom:box>
+ <geom:cylinder name="base_laser_cylinder1">
+ <xyz>0.0 0.0 0.041</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.02 0.013</size>
+ <mass>0.02</mass>
+
+ <visual>
+ <rpy>0 0 0</rpy>
+ <scale>0.04 0.04 0.013</scale>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
+
+ </geom:cylinder>
+
+ <geom:cylinder name="base_laser_cylinder2">
+ <xyz>0.0 0.0 0.054</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.018 0.009</size>
+ <mass>0.02</mass>
+
+ <visual>
+ <rpy>0 0 0</rpy>
+ <size>0.036 0.036 0.009</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
+
+ </geom:cylinder>
+
+ <geom:cylinder name="base_laser_cylinder3">
+ <xyz>0.0 0.0 0.063</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.0175 0.008</size>
+ <mass>0.02</mass>
+
+ <visual>
+ <rpy>0 0 0</rpy>
+ <size>0.035 0.035 0.008</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Grey</material>
+ </visual>
+
+ </geom:cylinder>
+
+
+ <sensor:ray name="base_laser_1">
+ <rayCount>68</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
+ <origin>0.0 0.0 0.05</origin>
+
+ <minAngle>-135</minAngle>
+ <maxAngle>135</maxAngle>
+
+ <minRange>0.05</minRange>
+ <maxRange>10.0</maxRange>
+
+ <updateRate>10.0</updateRate>
+
+ <controller:sicklms200_laser name="base_laser_controller_1">
+ <interface:laser name="base_laser_iface_1"/>
+ <updateRate>10</updateRate>
+ </controller:sicklms200_laser>
+ </sensor:ray>
+ <!--
+ -->
+ <!--
+ <sensor:ray2 name="laser_1">
+ <rayCount>683</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
+ <origin>0.0 0.0 0.05</origin>
+
+ <minAngle>-135</minAngle>
+ <maxAngle>135</maxAngle>
+
+ <minRange>0.05</minRange>
+ <maxRange>10.0</maxRange>
+
+ <updateRate>10.0</updateRate>
+
+ <controller:sick2lms200_laser name="laser_controller_1">
+ <interface:laser name="laser_iface_1"/>
+ <updateRate>10</updateRate>
+ </controller:sick2lms200_laser>
+ </sensor:ray2>
+ -->
+
+ </body:box>
+
+ <!-- attach hokuyo to torso -->
+ <joint:hinge name="hokuyo_base_y_joint">
+ <body1>base_body</body1>
+ <body2>base_laser_body</body2>
+ <anchor>base_laser_body</anchor>
+ <axis> 0.0 1.0 0.0 </axis>
+ </joint:hinge>
+ <joint:hinge name="hokuyo_base_z_joint">
+ <body1>base_body</body1>
+ <body2>base_laser_body</body2>
+ <anchor>base_laser_body</anchor>
+ <axis> 0.0 0.0 1.0 </axis>
+ </joint:hinge>
+
+
+
<!-- neck body -->
<canonicalBody>neck_body</canonicalBody>
<body:box name="neck_body">
@@ -1253,13 +1380,13 @@
<sensor:ray name="laser_1">
- <rayCount>40</rayCount>
+ <rayCount>68</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
<origin>0.0 0.0 0.05</origin>
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
+ <minAngle>-45</minAngle>
+ <maxAngle>45</maxAngle>
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sac...@us...> - 2008-06-11 17:49:49
|
Revision: 743
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=743&view=rev
Author: sachinchitta
Date: 2008-06-11 10:49:55 -0700 (Wed, 11 Jun 2008)
Log Message:
-----------
) Fixed inverted axes in Gazebo
(b) Added test code for inverse kinematics, IK now works with a parameterization of the first angle. TODO: further testing across configuration space, addition of two other IK methods, better tuning of low-level controllers
(c) Added test code for individual joint tests
(d) Added base "steering" cmd to steer the wheels in the right direction before motion is initiated
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/Makefile
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_pr2API.cpp
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/simulators/gazebo_plugin/Makefile
pkg/trunk/simulators/gazebo_plugin/SConstruct
pkg/trunk/simulators/gazebo_plugin/manifest.xml
pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
pkg/trunk/simulators/rosgazebo/world/pr2.model
pkg/trunk/simulators/rosgazebo/world/robot.world
pkg/trunk/util/kinematics/libKinematics/include/libKinematics/kinematics.h
pkg/trunk/util/kinematics/libKinematics/src/ik.cpp
pkg/trunk/util/kinematics/libKinematics/src/kinematics.cpp
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/Makefile
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/Makefile 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/Makefile 2008-06-11 17:49:55 UTC (rev 743)
@@ -1,7 +1,7 @@
PKG = libpr2API
CXX = g++
-CFLAGS = -ggdb -g -Wall $(shell rospack export/cpp/cflags $(PKG))
+CFLAGS = -g -Wall $(shell rospack export/cpp/cflags $(PKG))
CDFLAGS = $(shell rospack export/cpp/cflags gazebo) \
$(shell rospack export/cpp/cflags pr2Core) \
@@ -30,7 +30,7 @@
$(LIBS): pr2API.o
$(CXX) $(CDFLAGS) -shared -o $@ $^ $(LDFLAGS)
-test_libpr2API: src/test/test_pr2API.cpp
+test_libpr2API: src/test/test_pr2API.cpp pr2API.o
$(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
clean:
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-11 17:49:55 UTC (rev 743)
@@ -30,8 +30,8 @@
//ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
//POSSIBILITY OF SUCH DAMAGE.
-#ifndef PR2_API_HH
-#define PR2_API_HH
+#ifndef PR2NEW_API_HH
+#define PR2NEW_API_HH
#include <newmat10/newmat.h>
#include <libKinematics/ik.h>
#include <pr2Core/pr2Core.h>
@@ -47,6 +47,7 @@
class PR2Robot
{
+
/*! \fn
\brief Constructor
*/
@@ -73,7 +74,7 @@
/*! \fn
\brief Set the joint control mode
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs
\param mode - mode for joint control, possible options are torque control, position control or speed control
*/
public: PR2_ERROR_CODE SetJointControlMode(PR2_JOINT_ID id, PR2_JOINT_CONTROL_MODE mode);
@@ -81,7 +82,7 @@
/*! \fn
\brief Get the joint control mode
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs
\param mode - pointer to return value (mode for joint control, possible values are torque control, position control or speed control)
*/
public: PR2_ERROR_CODE GetJointControlMode(PR2_JOINT_ID id, PR2_JOINT_CONTROL_MODE *mode);
@@ -89,7 +90,7 @@
/*! \fn
\brief Set the controller gains
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs
\param pGain - proportional gain
\param iGain - integral gain
\param dGain - derivative gain
@@ -99,7 +100,7 @@
/*! \fn
\brief Get the controller gains
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs
\param pGain - pointer to proportional gain
\param iGain - pointer to integral gain
\param dGain - pointer to derivative gain
@@ -109,29 +110,29 @@
/*! \fn
\brief Enable the joint
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs
*/
public: PR2_ERROR_CODE EnableJoint(PR2_JOINT_ID id);
/*! \fn
\brief Disable the joint
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs
*/
public: PR2_ERROR_CODE DisableJoint(PR2_JOINT_ID id);
/*! \fn
\brief Return value corresponding to whether the joint is enabled or not, 0 - disabled, 1 - enabled
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs
*/
public: PR2_ERROR_CODE IsEnabledJoint(PR2_JOINT_ID id, int *enabled);
/*! \fn
\brief Set particular parameters for the joint (NOT IMPLEMENTED YET)
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
- \param pId - parameter ID corresponding to the parameter for the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
+ \param pId - parameter ID corresponding to the parameter for the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param value - parameter value
*/
public: PR2_ERROR_CODE SetJointParams(PR2_JOINT_ID id, PR2_JOINT_PARAM_ID pId, double value);
@@ -139,15 +140,15 @@
/*! \fn
\brief Get particular parameters for the joint (NOT IMPLEMENTED YET)
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
- \param pId - parameter ID corresponding to the parameter for the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
+ \param pId - parameter ID corresponding to the parameter for the joint, see pr2Core.h for a list of joint IDs and parameter IDs
*/
public: PR2_ERROR_CODE GetJointParams(PR2_JOINT_ID id, PR2_JOINT_PARAM_ID pId, double *value);
/*! \fn
\brief Command a desired joint position and speed
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param jointPosition - desired joint position (in radians or meters)
\param jointSpeed - desired joint speed (in rad/s or m/s)
*/
@@ -156,7 +157,7 @@
/*! \fn
\brief Get the commanded joint position and speed values
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param jointPosition - pointer to desired joint position (in radians or meters)
\param jointSpeed - desired joint speed (in rad/s or m/s)
*/
@@ -164,7 +165,7 @@
/*! \fn
\brief Get the actual joint position and speed values
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param jointPosition - pointer to desired joint position (in radians or meters)
\param jointSpeed - desired joint speed (in rad/s or m/s)
*/
@@ -172,7 +173,7 @@
/*! \fn
\brief Get the actual joint position and speed values
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param jointPosition - pointer to desired joint position (in radians or meters)
\param jointSpeed - desired joint speed (in rad/s or m/s)
*/
@@ -180,35 +181,35 @@
/*! \fn
\brief Command a desired joint torque or force (for prismatic joints)
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param torque - desired torque (Nm or N)
*/
public: PR2_ERROR_CODE SetJointTorque(PR2_JOINT_ID id, double torque);
/*! \fn
\brief Get the commanded joint torque or force (for prismatic joints)
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param torque - pointer to return value for commanded torque (Nm or N)
*/
public: PR2_ERROR_CODE GetJointTorqueCmd(PR2_JOINT_ID id, double *torque);
/*! \fn
\brief Get the actual joint torque or force (for prismatic joints)
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param torque - pointer to return value for actual torque (Nm or N)
*/
public: PR2_ERROR_CODE GetJointTorqueActual(PR2_JOINT_ID id, double *torque);
/*! \fn
\brief Command a desired joint speed
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param torque - desired speed (rad/s or m/s)
*/
public: PR2_ERROR_CODE SetJointSpeed(PR2_JOINT_ID id, double speed);
/*! \fn
\brief Get the commanded joint speed
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param torque - desired speed (rad/s or m/s)
*/
public: PR2_ERROR_CODE GetJointSpeedCmd(PR2_JOINT_ID id, double *speed);
@@ -216,7 +217,7 @@
/*! \fn
\brief Get the actual joint speed
- \param id - jointId corresponding to the joint, set pr2Core.h for a list of joint IDs and parameter IDs
+ \param id - jointId corresponding to the joint, see pr2Core.h for a list of joint IDs and parameter IDs
\param torque - desired speed (rad/s or m/s)
*/
public: PR2_ERROR_CODE GetJointSpeedActual(PR2_JOINT_ID id, double *speed);
@@ -224,7 +225,7 @@
/*! \fn
\brief Set the control mode for the arm
- \param id - modelId corresponding to the arm, set pr2Core.h for a list of model IDs
+ \param id - modelId corresponding to the arm, see pr2Core.h for a list of model IDs
\param mode - two choices (joint control or cartesian control)
*/
public: PR2_ERROR_CODE SetArmControlMode(PR2_MODEL_ID id, PR2_CONTROL_MODE mode);
@@ -232,7 +233,7 @@
/*! \fn
\brief Get the control mode for the arm
- \param id - modelId corresponding to the arm, set pr2Core.h for a list of model IDs
+ \param id - modelId corresponding to the arm, see pr2Core.h for a list of model IDs
\param mode - pointer to return value for mode, two choices (joint control or cartesian control)
*/
public: PR2_ERROR_CODE GetArmControlMode(PR2_MODEL_ID id, PR2_CONTROL_MODE *mode);
@@ -240,7 +241,7 @@
/*! \fn
\brief Enable the model (i.e. enable all actuators corresponding to a particular part of the robot)
- \param id - modelID, set pr2Core.h for a list of model IDs
+ \param id - modelID, see pr2Core.h for a list of model IDs
*/
public: PR2_ERROR_CODE EnableModel(PR2_MODEL_ID id);
@@ -263,14 +264,14 @@
/*! \fn
\brief Enable the arm (i.e. enable all actuators corresponding to an arm)
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
*/
public: PR2_ERROR_CODE EnableArm(PR2_MODEL_ID id);
/*! \fn
\brief Enable the gripper (i.e. enable all actuators corresponding to the gripper)
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
*/
public: PR2_ERROR_CODE EnableGripper(PR2_MODEL_ID id);
@@ -289,7 +290,7 @@
/*! \fn
\brief Disable the model (i.e. disable all actuators corresponding to a particular part of the robot)
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
*/
public: PR2_ERROR_CODE DisableModel(PR2_MODEL_ID id);
@@ -312,14 +313,14 @@
/*! \fn
\brief Disable the arm (i.e. disable all actuators corresponding to an arm)
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
*/
public: PR2_ERROR_CODE DisableArm(PR2_MODEL_ID id);
/*! \fn
\brief Disable the gripper (i.e. disable all actuators corresponding to a gripper)
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
*/
public: PR2_ERROR_CODE DisableGripper(PR2_MODEL_ID id);
@@ -338,7 +339,7 @@
/*! \fn
\brief Check whether all actuators in a particular part of the robot have been enabled
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param enabled - pointer to return value - 0 if any actuator in that part of the robot is disabled, 1 if all actuators in that part of the robot are enabled
*/
public: PR2_ERROR_CODE IsEnabledModel(PR2_MODEL_ID id, int *enabled);
@@ -346,7 +347,7 @@
/*! \fn
\brief Command the arm to go to a particular position in joint space
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param jointPosition - array of desired positions of all the joints
\param jointSpeed - array of desired speeds of all the joints
*/
@@ -355,7 +356,7 @@
/*! \fn
\brief Get the commanded joint values for an arm
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param jointPosition - array of desired positions of all the joints
\param jointSpeed - array of desired speeds of all the joints
*/
@@ -363,8 +364,27 @@
/*! \fn
+ \brief Get the actual wrist pose
+ \param id - model ID, see pr2Core.h for a list of model IDs
+ \param *x pointer to return value of x position of the wrist
+ \param *y pointer to return value of y position of the wrist
+ \param *z pointer to return value of z position of the wrist
+ \param *roll pointer to return value of roll of the wrist
+ \param *pitch pointer to return value of pitch of the wrist
+ \param *yaw pointer to return value of yaw of the wrist
+ */
+ public: PR2_ERROR_CODE GetWristPoseGroundTruth(PR2_MODEL_ID id, double *x, double *y, double *z, double *roll, double *pitch, double *yaw);
+
+
+ /*! \fn
+ \brief Get the actual wrist pose
+ \param id - model ID, see pr2Core.h for a list of model IDs
+ */
+ public: NEWMAT::Matrix GetWristPoseGroundTruth(PR2_MODEL_ID id);
+
+ /*! \fn
\brief Get the actual joint values for an arm
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param jointPosition - array of desired positions of all the joints
\param jointSpeed - array of desired speeds of all the joints
*/
@@ -373,14 +393,14 @@
/*! \fn
\brief Command a desired torque for all the joints in an arm
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param torque - array of desired torques for all the joints
*/
public: PR2_ERROR_CODE SetArmJointTorque(PR2_MODEL_ID id, double torque[]);
/*! \fn
\brief Get the commanded torque for all the joints in an arm
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param torque - array of desired torques for all the joints
*/
public: PR2_ERROR_CODE GetArmJointTorqueCmd(PR2_MODEL_ID id, double torque[]);
@@ -388,7 +408,7 @@
/*! \fn
\brief Get the actual torque for all the joints in an arm
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param torque - array of actual torques for all the joints
*/
public: PR2_ERROR_CODE GetArmJointTorqueActual(PR2_MODEL_ID id, double torque[]);
@@ -397,7 +417,7 @@
/*! \fn
\brief Command the arm to a particular speed in joint space
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param speed - array of desired speeds for all the joints
*/
public: PR2_ERROR_CODE SetArmJointSpeed(PR2_MODEL_ID id, double speed[]);
@@ -405,14 +425,14 @@
/*! \fn
\brief Get the commanded joint speeds for the entire arm
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param speed - array of desired speeds for all the joints
*/
public: PR2_ERROR_CODE GetArmJointSpeedCmd(PR2_MODEL_ID id, double speed[]);
/*! \fn
\brief Get the actual joint speeds for the entire arm
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param speed - array of desired speeds for all the joints
*/
public: PR2_ERROR_CODE GetArmJointSpeedActual(PR2_MODEL_ID id, double speed[]);
@@ -420,18 +440,16 @@
/*! \fn
\brief Command the end-effector to go to a particular position in cartesian space
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param effectorPosition - array of desired position of the end-effector
\param effectorSpeed - array of desired speed of the end-effector
*/
//public: PR2_ERROR_CODE SetArmCartesianPosition(PR2_MODEL_ID id, double effectorPosition[], double effectorSpeed[]);
public: PR2_ERROR_CODE SetArmCartesianPosition(PR2_MODEL_ID id, NEWMAT::Matrix g);
-
-#if 0
/*! \fn
\brief Get the commanded position and speed for the end-effector
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param effectorPosition - array of desired position of the end-effector
\param effectorSpeed - array of desired speed of the end-effector
*/
@@ -439,54 +457,69 @@
/*! \fn
\brief Get the actual cartesian position and speed for the end-effector
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param effectorPosition - array of desired position of the end-effector
\param effectorSpeed - array of desired speed of the end-effector
*/
public: PR2_ERROR_CODE GetArmCartesianPositionActual(PR2_MODEL_ID id, double effectorPosition[], double effectorSpeed[]);
/*! \fn
+ \brief Forward Kinematics - Compute cartesian position and speed for the end-effector
+ \param id - model ID, see pr2Core.h for a list of model IDs
+ \param angles - joint angles
+ */
+ public: NEWMAT::Matrix ComputeArmForwardKinematics(PR2_MODEL_ID id, double angles[]);
+
+ /*! \fn
+ \brief Inverse Kinematics - Compute joint angles corresponding to end-effector position and orientation
+ \param id - model ID, see pr2Core.h for a list of model IDs
+ \param g - representation of end-effector position and orientation
+ */
+ public: NEWMAT::Matrix ComputeArmInverseKinematics(PR2_MODEL_ID id, NEWMAT::Matrix g);
+
+ /*! \fn
\brief Command a desired end-effector force
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param force - desired end effector force
*/
public: PR2_ERROR_CODE SetArmCartesianForce(PR2_MODEL_ID id, double force[]);
/*! \fn
\brief Get the commanded end-effector force
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param force - desired end effector force
*/
public: PR2_ERROR_CODE GetArmCartesianForceCmd(PR2_MODEL_ID id, double force[]);
/*! \fn
\brief Get the actual end-effector force
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param force - desired end effector force
*/
public: PR2_ERROR_CODE GetArmCartesianForceActual(PR2_MODEL_ID id, double force[]);
/*! \fn
\brief Command a desired speed in cartesian space
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param speed - desired end effector speed
*/
public: PR2_ERROR_CODE SetArmCartesianSpeed(PR2_MODEL_ID id, double speed[]);
/*! \fn
\brief Get the commanded speed in cartesian space
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param force - desired end effector speed
*/
public: PR2_ERROR_CODE GetArmCartesianSpeedCmd(PR2_MODEL_ID id, double speed[]);
+
/*! \fn
\brief Get the actual end-effector speed in cartesian space
- \param id - model ID, set pr2Core.h for a list of model IDs
+ \param id - model ID, see pr2Core.h for a list of model IDs
\param force - desired end effector speed
*/
public: PR2_ERROR_CODE GetArmCartesianSpeedActual(PR2_MODEL_ID id, double speed[]);
-#endif
+
/*! \fn
\brief Set a control mode for the base
\param mode - two choices - Cartesian or Joint control
@@ -510,6 +543,14 @@
public: PR2_ERROR_CODE SetBaseCartesianSpeedCmd(double vx, double vy, double vw);
/*! \fn
+ \brief Command a steering angle to achieve the desired speed. At the end of this maneuver the wheels will be at the steering angles required to achieve the desired combination of translational and rotational speeds.
+ \param vx - forward speed
+ \param vy - sideways speed
+ \param vw - rotational speed
+ */
+ public: PR2_ERROR_CODE SetBaseSteeringAngle(double vx, double vy, double vw);
+
+ /*! \fn
\brief Retrieve commanded speed for the base in cartesian space in body coordinates
\param vx - forward speed
\param vy - sideways speed
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-11 17:24:12 UTC (rev 742)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-11 17:49:55 UTC (rev 743)
@@ -1,9 +1,9 @@
+#include <libpr2API/pr2API.h>
+#include <math.h>
+
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
-#include <libpr2API/pr2API.h>
-#include <math.h>
-
using namespace gazebo;
using namespace PR2;
@@ -21,7 +21,10 @@
static gazebo::CameraIface *pr2CameraHeadLeftIface;
static gazebo::CameraIface *pr2CameraHeadRightIface;
+static gazebo::PositionIface *pr2LeftWristIface;
+static gazebo::PositionIface *pr2RightWristIface;
+
point Rot2D(point p,double theta)
{
point q;
@@ -109,6 +112,9 @@
pr2CameraHeadLeftIface = new gazebo::CameraIface();
pr2CameraHeadRightIface = new gazebo::CameraIface();
+ pr2LeftWristIface = new gazebo::PositionIface();
+ pr2RightWristIface = new gazebo::PositionIface();
+
int serverId = 0;
/// Connect to the libgazebo server
@@ -246,8 +252,29 @@
//return PR2_ERROR;
}
+ try
+ {
+ pr2LeftWristIface->Open(client, "p3d_left_wrist_position");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the left wrist interface\n"
+ << e << "\n";
+ pr2LeftWristIface = NULL;
+ }
- return PR2_ALL_OK;
+ try
+ {
+ pr2RightWristIface->Open(client, "p3d_right_wrist_position");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the right wrist interface\n"
+ << e << "\n";
+ pr2RightWristIface = NULL;
+ }
+
+ return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::CalibrateRobot()
@@ -340,9 +367,8 @@
pr2Iface->Unlock();
}
return PR2_ALL_OK;
-}
+};
-
PR2_ERROR_CODE PR2Robot::EnableModel(PR2_MODEL_ID id)
{
if(IsHead(id))
@@ -584,6 +610,7 @@
g(2,4) = g(2,4) - SPINE_LEFT_ARM_OFFSET.y;
g(3,4) = g(3,4) - SPINE_LEFT_ARM_OFFSET.z;
}
+
theta = 0;
theta = myArm.ComputeIK(g,0.1);
for(int jj = 1; jj <= 8; jj++)
@@ -594,6 +621,7 @@
break;
}
}
+ validSolution = 1;
if(validSolution <= 8)
{
for(int ii = 0; ii < 7; ii++)
@@ -606,6 +634,54 @@
return PR2_ALL_OK;
};
+NEWMAT::Matrix PR2Robot::ComputeArmInverseKinematics(PR2_MODEL_ID id, NEWMAT::Matrix g)
+{
+ NEWMAT::Matrix theta(8,8);
+ double angles[7], speeds[7];
+ int validSolution;
+
+ if (id == PR2_RIGHT_ARM)
+ {
+ g(1,4) = g(1,4) - SPINE_RIGHT_ARM_OFFSET.x;
+ g(2,4) = g(2,4) - SPINE_RIGHT_ARM_OFFSET.y;
+ g(3,4) = g(3,4) - SPINE_RIGHT_ARM_OFFSET.z;
+ }
+
+ if (id == PR2_LEFT_ARM)
+ {
+ g(1,4) = g(1,4) - SPINE_LEFT_ARM_OFFSET.x;
+ g(2,4) = g(2,4) - SPINE_LEFT_ARM_OFFSET.y;
+ g(3,4) = g(3,4) - SPINE_LEFT_ARM_OFFSET.z;
+ }
+
+ theta = 0;
+ theta = myArm.ComputeIK(g,0.1);
+ return theta;
+};
+
+
+
+NEWMAT::Matrix PR2Robot::ComputeArmForwardKinematics(PR2_MODEL_ID id, double angles[])
+{
+ NEWMAT::Matrix g = myArm.ComputeFK(angles);
+
+ if (id == PR2_RIGHT_ARM)
+ {
+ g(1,4) = g(1,4) + SPINE_RIGHT_ARM_OFFSET.x;
+ g(2,4) = g(2,4) + SPINE_RIGHT_ARM_OFFSET.y;
+ g(3,4) = g(3,4) + SPINE_RIGHT_ARM_OFFSET.z;
+ }
+
+ if (id == PR2_LEFT_ARM)
+ {
+ g(1,4) = g(1,4) + SPINE_LEFT_ARM_OFFSET.x;
+ g(2,4) = g(2,4) + SPINE_LEFT_ARM_OFFSET.y;
+ g(3,4) = g(3,4) + SPINE_LEFT_ARM_OFFSET.z;
+ }
+ return g;
+};
+
+
PR2_ERROR_CODE PR2Robot::SetBaseControlMode(PR2_CONTROL_MODE mode)
{
baseControlMode = mode;
@@ -851,7 +927,7 @@
PR2_ERROR_CODE PR2Robot::SetArmJointPosition(PR2_MODEL_ID id, double jointPosition[], double jointSpeed[])
{
- if (id != PR2_RIGHT_ARM || id != PR2_LEFT_ARM)
+ if (id != PR2_RIGHT_ARM && id != PR2_LEFT_ARM)
return PR2_ERROR;
pr2Iface->Lock(1);
@@ -8...
[truncated message content] |
|
From: <hsu...@us...> - 2008-06-11 21:04:19
|
Revision: 754
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=754&view=rev
Author: hsujohnhsu
Date: 2008-06-11 14:04:28 -0700 (Wed, 11 Jun 2008)
Log Message:
-----------
remove advertise and publishing of "laser" message so 2d nav stack works.
put map3.png in perspective directories so roslaunch works.
would like to launch gazebo with roslaunch too if possible.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
Added Paths:
-----------
pkg/trunk/nav/amcl_player/map3.png
pkg/trunk/nav/nav_view/map3.png
pkg/trunk/nav/wavefront_player/map3.png
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-06-11 21:02:39 UTC (rev 753)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-06-11 21:04:28 UTC (rev 754)
@@ -1,6 +1,7 @@
<launch>
<ns name="wg">
+ <!--node pkg="rosgazebo" type="gazebo" args="world/robot.world" respawn="false" /-->
<node pkg="rosgazebo" type="rosgazebo" args="" respawn="false" />
<node pkg="amcl_player" type="amcl_player" args="map3.png 0.1" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" args="map3.png 0.1" respawn="false" />
Added: pkg/trunk/nav/amcl_player/map3.png
===================================================================
--- pkg/trunk/nav/amcl_player/map3.png (rev 0)
+++ pkg/trunk/nav/amcl_player/map3.png 2008-06-11 21:04:28 UTC (rev 754)
@@ -0,0 +1 @@
+link ../../3rdparty/gazebo/gazebo-git/world/Media/materials/textures/map3.png
\ No newline at end of file
Property changes on: pkg/trunk/nav/amcl_player/map3.png
___________________________________________________________________
Name: svn:special
+ *
Added: pkg/trunk/nav/nav_view/map3.png
===================================================================
--- pkg/trunk/nav/nav_view/map3.png (rev 0)
+++ pkg/trunk/nav/nav_view/map3.png 2008-06-11 21:04:28 UTC (rev 754)
@@ -0,0 +1 @@
+link ../../3rdparty/gazebo/gazebo-git/world/Media/materials/textures/map3.png
\ No newline at end of file
Property changes on: pkg/trunk/nav/nav_view/map3.png
___________________________________________________________________
Name: svn:special
+ *
Added: pkg/trunk/nav/wavefront_player/map3.png
===================================================================
--- pkg/trunk/nav/wavefront_player/map3.png (rev 0)
+++ pkg/trunk/nav/wavefront_player/map3.png 2008-06-11 21:04:28 UTC (rev 754)
@@ -0,0 +1 @@
+link ../../3rdparty/gazebo/gazebo-git/world/Media/materials/textures/map3.png
\ No newline at end of file
Property changes on: pkg/trunk/nav/wavefront_player/map3.png
___________________________________________________________________
Name: svn:special
+ *
Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-11 21:02:39 UTC (rev 753)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-11 21:04:28 UTC (rev 754)
@@ -237,7 +237,7 @@
int
GazeboNode::SubscribeModels()
{
- advertise<std_msgs::LaserScan>("laser");
+ //advertise<std_msgs::LaserScan>("laser");
advertise<std_msgs::LaserScan>("scan");
advertise<std_msgs::RobotBase2DOdom>("odom");
advertise<std_msgs::Image>("image");
@@ -356,7 +356,7 @@
this->laserMsg.header.stamp.sec = (unsigned long)floor(simTime);
this->laserMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( simTime - this->laserMsg.header.stamp.sec) );
- publish("laser",this->laserMsg); // for laser_view FIXME: can alias this at the commandline or launch script
+ //publish("laser",this->laserMsg); // for laser_view FIXME: can alias this at the commandline or launch script
publish("scan",this->laserMsg); // for rosstage
}
@@ -379,8 +379,8 @@
// Get position
double x,y,th;
this->myPR2->GetBasePositionActual(&x,&y,&th);
- this->odomMsg.pos.x = x;
- this->odomMsg.pos.y = y;
+ this->odomMsg.pos.x = x + 256.5;
+ this->odomMsg.pos.y = y + 256.5;
this->odomMsg.pos.th = th;
// this->odomMsg.stall = this->positionmodel->Stall();
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