Menu
▾
▴
personalrobots-commit
|
From: <ge...@us...> - 2008-11-05 23:24:37
|
Revision: 6294
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6294&view=rev
Author: gerkey
Date: 2008-11-05 23:24:34 +0000 (Wed, 05 Nov 2008)
Log Message:
-----------
Added -g option to rosstage to disable gui. Added example launch file that
uses this option.
Modified Paths:
--------------
pkg/trunk/simulators/rosstage/rosstage.cc
Added Paths:
-----------
pkg/trunk/demos/2dnav_stage/2dnav_stage_2.5cm.xml
Added: pkg/trunk/demos/2dnav_stage/2dnav_stage_2.5cm.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_2.5cm.xml (rev 0)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_2.5cm.xml 2008-11-05 23:24:34 UTC (rev 6294)
@@ -0,0 +1,26 @@
+<launch>
+
+ <group name="wg">
+ <node pkg="rosstage" type="rosstage" args="-g $(find 2dnav_stage)/willow-pr2-2.5cm.world" respawn="false" output="screen"/>
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/willow-full-0.025.pgm 0.025" respawn="false" output="screen"/>
+ <param name="pf_laser_max_beams" value="6"/>
+ <param name="pf_min_samples" value="100"/>
+ <param name="pf_max_samples" value="1000"/>
+ <param name="pf_odom_drift_xx" value="0.1"/>
+ <param name="pf_odom_drift_yy" value="0.1"/>
+ <param name="pf_odom_drift_aa" value="0.1"/>
+ <param name="pf_odom_drift_xa" value="0.1"/>
+ <param name="pf_min_d" value="0.25"/> <!-- 25cm -->
+ <param name="pf_min_a" value="0.349"/> <!-- 20 degrees -->
+ <node pkg="amcl_player" type="amcl_player" respawn="false" output="screen">
+ <remap from="scan" to="base_scan" />
+ </node>
+ <param name="robot_radius" value="0.325"/>
+ <node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen">
+ <remap from="scan" to="base_scan" />
+ </node>
+ <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
+ <!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/> -->
+ </group>
+</launch>
+
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-11-05 23:23:49 UTC (rev 6293)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-11-05 23:24:34 UTC (rev 6294)
@@ -119,7 +119,7 @@
public:
// Constructor; stage itself needs argc/argv. fname is the .world file
// that stage should load.
- StageNode(int argc, char** argv, const char* fname);
+ StageNode(int argc, char** argv, bool gui, const char* fname);
~StageNode();
// Subscribe to models of interest. Currently, we find and subscribe
@@ -135,7 +135,7 @@
void cmdvelReceived();
// The main simulator object
- Stg::StgWorldGui* world;
+ Stg::StgWorld* world;
};
void
@@ -164,7 +164,7 @@
this->lock.unlock();
}
-StageNode::StageNode(int argc, char** argv, const char* fname) :
+StageNode::StageNode(int argc, char** argv, bool gui, const char* fname) :
ros::node("rosstage"),
tf(*this)
{
@@ -173,9 +173,12 @@
// initialize libstage
Stg::Init( &argc, &argv );
- //StgWorld world;
- this->world = new Stg::StgWorldGui(800, 700, "Stage (ROS)");
+ if(gui)
+ this->world = new Stg::StgWorldGui(800, 700, "Stage (ROS)");
+ else
+ this->world = new Stg::StgWorld();
+
this->world->Load(fname);
this->world->ForEachModel((GHFunc)ghfunc, this);
@@ -313,8 +316,15 @@
ros::init(argc,argv);
- StageNode sn(argc,argv,argv[1]);
+ bool gui = true;
+ for(int i=0;i<(argc-1);i++)
+ {
+ if(!strcmp(argv[i], "-g"))
+ gui = false;
+ }
+ StageNode sn(argc,argv,gui,argv[argc-1]);
+
if(sn.SubscribeModels() != 0)
exit(-1);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-11-06 00:29:53
|
Revision: 6298
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6298&view=rev
Author: stuglaser
Date: 2008-11-06 00:29:49 +0000 (Thu, 06 Nov 2008)
Log Message:
-----------
Added a controller for calibrating the gripper
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h
pkg/trunk/mechanism/mechanism_model/src/gripper_transmission.cpp
Added Paths:
-----------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
Removed Paths:
-------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_manual_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_manual_calibration_controller.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2008-11-06 00:08:12 UTC (rev 6297)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2008-11-06 00:29:49 UTC (rev 6298)
@@ -16,6 +16,7 @@
src/caster_controller.cpp
src/caster_calibration_controller.cpp
src/wrist_calibration_controller.cpp
+ src/gripper_calibration_controller.cpp
)
rospack_add_executable(testBaseController test/test_base_controller.cpp)
Copied: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h (from rev 6297, pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h)
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h 2008-11-06 00:29:49 UTC (rev 6298)
@@ -0,0 +1,128 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+/*
+ Example XML:
+ <controller type="GripperCalibrationController">
+ <calibrate joint="upperarm_roll_right_joint"
+ actuator="upperarm_roll_right_act"
+ transmission="upperarm_roll_right_trans"
+ velocity="0.3" />
+ <pid p="15" i="0" d="0" iClamp="0" />
+ </controller>
+
+*/
+
+
+#include "mechanism_model/robot.h"
+#include "robot_mechanism_controllers/joint_velocity_controller.h"
+#include "misc_utils/realtime_publisher.h"
+#include "std_msgs/Empty.h"
+
+namespace controller
+{
+
+class GripperCalibrationController : public Controller
+{
+public:
+ GripperCalibrationController();
+ ~GripperCalibrationController();
+
+ virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+ virtual void update();
+
+ bool calibrated() { return state_ == CALIBRATED; }
+ void beginCalibration() {
+ if (state_ == INITIALIZED)
+ state_ = BEGINNING;
+ }
+
+protected:
+
+ enum { INITIALIZED, BEGINNING, STARTING, CLOSING, CALIBRATED };
+ int state_;
+ int count_;
+
+ double search_velocity_;
+ Actuator *actuator_;
+ mechanism::JointState *joint_;
+
+ double init_time;
+
+ controller::JointVelocityController vc_; /** The joint velocity controller used to sweep the joint.*/
+};
+
+
+/***************************************************/
+/*! \class controller::GripperCalibrationControllerNode
+ \brief Joint Limit Controller ROS Node
+
+ This class starts and stops the initialization sequence
+
+*/
+/***************************************************/
+
+class GripperCalibrationControllerNode : public Controller
+{
+public:
+ /*!
+ * \brief Default Constructor
+ *
+ */
+ GripperCalibrationControllerNode();
+
+ /*!
+ * \brief Destructor
+ */
+ ~GripperCalibrationControllerNode();
+
+ void update();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+
+private:
+ mechanism::RobotState* robot_;
+ GripperCalibrationController c_;
+
+ double last_publish_time_;
+ misc_utils::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
+};
+
+}
+
+
Copied: pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp (from rev 6297, pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp)
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp 2008-11-06 00:29:49 UTC (rev 6298)
@@ -0,0 +1,192 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include "pr2_mechanism_controllers/gripper_calibration_controller.h"
+#include <ros/time.h>
+
+using namespace std;
+using namespace controller;
+
+GripperCalibrationController::GripperCalibrationController()
+: state_(INITIALIZED), joint_(NULL)
+{
+}
+
+GripperCalibrationController::~GripperCalibrationController()
+{
+}
+
+bool GripperCalibrationController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ assert(robot);
+ assert(config);
+
+ TiXmlElement *cal = config->FirstChildElement("calibrate");
+ if (!cal)
+ {
+ std::cerr<<"GripperCalibrationController was not given calibration parameters"<<std::endl;
+ return false;
+ }
+
+ if(cal->QueryDoubleAttribute("velocity", &search_velocity_) != TIXML_SUCCESS)
+ {
+ std::cerr<<"Velocity value was not specified\n";
+ return false;
+ }
+
+ const char *joint_name = cal->Attribute("joint");
+ joint_ = joint_name ? robot->getJointState(joint_name) : NULL;
+ if (!joint_)
+ {
+ fprintf(stderr, "Error: GripperCalibrationController could not find joint \"%s\"\n",
+ joint_name);
+ return false;
+ }
+
+ const char *actuator_name = cal->Attribute("actuator");
+ actuator_ = actuator_name ? robot->model_->getActuator(actuator_name) : NULL;
+ if (!actuator_)
+ {
+ fprintf(stderr, "Error: GripperCalibrationController could not find actuator \"%s\"\n",
+ actuator_name);
+ return false;
+ }
+
+ control_toolbox::Pid pid;
+ TiXmlElement *pid_el = config->FirstChildElement("pid");
+ if (!pid_el)
+ {
+ fprintf(stderr, "Error: GripperCalibrationController was not given a pid element.\n");
+ return false;
+ }
+ if (!pid.initXml(pid_el))
+ return false;
+
+ if (!vc_.init(robot, joint_name, pid))
+ return false;
+
+ return true;
+}
+
+void GripperCalibrationController::update()
+{
+ assert(joint_);
+ assert(actuator_);
+
+ switch (state_)
+ {
+ case INITIALIZED:
+ state_ = BEGINNING;
+ return;
+ case BEGINNING:
+ count_ = 0;
+ joint_->calibrated_ = false;
+ actuator_->state_.zero_offset_ = 0.0;
+ vc_.setCommand(search_velocity_);
+ state_ = STARTING;
+ break;
+ case STARTING:
+ // Makes sure we start moving for a bit before checking if we've stopped.
+ if (++count_ > 500)
+ {
+ count_ = 0;
+ state_ = CLOSING;
+ }
+ break;
+ case CLOSING:
+ if (fabs(joint_->velocity_) < 0.001)
+ {
+ actuator_->state_.zero_offset_ = actuator_->state_.position_;
+ state_ = CALIBRATED;
+ joint_->calibrated_ = true;
+ vc_.setCommand(0);
+ }
+ break;
+ case CALIBRATED:
+ break;
+ }
+
+ if (state_ != CALIBRATED)
+ vc_.update();
+}
+
+
+ROS_REGISTER_CONTROLLER(GripperCalibrationControllerNode)
+
+GripperCalibrationControllerNode::GripperCalibrationControllerNode()
+: robot_(NULL)
+{
+}
+
+GripperCalibrationControllerNode::~GripperCalibrationControllerNode()
+{
+}
+
+void GripperCalibrationControllerNode::update()
+{
+ c_.update();
+
+ if (c_.calibrated())
+ {
+ if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ {
+ assert(pub_calibrated_);
+ if (pub_calibrated_->trylock())
+ {
+ last_publish_time_ = robot_->hw_->current_time_;
+ pub_calibrated_->unlockAndPublish();
+ }
+ }
+ }
+}
+
+bool GripperCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ robot_ = robot;
+ ros::node *node = ros::node::instance();
+
+ std::string topic = config->Attribute("name") ? config->Attribute("name") : "";
+ if (topic == "")
+ {
+ fprintf(stderr, "No name given to GripperCalibrationController\n");
+ return false;
+ }
+
+ if (!c_.initXml(robot, config))
+ return false;
+
+ pub_calibrated_ = new misc_utils::RealtimePublisher<std_msgs::Empty>(topic + "/calibrated", 1);
+
+ return true;
+}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2008-11-06 00:08:12 UTC (rev 6297)
+++ pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2008-11-06 00:29:49 UTC (rev 6298)
@@ -16,6 +16,5 @@
src/joint_pd_controller.cpp
src/joint_calibration_controller.cpp
src/joint_blind_calibration_controller.cpp
- src/joint_manual_calibration_controller.cpp
- src/lqr_controller.cpp)
-
+ src/lqr_controller.cpp
+ )
\ No newline at end of file
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h 2008-11-06 00:08:12 UTC (rev 6297)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h 2008-11-06 00:29:49 UTC (rev 6298)
@@ -58,11 +58,10 @@
/***************************************************/
-#include "joint_manual_calibration_controller.h"
+#include "mechanism_model/robot.h"
+#include "robot_mechanism_controllers/joint_velocity_controller.h"
#include "misc_utils/realtime_publisher.h"
#include "std_msgs/Empty.h"
-// Services
-#include <robot_mechanism_controllers/CalibrateJoint.h>
namespace controller
@@ -111,22 +110,9 @@
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- /** DEPRECATED. Listen to /topic/calibrated instead
- *
- * \brief initializes the calibration procedure (blocking service)
- * This service starts the calibration sequence of the joint and waits to return until the calibration sequence is finished.
- *
- * @param req
- * @param resp
- * @return
- */
- bool calibrateCommand(robot_mechanism_controllers::CalibrateJoint::request &req,
- robot_mechanism_controllers::CalibrateJoint::response &resp);
-
private:
mechanism::RobotState* robot_;
JointCalibrationController c_;
- AdvertisedServiceGuard guard_calibrate_;
double last_publish_time_;
misc_utils::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_manual_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_manual_calibration_controller.h 2008-11-06 00:08:12 UTC (rev 6297)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_manual_calibration_controller.h 2008-11-06 00:29:49 UTC (rev 6298)
@@ -1,168 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-#pragma once
-
-/***************************************************/
-/*! \class controller::JointCalibratonController
- \brief Joint Controller that finds zerop point
- \author Timothy Hunter <tjh...@wi...>
-
-
- This class moves the joint and reads the value of the clibration_reading_ field to find the zero position of the joint. Once these are determined, these values
- * are passed to the joint and enable the joint for the other controllers.
-
- */
-/***************************************************/
-
-
-#include <ros/node.h>
-#include <mechanism_model/controller.h>
-#include <robot_mechanism_controllers/joint_velocity_controller.h>
-#include <mechanism_model/robot.h>
-#include <hardware_interface/hardware_interface.h>
-
-// Services
-#include <robot_mechanism_controllers/CalibrateJoint.h>
-
-//FIXME: the editor messed up the indentation
-namespace controller
-{
-
- class JointManualCalibrationController : public Controller
- {
- public:
- /*!
- * \brief Default Constructor.
- *
- */
- JointManualCalibrationController();
-
- /*!
- * \brief Destructor.
- */
- virtual ~JointManualCalibrationController();
-
- /*!
- * \brief Functional way to initialize limits and gains.
- *
- */
- virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- /*!
- * \brief Sets the joint in motion to find the reference position.
- *
- * There are two operating modes: manual and automatic.
- * The following algorithm is currently used in automatic mode: the controller sets a search direction for the reference point: positive direction if the current calibration reading is low, negative otherwie. It uses a velocity controller to move the joint in this direction and moves the joint until the calibration reading changes value. It then sets the offset filed in the related transmission accordingly.
- * In manual mode, the joint velocity controller is used to find the min and max limits by exploring the space at low speed.
- *
- */
- virtual void beginCalibration();
-
- virtual void endCalibration();
-
- bool getOffset(double & joint_angle);
-
- /** \brief Sets the offset of the joint
- */
- void setOffset(double joint_angle);
-
- /*!
- * \brief Issues commands to the joint. Should be called at regular intervals
- */
- virtual void update();
-
- bool calibrated() const { assert(joint_state_); return joint_state_->calibrated_; }
-
-
- protected:
-
- double offset(double act_pos, double joint_ref_pos);
-
- enum ControllerState {Stop,Search,Initialized,Begin,Idle} ;
-
- mechanism::Joint* joint_; /**< Joint we're controlling. */
- Actuator* actuator_; /** The actuator corresponding to the joint */
- mechanism::Robot *robot_; /**< Pointer to robot structure. */
- mechanism::Transmission * transmission_; /** The transmission associated to the actuator and the joint. */
- mechanism::JointState* joint_state_; /**< Joint we're controlling. */
-
- int state_; /** The current state of the controller*/
-
- double min_; // in actuator position
- double max_; // in actuator position
-
- ros::thread::mutex state_mutex_; /** Mutex locked during the calibration procedure to prevent lousy code from trying to find the offset while it is already searching. */
- };
-
-
- /***************************************************/
-/*! \class controller::JointCalibrationControllerNode
- \brief Joint Limit Controller ROS Node
-
- This class starts and stops the initialization sequence
-
- */
- /***************************************************/
-
- class JointManualCalibrationControllerNode : public Controller
- {
- public:
- /*!
- * \brief Default Constructor
- *
- */
- JointManualCalibrationControllerNode();
-
- /*!
- * \brief Destructor
- */
- virtual ~JointManualCalibrationControllerNode();
-
- void update();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- // Services
- bool beginCalibrationCommand(robot_mechanism_controllers::CalibrateJoint::request &req,
- robot_mechanism_controllers::CalibrateJoint::response &resp);
- bool endCalibrationCommand(robot_mechanism_controllers::CalibrateJoint::request &req,
- robot_mechanism_controllers::CalibrateJoint::response &resp);
-
- private:
- JointManualCalibrationController *c_;
- };
-}
-
-
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-11-06 00:08:12 UTC (rev 6297)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-11-06 00:29:49 UTC (rev 6298)
@@ -41,7 +41,7 @@
//ROS_REGISTER_CONTROLLER(JointCalibrationController)
JointCalibrationController::JointCalibrationController()
-: state_(INITIALIZED)
+ : state_(INITIALIZED), actuator_(NULL), joint_(NULL), transmission_(NULL)
{
}
@@ -196,34 +196,20 @@
}
}
-
- bool JointCalibrationControllerNode::calibrateCommand(robot_mechanism_controllers::CalibrateJoint::request &req, robot_mechanism_controllers::CalibrateJoint::response &resp)
-{
- c_.beginCalibration();
- ros::Duration d=ros::Duration(0,1000000);
- while(!c_.calibrated())
- d.sleep();
- resp.offset = 0;
- return true;
-}
-
bool JointCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
assert(robot);
robot_ = robot;
ros::node *node = ros::node::instance();
- std::string topic = config->Attribute("topic") ? config->Attribute("topic") : "";
+ std::string topic = config->Attribute("name") ? config->Attribute("name") : "";
if (topic == "")
{
- fprintf(stderr, "No topic given to JointCalibrationController\n");
+ fprintf(stderr, "No name given to JointCalibrationController\n");
return false;
}
-
if (!c_.initXml(robot, config))
return false;
- node->advertise_service(topic + "/calibrate", &JointCalibrationControllerNode::calibrateCommand, this);
- guard_calibrate_.set(topic + "/calibrate");
pub_calibrated_ = new misc_utils::RealtimePublisher<std_msgs::Empty>(topic + "/calibrated", 1);
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_manual_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_manual_calibration_controller.cpp 2008-11-06 00:08:12 UTC (rev 6297)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_manual_calibration_controller.cpp 2008-11-06 00:29:49 UTC (rev 6298)
@@ -1,243 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-#include <robot_mechanism_controllers/joint_calibration_controller.h>
-#include <ros/time.h>
-
-using namespace std;
-using namespace controller;
-
-ROS_REGISTER_CONTROLLER(JointManualCalibrationController)
-
-JointManualCalibrationController::JointManualCalibrationController()
- : joint_(NULL),
- actuator_(NULL),
- robot_(NULL),
- transmission_(NULL),
- joint_state_(NULL),
- state_(Idle)
-{
- std::cout<<"JointManualCalibration created\n";
-}
-
-JointManualCalibrationController::~JointManualCalibrationController()
-{
- std::cout<<"JointManualCalibration destroyed\n";
-}
-
-bool JointManualCalibrationController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- assert(robot);
- assert(config);
- robot_ = robot->model_;
-
- TiXmlElement *j = config->FirstChildElement("param");
- if (!j)
- {
- std::cerr<<"JointManualCalibrationController was not given parameters"<<std::endl;
- return false;
- }
-
- const char *joint_name = j->Attribute("joint_name");
- joint_ = joint_name ? robot_->getJoint(joint_name) : NULL;
- if (!joint_)
- {
- fprintf(stderr, "JointManualCalibrationController could not find joint named \"%s\"\n", joint_name);
- return false;
- }
-
- const int i = robot_->getJointIndex(joint_name);
- if(i<0)
- {
- std::cout<<"Could not find joint state\n";
- return false;
- }
- joint_state_ = &(robot->joint_states_[i]);
-
- const char *act_name = j->Attribute("actuator_name");
- actuator_ = act_name ? robot_->getActuator(act_name) : NULL;
- if(!actuator_)
- {
- std::cout<<"JointManualCalibrationController could not find an actuator called "<<act_name<<std::endl;
- return false;
- }
-
- const char *trans_name = j->Attribute("transmission_name");
- transmission_ = trans_name ? robot_->getTransmission(trans_name) : NULL;
- if(!transmission_)
- {
- std::cout<<"JointManualCalibrationController could not find a transmission called "<<trans_name<<std::endl;
- return false;
- }
-
- return true;
-}
-
-void JointManualCalibrationController::beginCalibration()
-{
- // Upward exploration
- if(state_mutex_.trylock())
- {
- state_ = Begin;
- std::cout<<"Starting calibration sequence "<<std::endl;
- }
- else
- std::cerr<<"JointManualCalibrationController"<<joint_->name_<<" : You tried to find the offset while it is already looking for it.\n";
-}
-
-void JointManualCalibrationController::endCalibration()
-{
- if(state_ != Search)
- {
- std::cerr<<"You tried to stop the calibration procedure while it is not searching\n";
- }
- state_ = Stop;
-}
-
-void JointManualCalibrationController::update()
-{
- assert(joint_state_);
- assert(actuator_);
-
- if(state_ == Begin)
- {
- joint_state_->calibrated_ = false;
- min_ = actuator_->state_.position_;
- max_ = actuator_->state_.position_;
- state_ = Search;
- }
-
- if(state_ == Search)
- {
- min_= std::min(min_, actuator_->state_.position_);
- max_= std::max(max_, actuator_->state_.position_);
- }
-
- if(state_ == Stop)
- {
- // Compute the offset:
- std::cout<<">>"<<min_<<" "<<max_<<std::endl;
- const double offset_min = offset(min_, joint_->joint_limit_min_);
- const double offset_max = offset(max_, joint_->joint_limit_max_);
- const double offset_avg = 0.5*(offset_min+offset_max);
- std::cout<<"Offset found: "<<offset_min<<'\t'<<offset_max<<'\t'<<offset_avg<<std::endl;
- actuator_->state_.zero_offset_=offset_avg;
- state_ = Initialized;
-
- }
-
- if(state_ == Initialized)
- {
- joint_state_->calibrated_ = true;
- state_mutex_.unlock();
- }
-}
-
-bool JointManualCalibrationController::getOffset(double & value)
-{
- value = actuator_->state_.zero_offset_;
- return state_ == Initialized;
-}
-
-double JointManualCalibrationController::offset(double act_pos, double joint_ref_pos)
-{
- std::cout<<"act_pos = "<<act_pos<<'\n';
- std::cout<<"ref_pos = "<<joint_ref_pos<<'\n';
- assert(transmission_);
-
- Actuator act;
- mechanism::JointState jState;
- std::vector<Actuator *> acts;
- std::vector<mechanism::JointState*> jStates;
- acts.push_back(&act);
- jStates.push_back(&jState);
-
- jState.position_ = joint_ref_pos;
-
- transmission_->prop...
[truncated message content] |
|
From: <mee...@us...> - 2008-11-06 23:12:12
|
Revision: 6337
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6337&view=rev
Author: meeussen
Date: 2008-11-06 23:12:06 +0000 (Thu, 06 Nov 2008)
Log Message:
-----------
Add first version of odometry estimation using EKF
Added Paths:
-----------
pkg/trunk/estimation/
pkg/trunk/estimation/robot_pose_ekf/
pkg/trunk/estimation/robot_pose_ekf/Makefile
pkg/trunk/estimation/robot_pose_ekf/manifest.xml
pkg/trunk/estimation/robot_pose_ekf/nonlinearanalyticconditionalgaussianodo.cpp
pkg/trunk/estimation/robot_pose_ekf/nonlinearanalyticconditionalgaussianodo.h
pkg/trunk/estimation/robot_pose_ekf/odom_estimation.cpp
pkg/trunk/estimation/robot_pose_ekf/odom_estimation.h
pkg/trunk/estimation/robot_pose_ekf/robot_pose_ekf.xml
pkg/trunk/estimation/rospack_nosubdirs
Added: pkg/trunk/estimation/robot_pose_ekf/Makefile
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/Makefile (rev 0)
+++ pkg/trunk/estimation/robot_pose_ekf/Makefile 2008-11-06 23:12:06 UTC (rev 6337)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/estimation/robot_pose_ekf/manifest.xml
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/manifest.xml (rev 0)
+++ pkg/trunk/estimation/robot_pose_ekf/manifest.xml 2008-11-06 23:12:06 UTC (rev 6337)
@@ -0,0 +1,9 @@
+<package>
+<description>Package to merge sensor data from odometry,IMU, VO, etc, into an estimate for the robot pose.</description>
+<author>Wim Meeussen</author>
+<license>BSD</license>
+<depend package="roscpp" />
+<depend package="bfl_latest" />
+<depend package="std_msgs" />
+<depend package="kdl" />
+</package>
Added: pkg/trunk/estimation/robot_pose_ekf/nonlinearanalyticconditionalgaussianodo.cpp
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/nonlinearanalyticconditionalgaussianodo.cpp (rev 0)
+++ pkg/trunk/estimation/robot_pose_ekf/nonlinearanalyticconditionalgaussianodo.cpp 2008-11-06 23:12:06 UTC (rev 6337)
@@ -0,0 +1,81 @@
+// Copyright (C) 2008 Wim Meeussen <meeussen at willowgarage com>
+//
+// This program 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 program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+
+#include "nonlinearanalyticconditionalgaussianodo.h"
+#include <wrappers/rng/rng.h> // Wrapper around several rng
+ // libraries
+#define NUMCONDARGUMENTS_MOBILE 2
+
+namespace BFL
+{
+ using namespace MatrixWrapper;
+
+
+ NonLinearAnalyticConditionalGaussianOdo::NonLinearAnalyticConditionalGaussianOdo(const Gaussian& additiveNoise)
+ : AnalyticConditionalGaussianAdditiveNoise(additiveNoise,NUMCONDARGUMENTS_MOBILE),
+ df(6,6)
+ {
+ // initialize df matrix
+ for (unsigned int i=1; i<=6; i++){
+ for (unsigned int j=1; j<=6; j++){
+ if (i==j) df(i,j) = 1;
+ else df(i,j) = 0;
+ }
+ }
+ }
+
+
+ NonLinearAnalyticConditionalGaussianOdo::~NonLinearAnalyticConditionalGaussianOdo(){}
+
+ ColumnVector NonLinearAnalyticConditionalGaussianOdo::ExpectedValueGet() const
+ {
+ ColumnVector state = ConditionalArgumentGet(0);
+ ColumnVector vel = ConditionalArgumentGet(1);
+ state(1) += cos(state(6)) * vel(1);
+ state(2) += sin(state(6)) * vel(1);
+ state(6) += vel(2);
+ return state + AdditiveNoiseMuGet();
+ }
+
+ Matrix NonLinearAnalyticConditionalGaussianOdo::dfGet(unsigned int i) const
+ {
+ if (i==0)//derivative to the first conditional argument (x)
+ {
+ double vel_trans = ConditionalArgumentGet(1)(1);
+ double yaw = ConditionalArgumentGet(0)(6);
+
+ df(1,3)=-vel_trans*sin(yaw);
+ df(2,3)= vel_trans*cos(yaw);
+
+ return df;
+ }
+ else
+ {
+ if (i >= NumConditionalArgumentsGet())
+ {
+ cerr << "This pdf Only has " << NumConditionalArgumentsGet() << " conditional arguments\n";
+ exit(-BFL_ERRMISUSE);
+ }
+ else{
+ cerr << "The df is not implemented for the" <<i << "th conditional argument\n";
+ exit(-BFL_ERRMISUSE);
+ }
+ }
+ }
+
+}//namespace BFL
+
Added: pkg/trunk/estimation/robot_pose_ekf/nonlinearanalyticconditionalgaussianodo.h
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/nonlinearanalyticconditionalgaussianodo.h (rev 0)
+++ pkg/trunk/estimation/robot_pose_ekf/nonlinearanalyticconditionalgaussianodo.h 2008-11-06 23:12:06 UTC (rev 6337)
@@ -0,0 +1,61 @@
+// Copyright (C) 2008 Wim Meeussen <meeussen at willowgarage com>
+//
+// This program 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 program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+
+
+#ifndef __NON_LINEAR_SYSTEM_CONDITIONAL_GAUSSIAN_ODO__
+#define __NON_LINEAR_SYSTEM_CONDITIONAL_GAUSSIAN_ODO__
+
+#include <pdf/analyticconditionalgaussian_additivenoise.h>
+
+namespace BFL
+{
+ /// Non Linear Conditional Gaussian
+ /**
+ - \f$ \mu = Matrix[1] . ConditionalArguments[0] +
+ Matrix[2]. ConditionalArguments[1] + ... + Noise.\mu \f$
+ - Covariance is independent of the ConditionalArguments, and is
+ the covariance of the Noise pdf
+ */
+ class NonLinearAnalyticConditionalGaussianOdo : public AnalyticConditionalGaussianAdditiveNoise
+ {
+ public:
+ /// Constructor
+ /** @pre: Every Matrix should have the same amount of rows!
+ This is currently not checked. The same goes for the number
+ of columns, which should be equal to the number of rows of
+ the corresponding conditional argument!
+ @param ratio: vector containing the different matrices of
+ the linear relationship between the conditional arguments
+ and \f$\mu\f$
+ @param additiveNoise Pdf representing the additive Gaussian uncertainty
+ */
+ NonLinearAnalyticConditionalGaussianOdo( const Gaussian& additiveNoise);
+
+ /// Destructor
+ virtual ~NonLinearAnalyticConditionalGaussianOdo();
+
+ // redefine virtual functions
+ virtual MatrixWrapper::ColumnVector ExpectedValueGet() const;
+ virtual MatrixWrapper::Matrix dfGet(unsigned int i) const;
+
+ private:
+ mutable MatrixWrapper::Matrix df;
+ };
+
+} // End namespace BFL
+
+#endif //
Added: pkg/trunk/estimation/robot_pose_ekf/odom_estimation.cpp
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/odom_estimation.cpp (rev 0)
+++ pkg/trunk/estimation/robot_pose_ekf/odom_estimation.cpp 2008-11-06 23:12:06 UTC (rev 6337)
@@ -0,0 +1,313 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "odom_estimation.h"
+
+
+using namespace MatrixWrapper;
+using namespace BFL;
+using namespace KDL;
+using namespace std;
+using namespace std_msgs;
+using namespace ros;
+
+
+namespace estimation
+{
+ // constructor
+ odom_estimation::odom_estimation()
+ : ros::node("odom_estimation"),
+ _vel_desi(2),
+ _filter_initialized(false),
+ _odom_initialized(false),
+ _imu_initialized(false)
+ {
+ // create SYSTEM MODEL
+ ColumnVector sysNoise_Mu(6); sysNoise_Mu = 0;
+ double sys_covar_trans, sys_covar_rot;
+ param("sys_covar_trans",sys_covar_trans,1000.0);
+ param("sys_covar_rot", sys_covar_rot, 1000.0);
+ SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;
+ sysNoise_Cov(1,1) = pow(sys_covar_trans,2);
+ sysNoise_Cov(2,2) = pow(sys_covar_trans,2);
+ sysNoise_Cov(3,3) = pow(sys_covar_trans,2);
+ sysNoise_Cov(4,4) = pow(sys_covar_rot,2);
+ sysNoise_Cov(5,5) = pow(sys_covar_rot,2);
+ sysNoise_Cov(6,6) = pow(sys_covar_rot,2);
+ Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
+ _sys_pdf = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty);
+ _sys_model = new AnalyticSystemModelGaussianUncertainty(_sys_pdf);
+
+
+ // create MEASUREMENT MODEL ODOM
+ ColumnVector measNoiseOdom_Mu(3); measNoiseOdom_Mu = 0;
+ double meas_odom_covar_trans, meas_odom_covar_rot;
+ param("meas_odom_covar_trans",meas_odom_covar_trans,0.0000001);
+ param("meas_odom_covar_rot", meas_odom_covar_rot, 0.0000001);
+ SymmetricMatrix measNoiseOdom_Cov(3); measNoiseOdom_Cov = 0;
+ measNoiseOdom_Cov(1,1) = pow(meas_odom_covar_trans,2);
+ measNoiseOdom_Cov(2,2) = pow(meas_odom_covar_trans,2);
+ measNoiseOdom_Cov(3,3) = pow(meas_odom_covar_rot,2);
+ Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);
+ Matrix Hodom(3,6); Hodom = 0;
+ Hodom(1,1) = 1; Hodom(2,2) = 1; Hodom(3,6) = 1;
+ _odom_meas_pdf = new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom);
+ _odom_meas_model = new LinearAnalyticMeasurementModelGaussianUncertainty(_odom_meas_pdf);
+
+
+ // create MEASUREMENT MODEL IMU
+ ColumnVector measNoiseImu_Mu(3); measNoiseImu_Mu = 0;
+ double meas_imu_covar_trans, meas_imu_covar_rot;
+ param("meas_imu_covar_trans",meas_imu_covar_trans,1.0);
+ param("meas_imu_covar_rot", meas_imu_covar_rot, 1.0);
+ SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
+ measNoiseImu_Cov(1,1) = pow(meas_imu_covar_trans,2);
+ measNoiseImu_Cov(2,2) = pow(meas_imu_covar_trans,2);
+ measNoiseImu_Cov(3,3) = pow(meas_imu_covar_rot,2);
+ Gaussian measurement_Uncertainty_Imu(measNoiseImu_Mu, measNoiseImu_Cov);
+ Matrix Himu(3,6); Himu = 0;
+ Himu(1,1) = 1; Himu(2,2) = 1; Himu(3,6) = 1;
+ _imu_meas_pdf = new LinearAnalyticConditionalGaussian(Himu, measurement_Uncertainty_Imu);
+ _imu_meas_model = new LinearAnalyticMeasurementModelGaussianUncertainty(_imu_meas_pdf);
+
+
+ // advertise our estimation
+ advertise<std_msgs::RobotBase2DOdom>("odom_estimation",10);
+
+ // subscribe to messages
+ subscribe("cmd_vel", _vel, &odom_estimation::vel_callback, 1);
+ subscribe("odom", _odom, &odom_estimation::odom_callback, 10);
+ subscribe("eular_angles", _imu, &odom_estimation::imu_callback, 10);
+
+ _odom_file.open("odom_file.txt");
+ _pred_file.open("pred_file.txt");
+ _corr_file.open("corr_file.txt");
+ _diff_file.open("diff_file.txt");
+ _sum_file.open("sum_file.txt");
+ _vel_file.open("vel_file.txt");
+ };
+
+
+
+ // destructor
+ odom_estimation::~odom_estimation(){
+ delete _filter;
+ delete _prior;
+ delete _odom_meas_model;
+ delete _odom_meas_pdf;
+ delete _imu_meas_model;
+ delete _imu_meas_pdf;
+ delete _sys_pdf;
+ delete _sys_model;
+
+ _vel_desi = 0;
+ _odom_file.close();
+ _pred_file.close();
+ _corr_file.close();
+ _diff_file.close();
+ _sum_file.close();
+ _vel_file.close();
+ };
+
+
+
+
+ // initialize prior density of filter with odom data
+ void odom_estimation::Initialize_filter(const Frame& fr, double time)
+ {
+ // set prior of filter
+ ColumnVector prior_Mu(6); prior_Mu = 0;
+ prior_Mu(1) = fr.p(0); prior_Mu(2) = fr.p(1); prior_Mu(3) = fr.p(2);
+ fr.M.GetRPY( prior_Mu(4), prior_Mu(5), prior_Mu(6));
+ SymmetricMatrix prior_Cov(6);
+ for (unsigned int i=1; i<=6; i++) {
+ for (unsigned int j=1; j<=6; j++){
+ if (i==j) prior_Cov(i,j) = 0;
+ else prior_Cov(i,j) = pow(0.001,2);
+ }
+ }
+ _prior = new Gaussian(prior_Mu,prior_Cov);
+ _filter = new ExtendedKalmanFilter(_prior);
+
+ // set timestamp of prior
+ _filter_time = time;
+
+ _filter_initialized = true;
+ }
+
+
+
+ // callback function for vel data
+ void odom_estimation::vel_callback()
+ {
+ // receive data
+ _vel_mutex.lock();
+ _vel_desi(1) = _vel.vx; _vel_desi(2) = _vel.vw;
+ _vel_mutex.unlock();
+
+ // write vel data to file
+ _vel_file << _vel_desi(1) << " " << _vel_desi(2) << endl;
+ };
+
+
+
+
+ // callback function for odom data
+ void odom_estimation::odom_callback()
+ {
+ // receive data
+ _filter_mutex.lock();
+ _odom_time = _odom.header.stamp.to_double();
+ _odom_frame = Frame(Rotation::RPY(0,0,_odom.pos.th), Vector(_odom.pos.x, _odom.pos.y, 0));
+
+ if (_filter_initialized && _odom_initialized){
+ OutputFrame(_odom_file, _odom_frame);
+
+ // predict the state up to the time of new odom measurement
+ _vel_mutex.lock();
+ _filter->Update(_sys_model, _vel_desi * (_odom_time - _filter_time));
+ //_vel_desi = 0; _filter->Update(_sys_model, _vel_desi);
+ _filter_time = _odom_time;
+ _vel_mutex.unlock();
+ Frame pred; GetState(pred);
+ OutputFrame(_pred_file, pred);
+
+ // update the state with new odom measurement
+ Frame meas_frame = _odom_estimation * _odom_frame_old.Inverse() * _odom_frame;
+ OutputFrame(_diff_file, _odom_frame_old.Inverse() * _odom_frame);
+ ColumnVector meas_vec(3); double tmp;
+ meas_vec(1) = meas_frame.p(0); meas_vec(2) = meas_frame.p(1); meas_frame.M.GetRPY(tmp,tmp,meas_vec(3));
+
+ _filter->Update(_odom_meas_model, meas_vec);
+ Frame corr; GetState(corr);
+ OutputFrame(_corr_file, corr);
+
+ _odom_sum_diff = _odom_sum_diff * _odom_frame_old.Inverse() * _odom_frame;
+ OutputFrame(_sum_file, _odom_sum_diff);
+
+ // publish state
+ GetState(_output);
+ publish("odom_estimation", _output);
+ // remember last state estimate with odom measurement
+ GetState(_odom_estimation);
+ }
+
+ // initialize filter with odom data
+ if (!_filter_initialized) Initialize_filter(_odom_frame, _odom_time);
+
+ // we received first odom data
+ if (!_odom_initialized && _filter_initialized){
+ GetState(_odom_estimation);
+ _odom_sum_diff = _odom_estimation;
+ _odom_initialized = true;
+ }
+
+ // store last frame
+ _odom_frame_old = _odom_frame;
+
+ _filter_mutex.unlock();
+ };
+
+
+
+ // callback function for imu data
+ void odom_estimation::imu_callback()
+ {
+ // receive data
+ _filter_mutex.lock();
+ _imu_time = _imu.header.stamp.to_double();
+ _imu_frame = Frame(Rotation::RPY(_imu.roll, _imu.pitch, _imu.yaw), Vector(0,0,0));
+
+ // we received first imu data
+ if (!_imu_initialized) _imu_initialized = true;
+
+ // store last frame
+ _imu_frame_old = _imu_frame;
+ _filter_mutex.unlock();
+ };
+
+
+
+ void odom_estimation::OutputFrame(ofstream& f, const Frame& fr)
+ {
+ double r,p,y;
+ fr.M.GetRPY(r,p,y);
+ f << fr.p(0) << " " << fr.p(1) << " " << fr.p(2) << " " << r << " " << p << " " << y << endl;
+ };
+
+
+
+ // get filter state as frame
+ void odom_estimation::GetState(Frame& f)
+ {
+ ColumnVector estimation = _filter->PostGet()->ExpectedValueGet();
+ f = Frame(Rotation::RPY(estimation(4), estimation(5), estimation(6)),
+ Vector(estimation(1), estimation(2), estimation(3)));
+ };
+
+
+ // return filter state as pose
+ void odom_estimation::GetState(PoseStamped& p)
+ {
+ ColumnVector estimation = _filter->PostGet()->ExpectedValueGet();
+ p.pose.position.x = estimation(1); p.pose.position.y = estimation(2); p.pose.position.z = estimation(3);
+ p.pose.orientation.x = estimation(4); p.pose.orientation.y = estimation(5); p.pose.orientation.z = estimation(6);
+ p.pose.orientation.w = 1; p.header.stamp.from_double(_filter_time);
+ };
+
+
+
+
+}; // namespace
+
+
+
+using namespace estimation;
+
+
+int main(int argc, char **argv)
+{
+ // Initialize ROS
+ ros::init(argc, argv);
+
+ // create filter class
+ odom_estimation my_filter;
+
+ // wait for filter to finish
+ my_filter.spin();
+
+ // Clean up
+ ros::fini();
+ return 0;
+}
Added: pkg/trunk/estimation/robot_pose_ekf/odom_estimation.h
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/odom_estimation.h (rev 0)
+++ pkg/trunk/estimation/robot_pose_ekf/odom_estimation.h 2008-11-06 23:12:06 UTC (rev 6337)
@@ -0,0 +1,127 @@
+/*********************************************************************
+* 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:
+*
+* 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 __ODOM_ESTIMATION__
+#define __ODOM_ESTIMATION__
+
+// ros stuff
+#include "ros/node.h"
+#include "kdl/frames.hpp"
+
+// messages
+#include "std_msgs/RobotBase2DOdom.h"
+#include "std_msgs/EulerAngles.h"
+#include "std_msgs/BaseVel.h"
+#include "std_msgs/PoseStamped.h"
+
+// bayesian fitlering
+#include "filter/extendedkalmanfilter.h"
+#include "model/linearanalyticsystemmodel_gaussianuncertainty.h"
+#include "model/linearanalyticmeasurementmodel_gaussianuncertainty.h"
+#include "pdf/analyticconditionalgaussian.h"
+#include "pdf/linearanalyticconditionalgaussian.h"
+#include "nonlinearanalyticconditionalgaussianodo.h"
+
+// log files
+#include <fstream>
+
+namespace estimation
+{
+
+class odom_estimation: public ros::node
+{
+public:
+ // constructor
+ odom_estimation();
+
+ // destructor
+ virtual ~odom_estimation();
+
+ // callback function for vel data
+ void vel_callback();
+
+ // callback function for odo data
+ void odom_callback();
+
+ // callback function for imu data
+ void imu_callback();
+
+private:
+
+ // initialize filter in first step
+ void Initialize_filter(const KDL::Frame& fr, double time);
+
+ // get filter state
+ void GetState(KDL::Frame& f);
+ void GetState(std_msgs::PoseStamped& p);
+
+ void OutputFrame(std::ofstream& f, const KDL::Frame& fr);
+
+ // messages to receive
+ std_msgs::BaseVel _vel;
+ std_msgs::RobotBase2DOdom _odom;
+ std_msgs::EulerAngles _imu;
+
+ // estimated robot pose message to send
+ std_msgs::PoseStamped _output;
+
+ // pdf / model / filter
+ BFL::AnalyticSystemModelGaussianUncertainty* _sys_model;
+ BFL::NonLinearAnalyticConditionalGaussianOdo* _sys_pdf;
+ BFL::LinearAnalyticConditionalGaussian* _odom_meas_pdf;
+ BFL::LinearAnalyticMeasurementModelGaussianUncertainty* _odom_meas_model;
+ BFL::LinearAnalyticConditionalGaussian* _imu_meas_pdf;
+ BFL::LinearAnalyticMeasurementModelGaussianUncertainty* _imu_meas_model;
+ BFL::Gaussian* _prior;
+ BFL::ExtendedKalmanFilter* _filter;
+
+ MatrixWrapper::ColumnVector _vel_desi;
+ KDL::Frame _odom_estimation, _odom_frame, _odom_frame_old,_imu_frame, _imu_frame_old, _odom_sum_diff;
+
+ double _odom_time, _imu_time, _filter_time;
+ bool _filter_initialized, _odom_initialized, _imu_initialized;
+
+ // mutex
+ ros::thread::mutex _vel_mutex, _filter_mutex;
+
+ // log files for debugging
+ std::ofstream _odom_file, _pred_file,_corr_file, _vel_file, _diff_file, _sum_file
+;
+
+
+
+}; // class
+
+}; // namespace
+
+#endif
Added: pkg/trunk/estimation/robot_pose_ekf/robot_pose_ekf.xml
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/robot_pose_ekf.xml (rev 0)
+++ pkg/trunk/estimation/robot_pose_ekf/robot_pose_ekf.xml 2008-11-06 23:12:06 UTC (rev 6337)
@@ -0,0 +1,12 @@
+<launch>
+ <master auto="start"/>
+
+ <param name="odom_estimation/sys_covar_trans" type="double" value="0.01"/>
+ <param name="odom_estimation/sys_covar_rot" type="double" value="0.01"/>
+ <param name="odom_estimation/meas_odom_covar_trans" type="double" value="0.005"/>
+ <param name="odom_estimation/meas_odom_covar_rot" type="double" value="0.005"/>
+ <param name="odom_estimation/meas_imu_covar_trans" type="double" value="0.01"/>
+ <param name="odom_estimation/meas_imu_covar_rot" type="double" value="0.01"/>
+
+ <node pkg="robot_pose_ekf" type="odom_estimation" output="screen"/>
+</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-11-06 23:59:00
|
Revision: 6354
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6354&view=rev
Author: tfoote
Date: 2008-11-06 23:58:56 +0000 (Thu, 06 Nov 2008)
Log Message:
-----------
adding intrepid manifest changes
Modified Paths:
--------------
pkg/trunk/3rdparty/scipy/manifest.xml
pkg/trunk/grasp_module/grasp_learner/manifest.xml
Modified: pkg/trunk/3rdparty/scipy/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/scipy/manifest.xml 2008-11-06 23:57:29 UTC (rev 6353)
+++ pkg/trunk/3rdparty/scipy/manifest.xml 2008-11-06 23:58:56 UTC (rev 6354)
@@ -18,7 +18,9 @@
</export>
<sysdepend os="ubuntu" version="7.04-feisty" package="wget"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="wget"/>
+<sysdepend os="ubuntu" version="8.10-intrepid" package="wget"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="atlas3-sse2-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="atlas3-sse2-dev"/>
+<sysdepend os="ubuntu" version="8.10-intrepid" package="libatlas-sse2-dev"/>
</package>
Modified: pkg/trunk/grasp_module/grasp_learner/manifest.xml
===================================================================
--- pkg/trunk/grasp_module/grasp_learner/manifest.xml 2008-11-06 23:57:29 UTC (rev 6353)
+++ pkg/trunk/grasp_module/grasp_learner/manifest.xml 2008-11-06 23:58:56 UTC (rev 6354)
@@ -16,4 +16,6 @@
</export>
<sysdepend os="ubuntu" version="8.04-hardy" package="refblas3-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="lapack3-dev"/>
+<sysdepend os="ubuntu" version="8.10-intrepid" package="liblapack-dev"/>
+<sysdepend os="ubuntu" version="8.10-intrepid" package="libblas-dev"/>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-11-07 06:40:38
|
Revision: 6371
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6371&view=rev
Author: tfoote
Date: 2008-11-07 06:40:35 +0000 (Fri, 07 Nov 2008)
Log Message:
-----------
removing extrapolation from tf constructors
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
pkg/trunk/nav/slam_gmapping/src/tftest.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/util/tf/include/tf/tf.h
pkg/trunk/util/tf/include/tf/transform_listener.h
pkg/trunk/util/tf/src/tf.cpp
pkg/trunk/util/tf/test/tf_unittest_future.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2008-11-07 06:40:35 UTC (rev 6371)
@@ -160,7 +160,7 @@
ROS_REGISTER_CONTROLLER(HeadPanTiltControllerNode)
HeadPanTiltControllerNode::HeadPanTiltControllerNode()
-: Controller(), node_(ros::node::instance()), TF(*ros::node::instance(),false, 10000000000ULL, 1000000000ULL)
+: Controller(), node_(ros::node::instance()), TF(*ros::node::instance(),false, 10000000000ULL)
{
c_ = new HeadPanTiltController();
}
Modified: pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
===================================================================
--- pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp 2008-11-07 06:40:35 UTC (rev 6371)
@@ -56,7 +56,8 @@
/// @todo Disable extrapolation, and implement scan-buffering. This is
/// not urgent for a robot like the PR2, where odometry is being
/// published several times faster than laser scans.
- tf_ = new tf::TransformListener(*node_, true, 10000000000ULL, 200000000ULL);
+ tf_ = new tf::TransformListener(*node_, true, 10000000000ULL);
+ tf_->setExtrapolationLimit((int64_t) 200000000ULL );
ROS_ASSERT(tf_);
gsp_laser_ = NULL;
Modified: pkg/trunk/nav/slam_gmapping/src/tftest.cpp
===================================================================
--- pkg/trunk/nav/slam_gmapping/src/tftest.cpp 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/nav/slam_gmapping/src/tftest.cpp 2008-11-07 06:40:35 UTC (rev 6371)
@@ -9,7 +9,8 @@
{
node_ = new ros::node("test");
tf_ = new tf::TransformListener(*node_, true,
- 10000000000ULL, 200000000ULL);
+ 10000000000ULL);
+ tf_->setExtrapolationLimit( ros::Duration((int64_t)200000000ULL));
node_->subscribe("base_scan", scan_, &Test::laser_cb, this, -1);
}
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-11-07 06:40:35 UTC (rev 6371)
@@ -289,7 +289,7 @@
avmax(DTOR(80.0)),
amin(DTOR(10.0)),
amax(DTOR(40.0)),
- tf(*this, true, 10000000000ULL, 0)// cache for 10 sec, no extrapolation
+ tf(*this, true, 10000000000ULL)// cache for 10 sec, no extrapolation
//tf(*this, true, 200000000ULL, 200000000ULL) //nanoseconds
{
// Initialize global pose. Will be set in control loop based on actual data.
@@ -376,8 +376,8 @@
/* this->tf.setWithEulers("base_laser",
"base",
laser_x_offset, 0.0, 0.0, 0.0, 0.0, 0.0, 0);*/
- this->tf.setTransform(tf::Stamped<btTransform>(btTransform(btQuaternion(0,0,0), btVector3(laser_x_offset, 0,0)), 0, "base_laser", "base"));
- this->tf.setTransform(tf::Stamped<btTransform>(btTransform(btQuaternion(0,0,0), btVector3(laser_x_offset, 0,0)), 0, "map", "other"));///\todo fixme hack to get around short list edge case
+ this->tf.setTransform(tf::Stamped<btTransform>(btTransform(btQuaternion(0,0,0), btVector3(laser_x_offset, 0,0)), ros::Time(0ULL), "base_laser", "base"));
+ this->tf.setTransform(tf::Stamped<btTransform>(btTransform(btQuaternion(0,0,0), btVector3(laser_x_offset, 0,0)), ros::Time(0ULL), "map", "other"));///\todo fixme hack to get around short list edge case
advertise<std_msgs::Planner2DState>("state",1);
@@ -611,7 +611,7 @@
tf::Stamped<tf::Pose> robotPose;
robotPose.setIdentity();
robotPose.frame_id_ = "base";
- robotPose.stamp_ = 0; // request most recent pose
+ robotPose.stamp_ = ros::Time(0ULL); // request most recent pose
//robotPose.time = laserMsg.header.stamp.sec * 1000000000ULL +
// laserMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else
try
Modified: pkg/trunk/util/tf/include/tf/tf.h
===================================================================
--- pkg/trunk/util/tf/include/tf/tf.h 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/util/tf/include/tf/tf.h 2008-11-07 06:40:35 UTC (rev 6371)
@@ -85,11 +85,10 @@
/** Constructor
* \param interpolating Whether to interpolate, if this is false the closest value will be returned
* \param cache_time How long to keep a history of transforms in nanoseconds
- * \param max_extrapolation_distance How far to extrapolate before throwing an exception
+ *
*/
Transformer(bool interpolating = true,
- ros::Duration cache_time_ = ros::Duration(DEFAULT_CACHE_TIME),
- ros::Duration max_extrapolation_distance_ = ros::Duration(DEFAULT_MAX_EXTRAPOLATION_DISTANCE));
+ ros::Duration cache_time_ = ros::Duration(DEFAULT_CACHE_TIME));
virtual ~Transformer(void);
/** \brief Clear all data */
@@ -167,6 +166,11 @@
* Returns true unless "NO_PARENT" */
bool getParent(const std::string& frame_id, ros::Time time, std::string& parent);
+ /**@brief Set the distance which tf is allow to extrapolate
+ * \param distance How far to extrapolate before throwing an exception
+ * default is zero */
+ void setExtrapolationLimit(const ros::Duration& distance);
+
protected:
/** \brief The internal storage class for ReferenceTransform.
Modified: pkg/trunk/util/tf/include/tf/transform_listener.h
===================================================================
--- pkg/trunk/util/tf/include/tf/transform_listener.h 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/util/tf/include/tf/transform_listener.h 2008-11-07 06:40:35 UTC (rev 6371)
@@ -55,13 +55,15 @@
rosTF::TransformArray tfArrayIn;
public:
+ /**@brief Constructor for transform listener
+ * \param rosnode A reference to an instance of a ros::node for communication
+ * \param interpolating Whether to interpolate or return the closest
+ * \param max_cache_time How long to store transform information */
TransformListener(ros::node & rosnode,
- bool interpolating = true,
- int64_t max_cache_time = DEFAULT_CACHE_TIME,
- int64_t max_extrapolation_distance = DEFAULT_MAX_EXTRAPOLATION_DISTANCE):
+ bool interpolating = true,
+ int64_t max_cache_time = DEFAULT_CACHE_TIME):
Transformer(interpolating,
- max_cache_time,
- max_extrapolation_distance),
+ max_cache_time),
node_(rosnode)
{
// printf("Constructed rosTF\n");
Modified: pkg/trunk/util/tf/src/tf.cpp
===================================================================
--- pkg/trunk/util/tf/src/tf.cpp 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/util/tf/src/tf.cpp 2008-11-07 06:40:35 UTC (rev 6371)
@@ -34,11 +34,10 @@
using namespace tf;
Transformer::Transformer(bool interpolating,
- ros::Duration cache_time,
- ros::Duration max_extrapolation_distance):
+ ros::Duration cache_time):
cache_time(cache_time),
interpolating (interpolating),
- max_extrapolation_distance_(max_extrapolation_distance)
+ max_extrapolation_distance_(DEFAULT_MAX_EXTRAPOLATION_DISTANCE)
{
frameIDs_["NO_PARENT"] = 0;
frames_.push_back(NULL);// new TimeCache(interpolating, cache_time, max_extrapolation_distance));//unused but needed for iteration over all elements
@@ -130,6 +129,10 @@
};
+void Transformer::setExtrapolationLimit(const ros::Duration& distance)
+{
+ max_extrapolation_distance_ = distance;
+}
TransformLists Transformer::lookupLists(unsigned int target_frame, ros::Time time, unsigned int source_frame)
Modified: pkg/trunk/util/tf/test/tf_unittest_future.cpp
===================================================================
--- pkg/trunk/util/tf/test/tf_unittest_future.cpp 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/util/tf/test/tf_unittest_future.cpp 2008-11-07 06:40:35 UTC (rev 6371)
@@ -17,7 +17,7 @@
TEST(tf, NoExtrapolationExceptionFromParent)
{
- tf::Transformer mTR(true, ros::Duration((int64_t)1000000LL), ros::Duration((int64_t)0LL));
+ tf::Transformer mTR(true, ros::Duration((int64_t)1000000LL));
@@ -54,7 +54,7 @@
TEST(tf, ExtrapolationFromOneValue)
{
- tf::Transformer mTR(true, ros::Duration((int64_t)1000000LL), ros::Duration((int64_t)0LL));
+ tf::Transformer mTR(true, ros::Duration((int64_t)1000000LL));
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp 2008-11-07 04:23:22 UTC (rev 6370)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp 2008-11-07 06:40:35 UTC (rev 6371)
@@ -86,7 +86,8 @@
}
ROS_ASSERT( ros_node_ );
- tf_ = new tf::TransformListener( *ros_node_, true, 10000000000ULL, 1000000000ULL );
+ tf_ = new tf::TransformListener( *ros_node_, true, 10000000000ULL);
+ tf_->setExtrapolationLimit( ros::Duration((int64_t)1000000000ULL) );
scene_manager_ = ogre_root_->createSceneManager( Ogre::ST_GENERIC );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-11-07 09:18:11
|
Revision: 6372
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6372&view=rev
Author: tfoote
Date: 2008-11-07 09:18:06 +0000 (Fri, 07 Nov 2008)
Log Message:
-----------
all changes necessary for roscpp buildtest with ros::Time(double)
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
pkg/trunk/grasp_module/grasp_learner/src/voxel_processor.cpp
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/laser_pose_interpolator/gen_carmen_laser.cpp
pkg/trunk/util/logging/include/logging/LogPlayer.h
pkg/trunk/util/logging/src/checklog/checklog.cpp
pkg/trunk/util/logging/src/demo/demo.cpp
pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp
pkg/trunk/util/logsetta/imu/imu_extract.cpp
pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp
pkg/trunk/util/logsetta/odom/odom_extract.cpp
pkg/trunk/util/tf/src/cache.cpp
pkg/trunk/util/tf/test/tf_unittest.cpp
pkg/trunk/vision/calib_converter/src/calib_converter.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp
pkg/trunk/visualization/scene_labeler/src/scene_labeler/scene_labeler_stereo.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -279,7 +279,7 @@
try
{
- TF.lookupTransform(point.frame_id_,"head_pan",0,frame);
+ TF.lookupTransform(point.frame_id_,"head_pan",ros::Time(0.0),frame);
}
catch(tf::TransformException& ex)
{
Modified: pkg/trunk/grasp_module/grasp_learner/src/voxel_processor.cpp
===================================================================
--- pkg/trunk/grasp_module/grasp_learner/src/voxel_processor.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/grasp_module/grasp_learner/src/voxel_processor.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -68,7 +68,7 @@
virtual void processData() = 0;
virtual bool loadData(std::string filename) {
- if (!mLp.open(filename, ros::Time(0))) return false;
+ if (!mLp.open(filename, ros::Time(0.0))) return false;
mLp.addHandler<OctreeLearningMsg>(std::string("grasp_learning_bus"),
©Msg<OctreeLearningMsg>, (void*)this, true);
mNewMsg = false;
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-11-07 09:18:06 UTC (rev 6372)
@@ -741,7 +741,7 @@
{
tf::Stamped<tf::Pose> ident (btTransform(btQuaternion(0,0,0),
btVector3(0,0,0)),
- 0, laserMsg.header.frame_id);
+ ros::Time((uint64_t)0ull), laserMsg.header.frame_id);
tf::Stamped<btTransform> laser_pose;
try
{
Modified: pkg/trunk/nav/laser_pose_interpolator/gen_carmen_laser.cpp
===================================================================
--- pkg/trunk/nav/laser_pose_interpolator/gen_carmen_laser.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/nav/laser_pose_interpolator/gen_carmen_laser.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -94,7 +94,7 @@
for (int i = 1; i < argc; i++)
files.push_back(argv[i]);
- player.open(files, ros::Time(0));
+ player.open(files, ros::Time(0.0));
player.addHandler<laser_pose_interpolator::PoseLaserScan>(string("pose_scan"), &scan_callback, NULL);
Modified: pkg/trunk/util/logging/include/logging/LogPlayer.h
===================================================================
--- pkg/trunk/util/logging/include/logging/LogPlayer.h 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/util/logging/include/logging/LogPlayer.h 2008-11-07 09:18:06 UTC (rev 6372)
@@ -334,7 +334,7 @@
if (!done_)
return next_msg_time_;
else
- return ros::Time(0);
+ return ros::Time(0.0);
}
bool nextMsg()
@@ -487,7 +487,7 @@
{
LogPlayer* next_player = 0;
- ros::Time min_t = ros::Time(-1); // This should be the maximum unsigned int;
+ ros::Time min_t = ros::Time((uint64_t)-1); // This should be the maximum unsigned int;
bool remaining = false;
Modified: pkg/trunk/util/logging/src/checklog/checklog.cpp
===================================================================
--- pkg/trunk/util/logging/src/checklog/checklog.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/util/logging/src/checklog/checklog.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -68,7 +68,7 @@
LogPlayer player;
- if (player.open(string(argv[1]), ros::Time(0)))
+ if (player.open(string(argv[1]), ros::Time(0.0)))
{
player.addHandler<AnyMsg>(string("*"), &checkFile, NULL, false);
}
Modified: pkg/trunk/util/logging/src/demo/demo.cpp
===================================================================
--- pkg/trunk/util/logging/src/demo/demo.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/util/logging/src/demo/demo.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -80,7 +80,7 @@
int counter = 0;
- if (player.open(string(argv[1]), ros::Time(0)))
+ if (player.open(string(argv[1]), ros::Time(0.0)))
{
player.addHandler<AnyMsg>(string("*"), &all_handler, &counter, false);
player.addHandler<std_msgs::String>(string("chatter"), &string_handler, NULL);
Modified: pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp
===================================================================
--- pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -125,7 +125,7 @@
for (int i = 1; i < argc; i++)
files.push_back(argv[i]);
- player.open(files, ros::Time(0));
+ player.open(files, ros::Time(0.0));
player.addHandler<std_msgs::RobotBase2DOdom>(string("*"), &odom_callback, NULL);
player.addHandler<std_msgs::LaserScan>(string("*"), &scan_callback, NULL);
Modified: pkg/trunk/util/logsetta/imu/imu_extract.cpp
===================================================================
--- pkg/trunk/util/logsetta/imu/imu_extract.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/util/logsetta/imu/imu_extract.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -57,7 +57,7 @@
LogPlayer player;
- player.open(std::string(argv[1]), ros::Time(0));
+ player.open(std::string(argv[1]), ros::Time(0.0));
int count;
Modified: pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp
===================================================================
--- pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -57,7 +57,7 @@
LogPlayer player;
- player.open(std::string(argv[1]), ros::Time(0));
+ player.open(std::string(argv[1]), ros::Time(0.0));
int count;
Modified: pkg/trunk/util/logsetta/odom/odom_extract.cpp
===================================================================
--- pkg/trunk/util/logsetta/odom/odom_extract.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/util/logsetta/odom/odom_extract.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -62,7 +62,7 @@
LogPlayer player;
- player.open(std::string(argv[1]), ros::Time(0));
+ player.open(std::string(argv[1]), ros::Time(0.0));
int count;
Modified: pkg/trunk/util/tf/src/cache.cpp
===================================================================
--- pkg/trunk/util/tf/src/cache.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/util/tf/src/cache.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -83,7 +83,7 @@
}
//If time == 0 return the latest
- if (target_time == 0ULL)
+ if (target_time < ros::Time(0.000001))
{
one = storage_.front();
time_diff = ros::Time::now() - storage_.front().stamp_; ///@todo what should this be?? difference from "now"?
Modified: pkg/trunk/util/tf/test/tf_unittest.cpp
===================================================================
--- pkg/trunk/util/tf/test/tf_unittest.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/util/tf/test/tf_unittest.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -536,9 +536,9 @@
yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
- Stamped<btTransform> tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), 10 + i, "child", "my_parent");
+ Stamped<btTransform> tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time((uint64_t)10 + i), "child", "my_parent");
mTR.setTransform(tranStamped);
- Stamped<btTransform> tranStamped2(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), 10 + i, "grandchild", "child");
+ Stamped<btTransform> tranStamped2(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), ros::Time((uint64_t)10 + i), "grandchild", "child");
mTR.setTransform(tranStamped2);
}
@@ -548,7 +548,7 @@
for ( unsigned int i = 0; i < runs ; i++ )
{
- Stamped<btTransform> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), 10 + i, "grandchild");
+ Stamped<btTransform> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time((uint64_t)10 + i), "grandchild");
try{
Stamped<Pose> outpose;
Modified: pkg/trunk/vision/calib_converter/src/calib_converter.cpp
===================================================================
--- pkg/trunk/vision/calib_converter/src/calib_converter.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/vision/calib_converter/src/calib_converter.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -81,7 +81,7 @@
// -- Load the messages.
- lp.open(fullname, ros::Time(0));
+ lp.open(fullname, ros::Time(0.0));
lp.addHandler<std_msgs::ImageArray>(string("videre/images"), ©Msg<std_msgs::ImageArray>, (void*)(&image_msg), true);
lp.addHandler<std_msgs::String>(string("videre/cal_params"), ©Msg<std_msgs::String>, (void*)(&calparams), true);
lp.addHandler<std_msgs::PointCloud>(string("full_cloud"), ©Msg<std_msgs::PointCloud>, (void*)(&cloud), true);
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -69,8 +69,8 @@
, vis_panel_( panel )
, needs_reset_( false )
, new_ros_time_( false )
-, wall_clock_begin_( 0 )
-, ros_time_begin_( 0 )
+, wall_clock_begin_( 0.0 )
+, ros_time_begin_( 0.0 )
{
initializeCommon();
registerFactories( this );
@@ -235,8 +235,8 @@
resetVisualizers();
tf_->clear();
- ros_time_begin_ = ros::Time( 0 );
- wall_clock_begin_ = ros::Time( 0 );
+ ros_time_begin_ = ros::Time( 0.0 );
+ wall_clock_begin_ = ros::Time( 0.0 );
}
static float time_update_timer = 0.0f;
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -60,7 +60,7 @@
, height_( 0.0f )
, load_timer_( 2.0f )
, new_metadata_( false )
-, last_loaded_map_time_( 0 )
+, last_loaded_map_time_( 0.0 )
, service_property_( NULL )
, resolution_property_( NULL )
, width_property_( NULL )
Modified: pkg/trunk/visualization/scene_labeler/src/scene_labeler/scene_labeler_stereo.cpp
===================================================================
--- pkg/trunk/visualization/scene_labeler/src/scene_labeler/scene_labeler_stereo.cpp 2008-11-07 06:40:35 UTC (rev 6371)
+++ pkg/trunk/visualization/scene_labeler/src/scene_labeler/scene_labeler_stereo.cpp 2008-11-07 09:18:06 UTC (rev 6372)
@@ -12,7 +12,7 @@
cout << "Loading messages... "; flush(cout);
- lp.open(file, ros::Time(0));
+ lp.open(file, ros::Time(0.0));
lp.addHandler<std_msgs::ImageArray>(string("videre/images"), ©Msg<std_msgs::ImageArray>, (void*)(&images_msg_), true);
lp.addHandler<std_msgs::ImageArray>(string("labeled_images"), ©Msg<std_msgs::ImageArray>, (void*)(&labeled_images_msg_), true);
lp.addHandler<std_msgs::PointCloud>(ptcld_topic_, ©Msg<std_msgs::PointCloud>, (void*)(&cloud_), true);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-11-07 09:24:34
|
Revision: 6373
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6373&view=rev
Author: jleibs
Date: 2008-11-07 09:24:30 +0000 (Fri, 07 Nov 2008)
Log Message:
-----------
Checking in new image_msg that uses multiarrays under the hood.
Added Paths:
-----------
pkg/trunk/image_msgs/
pkg/trunk/image_msgs/CMakeLists.txt
pkg/trunk/image_msgs/Makefile
pkg/trunk/image_msgs/include/
pkg/trunk/image_msgs/include/image_msgs/
pkg/trunk/image_msgs/include/image_msgs/ImageWrapper.h
pkg/trunk/image_msgs/manifest.xml
pkg/trunk/image_msgs/msg/
pkg/trunk/image_msgs/msg/Image.msg
Added: pkg/trunk/image_msgs/CMakeLists.txt
===================================================================
--- pkg/trunk/image_msgs/CMakeLists.txt (rev 0)
+++ pkg/trunk/image_msgs/CMakeLists.txt 2008-11-07 09:24:30 UTC (rev 6373)
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(image_msgs)
+genmsg()
+
Added: pkg/trunk/image_msgs/Makefile
===================================================================
--- pkg/trunk/image_msgs/Makefile (rev 0)
+++ pkg/trunk/image_msgs/Makefile 2008-11-07 09:24:30 UTC (rev 6373)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/image_msgs/include/image_msgs/ImageWrapper.h
===================================================================
--- pkg/trunk/image_msgs/include/image_msgs/ImageWrapper.h (rev 0)
+++ pkg/trunk/image_msgs/include/image_msgs/ImageWrapper.h 2008-11-07 09:24:30 UTC (rev 6373)
@@ -0,0 +1,110 @@
+/*********************************************************************
+* 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 IMAGEWRAPPER_HH
+#define IMAGEWRAPPER_HH
+
+#include "image_msgs/Image.h"
+#include "image.h"
+#include "opencv/cxcore.h"
+
+
+namespace image_msgs
+{
+
+ class ImageWrapper : public image_msgs::Image
+ {
+ public:
+ void fromData(std::string label_arg,
+ uint32_t height_arg, uint32_t width_arg, uint32_t channel_arg,
+ std::string encoding_arg, std::string depth_arg,
+ void* data_arg,
+ uint32_t channel_step = 0, uint32_t width_step = 0, uint32_t height_step = 0)
+ {
+ label = label_arg;
+ encoding = encoding_arg;
+ depth = depth_arg;
+
+ if (channel_step == 0)
+ channel_step = channel_arg;
+
+ if (width_step == 0)
+ width_step = width_arg * channel_step;
+
+ if (height_step == 0)
+ height_step = height_arg * width_step;
+
+ if (depth == "byte")
+ fromDataHelper(byte_data,
+ height_arg, height_step,
+ width_arg, width_step,
+ channel_arg, channel_step,
+ data_arg);
+
+ }
+
+ IplImage* asIplImage()
+ {
+ IplImage* img = cvCreateImageHeader(cvSize(byte_data.layout.dim[1].size, byte_data.layout.dim[0].size), IPL_DEPTH_8U, 1);
+ cvSetData(img, &(byte_data.data[0]), byte_data.layout.dim[1].stride);
+ return img;
+ }
+
+
+ private:
+ template <class M>
+ void fromDataHelper(M &m,
+ uint32_t sz0, uint32_t st0,
+ uint32_t sz1, uint32_t st1,
+ uint32_t sz2, uint32_t st2,
+ void *d)
+ {
+ m.layout.dim.resize(3);
+ m.layout.dim[0].label = "height";
+ m.layout.dim[0].size = sz0;
+ m.layout.dim[0].stride = st0;
+ m.layout.dim[1].label = "width";
+ m.layout.dim[1].size = sz1;
+ m.layout.dim[1].stride = st1;
+ m.layout.dim[2].label = "channel";
+ m.layout.dim[2].size = sz2;
+ m.layout.dim[2].stride = st2;
+ m.data.resize(st0);
+ memcpy((char*)(&m.data[0]), (char*)(d), st0*sizeof(m.data[0]));
+ }
+ };
+}
+
+
+#endif
Added: pkg/trunk/image_msgs/manifest.xml
===================================================================
--- pkg/trunk/image_msgs/manifest.xml (rev 0)
+++ pkg/trunk/image_msgs/manifest.xml 2008-11-07 09:24:30 UTC (rev 6373)
@@ -0,0 +1,12 @@
+<package>
+ <description>A package containing ROS messages for dealing with images and associated utilities.</description>
+ <author>Jeremy Leibs</author>
+ <license>BSD</license>
+ <depend package="std_msgs"/>
+ <depend package="opencv_latest"/>
+ <depend package="dcam"/>
+ <export>
+ <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp"/>
+ </export>
+ <url></url>
+</package>
Added: pkg/trunk/image_msgs/msg/Image.msg
===================================================================
--- pkg/trunk/image_msgs/msg/Image.msg (rev 0)
+++ pkg/trunk/image_msgs/msg/Image.msg 2008-11-07 09:24:30 UTC (rev 6373)
@@ -0,0 +1,37 @@
+Header header # Header
+string label # Label for the image
+string encoding # Specifies the color encoding of the data
+ # Acceptable values are:
+ # 1 channel types:
+ # mono, bayer_rggb, bayer_gbrg, bayer_grbg, bayer_bggr
+ # 3 channel types:
+ # rgb, bgr
+ # 4 channel types:
+ # rgba, bgra, yuv422
+ # 6 channel types:
+ # yuv411
+ # N channel types:
+ # other
+string depth # Specifies the depth of the data:
+ # Acceptable values:
+ # byte, uint16, int16, uint32, int32, uint64, int64, float32, float64
+
+# Based on depth ONE of the following MultiArrays may contain data.
+# The multi-array MUST have 3 dimensions, labeled as "height",
+# "width", and "channel", though depending on usage the ordering of
+# the dimensions may very. Note that IPL Image convention will order
+# these as: height, width, channel, which is the preferred ordering
+# unless performance dictates otherwise.
+#
+# Height, width, and number of channels are specified in the dimension
+# sizes within the appropriate MultiArray
+
+std_msgs/ByteMultiArray byte_data
+std_msgs/UInt16MultiArray uint16_data
+std_msgs/Int16MultiArray int16_data
+std_msgs/UInt32MultiArray uint32_data
+std_msgs/Int32MultiArray int32_data
+std_msgs/UInt64MultiArray uint64_data
+std_msgs/Int64MultiArray int64_data
+std_msgs/Float32MultiArray float32_data
+std_msgs/Float64MultiArray float64_data
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-07 16:59:47
|
Revision: 6377
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6377&view=rev
Author: hsujohnhsu
Date: 2008-11-07 16:59:35 +0000 (Fri, 07 Nov 2008)
Log Message:
-----------
* removed dependency on gazebo_joints xml's
* damping coefficients moved to joint_properties, parsed by initXml in joint.cpp and by URDF.cpp
* updates to gazebo_mechanism_control to use damping from jonint.h in mechanism_model
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
pkg/trunk/mechanism/mechanism_model/src/joint.cpp
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/controllers_gazebo_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/controllers_gazebo_multi_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/Makefile
pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/one_armed_bandit.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm/pr2d2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm/pr2d2_tilt_laser.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/gazebo/controllers_gazebo_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/gazebo/controllers_gazebo_arm_headless.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/controllers_gazebo_prototype1.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/pr2_prototype1.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/body_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/gazebo/controllers_gazebo_single_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/pr2_single_link.xml
pkg/trunk/robot_descriptions/wg_robot_description_parser/include/urdf/URDF.h
pkg/trunk/robot_descriptions/wg_robot_description_parser/src/URDF.cpp
Added Paths:
-----------
pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/groups_multi_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/transmissions_multi_link.xml
Removed Paths:
-------------
pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_arm.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_d2.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_dual_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_multi_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_prototype1.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_single_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/gazebo_joints.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm/gazebo_joints_d2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/gazebo/gazebo_joints_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/gazebo_joints_prototype1.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gazebo.xml
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-11-07 16:59:35 UTC (rev 6377)
@@ -6,8 +6,8 @@
gensrv()
include_directories(srv/cpp)
-#add_definitions(-Wall -DNDEBUG -O3)
-set(ROS_BUILD_TYPE Release)
+add_definitions(-Wall -DNDEBUG -O3)
+#set(ROS_BUILD_TYPE Release)
rospack_add_library(Ros_Stereo_Camera src/Ros_Stereo_Camera.cc)
rospack_add_library(Ros_Camera src/Ros_Camera.cc)
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h 2008-11-07 16:59:35 UTC (rev 6377)
@@ -149,26 +149,21 @@
mechanism::RobotState *fake_state_;
std::vector<gazebo::Joint*> joints_;
- // added for joint damping coefficients
- std::vector<double> joints_damping_;
- std::map<std::string,double> joints_damping_map_;
-
/*
* \brief read pr2.xml for actuators, and pass tinyxml node to mechanism control node's initXml.
*/
void ReadPr2Xml(XMLConfigNode *node);
/*
- * \brief read gazebo_joints.xml for joint damping and additional simulation parameters for joints
+ * \brief pointer to ros node
*/
- void ReadGazeboPhysics(XMLConfigNode *node);
+ ros::node *rosnode_;
/*
- * \brief pointer to ros node
+ * \brief tmp vars for performance checking
*/
- ros::node *rosnode_;
+ double wall_start, sim_start;
-
};
/** \} */
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp 2008-11-07 16:59:35 UTC (rev 6377)
@@ -69,6 +69,12 @@
printf("-------------------- starting node in Gazebo Mechanism Control \n");
}
+ if (getenv("CHECK_SPEEDUP"))
+ {
+ wall_start = Simulator::Instance()->GetWallTime();
+ sim_start = Simulator::Instance()->GetSimTime();
+ }
+
}
GazeboMechanismControl::~GazeboMechanismControl()
@@ -84,10 +90,6 @@
// Initializes the fake state (for running the transmissions backwards).
fake_state_ = new mechanism::RobotState(&mc_.model_, &hw_);
- // Get gazebo joint properties such as explicit damping coefficient, simulation specific.
- // Currently constructs a map of joint-name/damping-value pairs.
- ReadGazeboPhysics(node);
-
// The gazebo joints and mechanism joints should match up.
for (unsigned int i = 0; i < mc_.model_.joints_.size(); ++i)
{
@@ -105,18 +107,6 @@
joints_.push_back(NULL);
}
- // fill in gazebo joints / damping value pairs
- std::map<std::string,double>::iterator jt = joints_damping_map_.find(joint_name);
- if (jt!=joints_damping_map_.end())
- {
- joints_damping_.push_back(jt->second);
- //std::cout << "adding gazebo joint name (" << joint_name << ") with damping=" << jt->second << std::endl;
- }
- else
- {
- joints_damping_.push_back(0); // no damping
- //std::cout << "adding gazebo joint name (" << joint_name << ") with no damping." << std::endl;
- }
}
hw_.current_time_ = Simulator::Instance()->GetSimTime();
@@ -130,8 +120,16 @@
void GazeboMechanismControl::UpdateChild()
{
+ if (getenv("CHECK_SPEEDUP"))
+ {
+ double wall_elapsed = Simulator::Instance()->GetWallTime() - wall_start;
+ double sim_elapsed = Simulator::Instance()->GetSimTime() - sim_start;
+ std::cout << " real time: " << wall_elapsed
+ << " sim time: " << sim_elapsed
+ << " speed up: " << sim_elapsed / wall_elapsed
+ << std::endl;
+ }
assert(joints_.size() == fake_state_->joint_states_.size());
- assert(joints_.size() == joints_damping_.size());
//--------------------------------------------------
// Pushes out simulation state
@@ -201,11 +199,15 @@
switch (joints_[i]->GetType())
{
case Joint::HINGE:
- damping_force = joints_damping_[i] * ((HingeJoint*)joints_[i])->GetAngleRate();
+ //std::cout << " hinge joint name " << mc_.model_.joints_[i]->name_ << std::endl;
+ //std::cout << " check damping coef " << mc_.model_.joints_[i]->joint_damping_coefficient_ << std::endl;
+ damping_force = mc_.model_.joints_[i]->joint_damping_coefficient_ * ((HingeJoint*)joints_[i])->GetAngleRate();
((HingeJoint*)joints_[i])->SetTorque(effort - damping_force);
break;
case Joint::SLIDER:
- damping_force = joints_damping_[i] * ((SliderJoint*)joints_[i])->GetPositionRate();
+ //std::cout << " slider joint name " << mc_.model_.joints_[i]->name_ << std::endl;
+ //std::cout << " check damping coef " << mc_.model_.joints_[i]->joint_damping_coefficient_ << std::endl;
+ damping_force = mc_.model_.joints_[i]->joint_damping_coefficient_ * ((SliderJoint*)joints_[i])->GetPositionRate();
((SliderJoint*)joints_[i])->SetSliderForce(effort - damping_force);
break;
default:
@@ -283,56 +285,4 @@
mc_.state_->joint_states_[i].calibrated_ = true;
}
-void GazeboMechanismControl::ReadGazeboPhysics(XMLConfigNode *node)
-{
- XMLConfigNode *robot = node->GetChild("gazebo_physics");
- if (!robot)
- {
- fprintf(stderr, "Error loading gazebo_physics config: no robot element\n");
- return;
- }
-
- std::string filename = robot->GetFilename("filename", "", 1);
- printf("Loading %s\n", filename.c_str());
-
- TiXmlDocument doc(filename);
- if (!doc.LoadFile())
- {
- fprintf(stderr, "Error: Could not load the gazebo mechanism_control plugin's configuration file for gazebo physics: %s\n",
- filename.c_str());
- abort();
- }
- // Pulls out the list of joints used in the gazebo physics configuration.
- struct GetJoints : public TiXmlVisitor
- {
- std::map<const char*,double> joints;
- virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *)
- {
- if (elt.ValueStr() == std::string("joint") && elt.Attribute("name"))
- {
- double damp;
- //extract damping coefficient value
- if (elt.FirstChildElement("explicitDampingCoefficient"))
- //std::cout << "damp : " << elt.FirstChildElement("explicitDampingCoefficient")->GetText() << std::endl;
- damp = atof(elt.FirstChildElement("explicitDampingCoefficient")->GetText());
- else
- damp = 0;
-
- //std::cout << "inserting pair to map " << elt.Attribute("name") << " " << damp << std::endl;
- joints.insert(make_pair(elt.Attribute("name"),damp));
- }
- return true;
- }
- } get_joints;
- doc.RootElement()->Accept(&get_joints);
-
- // Copy the found joint/damping pairs into the class variable
- std::map<const char*,double>::iterator it;
- for (it = get_joints.joints.begin(); it != get_joints.joints.end(); ++it)
- {
- std::string *jn = new std::string((it->first));
- joints_damping_map_.insert(make_pair(*jn,it->second));
- }
-
-}
} // namespace gazebo
Modified: pkg/trunk/mechanism/mechanism_model/src/joint.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2008-11-07 16:59:35 UTC (rev 6377)
@@ -190,26 +190,24 @@
}
}
- if (type_ == JOINT_ROTARY || type_ == JOINT_CONTINUOUS || type_ == JOINT_PRISMATIC)
+ // Parses out the joint properties, this is done for all joints as default
+ TiXmlElement *prop_el = elt->FirstChildElement("joint_properties");
+ if (!prop_el)
{
- // Parses out the joint properties
- TiXmlElement *prop_el = elt->FirstChildElement("joint_properties");
- if (!prop_el)
- {
- fprintf(stderr, "Warning: Joint \"%s\" did not specify any joint properties, default to 0.\n", name_.c_str());
- joint_damping_coefficient_ = 0.0;
- joint_friction_coefficient_ = 0.0;
- }
- else
- {
- double tmp_damping;
- double tmp_friction;
- if (prop_el->QueryDoubleAttribute("damping", &joint_damping_coefficient_) != TIXML_SUCCESS)
- fprintf(stderr,"damping is not specified\n");
- if (prop_el->QueryDoubleAttribute("friction", &joint_friction_coefficient_) != TIXML_SUCCESS)
- fprintf(stderr,"friction is not specified\n");
- }
+ fprintf(stderr, "Warning: Joint \"%s\" did not specify any joint properties, default to 0.\n", name_.c_str());
+ joint_damping_coefficient_ = 0.0;
+ joint_friction_coefficient_ = 0.0;
+ }
+ else
+ {
+ if (prop_el->QueryDoubleAttribute("damping", &joint_damping_coefficient_) != TIXML_SUCCESS)
+ fprintf(stderr,"damping is not specified\n");
+ if (prop_el->QueryDoubleAttribute("friction", &joint_friction_coefficient_) != TIXML_SUCCESS)
+ fprintf(stderr,"friction is not specified\n");
+ }
+ if (type_ == JOINT_ROTARY || type_ == JOINT_CONTINUOUS || type_ == JOINT_PRISMATIC)
+ {
// Parses out the joint axis
TiXmlElement *axis_el = elt->FirstChildElement("axis");
if (!axis_el)
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints.xml 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints.xml 2008-11-07 16:59:35 UTC (rev 6377)
@@ -1 +0,0 @@
-link ../../wg_robot_description/pr2/gazebo/gazebo_joints.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_arm.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_arm.xml 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_arm.xml 2008-11-07 16:59:35 UTC (rev 6377)
@@ -1 +0,0 @@
-link ../../wg_robot_description/pr2_arm_test/gazebo/gazebo_joints_arm.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_d2.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_d2.xml 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_d2.xml 2008-11-07 16:59:35 UTC (rev 6377)
@@ -1 +0,0 @@
-link ../../wg_robot_description/pr2_arm/gazebo_joints_d2.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_dual_link.xml 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_dual_link.xml 2008-11-07 16:59:35 UTC (rev 6377)
@@ -1 +0,0 @@
-link ../../wg_robot_description/dual_link_test/gazebo/gazebo_joints_dual_link.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_multi_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_multi_link.xml 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_multi_link.xml 2008-11-07 16:59:35 UTC (rev 6377)
@@ -1 +0,0 @@
-link ../../wg_robot_description/multi_link_test/gazebo/gazebo_joints_multi_link.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_prototype1.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_prototype1.xml 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_prototype1.xml 2008-11-07 16:59:35 UTC (rev 6377)
@@ -1 +0,0 @@
-link ../../wg_robot_description/pr2_prototype1/gazebo/gazebo_joints_prototype1.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_single_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_single_link.xml 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_single_link.xml 2008-11-07 16:59:35 UTC (rev 6377)
@@ -1 +0,0 @@
-link ../../wg_robot_description/single_link_test/gazebo/gazebo_joints_single_link.xml
\ No newline at end of file
Modified: pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/controllers_gazebo_dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/controllers_gazebo_dual_link.xml 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/controllers_gazebo_dual_link.xml 2008-11-07 16:59:35 UTC (rev 6377)
@@ -14,8 +14,6 @@
<robot filename="pr2_dual_link.xml" />
- <gazebo_physics filename="gazebo_joints_dual_link.xml" /> <!-- for simulator/physics specific settigs -->
-
<interface:audio name="gazebo_mechanism_control_dummy_iface" />
</controller:gazebo_mechanism_control>
@@ -28,6 +26,18 @@
<verbatim key="controllers"> <!-- The key attribute is needed noly if multiple <xml> tags are in the same <map> tag -->
<!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_link1_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>link1</bodyName>
+ <topicName>link1_pose</topicName>
+ <frameName>link1_frame</frameName>
+ <xyzOffsets>0 0 0.5</xyzOffsets> <!-- at pivot of link2 -->
+ <rpyOffsets>0 0 0.0</rpyOffsets>
+ <interface:position name="p3d_link1_position"/>
+ </controller:P3D>
+
+ <!-- P3D for position groundtruth -->
<controller:P3D name="p3d_link2_controller" plugin="libP3D.so">
<alwaysOn>true</alwaysOn>
<updateRate>1000.0</updateRate>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml 2008-11-07 16:59:35 UTC (rev 6377)
@@ -114,7 +114,6 @@
<!-- Define groups of links; a link may be part of multiple groups -->
<include>groups_dual_link.xml</include>
<!-- mechanism controls -->
- <include>actuators_dual_link.xml</include>
<include>transmissions_dual_link.xml</include>
</robot>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/controllers_gazebo_multi_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/controllers_gazebo_multi_link.xml 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/controllers_gazebo_multi_link.xml 2008-11-07 16:59:35 UTC (rev 6377)
@@ -14,8 +14,6 @@
<robot filename="pr2_multi_link.xml" />
- <gazebo_physics filename="gazebo_joints_multi_link.xml" /> <!-- for simulator/physics specific settigs -->
-
<interface:audio name="gazebo_mechanism_control_dummy_iface" />
</controller:gazebo_mechanism_control>
Added: pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/groups_multi_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/groups_multi_link.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/groups_multi_link.xml 2008-11-07 16:59:35 UTC (rev 6377)
@@ -0,0 +1 @@
+link ../groups_multi_link.xml
\ No newline at end of file
Property changes on: pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/groups_multi_link.xml
___________________________________________________________________
Added: svn:special
+ *
Added: pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/transmissions_multi_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/transmissions_multi_link.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/transmissions_multi_link.xml 2008-11-07 16:59:35 UTC (rev 6377)
@@ -0,0 +1 @@
+link ../transmissions_multi_link.xml
\ No newline at end of file
Property changes on: pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/transmissions_multi_link.xml
___________________________________________________________________
Added: svn:special
+ *
Modified: pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/Makefile
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/Makefile 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/Makefile 2008-11-07 16:59:35 UTC (rev 6377)
@@ -7,7 +7,6 @@
$(XACRO) $^ > $^.expanded
$(URDF2GAZEBO) $^.expanded $@
$(RM) $^.expanded
- sed -e 's#gazebo_joints\.xml#../pr2_robot_defs/gazebo.xml#' < $@ > $@.temp
mv $@.temp $@
clean:
Modified: pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/one_armed_bandit.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/one_armed_bandit.xml 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/one_armed_bandit.xml 2008-11-07 16:59:35 UTC (rev 6377)
@@ -94,7 +94,6 @@
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
<robot filename="pr2.xml" />
- <gazebo_physics filename="gazebo_joints.xml" />
<interface:audio name="gazebo_mechanism_control_dummy_iface" />
</controller:gazebo_mechanism_control>
</verbatim>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml 2008-11-07 11:20:11 UTC (rev 6376)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml 2008-11-07 16:59:35 UTC (rev 6377)
@@ -5,9 +5,9 @@
<!-- Begin template definitions for ease of reuse -->
<const name="M_PI" value="3.14159" /> <!-- FIXME @todo needed to make M_PI interpreted correctly. -->
- <const name="robot_inital_position_x" value=" 0.0 " /> <!-- keep 0 for now, because of hardcoded sensor/gripper positions in controllers.cml -->
- <const name="robot_inital_position_y" value=" 0.0 " /> <!-- keep 0 for now, because of hardcoded sensor/gripper positions in controllers.cml -->
- <const name="robot_inital_position_z" value=" 0.0*(wheel_radius-base_caster_offset_z/2)/2 " /> <!-- keep 0 for now, because of hardcoded sensor/gripper positions in controllers.cml -->
+ <const name="robot_inital_position_x" value="0.0" /> <!-- keep 0 for now, because of hardcoded sensor/gripper positions in controllers.cml -->
+ <const name="robot_inital_position_y" value="0.0" /> <!-- keep 0 for now, because of hardcoded sensor/gripper positions in controllers.cml -->
+ <const name="robot_inital_position_z" value="0.0*(wheel_radius-base_caster_offset_z/2)/2" /> <!-- keep 0 for now, because of hardcoded sensor/gripper positions in controllers.cml -->
<const name="wheel_clearance_offset" value="0.02" /> <!-- placement robot model origin so wheel are not under ground -->
<const name="base_size_x" value="0.65" /> <!-- for defining collision geometry -->
@@ -28,31 +28,31 @@
<const name="caster_size_z" value="0.013" /> <!-- for collision geometry -->
<const name="caster_collision_center_box_center_offset_z" value="0.07" />
- <const name="torso_size_x" value=".432 " /> <!-- for defining collision geometry -->
- <const name="torso_size_y" value=".620 " /> <!-- for defining collision geometry -->
- <const name="torso_size_z" value=".823 " /> <!-- for defining collision geometry -->
+ <const name="torso_size_x" value=".432" /> <!-- for defining collision geometry -->
+ <const name="torso_size_y" value=".620" /> <!-- for defining collision geometry -->
+ <const name="torso_size_z" value=".823" /> <!-- for defining collision geometry -->
- <const name="base_torso_offset_x" value="-0.05 " /> <!-- mp 20080801 -->
- <const name="base_torso_offset_z" value=" 0.739675" /> <!-- mp 20080801 this is the offset for home position: lowest spine setting -->
+ <const name="base_torso_offset_x" value="-0.05" /> <!-- mp 20080801 -->
+ <const name="base_torso_offset_z" value="0.739675" /> <!-- mp 20080801 this is the offset for home position: lowest spine setting -->
<const name="torso_center_box_center_offset_x" value="-.10" /> <!-- FIXME -->
<const name="torso_center_box_center_offset_z" value="-.50" /> <!-- FIXME -->
- <const name="torso_max_travel_range" value=" 0.396 " /> <!-- FIXME -->
+ <const name="torso_max_travel_range" value="0.396" /> <!-- FIXME -->
<const name="shoulder_pan_size_x" value="0.347" /> <!-- for defining collision geometry -->
<const name="shoulder_pan_size_y" value="0.254" /> <!-- for defining collision geometry -->
<const name="shoulder_pan_size_z" value="0.646" /> <!-- for defining collision geometry -->
- <const name="shoulder_pan_center_box_center_offset_x" value=" .05" />
+ <const name="shoulder_pan_center_box_center_offset_x" value=".05" />
<const name="shoulder_pan_center_box_center_offset_z" value="-.20" />
- <const name="shoulder_pan_min_limit" value="-M_PI/2 " /> <!-- FIXME -->
- <const name="shoulder_pan_max_limit" value="M_PI/2 " /> <!-- FIXME -->
+ <const name="shoulder_pan_min_limit" value="-M_PI/2" /> <!-- FIXME -->
+ <const name="shoulder_pan_max_limit" value="M_PI/2" /> <!-- FIXME -->
<const name="torso_shoulder_pan_offset_y" value="0.188" /> <!-- mp 20080801 -->
- <const name="shoulder_pitch_min_limit" value=" -0.6109 " />
- <const name="shoulder_pitch_max_limit" value=" 1.3090 " />
+ <const name="shoulder_pitch_min_limit" value="-0.6109" />
+ <const name="shoulder_pitch_max_limit" value="1.3090" />
<const name="shoulder_pitch_length" value="0.10" /> <!-- for defining collision geometry -->
<const name="shoulder_pitch_radius" value="0.12" /> <!-- for defining collision geometry -->
@@ -65,28 +65,28 @@
<const name="upperarm_roll_center_box_center_offset_x" value=".30" /> <!-- from origin of mesh to center of box-geom -->
- <const name="elbow_flex_min_limit" value=" -M_PI/2 " />
- <const name="elbow_flex_max_limit" value=" 0.8727 " />
+ <const name="elbow_flex_min_limit" value="-M_PI/2" />
+ <const name="elbow_flex_max_limit" value="0.8727" />
<const name="elbow_flex_length" value="0.08" /> <!-- for defining collision geometry -->
<const name="elbow_flex_radius" value="0.1" /> <!-- for defining collision geometry -->
<const name="upperarm_roll_elbow_flex_offset_x" value="0.4" /> <!-- mp 20080801 -->
<const name="upperarm_roll_min_limit" value="-2.3562" />
- <const name="upperarm_roll_max_limit" value=" 2.3562" />
+ <const name="upperarm_roll_max_limit" value="2.3562" />
<const name="forearm_roll_size_x" value="0.27" /> <!-- for defining collision geometry -->
<const name="forearm_roll_size_y" value="0.12" /> <!-- for defining collision geometry -->
<const name="forearm_roll_size_z" value="0.08" /> <!-- for defining collision geometry -->
- <const name="forearm_roll_center_box_center_offset_x" value=" 0.22 " /> <!-- FIXME -->
+ <const name="forearm_roll_center_box_center_offset_x" value="0.22" /> <!-- FIXME -->
<const name="elbow_roll_wrist_flex_offset_x" value="0.32025" /> <!-- mp 20080801 -->
- <const name="wrist_flex_radius" value="0.033 " /> <!-- for defining collision geometry -->
- <const name="wrist_flex_length" value="0.103 " /> <!-- for defining collision geometry -->
+ <const name="wrist_flex_radius" value="0.033" /> <!-- for defining collision geometry -->
+ <const name="wrist_flex_length" value="0.103" /> <!-- for defining collision geometry -->
- <const name="wrist_flex_min_limit" value=" -1 " />
- <const name="wrist_flex_max_limit" value=" 1 " />
+ <const name="wrist_flex_min_limit" value="-1" />
+ <const name="wrist_flex_max_limit" value="1" />
<const name="gripper_roll_size_x" value="0.11" /> <!-- for defining collision geometry -->
<const name="gripper_roll_size_y" value="0.10" /> <!-- for defining collision geometry -->
@@ -106,8 +106,8 @@
<const name="head_pan_size_z" value="0.137" /> <!-- for defining collision geometry -->
- <const name="head_tilt_min_limit" value=" -M_PI/6 " /> <!-- FIXME -->
- <const name="head_tilt_max_li...
[truncated message content] |
|
From: <jfa...@us...> - 2008-11-07 18:31:25
|
Revision: 6381
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6381&view=rev
Author: jfaustwg
Date: 2008-11-07 18:31:21 +0000 (Fri, 07 Nov 2008)
Log Message:
-----------
r11741@iad: jfaust | 2008-11-06 16:56:45 -0800
Clear out the map when deleting the map display
Modified Paths:
--------------
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11511
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11741
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp 2008-11-07 18:02:16 UTC (rev 6380)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp 2008-11-07 18:31:21 UTC (rev 6381)
@@ -84,6 +84,8 @@
MapVisualizer::~MapVisualizer()
{
unsubscribe();
+
+ clear();
}
void MapVisualizer::onEnable()
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2008-11-07 18:32:43
|
Revision: 6382
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6382&view=rev
Author: jfaustwg
Date: 2008-11-07 18:32:40 +0000 (Fri, 07 Nov 2008)
Log Message:
-----------
r11742@iad: jfaust | 2008-11-06 16:57:36 -0800
First pass at renaming "visualizer" to "display" to make the code terminology the same as the UI/docs
Added Paths:
-----------
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.i
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/
Removed Paths:
-------------
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.i
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11741
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11742
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Copied: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.cpp (from rev 6082, pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.cpp)
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.cpp (rev 0)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.cpp 2008-11-07 18:32:40 UTC (rev 6382)
@@ -0,0 +1,155 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "visualizer_base.h"
+#include "visualization_manager.h"
+#include "properties/property_manager.h"
+#include "properties/property.h"
+
+namespace ogre_vis
+{
+
+VisualizerBase::VisualizerBase( const std::string& name, VisualizationManager* manager )
+: vis_manager_( manager )
+, scene_manager_( manager->getSceneManager() )
+, name_( name )
+, enabled_( false )
+, target_frame_( "base" )
+, ros_node_( manager->getROSNode() )
+, tf_( manager->getTFClient() )
+, property_prefix_( name_ + "." )
+, property_manager_( NULL )
+, parent_category_( NULL )
+{
+}
+
+VisualizerBase::~VisualizerBase()
+{
+ if ( property_manager_ )
+ {
+ property_manager_->deleteByUserData( this );
+ }
+}
+
+void VisualizerBase::enable( bool force )
+{
+ if ( enabled_ && !force )
+ {
+ return;
+ }
+
+ enabled_ = true;
+
+ onEnable();
+}
+
+void VisualizerBase::disable( bool force )
+{
+ if ( !enabled_ && !force )
+ {
+ return;
+ }
+
+ enabled_ = false;
+
+ onDisable();
+}
+
+void VisualizerBase::setRenderCallback( boost::function<void ()> func )
+{
+ render_callback_ = func;
+}
+
+void VisualizerBase::setLockRenderCallback( boost::function<void ()> func )
+{
+ render_lock_ = func;
+}
+
+void VisualizerBase::setUnlockRenderCallback( boost::function<void ()> func )
+{
+ render_unlock_ = func;
+}
+
+
+void VisualizerBase::causeRender()
+{
+ if ( render_callback_ )
+ {
+ render_callback_();
+ }
+}
+
+void VisualizerBase::lockRender()
+{
+ if ( render_lock_ )
+ {
+ render_lock_();
+ }
+}
+
+void VisualizerBase::unlockRender()
+{
+ if ( render_unlock_ )
+ {
+ render_unlock_();
+ }
+}
+
+void VisualizerBase::setTargetFrame( const std::string& frame )
+{
+ target_frame_ = frame;
+
+ if ( isEnabled() )
+ {
+ targetFrameChanged();
+ }
+}
+
+void VisualizerBase::setFixedFrame( const std::string& frame )
+{
+ fixed_frame_ = frame;
+
+ if ( isEnabled() )
+ {
+ fixedFrameChanged();
+ }
+}
+
+void VisualizerBase::setPropertyManager( PropertyManager* manager, CategoryProperty* parent )
+{
+ property_manager_ = manager;
+ parent_category_ = parent;
+
+ property_manager_->createProperty<BoolProperty>( "Enabled", property_prefix_, boost::bind( &VisualizerBase::isEnabled, this ),
+ boost::bind( &VisualizationManager::setVisualizerEnabled, vis_manager_, this, _1 ), parent, this );
+
+ createProperties();
+}
+
+} // namespace ogre_vis
Copied: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.h (from rev 6082, pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h)
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.h (rev 0)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.h 2008-11-07 18:32:40 UTC (rev 6382)
@@ -0,0 +1,236 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef OGRE_VISUALIZER_VISUALIZER_BASE
+#define OGRE_VISUALIZER_VISUALIZER_BASE
+
+#include <string>
+#include <boost/function.hpp>
+
+#include <wx/string.h>
+
+namespace Ogre
+{
+class SceneManager;
+class MovableObject;
+}
+
+namespace ros
+{
+class node;
+}
+
+namespace tf
+{
+class TransformListener;
+}
+
+class wxPanel;
+class wxWindow;
+class wxPropertyGrid;
+class wxPropertyGridEvent;
+class wxPGProperty;
+class wxConfigBase;
+class wxString;
+
+namespace ogre_vis
+{
+
+class PropertyManager;
+class CategoryProperty;
+class BoolProperty;
+
+class VisualizationManager;
+
+/**
+ * \class VisualizerBase
+ * \brief Abstract base class for all visualizers.
+ *
+ * Provides a common interface for the visualization panel to interact with,
+ * so that new visualizers can be added without the visualization panel knowing anything about them.
+ */
+class VisualizerBase
+{
+public:
+ VisualizerBase( const std::string& name, VisualizationManager* manager );
+ virtual ~VisualizerBase();
+
+ /**
+ * \brief Enable this visualizer
+ * @param force If false, does not re-enable if this visualizer is already enabled. If true, it does.
+ */
+ void enable( bool force = false );
+ /**
+ * \brief Disable this visualizer
+ * @param force If false, does not re-disable if this visualizer is already disabled. If true, it does.
+ */
+ void disable( bool force = false );
+
+ bool isEnabled() { return enabled_; }
+ const std::string& getName() { return name_; }
+
+ /**
+ * \brief Called periodically by the visualization panel
+ * @param dt Time, in seconds, since the last time the update list was run through.
+ */
+ virtual void update( float dt ) {}
+
+ ///
+ /**
+ * \brief Set the callback used for causing a render to happen
+ * @param func a void(void) function that will cause a render to happen from the correct thread
+ */
+ void setRenderCallback( boost::function<void ()> func );
+
+ /// Set the callback used to lock the renderer
+ void setLockRenderCallback( boost::function<void ()> func );
+ /// Set the callback used to unlock the renderer
+ void setUnlockRenderCallback( boost::function<void ()> func );
+
+ /**
+ * \brief Sets the property manager and parent category for this visualizer
+ * @param manager The property manager
+ * @param parent The parent category
+ */
+ void setPropertyManager( PropertyManager* manager, CategoryProperty* parent );
+
+ /**
+ * \brief Called from setPropertyManager, gives the visualizer a chance to create some properties immediately.
+ *
+ * Once this function is called, the property_manager_ member is valid and will stay valid
+ */
+ virtual void createProperties() {}
+
+ /// Set the target frame of this visualizer. This is a frame id which should match something being broadcast through TF.
+ void setTargetFrame( const std::string& frame );
+
+ /**
+ * \brief Called from within setTargetFrame, notifying child classes that the target frame has changed
+ */
+ virtual void targetFrameChanged() = 0;
+
+ /**
+ * \brief Set the fixed frame of this visualizer. This is a frame id which should generally be the top-level frame being broadcast through TF
+ * @param frame The fixed frame
+ */
+ void setFixedFrame( const std::string& frame );
+
+ /**
+ * \brief Called from within setFixedFrame, notifying child classes that the fixed frame has changed
+ */
+ virtual void fixedFrameChanged() = 0;
+
+ /**
+ * \brief Returns whether an object owned by this visualizer is pickable/mouse selectable
+ * @param object The Ogre::MovableObject to check
+ */
+ virtual bool isObjectPickable( const Ogre::MovableObject* object ) const { return false; }
+
+ /**
+ * \brief Returns the type name of this visualizer. Does not need to be exactly the same as the class name. Can contains spaces/punctuation, etc.
+ * @return The type name
+ */
+ virtual const char* getType() = 0;
+
+ /**
+ * \brief Called to tell the visualizer to clear its state
+ */
+ virtual void reset() {}
+
+protected:
+ /// Derived classes override this to do the actual work of enabling themselves
+ virtual void onEnable() = 0;
+ /// Derived classes override this to do the actual work of disabling themselves
+ virtual void onDisable() = 0;
+
+ ///
+ /**
+ * \brief Cause the scene we're in to be rendered.
+ * \note This does not immediately cause a render -- instead, one is queued and happens next run through the event loop.
+ */
+ void causeRender();
+
+ /// Lock the renderer
+ void lockRender();
+ /// Unlock the renderer
+ void unlockRender();
+
+ VisualizationManager* vis_manager_;
+
+ Ogre::SceneManager* scene_manager_; ///< The scene manager we're associated with
+ std::string name_; ///< The name of this visualizer
+ bool enabled_; ///< Are we enabled?
+
+ std::string target_frame_; ///< The frame we should transform all periodically-updated data into
+ std::string fixed_frame_; ///< The frame we should transform all fixed data into
+
+ boost::function<void ()> render_callback_; ///< Render callback
+ boost::function<void ()> render_lock_; ///< Render lock callback
+ boost::function<void ()> render_unlock_; ///< Render unlock callback
+
+ ros::node* ros_node_; ///< ros node
+ tf::TransformListener* tf_; ///< tf client
+
+ std::string property_prefix_; ///< Prefix to prepend to our properties
+
+ PropertyManager* property_manager_; ///< The property manager to use to create properties
+ CategoryProperty* parent_category_; ///< The parent category to use when creating properties
+ BoolProperty* enabled_property_;
+
+ friend class RenderAutoLock;
+};
+
+/**
+ * \class RenderAutoLock
+ * \brief A scoped lock on the renderer
+ *
+ * Constructor calls VisualizerBase::lockRender<br>
+ * Destructor calls VisualizerBase::unlockRender
+ */
+class RenderAutoLock
+{
+public:
+ RenderAutoLock( VisualizerBase* visualizer )
+ : visualizer_( visualizer )
+ {
+ visualizer_->lockRender();
+ }
+
+ ~RenderAutoLock()
+ {
+ visualizer_->unlockRender();
+ }
+
+private:
+ VisualizerBase* visualizer_;
+};
+
+} // namespace ogre_vis
+
+#endif
Copied: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.i (from rev 6082, pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.i)
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.i (rev 0)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.i 2008-11-07 18:32:40 UTC (rev 6382)
@@ -0,0 +1,19 @@
+%{
+#include "visualizer_base.h"
+%}
+
+%include typemaps.i
+%include my_typemaps.i
+
+%import core.i
+%import windows.i
+
+%pythonAppend VisualizerBase "self._setOORInfo(self)"
+
+%include "visualizer_base.h"
+
+%init %{
+
+%}
+
+
Deleted: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.cpp 2008-11-07 18:31:21 UTC (rev 6381)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.cpp 2008-11-07 18:32:40 UTC (rev 6382)
@@ -1,155 +0,0 @@
-/*
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include "visualizer_base.h"
-#include "visualization_manager.h"
-#include "properties/property_manager.h"
-#include "properties/property.h"
-
-namespace ogre_vis
-{
-
-VisualizerBase::VisualizerBase( const std::string& name, VisualizationManager* manager )
-: vis_manager_( manager )
-, scene_manager_( manager->getSceneManager() )
-, name_( name )
-, enabled_( false )
-, target_frame_( "base" )
-, ros_node_( manager->getROSNode() )
-, tf_( manager->getTFClient() )
-, property_prefix_( name_ + "." )
-, property_manager_( NULL )
-, parent_category_( NULL )
-{
-}
-
-VisualizerBase::~VisualizerBase()
-{
- if ( property_manager_ )
- {
- property_manager_->deleteByUserData( this );
- }
-}
-
-void VisualizerBase::enable( bool force )
-{
- if ( enabled_ && !force )
- {
- return;
- }
-
- enabled_ = true;
-
- onEnable();
-}
-
-void VisualizerBase::disable( bool force )
-{
- if ( !enabled_ && !force )
- {
- return;
- }
-
- enabled_ = false;
-
- onDisable();
-}
-
-void VisualizerBase::setRenderCallback( boost::function<void ()> func )
-{
- render_callback_ = func;
-}
-
-void VisualizerBase::setLockRenderCallback( boost::function<void ()> func )
-{
- render_lock_ = func;
-}
-
-void VisualizerBase::setUnlockRenderCallback( boost::function<void ()> func )
-{
- render_unlock_ = func;
-}
-
-
-void VisualizerBase::causeRender()
-{
- if ( render_callback_ )
- {
- render_callback_();
- }
-}
-
-void VisualizerBase::lockRender()
-{
- if ( render_lock_ )
- {
- render_lock_();
- }
-}
-
-void VisualizerBase::unlockRender()
-{
- if ( render_unlock_ )
- {
- render_unlock_();
- }
-}
-
-void VisualizerBase::setTargetFrame( const std::string& frame )
-{
- target_frame_ = frame;
-
- if ( isEnabled() )
- {
- targetFrameChanged();
- }
-}
-
-void VisualizerBase::setFixedFrame( const std::string& frame )
-{
- fixed_frame_ = frame;
-
- if ( isEnabled() )
- {
- fixedFrameChanged();
- }
-}
-
-void VisualizerBase::setPropertyManager( PropertyManager* manager, CategoryProperty* parent )
-{
- property_manager_ = manager;
- parent_category_ = parent;
-
- property_manager_->createProperty<BoolProperty>( "Enabled", property_prefix_, boost::bind( &VisualizerBase::isEnabled, this ),
- boost::bind( &VisualizationManager::setVisualizerEnabled, vis_manager_, this, _1 ), parent, this );
-
- createProperties();
-}
-
-} // namespace ogre_vis
Deleted: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h 2008-11-07 18:31:21 UTC (rev 6381)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h 2008-11-07 18:32:40 UTC (rev 6382)
@@ -1,236 +0,0 @@
-/*
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef OGRE_VISUALIZER_VISUALIZER_BASE
-#define OGRE_VISUALIZER_VISUALIZER_BASE
-
-#include <string>
-#include <boost/function.hpp>
-
-#include <wx/string.h>
-
-namespace Ogre
-{
-class SceneManager;
-class MovableObject;
-}
-
-namespace ros
-{
-class node;
-}
-
-namespace tf
-{
-class TransformListener;
-}
-
-class wxPanel;
-class wxWindow;
-class wxPropertyGrid;
-class wxPropertyGridEvent;
-class wxPGProperty;
-class wxConfigBase;
-class wxString;
-
-namespace ogre_vis
-{
-
-class PropertyManager;
-class CategoryProperty;
-class BoolProperty;
-
-class VisualizationManager;
-
-/**
- * \class VisualizerBase
- * \brief Abstract base class for all visualizers.
- *
- * Provides a common interface for the visualization panel to interact with,
- * so that new visualizers can be added without the visualization panel knowing anything about them.
- */
-class VisualizerBase
-{
-public:
- VisualizerBase( const std::string& name, VisualizationManager* manager );
- virtual ~VisualizerBase();
-
- /**
- * \brief Enable this visualizer
- * @param force If false, does not re-enable if this visualizer is already enabled. If true, it does.
- */
- void enable( bool force = false );
- /**
- * \brief Disable this visualizer
- * @param force If false, does not re-disable if this visualizer is already disabled. If true, it does.
- */
- void disable( bool force = false );
-
- bool isEnabled() { return enabled_; }
- const std::string& getName() { return name_; }
-
- /**
- * \brief Called periodically by the visualization panel
- * @param dt Time, in seconds, since the last time the update list was run through.
- */
- virtual void update( float dt ) {}
-
- ///
- /**
- * \brief Set the callback used for causing a render to happen
- * @param func a void(void) function that will cause a render to happen from the correct thread
- */
- void setRenderCallback( boost::function<void ()> func );
-
- /// Set the callback used to lock the renderer
- void setLockRenderCallback( boost::function<void ()> func );
- /// Set the callback used to unlock the renderer
- void setUnlockRenderCallback( boost::function<void ()> func );
-
- /**
- * \brief Sets the property manager and parent category for this visualizer
- * @param manager The property manager
- * @param parent The parent category
- */
- void setPropertyManager( PropertyManager* manager, CategoryProperty* parent );
-
- /**
- * \brief Called from setPropertyManager, gives the visualizer a chance to create some properties immediately.
- *
- * Once this function is called, the property_manager_ member is valid and will stay valid
- */
- virtual void createProperties() {}
-
- /// Set the target frame of this visualizer. This is a frame id which should match something being broadcast through TF.
- void setTargetFrame( const std::string& frame );
-
- /**
- * \brief Called from within setTargetFrame, notifying child classes that the target frame has changed
- */
- virtual void targetFrameChanged() = 0;
-
- /**
- * \brief Set the fixed frame of this visualizer. This is a frame id which should generally be the top-level frame being broadcast through TF
- * @param frame The fixed frame
- */
- void setFixedFrame( const std::string& frame );
-
- /**
- * \brief Called from within setFixedFrame, notifying child classes that the fixed frame has changed
- */
- virtual void fixedFrameChanged() = 0;
-
- /**
- * \brief Returns whether an object owned by this visualizer is pickable/mouse selectable
- * @param object The Ogre::MovableObject to check
- */
- virtual bool isObjectPickable( const Ogre::MovableObject* object ) const { return false; }
-
- /**
- * \brief Returns the type name of this visualizer. Does not need to be exactly the same as the class name. Can contains spaces/punctuation, etc.
- * @return The type name
- */
- virtual const char* getType() = 0;
-
- /**
- * \brief Called to tell the visualizer to clear its state
- */
- virtual void reset() {}
-
-protected:
- /// Derived classes override this to do the actual work of enabling themselves
- virtual void onEnable() = 0;
- /// Derived classes override this to do the actual work of disabling themselves
- virtual void onDisable() = 0;
-
- ///
- /**
- * \brief Cause the scene we're in to be rendered.
- * \note This does not immediately cause a render -- instead, one is queued and happens next run through the event loop.
- */
- void causeRender();
-
- /// Lock the renderer
- void lockRender();
- /// Unlock the renderer
- void unlockRender();
-
- VisualizationManager* vis_manager_;
-
- Ogre::SceneManager* scene_manager_; ///< The scene manager we're associated with
- std::string name_; ///< The name of this visualizer
- bool enabled_; ///< Are we enabled?
-
- std::string target_frame_; ///< The frame we should transform all periodically-updated data into
- std::string fixed_frame_; ///< The frame we should transform all fixed data into
-
- boost::function<void ()> render_callback_; ///< Render callback
- boost::function<void ()> render_lock_; ///< Render lock callback
- boost::function<void ()> render_unlock_; ///< Render unlock callback
-
- ros::node* ros_node_; ///< ros node
- tf::TransformListener* tf_; ///< tf client
-
- std::string property_prefix_; ///< Prefix to prepend to our properties
-
- PropertyManager* property_manager_; ///< The property manager to use to create properties
- CategoryProperty* parent_category_; ///< The parent category to use when creating properties
- BoolProperty* enabled_property_;
-
- friend class RenderAutoLock;
-};
-
-/**
- * \class RenderAutoLock
- * \brief A scoped lock on the renderer
- *
- * Constructor calls VisualizerBase::lockRender<br>
- * Destructor calls VisualizerBase::unlockRender
- */
-class RenderAutoLock
-{
-public:
- RenderAutoLock( VisualizerBase* visualizer )
- : visualizer_( visualizer )
- {
- visualizer_->lockRender();
- }
-
- ~RenderAutoLock()
- {
- visualizer_->unlockRender();
- }
-
-private:
- VisualizerBase* visualizer_;
-};
-
-} // namespace ogre_vis
-
-#endif
Deleted: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.i
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.i 2008-11-07 18:31:21 UTC (rev 6381)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.i 2008-11-07 18:32:40 UTC (rev 6382)
@@ -1,19 +0,0 @@
-%{
-#include "visualizer_base.h"
-%}
-
-%include typemaps.i
-%include my_typemaps.i
-
-%import core.i
-%import windows.i
-
-%pythonAppend VisualizerBase "self._setOORInfo(self)"
-
-%include "visualizer_base.h"
-
-%init %{
-
-%}
-
-
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2008-11-07 18:33:56
|
Revision: 6383
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6383&view=rev
Author: jfaustwg
Date: 2008-11-07 18:33:52 +0000 (Fri, 07 Nov 2008)
Log Message:
-----------
r11744@iad: jfaust | 2008-11-06 17:04:42 -0800
2nd pass at renaming visualizer->display. Rename the individual display source files
Added Paths:
-----------
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/octree_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/octree_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/particle_cloud_2d_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/particle_cloud_2d_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/point_cloud_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/point_cloud_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/poly_line_2d_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/poly_line_2d_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_base2d_pose_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_base2d_pose_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_model_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_model_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/tf_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/tf_display.h
Removed Paths:
-------------
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/octree_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/octree_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/particle_cloud_2d_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/particle_cloud_2d_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/point_cloud_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/point_cloud_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/poly_line_2d_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/poly_line_2d_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_base2d_pose_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_base2d_pose_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_model_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_model_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/tf_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/tf_visualizer.h
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11742
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11744
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Copied: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.cpp (from rev 6382, pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_visualizer.cpp)
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.cpp (rev 0)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.cpp 2008-11-07 18:33:52 UTC (rev 6383)
@@ -0,0 +1,127 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "axes_visualizer.h"
+#include "visualization_manager.h"
+#include "properties/property.h"
+#include "properties/property_manager.h"
+#include "common.h"
+
+#include "ogre_tools/axes.h"
+
+#include <boost/bind.hpp>
+
+#include <OgreSceneNode.h>
+#include <OgreSceneManager.h>
+
+namespace ogre_vis
+{
+
+AxesVisualizer::AxesVisualizer( const std::string& name, VisualizationManager* manager )
+: VisualizerBase( name, manager )
+, length_( 1.0 )
+, radius_( 0.1 )
+{
+ axes_ = new ogre_tools::Axes( scene_manager_, manager->getTargetRelativeNode(), length_, radius_ );
+
+ axes_->getSceneNode()->setVisible( isEnabled() );
+
+ Ogre::Quaternion orient( Ogre::Quaternion::IDENTITY );
+ robotToOgre( orient );
+ axes_->setOrientation( orient );
+ axes_->setUserData( Ogre::Any( (void*)this ) );
+}
+
+AxesVisualizer::~AxesVisualizer()
+{
+ delete axes_;
+}
+
+void AxesVisualizer::onEnable()
+{
+ axes_->getSceneNode()->setVisible( true );
+}
+
+void AxesVisualizer::onDisable()
+{
+ axes_->getSceneNode()->setVisible( false );
+}
+
+void AxesVisualizer::create()
+{
+ axes_->set( length_, radius_ );
+
+ causeRender();
+}
+
+void AxesVisualizer::set( float length, float radius )
+{
+ length_ = length;
+ radius_ = radius;
+
+ create();
+
+ if ( length_property_ )
+ {
+ length_property_->changed();
+ }
+
+ if ( radius_property_ )
+ {
+ radius_property_->changed();
+ }
+}
+
+void AxesVisualizer::setLength( float length )
+{
+ set( length, radius_ );
+}
+
+void AxesVisualizer::setRadius( float radius )
+{
+ set( length_, radius );
+}
+
+void AxesVisualizer::createProperties()
+{
+ length_property_ = property_manager_->createProperty<FloatProperty>( "Length", property_prefix_, boost::bind( &AxesVisualizer::getLength, this ),
+ boost::bind( &AxesVisualizer::setLength, this, _1 ), parent_category_, this );
+ length_property_->setMin( 0.0001 );
+
+ radius_property_ = property_manager_->createProperty<FloatProperty>( "Radius", property_prefix_, boost::bind( &AxesVisualizer::getRadius, this ),
+ boost::bind( &AxesVisualizer::setRadius, this, _1 ), parent_category_, this );
+ radius_property_->setMin( 0.0001 );
+}
+
+const char* AxesVisualizer::getDescription()
+{
+ return "Displays a set of Axes at the origin of the target frame of reference.";
+}
+
+} // namespace ogre_vis
Copied: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.h (from rev 6382, pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_visualizer.h)
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.h (rev 0)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.h 2008-11-07 18:33:52 UTC (rev 6383)
@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef OGRE_VISUALIZER_AXES_VISUALIZER_H
+#define OGRE_VISUALIZER_AXES_VISUALIZER_H
+
+#include "visualizer_base.h"
+
+namespace ogre_tools
+{
+class Axes;
+}
+
+namespace ogre_vis
+{
+
+class FloatProperty;
+
+/**
+ * \class AxesVisualizer
+ * \brief Displays a set of XYZ axes at the origin
+ */
+class AxesVisualizer : public VisualizerBase
+{
+public:
+ AxesVisualizer( const std::string& name, VisualizationManager* manager );
+ virtual ~AxesVisualizer();
+
+ /**
+ * \brief Set the parameters for the axes
+ * @param length Length of each axis
+ * @param radius Radius of each axis
+ */
+ void set( float length, float radius );
+
+ void setLength( float length );
+ float getLength() { return length_; }
+ void setRadius( float radius );
+ float getRadius() { return radius_; }
+
+ // Overrides from VisualizerBase
+ virtual void targetFrameChanged() {}
+ virtual void fixedFrameChanged() {}
+ virtual void createProperties();
+
+ static const char* getTypeStatic() { return "Axes"; }
+ virtual const char* getType() { return getTypeStatic(); }
+ static const char* getDescription();
+
+protected:
+ /**
+ * \brief Create the axes with the current parameters
+ */
+ void create();
+
+ // overrides from VisualizerBase
+ virtual void onEnable();
+ virtual void onDisable();
+
+ float length_; ///< Length of each axis
+ float radius_; ///< Radius of each axis
+ ogre_tools::Axes* axes_; ///< Handles actually drawing the axes
+
+ FloatProperty* length_property_;
+ FloatProperty* radius_property_;
+};
+
+} // namespace ogre_vis
+
+ #endif
Deleted: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_visualizer.cpp 2008-11-07 18:32:40 UTC (rev 6382)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_visualizer.cpp 2008-11-07 18:33:52 UTC (rev 6383)
@@ -1,127 +0,0 @@
-/*
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include "axes_visualizer.h"
-#include "visualization_manager.h"
-#include "properties/property.h"
-#include "properties/property_manager.h"
-#include "common.h"
-
-#include "ogre_tools/axes.h"
-
-#include <boost/bind.hpp>
-
-#include <OgreSceneNode.h>
-#include <OgreSceneManager.h>
-
-namespace ogre_vis
-{
-
-AxesVisualizer::AxesVisualizer( const std::string& name, VisualizationManager* manager )
-: VisualizerBase( name, manager )
-, length_( 1.0 )
-, radius_( 0.1 )
-{
- axes_ = new ogre_tools::Axes( scene_manager_, manager->getTargetRelativeNode(), length_, radius_ );
-
- axes_->getSceneNode()->setVisible( isEnabled() );
-
- Ogre::Quaternion orient( Ogre::Quaternion::IDENTITY );
- robotToOgre( orient );
- axes_->setOrientation( orient );
- axes_->setUserData( Ogre::Any( (void*)this ) );
-}
-
-AxesVisualizer::~AxesVisualizer()
-{
- delete axes_;
-}
-
-void AxesVisualizer::onEnable()
-{
- axes_->getSceneNode()->setVisible( true );
-}
-
-void AxesVisualizer::onDisable()
-{
- axes_->getSceneNode()->setVisible( false );
-}
-
-void AxesVisualizer::create()
-{
- axes_->set( length_, radius_ );
-
- causeRender();
-}
-
-void AxesVisualizer::set( float length, float radius )
-{
- length_ = length;
- radius_ = radius;
-
- create();
-
- if ( length_property_ )
- {
- length_property_->changed();
- }
-
- if ( radius_property_ )
- {
- radius_property_->changed();
- }
-}
-
-void AxesVisualizer::setLength( float length )
-{
- set( length, radius_ );
-}
-
-void AxesVisualizer::setRadius( float radius )
-{
- set( length_, radius );
-}
-
-void AxesVisualizer::createProperties()
-{
- length_property_ = property_manager_->createProperty<FloatProperty>( "Length", property_prefix_, boost::bind( &AxesVisualizer::getLength, this ),
- boost::bind( &AxesVisualizer::setLength, this, _1 ), parent_category_, this );
- length_property_->setMin( 0.0001 );
-
- radius_property_ = property_manager_->createProperty<FloatProperty>( "Radius", property_prefix_, boost::bind( &AxesVisualizer::getRadius, this ),
- boost::bind( &AxesVisualizer::setRadius, this, _1 ), parent_category_, this );
- radius_property_->setMin( 0.0001 );
-}
-
-const char* AxesVisualizer::getDescription()
-{
- return "Displays a set of Axes at the origin of the target frame of reference.";
-}
-
-} // namespace ogre_vis
Deleted: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_visualizer.h 2008-11-07 18:32:40 UTC (rev 6382)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_visualizer.h 2008-11-07 18:33:52 UTC (rev 6383)
@@ -1,96 +0,0 @@
-/*
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef OGRE_VISUALIZER_AXES_VISUALIZER_H
-#define OGRE_VISUALIZER_AXES_VISUALIZER_H
-
-#include "visualizer_base.h"
-
-namespace ogre_tools
-{
-class Axes;
-}
-
-namespace ogre_vis
-{
-
-class FloatProperty;
-
-/**
- * \class AxesVisualizer
- * \brief Displays a set of XYZ axes at the origin
- */
-class AxesVisualizer : public VisualizerBase
-{
-public:
- AxesVisualizer( const std::string& name, VisualizationManager* manager );
- virtual ~AxesVisualizer();
-
- /**
- * \brief Set the parameters for the axes
- * @param length Length of each axis
- * @param radius Radius of each axis
- */
- void set( float length, float radius );
-
- void setLength( float length );
- float getLength() { return length_; }
- void setRadius( float radius );
- float getRadius() { return radius_; }
-
- // Overrides from VisualizerBase
- virtual void targetFrameChanged() {}
- virtual void fixedFrameChanged() {}
- virtual void createProperties();
-
- static const char* getTypeStatic() { return "Axes"; }
- virtual const char* getType() { return getTypeStatic(); }
- static const char* getDescription();
-
-protected:
- /**
- * \brief Create the axes with the current parameters
- */
- void create();
-
- // overrides from VisualizerBase
- virtual void onEnable();
- virtual void onDisable();
-
- float length_; ///< Length of each axis
- float radius_; ///< Radius of each axis
- ogre_tools::Axes* axes_; ///< Handles actually drawing the axes
-
- FloatProperty* length_property_;
- FloatProperty* radius_property_;
-};
-
-} // namespace ogre_vis
-
- #endif
Copied: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_display.cpp (from rev 6382, pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_visualizer.cpp)
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_display.cpp (rev 0)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_display.cpp 2008-11-07 18:33:52 UTC (rev 6383)
@@ -0,0 +1,142 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "grid_visualizer.h"
+#include "common.h"
+#include "visualization_manager.h"
+#include "properties/property.h"
+#include "properties/property_manager.h"
+
+#include "ogre_tools/grid.h"
+
+#include <boost/bind.hpp>
+
+#include <OgreSceneNode.h>
+#include <OgreSceneManager.h>
+
+namespace ogre_vis
+{
+
+GridVisualizer::GridVisualizer( const std::string& name, VisualizationManager* manager )
+: VisualizerBase( name, manager )
+, cell_size_( 1.0f )
+, cell_count_( 10 )
+, color_( 0.5, 0.5, 0.5 )
+, cellcount_property_( NULL )
+, cellsize_property_( NULL )
+, color_property_( NULL )
+{
+ grid_ = new ogre_tools::Grid( scene_manager_, manager->getTargetRelativeNode(), cell_count_, cell_size_, color_.r_, color_.g_, color_.b_ );
+
+ /*Ogre::Quaternion orient( Ogre::Quaternion::IDENTITY );
+ ogreToRobot( orient );
+ grid_->getSceneNode()->setOrientation( orient );*/
+}
+
+GridVisualizer::~GridVisualizer()
+{
+ delete grid_;
+}
+
+void GridVisualizer::onEnable()
+{
+ grid_->getSceneNode()->setVisible( true );
+}
+
+void GridVisualizer::onDisable()
+{
+ grid_->getSceneNode()->setVisible( false );
+}
+
+void GridVisualizer::create()
+{
+ grid_->set( cell_count_, cell_size_, color_.r_, color_.g_, color_.b_ );
+
+ causeRender();
+}
+
+void GridVisualizer::set( uint32_t cell_count, float cell_size, const Color& color )
+{
+ cell_count_ = cell_count;
+ cell_size_ = cell_size;
+ color_ = color;
+
+ create();
+
+ if ( cellcount_property_ )
+ {
+ cellcount_property_->changed();
+ }
+
+ if ( cellsize_property_ )
+ {
+ cellsize_property_->changed();
+ }
+
+ if ( color_property_ )
+ {
+ color_property_->changed();
+ }
+}
+
+void GridVisualizer::setCellSize( float size )
+{
+ set( cell_count_, size, color_ );
+}
+
+void GridVisualizer::setCellCount( uint32_t count )
+{
+ set( count, cell_size_, color_ );
+}
+
+void GridVisualizer::setColor( const Color& color )
+{
+ set( cell_count_, cell_size_, color );
+}
+
+void GridVisualizer::createProperties()
+{
+ cellcount_property_ = property_manager_->createProperty<IntProperty>( "Cell Count", property_prefix_, boost::bind( &GridVisualizer::getCellCount, this ),
+ boost::bind( &GridVisualizer::setCellCount, this, _1 ), parent_category_, this );
+ cellcount_property_->setMin( 1 );
+
+ cellsize_property_ = property_manager_->createProperty<FloatProperty>( "Cell Size", property_prefix_, boost::bind( &GridVisualizer::getCellSize, this ),
+ boost::bind( &GridVisualizer::setCellSize, this, _1 ), parent_category_, this );
+ cellsize_property_->setMin( 0.0001 );
+
+ color_property_ = property_manager_->createProperty<ColorProperty>( "Color", property_prefix_, boost::bind( &GridVisualizer::getColor, this ),
+ boost::bind( &GridVisualizer::setColor, this, _1 ), parent_category_, this );
+}
+
+const char* GridVisualizer::getDescription()
+{
+ return "Displays a grid along the ground plane, centered at the origin of the target frame of reference.";
+}
+
+} // namespace ogre_vis
Copied: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_display.h (from rev 6382, pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_visualizer.h)
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_display.h (rev 0)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_display.h 2008-11-07 18:33:52 UTC (rev 6383)
@@ -0,0 +1,128 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef OGRE_VISUALIZER_GRID_VISUALIZER_H
+#define OGRE_VISUALIZER_GRID_VISUALIZER_H
+
+#include "visualizer_base.h"
+#include "helpers/color.h"
+
+namespace ogre_tools
+{
+class Grid;
+}
+
+class wxFoc...
[truncated message content] |
|
From: <jfa...@us...> - 2008-11-07 18:34:55
|
Revision: 6384
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6384&view=rev
Author: jfaustwg
Date: 2008-11-07 18:34:47 +0000 (Fri, 07 Nov 2008)
Log Message:
-----------
r11754@iad: jfaust | 2008-11-06 17:54:46 -0800
3rd pass rename of visualizer->display: the code
Modified Paths:
--------------
pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.i
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/octree_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/octree_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/particle_cloud_2d_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/particle_cloud_2d_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/point_cloud_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/point_cloud_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/poly_line_2d_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/poly_line_2d_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_base2d_pose_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_base2d_pose_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_model_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_model_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/tf_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/tf_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/factory.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/factory.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/new_display_dialog.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/new_display_dialog.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/ogre_visualizer.i
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/selection_tool.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.i
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_panel.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_panel.h
pkg/trunk/visualization/ogre_visualizer/src/test/visualizer_test.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11744
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11754
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt 2008-11-07 18:33:52 UTC (rev 6383)
+++ pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt 2008-11-07 18:34:47 UTC (rev 6384)
@@ -36,27 +36,27 @@
${SWIG_OUTPUT_PYTHON_FILE}
COMMAND ${WXSWIG_EXECUTABLE} ${SWIG_FLAGS} -o ${SWIG_OUTPUT_CPP_FILE} -outdir ../lib -module ${PROJECT_NAME} ${SWIG_INTERFACE_FILE}
DEPENDS ${SWIG_INTERFACE_FILE}
- src/ogre_visualizer/visualizer_base.i
+ src/ogre_visualizer/display.i
src/ogre_visualizer/visualization_panel.i
src/ogre_visualizer/visualization_manager.i
src/ogre_visualizer/helpers/color.i
src/ogre_visualizer/visualization_panel.h
src/ogre_visualizer/visualization_manager.h
src/ogre_visualizer/generated/visualization_panel_generated.h
- src/ogre_visualizer/visualizer_base.h
- src/ogre_visualizer/visualizers/axes_visualizer.h
- src/ogre_visualizer/visualizers/grid_visualizer.h
- src/ogre_visualizer/visualizers/laser_scan_visualizer.h
- src/ogre_visualizer/visualizers/marker_visualizer.h
- src/ogre_visualizer/visualizers/octree_visualizer.h
- src/ogre_visualizer/visualizers/planning_visualizer.h
- src/ogre_visualizer/visualizers/point_cloud_visualizer.h
- src/ogre_visualizer/visualizers/robot_model_visualizer.h
- src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.h
- src/ogre_visualizer/visualizers/particle_cloud_2d_visualizer.h
- src/ogre_visualizer/visualizers/poly_line_2d_visualizer.h
- src/ogre_visualizer/visualizers/map_visualizer.h
- src/ogre_visualizer/visualizers/tf_visualizer.h
+ src/ogre_visualizer/display.h
+ src/ogre_visualizer/displays/axes_display.h
+ src/ogre_visualizer/displays/grid_display.h
+ src/ogre_visualizer/displays/laser_scan_display.h
+ src/ogre_visualizer/displays/marker_display.h
+ src/ogre_visualizer/displays/octree_display.h
+ src/ogre_visualizer/displays/planning_display.h
+ src/ogre_visualizer/displays/point_cloud_display.h
+ src/ogre_visualizer/displays/robot_model_display.h
+ src/ogre_visualizer/displays/robot_base2d_pose_display.h
+ src/ogre_visualizer/displays/particle_cloud_2d_display.h
+ src/ogre_visualizer/displays/poly_line_2d_display.h
+ src/ogre_visualizer/displays/map_display.h
+ src/ogre_visualizer/displays/tf_display.h
src/ogre_visualizer/helpers/color.h)
rospack_add_library(${PROJECT_NAME} src/ogre_visualizer/properties/property.cpp
@@ -67,25 +67,25 @@
src/ogre_visualizer/tools/selection_tool.cpp
src/ogre_visualizer/visualization_panel.cpp
src/ogre_visualizer/visualization_manager.cpp
- src/ogre_visualizer/visualizer_base.cpp
+ src/ogre_visualizer/display.cpp
src/ogre_visualizer/common.cpp
src/ogre_visualizer/factory.cpp
src/ogre_visualizer/ros_topic_property.cpp
src/ogre_visualizer/new_display_dialog.cpp
src/ogre_visualizer/helpers/robot.cpp
- src/ogre_visualizer/visualizers/marker_visualizer.cpp
- src/ogre_visualizer/visualizers/axes_visualizer.cpp
- src/ogre_visualizer/visualizers/grid_visualizer.cpp
- src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp
- src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp
- src/ogre_visualizer/visualizers/robot_model_visualizer.cpp
- src/ogre_visualizer/visualizers/planning_visualizer.cpp
- src/ogre_visualizer/visualizers/octree_visualizer.cpp
- src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.cpp
- src/ogre_visualizer/visualizers/particle_cloud_2d_visualizer.cpp
- src/ogre_visualizer/visualizers/poly_line_2d_visualizer.cpp
- src/ogre_visualizer/visualizers/map_visualizer.cpp
- src/ogre_visualizer/visualizers/tf_visualizer.cpp
+ src/ogre_visualizer/displays/marker_display.cpp
+ src/ogre_visualizer/displays/axes_display.cpp
+ src/ogre_visualizer/displays/grid_display.cpp
+ src/ogre_visualizer/displays/point_cloud_display.cpp
+ src/ogre_visualizer/displays/laser_scan_display.cpp
+ src/ogre_visualizer/displays/robot_model_display.cpp
+ src/ogre_visualizer/displays/planning_display.cpp
+ src/ogre_visualizer/displays/octree_display.cpp
+ src/ogre_visualizer/displays/robot_base2d_pose_display.cpp
+ src/ogre_visualizer/displays/particle_cloud_2d_display.cpp
+ src/ogre_visualizer/displays/poly_line_2d_display.cpp
+ src/ogre_visualizer/displays/map_display.cpp
+ src/ogre_visualizer/displays/tf_display.cpp
src/ogre_visualizer/generated/visualization_panel_generated.cpp
${SWIG_OUTPUT_CPP_FILE})
target_link_libraries(${PROJECT_NAME} ${wxWidgets_LIBRARIES} boost_signals ${OGRE_LIBRARIES})
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.cpp 2008-11-07 18:33:52 UTC (rev 6383)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.cpp 2008-11-07 18:34:47 UTC (rev 6384)
@@ -27,7 +27,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
-#include "visualizer_base.h"
+#include "display.h"
#include "visualization_manager.h"
#include "properties/property_manager.h"
#include "properties/property.h"
@@ -35,7 +35,7 @@
namespace ogre_vis
{
-VisualizerBase::VisualizerBase( const std::string& name, VisualizationManager* manager )
+Display::Display( const std::string& name, VisualizationManager* manager )
: vis_manager_( manager )
, scene_manager_( manager->getSceneManager() )
, name_( name )
@@ -49,7 +49,7 @@
{
}
-VisualizerBase::~VisualizerBase()
+Display::~Display()
{
if ( property_manager_ )
{
@@ -57,7 +57,7 @@
}
}
-void VisualizerBase::enable( bool force )
+void Display::enable( bool force )
{
if ( enabled_ && !force )
{
@@ -69,7 +69,7 @@
onEnable();
}
-void VisualizerBase::disable( bool force )
+void Display::disable( bool force )
{
if ( !enabled_ && !force )
{
@@ -81,23 +81,23 @@
onDisable();
}
-void VisualizerBase::setRenderCallback( boost::function<void ()> func )
+void Display::setRenderCallback( boost::function<void ()> func )
{
render_callback_ = func;
}
-void VisualizerBase::setLockRenderCallback( boost::function<void ()> func )
+void Display::setLockRenderCallback( boost::function<void ()> func )
{
render_lock_ = func;
}
-void VisualizerBase::setUnlockRenderCallback( boost::function<void ()> func )
+void Display::setUnlockRenderCallback( boost::function<void ()> func )
{
render_unlock_ = func;
}
-void VisualizerBase::causeRender()
+void Display::causeRender()
{
if ( render_callback_ )
{
@@ -105,7 +105,7 @@
}
}
-void VisualizerBase::lockRender()
+void Display::lockRender()
{
if ( render_lock_ )
{
@@ -113,7 +113,7 @@
}
}
-void VisualizerBase::unlockRender()
+void Display::unlockRender()
{
if ( render_unlock_ )
{
@@ -121,7 +121,7 @@
}
}
-void VisualizerBase::setTargetFrame( const std::string& frame )
+void Display::setTargetFrame( const std::string& frame )
{
target_frame_ = frame;
@@ -131,7 +131,7 @@
}
}
-void VisualizerBase::setFixedFrame( const std::string& frame )
+void Display::setFixedFrame( const std::string& frame )
{
fixed_frame_ = frame;
@@ -141,13 +141,13 @@
}
}
-void VisualizerBase::setPropertyManager( PropertyManager* manager, CategoryProperty* parent )
+void Display::setPropertyManager( PropertyManager* manager, CategoryProperty* parent )
{
property_manager_ = manager;
parent_category_ = parent;
- property_manager_->createProperty<BoolProperty>( "Enabled", property_prefix_, boost::bind( &VisualizerBase::isEnabled, this ),
- boost::bind( &VisualizationManager::setVisualizerEnabled, vis_manager_, this, _1 ), parent, this );
+ property_manager_->createProperty<BoolProperty>( "Enabled", property_prefix_, boost::bind( &Display::isEnabled, this ),
+ boost::bind( &VisualizationManager::setDisplayEnabled, vis_manager_, this, _1 ), parent, this );
createProperties();
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.h 2008-11-07 18:33:52 UTC (rev 6383)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.h 2008-11-07 18:34:47 UTC (rev 6384)
@@ -27,8 +27,8 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
-#ifndef OGRE_VISUALIZER_VISUALIZER_BASE
-#define OGRE_VISUALIZER_VISUALIZER_BASE
+#ifndef OGRE_VISUALIZER_DISPLAY_H
+#define OGRE_VISUALIZER_DISPLAY_H
#include <string>
#include <boost/function.hpp>
@@ -69,26 +69,26 @@
class VisualizationManager;
/**
- * \class VisualizerBase
- * \brief Abstract base class for all visualizers.
+ * \class Display
+ * \brief Abstract base class for all displays.
*
* Provides a common interface for the visualization panel to interact with,
- * so that new visualizers can be added without the visualization panel knowing anything about them.
+ * so that new displays can be added without the visualization panel knowing anything about them.
*/
-class VisualizerBase
+class Display
{
public:
- VisualizerBase( const std::string& name, VisualizationManager* manager );
- virtual ~VisualizerBase();
+ Display( const std::string& name, VisualizationManager* manager );
+ virtual ~Display();
/**
- * \brief Enable this visualizer
- * @param force If false, does not re-enable if this visualizer is already enabled. If true, it does.
+ * \brief Enable this display
+ * @param force If false, does not re-enable if this display is already enabled. If true, it does.
*/
void enable( bool force = false );
/**
- * \brief Disable this visualizer
- * @param force If false, does not re-disable if this visualizer is already disabled. If true, it does.
+ * \brief Disable this display
+ * @param force If false, does not re-disable if this display is already disabled. If true, it does.
*/
void disable( bool force = false );
@@ -114,20 +114,20 @@
void setUnlockRenderCallback( boost::function<void ()> func );
/**
- * \brief Sets the property manager and parent category for this visualizer
+ * \brief Sets the property manager and parent category for this display
* @param manager The property manager
* @param parent The parent category
*/
void setPropertyManager( PropertyManager* manager, CategoryProperty* parent );
/**
- * \brief Called from setPropertyManager, gives the visualizer a chance to create some properties immediately.
+ * \brief Called from setPropertyManager, gives the display a chance to create some properties immediately.
*
* Once this function is called, the property_manager_ member is valid and will stay valid
*/
virtual void createProperties() {}
- /// Set the target frame of this visualizer. This is a frame id which should match something being broadcast through TF.
+ /// Set the target frame of this display. This is a frame id which should match something being broadcast through TF.
void setTargetFrame( const std::string& frame );
/**
@@ -136,7 +136,7 @@
virtual void targetFrameChanged() = 0;
/**
- * \brief Set the fixed frame of this visualizer. This is a frame id which should generally be the top-level frame being broadcast through TF
+ * \brief Set the fixed frame of this display. This is a frame id which should generally be the top-level frame being broadcast through TF
* @param frame The fixed frame
*/
void setFixedFrame( const std::string& frame );
@@ -147,19 +147,19 @@
virtual void fixedFrameChanged() = 0;
/**
- * \brief Returns whether an object owned by this visualizer is pickable/mouse selectable
+ * \brief Returns whether an object owned by this display is pickable/mouse selectable
* @param object The Ogre::MovableObject to check
*/
virtual bool isObjectPickable( const Ogre::MovableObject* object ) const { return false; }
/**
- * \brief Returns the type name of this visualizer. Does not need to be exactly the same as the class name. Can contains spaces/punctuation, etc.
+ * \brief Returns the type name of this display. Does not need to be exactly the same as the class name. Can contains spaces/punctuation, etc.
* @return The type name
*/
virtual const char* getType() = 0;
/**
- * \brief Called to tell the visualizer to clear its state
+ * \brief Called to tell the display to clear its state
*/
virtual void reset() {}
@@ -184,7 +184,7 @@
VisualizationManager* vis_manager_;
Ogre::SceneManager* scene_manager_; ///< The scene manager we're associated with
- std::string name_; ///< The name of this visualizer
+ std::string name_; ///< The name of this display
bool enabled_; ///< Are we enabled?
std::string target_frame_; ///< The frame we should transform all periodically-updated data into
@@ -210,25 +210,25 @@
* \class RenderAutoLock
* \brief A scoped lock on the renderer
*
- * Constructor calls VisualizerBase::lockRender<br>
- * Destructor calls VisualizerBase::unlockRender
+ * Constructor calls Display::lockRender<br>
+ * Destructor calls Display::unlockRender
*/
class RenderAutoLock
{
public:
- RenderAutoLock( VisualizerBase* visualizer )
- : visualizer_( visualizer )
+ RenderAutoLock( Display* display )
+ : display_( display )
{
- visualizer_->lockRender();
+ display_->lockRender();
}
~RenderAutoLock()
{
- visualizer_->unlockRender();
+ display_->unlockRender();
}
private:
- VisualizerBase* visualizer_;
+ Display* display_;
};
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.i
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.i 2008-11-07 18:33:52 UTC (rev 6383)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/display.i 2008-11-07 18:34:47 UTC (rev 6384)
@@ -1,5 +1,5 @@
%{
-#include "visualizer_base.h"
+#include "display.h"
%}
%include typemaps.i
@@ -10,7 +10,7 @@
%pythonAppend VisualizerBase "self._setOORInfo(self)"
-%include "visualizer_base.h"
+%include "display.h"
%init %{
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.cpp 2008-11-07 18:33:52 UTC (rev 6383)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.cpp 2008-11-07 18:34:47 UTC (rev 6384)
@@ -27,7 +27,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
-#include "axes_visualizer.h"
+#include "axes_display.h"
#include "visualization_manager.h"
#include "properties/property.h"
#include "properties/property_manager.h"
@@ -43,8 +43,8 @@
namespace ogre_vis
{
-AxesVisualizer::AxesVisualizer( const std::string& name, VisualizationManager* manager )
-: VisualizerBase( name, manager )
+AxesDisplay::AxesDisplay( const std::string& name, VisualizationManager* manager )
+: Display( name, manager )
, length_( 1.0 )
, radius_( 0.1 )
{
@@ -58,29 +58,29 @@
axes_->setUserData( Ogre::Any( (void*)this ) );
}
-AxesVisualizer::~AxesVisualizer()
+AxesDisplay::~AxesDisplay()
{
delete axes_;
}
-void AxesVisualizer::onEnable()
+void AxesDisplay::onEnable()
{
axes_->getSceneNode()->setVisible( true );
}
-void AxesVisualizer::onDisable()
+void AxesDisplay::onDisable()
{
axes_->getSceneNode()->setVisible( false );
}
-void AxesVisualizer::create()
+void AxesDisplay::create()
{
axes_->set( length_, radius_ );
causeRender();
}
-void AxesVisualizer::set( float length, float radius )
+void AxesDisplay::set( float length, float radius )
{
length_ = length;
radius_ = radius;
@@ -98,28 +98,28 @@
}
}
-void AxesVisualizer::setLength( float length )
+void AxesDisplay::setLength( float length )
{
set( length, radius_ );
}
-void AxesVisualizer::setRadius( float radius )
+void AxesDisplay::setRadius( float radius )
{
set( length_, radius );
}
-void AxesVisualizer::createProperties()
+void AxesDisplay::createProperties()
{
- length_property_ = property_manager_->createProperty<FloatProperty>( "Length", property_prefix_, boost::bind( &AxesVisualizer::getLength, this ),
- boost::bind( &AxesVisualizer::setLength, this, _1 ), parent_category_, this );
+ length_property_ = property_manager_->createProperty<FloatProperty>( "Length", property_prefix_, boost::bind( &AxesDisplay::getLength, this ),
+ boost::bind( &AxesDisplay::setLength, this, _1 ), parent_category_, this );
length_property_->setMin( 0.0001 );
- radius_property_ = property_manager_->createProperty<FloatProperty>( "Radius", property_prefix_, boost::bind( &AxesVisualizer::getRadius, this ),
- boost::bind( &AxesVisualizer::setRadius, this, _1 ), parent_category_, this );
+ radius_property_ = property_manager_->createProperty<FloatProperty>( "Radius", property_prefix_, boost::bind( &AxesDisplay::getRadius, this ),
+ boost::bind( &AxesDisplay::setRadius, this, _1 ), parent_category_, this );
radius_property_->setMin( 0.0001 );
}
-const char* AxesVisualizer::getDescription()
+const char* AxesDisplay::getDescription()
{
return "Displays a set of Axes at the origin of the target frame of reference.";
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.h 2008-11-07 18:33:52 UTC (rev 6383)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/axes_display.h 2008-11-07 18:34:47 UTC (rev 6384)
@@ -27,10 +27,10 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
-#ifndef OGRE_VISUALIZER_AXES_VISUALIZER_H
-#define OGRE_VISUALIZER_AXES_VISUALIZER_H
+#ifndef OGRE_VISUALIZER_AXES_DISPLAY_H
+#define OGRE_VISUALIZER_AXES_DISPLAY_H
-#include "visualizer_base.h"
+#include "display.h"
namespace ogre_tools
{
@@ -43,14 +43,14 @@
class FloatProperty;
/**
- * \class AxesVisualizer
+ * \class AxesDisplay
* \brief Displays a set of XYZ axes at the origin
*/
-class AxesVisualizer : public VisualizerBase
+class AxesDisplay : public Display
{
public:
- AxesVisualizer( const std::string& name, VisualizationManager* manager );
- virtual ~AxesVisualizer();
+ AxesDisplay( const std::string& name, VisualizationManager* manager );
+ virtual ~AxesDisplay();
/**
* \brief Set the parameters for the axes
@@ -64,7 +64,7 @@
void setRadius( float radius );
float getRadius() { return radius_; }
- // Overrides from VisualizerBase
+ // Overrides from Display
virtual void targetFrameChanged() {}
virtual void fixedFrameChanged() {}
virtual void createProperties();
@@ -79,7 +79,7 @@
*/
void create();
- // overrides from VisualizerBase
+ // overrides from Display
virtual void onEnable();
virtual void onDisable();
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_display.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_display.cpp 2008-11-07 18:33:52 UTC (rev 6383)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/grid_display.cpp 2008-11-07 18:34:47 UTC (rev 6384)
@@ -27,7 +27,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
-#include "grid_visualizer.h"
+#include "grid_display.h"
#include "common.h"
#include "visualization_manager.h"
#include "properties/property.h"
@@ -43,8 +43,8 @@
namespace ogre_vis
{
-GridVisualizer::GridVisualizer( const std::string& name, VisualizationManager* manager )
-: VisualizerBase( name, manager )
+GridDisplay::GridDisplay( const std::string& name, VisualizationManager* manager )
+: Display( name, manager )
, cell_size_( 1.0f )
, cell_count_( 10 )
, color_( 0.5, 0.5, 0.5 )
@@ -59,29 +59,29 @@
grid_->getSceneNode()->setOrientation( orient );*/
}
-GridVisualizer::~GridVisualizer()
+GridDisplay::~GridDisplay()
{
delete grid_;
}
-void GridVisualizer::onEnable()
+void GridDisplay::onEnable()
{
grid_->getSceneNode()->setVisible( true );
}
-void GridVisualizer::onDisable()
+void GridDisplay::onDisable()
{
grid_->getSceneNode()->setVisible( false );
}
-void GridVisualizer::create()
+void GridDisplay::create()
{
grid_->set( cell_count_, cell_size_, color_.r_, color_.g_, color_.b_ );
causeRender();
}
-void GridVisualizer::set( uint32_t cell_count, float cell_size, const Color& color )
+void GridDisplay::set( uint32_t cell_count, float cell_size, const Color& color )
{
cell_count_ = cell_count;
cell_size_ = cell_size;
@@ -105,36 +105,36 @@
}
}
-void GridVisualizer::setCellSize( float size )
+void GridDisplay::setCellSize( float size )
{
set( cell_count_, size, color_ );
}
-void GridVisualizer::setCellCount( uint32_t count )
+void GridDisplay::setCellCount( uint32_t count )
{
set( count, cell_size_, color_ );
}
-void GridVisualizer::setColor( const Color& color )
+void GridDisplay::setColor( const Color& color )
{
set( cell_count_, cell_size_, color );
}
-void GridVisualizer::createProperties()
+void GridDisplay::createProperties()
{
- cellcount_property_ = property_manager_->createProperty<IntProperty>( "Cell Count", property_prefix_, boost::bind( &GridVisualizer::getCellCount, this ),
- boost::bind( &GridVisualizer::setCellCount, this, _1 ), parent_category_, this );
+ cellcount_property_ = property_manager_->createProperty<IntProperty>( "Cell Count", property_prefix_, boost::bind( &GridDisplay::getCellCount, this ),
+ boost::bind( &GridDisplay::setCellCount, this, _1 ), parent_category_, this );
cellcount_property_->setMin( 1 );
- cellsize_property_ = property_manager_->createProperty<FloatProperty>( "Cell Size", property_prefix_, boost::bind( &GridVisualizer::getCellSize, this ),
- boost::bind( &GridVisualizer::setCellSize, this, _1 ), parent_category_, this );
+ cellsize_property_ = property_manager_->createProperty<FloatProperty>( "Cell Size", property_prefix_, boost::bind( &GridDisplay::getCellSize, this ),
+ boost::bind( &GridDisplay::setCellSize, this, _1 ), parent_category_, this );
cellsize_property_->setMin( 0.0001 );
- color_property_ = property_manager_->createProperty<ColorProperty>( "Color", property_prefix_, boost::bind( &GridVisualizer::getColor, this ),
- boost::bind( &GridVisualizer::setColor, this, _1 ), parent_category_, this );
+ color_property_ = property_manager_->createProperty<ColorProperty>( "Color", property_prefix_, boost::bind( &GridDisplay::getColor, this ),
+ ...
[truncated message content] |
|
From: <jfa...@us...> - 2008-11-07 18:35:06
|
Revision: 6385
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6385&view=rev
Author: jfaustwg
Date: 2008-11-07 18:35:00 +0000 (Fri, 07 Nov 2008)
Log Message:
-----------
r11755@iad: jfaust | 2008-11-06 18:01:37 -0800
Made pr2_gui use the new pr2 config instead of instantiating the displays directly in code
Modified Paths:
--------------
pkg/trunk/visualization/pr2_gui/src/pr2_gui/visualizer_panel.py
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11754
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/visualization/pr2_gui/src/pr2_gui/visualizer_panel.py
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/pr2_gui/visualizer_panel.py 2008-11-07 18:34:47 UTC (rev 6384)
+++ pkg/trunk/visualization/pr2_gui/src/pr2_gui/visualizer_panel.py 2008-11-07 18:35:00 UTC (rev 6385)
@@ -43,45 +43,7 @@
def createDefaultVisualizers(self):
manager = self.getManager();
- manager.createGridVisualizer( "Grid", True )
- manager.createAxesVisualizer( "Origin Axes", False )
- manager.createMarkerVisualizer( "Visualization Markers", True )
+ config_path = rostools.packspec.get_pkg_dir( "ogre_visualizer" ) + "/configs/pr2.vcg"
+ config = wx.FileConfig(localFilename=config_path)
+ manager.loadConfig(config)
- robot_vis = manager.createRobotModelVisualizer( "Robot Model", False )
- robot_vis.setRobotDescription( "robotdesc/pr2" )
-
- manager.createRobotBase2DPoseVisualizer( "2D Pose: Odom", True )
- localized_pose = manager.createRobotBase2DPoseVisualizer("2D Pose: Localized", True)
- localized_pose.setTopic("localizedpose")
- localized_pose.setColor(ogre_visualizer.Color(0.0, 0.1, 0.8))
-
- manager.createMapVisualizer("Map", True)
-
- planning = manager.createPlanningVisualizer( "Planning", False )
- planning.initialize( "robotdesc/pr2", "display_kinematic_path" )
-
- point_cloud = manager.createPointCloudVisualizer( "Stereo Full Cloud", True )
- point_cloud.setTopic( "videre/cloud" )
- point_cloud.setColor( ogre_visualizer.Color(1.0, 1.0, 1.0) )
-
- point_cloud = manager.createPointCloudVisualizer( "Head Full Cloud", True )
- point_cloud.setTopic( "full_cloud" )
- point_cloud.setColor( ogre_visualizer.Color(1.0, 1.0, 0.0) )
-
- point_cloud = manager.createPointCloudVisualizer( "World 3D Map", True )
- point_cloud.setTopic( "world_3d_map" )
- point_cloud.setColor( ogre_visualizer.Color(1.0, 0.0, 0.0) )
- point_cloud.setBillboardSize( 0.01 )
-
- laser_scan = manager.createLaserScanVisualizer( "Head Scan", True )
- laser_scan.setScanTopic( "tilt_scan" )
- laser_scan.setColor( ogre_visualizer.Color(1.0, 0.0, 0.0) )
- laser_scan.setDecayTime( 30.0 )
-
- laser_scan = manager.createLaserScanVisualizer( "Floor Scan", True )
- laser_scan.setScanTopic( "base_scan" )
- laser_scan.setColor( ogre_visualizer.Color(0.0, 1.0, 0.0) )
- laser_scan.setDecayTime( 0.0 )
-
- manager.createOctreeVisualizer( "Octree", True ).setOctreeTopic( "full_octree" )
-
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-07 20:05:21
|
Revision: 6391
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6391&view=rev
Author: hsujohnhsu
Date: 2008-11-07 20:05:16 +0000 (Fri, 07 Nov 2008)
Log Message:
-----------
ability to tune quick step in xml.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test_headless.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_headless.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1_headless.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg_headless.world
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-11-07 19:50:50 UTC (rev 6390)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-11-07 20:05:16 UTC (rev 6391)
@@ -194,30 +194,6 @@
/// Pointer to the stereo data
public: StereoCameraData *data;
};
-Index: server/physics/MapGeom.cc
-===================================================================
---- server/physics/MapGeom.cc (revision 7049)
-+++ server/physics/MapGeom.cc (working copy)
-@@ -307,7 +307,9 @@
-
- int diff = labs(freePixels - occPixels);
-
-- if (diff > this->granularityP->GetValue())
-+ //if (diff > this->granularityP->GetValue())
-+ // use pixel size for granularity instead of occupancy
-+ if (node->width*node->height > this->granularityP->GetValue())
- {
- float newX, newY;
- float newW, newH;
-@@ -353,7 +355,7 @@
- newY += ceil(newH);
- }
-
-- node->occupied = true;
-+ node->occupied = false;
- node->leaf = false;
- }
- else if (occPixels == 0)
Index: server/physics/SphereGeom.cc
===================================================================
--- server/physics/SphereGeom.cc (revision 7049)
@@ -354,6 +330,30 @@
}
//////////////////////////////////////////////////////////////////////////////
+Index: server/physics/MapGeom.cc
+===================================================================
+--- server/physics/MapGeom.cc (revision 7049)
++++ server/physics/MapGeom.cc (working copy)
+@@ -307,7 +307,9 @@
+
+ int diff = labs(freePixels - occPixels);
+
+- if (diff > this->granularityP->GetValue())
++ //if (diff > this->granularityP->GetValue())
++ // use pixel size for granularity instead of occupancy
++ if (node->width*node->height > this->granularityP->GetValue())
+ {
+ float newX, newY;
+ float newW, newH;
+@@ -353,7 +355,7 @@
+ newY += ceil(newH);
+ }
+
+- node->occupied = true;
++ node->occupied = false;
+ node->leaf = false;
+ }
+ else if (occPixels == 0)
Index: server/physics/Geom.cc
===================================================================
--- server/physics/Geom.cc (revision 7049)
@@ -726,11 +726,13 @@
===================================================================
--- server/physics/ode/ODEPhysics.hh (revision 7049)
+++ server/physics/ode/ODEPhysics.hh (working copy)
-@@ -133,6 +133,7 @@
+@@ -133,6 +133,9 @@
private: ParamT<double> *globalCFMP;
private: ParamT<double> *globalERPP;
+ private: ParamT<bool> *quickStepP;
++ private: ParamT<int> *quickStepItersP;
++ private: ParamT<double> *quickStepWP;
};
/** \}*/
@@ -749,15 +751,17 @@
using namespace gazebo;
////////////////////////////////////////////////////////////////////////////////
-@@ -70,6 +74,7 @@
+@@ -70,6 +74,9 @@
Param::Begin(&this->parameters);
this->globalCFMP = new ParamT<double>("cfm", 10e-5, 0);
this->globalERPP = new ParamT<double>("erp", 0.2, 0);
+ this->quickStepP = new ParamT<bool>("quickStep", false, 0);
++ this->quickStepItersP = new ParamT<int>("quickStepIters", 20, 0);
++ this->quickStepWP = new ParamT<double>("quickStepW", 1.3, 0); /// over_relaxation value for SOR
Param::End();
}
-@@ -88,6 +93,7 @@
+@@ -88,6 +95,7 @@
delete this->globalCFMP;
delete this->globalERPP;
@@ -765,15 +769,17 @@
}
////////////////////////////////////////////////////////////////////////////////
-@@ -103,6 +109,7 @@
+@@ -103,6 +111,9 @@
this->updateRateP->Load(cnode);
this->globalCFMP->Load(cnode);
this->globalERPP->Load(cnode);
+ this->quickStepP->Load(cnode);
++ this->quickStepItersP->Load(cnode);
++ this->quickStepWP->Load(cnode);
}
////////////////////////////////////////////////////////////////////////////////
-@@ -115,6 +122,7 @@
+@@ -115,6 +126,7 @@
stream << prefix << " " << *(this->updateRateP) << "\n";
stream << prefix << " " << *(this->globalCFMP) << "\n";
stream << prefix << " " << *(this->globalERPP) << "\n";
@@ -781,7 +787,16 @@
stream << prefix << "</physics:ode>\n";
}
-@@ -133,13 +141,29 @@
+@@ -126,20 +138,37 @@
+ dWorldSetGravity(this->worldId, g.x, g.y, g.z);
+ dWorldSetCFM(this->worldId, this->globalCFMP->GetValue());
+ dWorldSetERP(this->worldId, this->globalERPP->GetValue());
+-
++ dWorldSetQuickStepNumIterations(this->worldId, this->quickStepItersP->GetValue() );
++ dWorldSetQuickStepW(this->worldId, this->quickStepWP->GetValue() );
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
// Update the ODE engine
void ODEPhysics::Update()
{
@@ -813,7 +828,7 @@
// Very important to clear out the contact group
dJointGroupEmpty( this->contactGroup );
-@@ -266,15 +290,16 @@
+@@ -266,15 +295,16 @@
contact.surface.mode = dContactSlip1 | dContactSlip2 |
dContactSoftERP | dContactSoftCFM |
dContactBounce | dContactMu2 | dContactApprox1;
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test_headless.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test_headless.world 2008-11-07 19:50:50 UTC (rev 6390)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test_headless.world 2008-11-07 20:05:16 UTC (rev 6391)
@@ -30,6 +30,8 @@
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
<quickStep>true</quickStep>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
</physics:ode>
<!-- Rendering Engine -->
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_headless.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_headless.world 2008-11-07 19:50:50 UTC (rev 6390)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_headless.world 2008-11-07 20:05:16 UTC (rev 6391)
@@ -29,6 +29,8 @@
<cfm>0.000000000001</cfm>
<erp>0.2</erp>
<quickStep>true</quickStep>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
</physics:ode>
<geo:origin>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1_headless.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1_headless.world 2008-11-07 19:50:50 UTC (rev 6390)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1_headless.world 2008-11-07 20:05:16 UTC (rev 6391)
@@ -29,6 +29,8 @@
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
<quickStep>true</quickStep>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
</physics:ode>
<geo:origin>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg_headless.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg_headless.world 2008-11-07 19:50:50 UTC (rev 6390)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg_headless.world 2008-11-07 20:05:16 UTC (rev 6391)
@@ -29,6 +29,8 @@
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
<quickStep>true</quickStep>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
</physics:ode>
<geo:origin>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-07 20:37:16
|
Revision: 6395
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6395&view=rev
Author: hsujohnhsu
Date: 2008-11-07 20:37:09 +0000 (Fri, 07 Nov 2008)
Log Message:
-----------
all simulations should run at 1kHz with throttle gui updates.
removed all headless simulations.
Modified Paths:
--------------
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-fake_localization.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-fake_localization.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-params.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_multi_link.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_obs.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_obstacle.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_pr2d2.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_single_link.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_test_coord.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/controllers_gazebo_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/controllers_gazebo_multi_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/gazebo/controllers_gazebo_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/gazebo/controllers_gazebo_single_link.xml
Removed Paths:
-------------
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-headless-fake_localization.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-headless.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-headless-fake_localization.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-headless.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-headless-fake_localization.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-headless.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/controllers_gazebo_headless.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_gazebo_actuators_headless.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test_headless.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_headless.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1_headless.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg_headless.world
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/pr2_gazebo_actuators_headless.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/gazebo/controllers_gazebo_arm_headless.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/gazebo/pr2_arm_gazebo_actuators_headless.xml
pkg/trunk/robot_descriptions/wg_robot_description/send_arm_test_headless.xml
Deleted: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-headless-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-headless-fake_localization.xml 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-headless-fake_localization.xml 2008-11-07 20:37:09 UTC (rev 6395)
@@ -1,19 +0,0 @@
-<launch>
- <master auto="start"/>
- <group name="wg">
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_headless.launch"/>
- <include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" output="screen" />
- <node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
- <node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
-
- <!-- for visualization -->
- <!--node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
-
- <!-- for manual control -->
- <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
- <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
- </group>
-</launch>
-
Deleted: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-headless.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-headless.xml 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-headless.xml 2008-11-07 20:37:09 UTC (rev 6395)
@@ -1,19 +0,0 @@
-<launch>
- <master auto="start"/>
- <group name="wg">
- <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_headless.launch"/>
- <include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" output="screen" />
- <node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" />
- <node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
-
- <!-- for visualization -->
- <!--node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
-
- <!-- for manual control -->
- <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
- <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
- </group>
-</launch>
-
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-fake_localization.xml 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-fake_localization.xml 2008-11-07 20:37:09 UTC (rev 6395)
@@ -7,7 +7,13 @@
<node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
<node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
+
+ <!-- for visualization -->
+ <!--node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
+
+ <!-- for manual control -->
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
</group>
</launch>
Deleted: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-headless-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-headless-fake_localization.xml 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-headless-fake_localization.xml 2008-11-07 20:37:09 UTC (rev 6395)
@@ -1,19 +0,0 @@
-<launch>
- <master auto="start"/>
- <group name="wg">
- <include file="$(find pr2_gazebo)/pr2_headless.launch"/>
- <include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" output="screen" />
- <node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
- <node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
-
- <!-- for visualization -->
- <!--node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
-
- <!-- for manual control -->
- <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
- <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
- </group>
-</launch>
-
Deleted: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-headless.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-headless.xml 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple-headless.xml 2008-11-07 20:37:09 UTC (rev 6395)
@@ -1,19 +0,0 @@
-<launch>
- <master auto="start"/>
- <group name="wg">
- <include file="$(find pr2_gazebo)/pr2_headless.launch"/>
- <include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" output="screen" />
- <node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" output="screen" />
- <node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
-
- <!-- for visualization -->
- <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
-
- <!-- for manual control -->
- <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
- <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
- </group>
-</launch>
-
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple.xml 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-simple.xml 2008-11-07 20:37:09 UTC (rev 6395)
@@ -1,24 +1,19 @@
<launch>
<master auto="start"/>
<group name="wg">
- <!-- this is another script that starts up a simple world with PR2 in it -->
<include file="$(find pr2_gazebo)/pr2.launch"/>
<include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" output="screen" />
<node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" output="screen" />
<node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
- <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
- </group>
- <!-- Test for publication of 'tilt_scan' topic. -->
- <test test-name="hztest_test_amcl1" pkg="rostest" type="hztest" name="amcl_test">
- <param name="topic" value="localizedpose" />
- <param name="hz" value="5.0" />
- <param name="hzerror" value="3.0" />
- <param name="test_duration" value="5.0" />
- <param name="check_intervals" value="false" />
- </test>
+ <!-- for visualization -->
+ <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
+ <!-- for manual control -->
+ <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
+ </group>
</launch>
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-fake_localization.xml 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-fake_localization.xml 2008-11-07 20:37:09 UTC (rev 6395)
@@ -9,7 +9,7 @@
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
<!-- for visualization -->
- <!--node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
+ <!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
<!-- for manual control -->
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
Deleted: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-headless-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-headless-fake_localization.xml 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-headless-fake_localization.xml 2008-11-07 20:37:09 UTC (rev 6395)
@@ -1,19 +0,0 @@
-<launch>
- <master auto="start"/>
- <group name="wg">
- <include file="$(find pr2_gazebo)/pr2_wg_headless.launch"/>
- <include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" output="screen" />
- <node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
- <node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
-
- <!-- for visualization -->
- <!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
-
- <!-- for manual control -->
- <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
- <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
- </group>
-</launch>
-
Deleted: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-headless.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-headless.xml 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg-headless.xml 2008-11-07 20:37:09 UTC (rev 6395)
@@ -1,19 +0,0 @@
-<launch>
- <master auto="start"/>
- <group name="wg">
- <include file="$(find pr2_gazebo)/pr2_wg_headless.launch"/>
- <include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" output="screen" />
- <node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" />
- <node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
-
- <!-- for visualization -->
- <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
-
- <!-- for manual control -->
- <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
- <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
- </group>
-</launch>
-
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg.xml 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-wg.xml 2008-11-07 20:37:09 UTC (rev 6395)
@@ -3,11 +3,17 @@
<group name="wg">
<include file="$(find pr2_gazebo)/pr2_wg.launch"/>
<include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" />
+ <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" output="screen" />
<node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" />
- <node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" />
- <node pkg="nav_view" type="nav_view" respawn="false" />
- <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" /-->
+ <node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
+ <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
+
+ <!-- for visualization -->
+ <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
+
+ <!-- for manual control -->
+ <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
</group>
</launch>
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-params.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-params.xml 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-params.xml 2008-11-07 20:37:09 UTC (rev 6395)
@@ -6,7 +6,7 @@
<param name="robot_x_start" value="0" />
<param name="robot_y_start" value="0" />
<param name="robot_th_start" value="0" />
- <param name="robot_radius" value="0.325" />
+ <param name="robot_radius" value="0.3" />
<param name="dist_penalty" value="2.0" />
<param name="pf_laser_max_beams" value="20"/>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt 2008-11-07 20:37:09 UTC (rev 6395)
@@ -18,11 +18,6 @@
DEPENDS urdf2gazebo
COMMENT "Building Gazebo model for PR2 Standard Model")
-add_custom_target(pr2_gazebo_model_headless ALL
- COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/pr2/gazebo/pr2_gazebo_actuators_headless.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_headless.model
- DEPENDS urdf2gazebo
- COMMENT "Building Gazebo model for PR2 Gazebo Actuators")
-
add_custom_target(pr2_gazebo_model_nolimit ALL
COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/pr2/gazebo/pr2_gazebo_actuators.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_nolimit.model 1
DEPENDS urdf2gazebo
@@ -40,11 +35,6 @@
DEPENDS urdf2gazebo
COMMENT "Building Gazebo model for PR2 Arm Test")
-add_custom_target(pr2_gazebo_model_arm_headless ALL
- COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/pr2_arm_test/gazebo/pr2_arm_gazebo_actuators_headless.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_arm_headless.model
- DEPENDS urdf2gazebo
- COMMENT "Building Gazebo model for PR2 Arm Test Headless")
-
# single_link_test
add_custom_target(pr2_gazebo_model_single_link ALL
COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/single_link_test/gazebo/pr2_single_link_gazebo_actuators.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_single_link.model
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/controllers_gazebo_headless.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/controllers_gazebo_headless.xml 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/controllers_gazebo_headless.xml 2008-11-07 20:37:09 UTC (rev 6395)
@@ -1 +0,0 @@
-link ../../wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_gazebo_actuators_headless.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_gazebo_actuators_headless.xml 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_gazebo_actuators_headless.xml 2008-11-07 20:37:09 UTC (rev 6395)
@@ -1 +0,0 @@
-link ../../wg_robot_description/pr2/gazebo/pr2_gazebo_actuators_headless.xml
\ No newline at end of file
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-07 20:37:09 UTC (rev 6395)
@@ -24,11 +24,13 @@
<!-- erp is typically .1-.8 -->
<!-- here's the global contact cfm/erp -->
<physics:ode>
- <stepTime>0.01</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
- <cfm>0.0000000001</cfm>
+ <cfm>0.000000000001</cfm>
<erp>0.2</erp>
- <quickStep>false</quickStep>
+ <quickStep>true</quickStep>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
</physics:ode>
<geo:origin>
@@ -57,7 +59,7 @@
</sky>
<gazeboPath>media</gazeboPath>
<grid>false</grid>
- <maxUpdateRate>100</maxUpdateRate>
+ <maxUpdateRate>10</maxUpdateRate>
</rendering:ogre>
@@ -319,7 +321,7 @@
<controller:ros_time name="ros_time" plugin="libRos_Time.so">
<alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
+ <updateRate>1000.0</updateRate>
<interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
</controller:ros_time>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world 2008-11-07 20:37:09 UTC (rev 6395)
@@ -25,11 +25,13 @@
<!-- erp is typically .1-.8 -->
<!-- here's the global contact cfm/erp -->
<physics:ode>
- <stepTime>0.01</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
- <quickStep>false</quickStep>
+ <quickStep>true</quickStep>
+ <quickStepIters>3</quickStepIters>
+ <quickStepW>1.3</quickStepW>
</physics:ode>
<!-- Rendering Engine -->
@@ -55,7 +57,7 @@
</sky>
<gazeboPath>media</gazeboPath>
<grid>false</grid>
- <maxUpdateRate>100</maxUpdateRate>
+ <maxUpdateRate>10</maxUpdateRate>
</rendering:ogre>
<!-- White Directional light -->
@@ -68,7 +70,7 @@
<attenuation>1 0.0 1.0 0.4</attenuation>
</light>
</model:renderable>
-
+
<!-- Ground plane -->
<model:physical name="gplane">
@@ -158,7 +160,7 @@
<!-- Broadcat time over ROS -->
<controller:ros_time name="ros_time" plugin="libRos_Time.so">
<alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
+ <updateRate>1000.0</updateRate>
<interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
</controller:ros_time>
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test_headless.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test_headless.world 2008-11-07 20:35:19 UTC (rev 6394)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test_headless.world 2008-11-07 20:37:09 UTC (rev 6395)
@@ -1,181 +0,0 @@
-<?xml version="1.0"?>
-
-<gazebo:world
- xmlns:xi="http://www.w3.org/2001/XInclude"
- xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
- xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
- xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
- xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
- xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
- xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
- xmlns:geo="http://willowgarage.com/xmlschema/#geo"
- xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
- xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
- xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
- xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
- xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
- xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
- xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
- xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >
-
- <verbosity>5</verbosity>
-
- <!-- Physics Engine: ODE parameters -->
- <!-- cfm is 1e-5 for single precision -->
- <!-- erp is typically .1-.8 -->
- <!-- here's the global contact cfm/erp -->
- <physics:ode>
- <stepTime>0.001</stepTime>
- <gravity>0 0 -9.8</gravity>
- <cfm>0.0000000001</cfm>
- <erp>0.2</erp>
- <quickStep>true</quickStep>
- <quickStepIters>3</quickStepIters>
- <quickStepW>1.3</quickStepW>
- </physics:ode>
-
- <!-- Rendering Engine -->
- <rendering:gui>
- <type>fltk</type>
- <size>1024 800</size>
- <pos>0 0</pos>
- <frames>
- <row height="100%">
- <camera width="100%">
- <xyz> .25 1.5 2.5</xyz>
- <rpy> 0 50 -90</rpy>
- </camera>
- </row>
- </frames>
- </rendering:gui>
-
- <!-- GUI Camera -->
- <rendering:ogre>
- <ambient>1.0 1.0 1.0 1.0</ambient>
- <sky>
- <material>Gazebo/CloudySky</material>
- </sky>
- <gazeboPath>media</gazeboPath>
- <grid>false</grid>
- <maxUpdateRate>100</maxUpdateRate>
- </rendering:ogre>
-
- <!-- White Directional light -->
- <model:renderable name="directional_white">
- <light>
- <type>directional</type>
- <direction>0 -0.5 -0.5</direction>
- <diffuseColor>0.4 0.4 0.4</diffuseColor>
- <specularColor>0.0 0.0 0.0</specularColor>
- <attenuation>1 0.0 1.0 0.4</attenuation>
- </light>
- </model:renderable>
-
-
- <!-- Ground plane -->
- <model:physical name="gplane">
- <xyz>0 0 0</xyz>
- <rpy>0 0 0</rpy>
- <static>true</static>
-
- <body:plane name="plane">
- <geom:plane name="plane">
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <normal>0 0 1</normal>
- <size>51.3 51.3</size>
- <material>Gazebo/White</material>
- </geom:plane>
- </body:plane>
- </model:physical>
-
- <!-- Object1: The small cylinder "cup" -->
- <model:physical name="cylinder1_model">
- <xyz> 0.5 0.2 0.7</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
- <static>true</static>
- <body:cylinder name="cylinder1_body">
- <geom:cylinder name="cylinder1_geom">
- <kp>100000000.0</kp>
- <kd>0.1</kd>
- <mesh>default</mesh>
- <size>0.025 0.075</size>
- <mass> 0.05</mass>
- <visual>
- <size> 0.05 0.05 0.075</size>
- <material>Gazebo/PioneerBody</material>
- <mesh>unit_cylinder</mesh>
- </visual>
- </geom:cylinder>
- <geom:box name="cylinder1_base_geom">
- <kp>100000000.0</kp>
- <kd>0.1</kd>
- <mesh>default</mesh>
- <xyz>0.0 0.0 -0.033</xyz>
- <size>0.05 0.05 0.01</size>
- <mass> 0.01</mass>
- <visual>
- <size> 0.05 0.05 0.01</size>
- <material>Gazebo/Fish</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:cylinder>
- </model:physical>
-
- <!-- Object2: The small box "cup" -->
- <model:physical name="object1_model">
- <xyz> 0.835 -0.55 0.5</xyz>
- <rpy> 0.0 0.0 30.0</rpy>
- <static>true</static>
- <body:box name="object1_body">
- <geom:box name="object1_geom">
- <kp>100000000.0</kp>
- <kd>0.1</kd>
- <mesh>default</mesh>
- <size>0.1 0.03 0.03</size>
- <mass> 0.05</mass>
- <visual>
- <size> 0.1 0.030 0.03</size>
- <material>Gazebo/PioneerBody</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:box>
- <controller:P3D name="p3d_object_controller" plugin="libP3D.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <bodyName>object1_body</bodyName>
- <topicName>object1_ground_truty</topicName>
- <frameName>map</frameName>
- <interface:position name="p3d_object_position"/>
- </controller:P3D>
- </model:physical>
-
-
-
- <!-- Robot Arm -->
- <model:physical name="robot_model1">
-
- <!-- Broadcat time over ROS -->
- <controller:ros_time name="ros_time" plugin="libRos_Time.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
- <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
- </controller:ros_time>
-
- <!-- location of robot in the world -->
- <xyz>0.0 0.0 0.0408</xyz>
- <rpy>0.0 0.0 0.0</rpy>
-
- <!-- base and arm -->
- <include embedded="true">
- ...
[truncated message content] |
|
From: <hsu...@us...> - 2008-11-08 00:56:03
|
Revision: 6422
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6422&view=rev
Author: hsujohnhsu
Date: 2008-11-08 00:55:51 +0000 (Sat, 08 Nov 2008)
Log Message:
-----------
no more controllers_gazebo_*.xml
controllers moved into world file, pending move to urdf2gazebo.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/arm.launch
pkg/trunk/demos/examples_gazebo/dual_link.xml
pkg/trunk/demos/examples_gazebo/multi_link.xml
pkg/trunk/demos/examples_gazebo/pr2d2.xml
pkg/trunk/demos/examples_gazebo/single_link.xml
pkg/trunk/demos/pr2_gazebo/pr2.launch
pkg/trunk/demos/pr2_gazebo/pr2_wg.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_multi_link.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_single_link.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/base_defs.xml
Added Paths:
-----------
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_tilt_laser_torso_gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/pr2_prototype1_xacroed.xml
Removed Paths:
-------------
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/
pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/gazebo/
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/controllers_gazebo_prototype1.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/controllers_head_tilt_laser_torso_gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/pr2_prototype1.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/pr2_prototype1_gazebo_actuators.xml
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/gazebo/
Modified: pkg/trunk/demos/arm_gazebo/arm.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm.launch 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/demos/arm_gazebo/arm.launch 2008-11-08 00:55:51 UTC (rev 6422)
@@ -1,5 +1,8 @@
<launch>
<group name="wg">
+ <!-- create model file for gazebo -->
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2_arm_test/pr2_arm.xml $(find gazebo_robot_description)/world/pr2_xml_arm.model" />
+ <!-- send pr2_arm.xml to param server -->
<param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2_arm_test/pr2_arm.xml"" />
<!-- -g flag runs gazebo in gui-less mode -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_arm_test.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/examples_gazebo/dual_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/dual_link.xml 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/demos/examples_gazebo/dual_link.xml 2008-11-08 00:55:51 UTC (rev 6422)
@@ -1,5 +1,8 @@
<launch>
<group name="wg">
+ <!-- create model file for gazebo -->
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/dual_link_test/pr2_dual_link.xml $(find gazebo_robot_description)/world/pr2_xml_dual_link.model" />
+ <!-- send pr2_arm.xml to param server -->
<param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/dual_link_test/pr2_dual_link.xml"" />
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_dual_link.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
Modified: pkg/trunk/demos/examples_gazebo/multi_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/multi_link.xml 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/demos/examples_gazebo/multi_link.xml 2008-11-08 00:55:51 UTC (rev 6422)
@@ -1,5 +1,8 @@
<launch>
<group name="wg">
+ <!-- create model file for gazebo -->
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/multi_link_test/pr2_multi_link.xml $(find gazebo_robot_description)/world/pr2_xml_multi_link.model" />
+ <!-- send pr2_arm.xml to param server -->
<param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/multi_link_test/pr2_multi_link.xml"" />
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_multi_link.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
Modified: pkg/trunk/demos/examples_gazebo/pr2d2.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/pr2d2.xml 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/demos/examples_gazebo/pr2d2.xml 2008-11-08 00:55:51 UTC (rev 6422)
@@ -2,6 +2,9 @@
<!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/wg_robot_description/pr2_arm -->
<master auto="start" />
<group name="wg">
+ <!-- create model file for gazebo -->
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2_arm/pr2d2.xml $(find gazebo_robot_description)/world/pr2d2_xml.model" />
+ <!-- send pr2_arm.xml to param server -->
<param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2_arm/pr2d2.xml"" />
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_pr2d2.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
Modified: pkg/trunk/demos/examples_gazebo/single_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/single_link.xml 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/demos/examples_gazebo/single_link.xml 2008-11-08 00:55:51 UTC (rev 6422)
@@ -1,5 +1,8 @@
<launch>
<group name="wg">
+ <!-- create model file for gazebo -->
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/single_link_test/pr2_single_link.xml $(find gazebo_robot_description)/world/pr2_xml_single_link.model" />
+ <!-- send pr2_arm.xml to param server -->
<param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/single_link_test/pr2_single_link.xml"" />
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_single_link.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
Modified: pkg/trunk/demos/pr2_gazebo/pr2.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2.launch 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/demos/pr2_gazebo/pr2.launch 2008-11-08 00:55:51 UTC (rev 6422)
@@ -1,6 +1,9 @@
<launch>
<!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/wg_robot_description/pr2 -->
<group name="wg">
+ <!-- create model file for gazebo -->
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml.model" />
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml_nolimit.model 1" />
<param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
Modified: pkg/trunk/demos/pr2_gazebo/pr2_wg.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_wg.launch 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/demos/pr2_gazebo/pr2_wg.launch 2008-11-08 00:55:51 UTC (rev 6422)
@@ -1,5 +1,8 @@
<launch>
<group name="wg">
+ <!-- create model file for gazebo -->
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml.model" />
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml_nolimit.model 1" />
<param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
<!-- -g flag runs gazebo in gui-less mode -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_wg.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch 2008-11-08 00:55:51 UTC (rev 6422)
@@ -2,9 +2,12 @@
<!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/wg_robot_description/pr2 -->
<!-- if needed, group tag allows pushing components into namespace via ns="namespace" -->
<group name="wg">
+ <!-- create model file for gazebo -->
+ <!--node pkg="xacro" type="xacro.py" args="$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml $(find wg_robot_description)/pr2_prototype1/pr2_prototype1_xacroed.xml" /-->
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2_prototype1/pr2_prototype1_xacroed.xml $(find gazebo_robot_description)/world/pr2_xml_prototype1.model" />
<!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
(Mechanism Control, BaseControllerNode, etc...) -->
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2_prototype1/gazebo/pr2_prototype1.xml"" />
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2_prototype1/pr2_prototype1_xacroed.xml"" />
<!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_prototype1.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch 2008-11-08 00:55:51 UTC (rev 6422)
@@ -2,7 +2,7 @@
<!-- use mech.py to spawn all controllers listed in controllers.xml -->
<param name="base_controller/odom_publish_rate" value="10" />
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_base_lab.xml" output="screen"/>
- <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/gazebo/controllers_head_tilt_laser_torso_gazebo.xml" output="screen"/>
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_head_tilt_laser_torso_gazebo.xml" output="screen"/>
<!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
for details of the profile, rates, see controller::LaserScannerControllerNode. -->
<node pkg="pr2_mechanism_controllers" type="control_laser.py" args="tilt_laser_controller sine 20 0.872 0.3475" respawn="false" output="screen" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-11-08 00:55:51 UTC (rev 6422)
@@ -6,8 +6,8 @@
gensrv()
include_directories(srv/cpp)
-add_definitions(-Wall -DNDEBUG -O3)
-#set(ROS_BUILD_TYPE Release)
+#add_definitions(-Wall -DNDEBUG -O3)
+set(ROS_BUILD_TYPE Release)
rospack_add_library(Ros_Stereo_Camera src/Ros_Stereo_Camera.cc)
rospack_add_library(Ros_Camera src/Ros_Camera.cc)
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt 2008-11-08 00:55:51 UTC (rev 6422)
@@ -12,51 +12,3 @@
message(${wg_robot_description_PACKAGE_PATH}/pr2/pr2.xml)
message(${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml.model)
-# we do not know all the dependencies of pr2.xml so we always build the target
-add_custom_target(pr2_gazebo_model ALL
- COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/pr2/gazebo/pr2_gazebo_actuators.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml.model
- DEPENDS urdf2gazebo
- COMMENT "Building Gazebo model for PR2 Standard Model")
-
-add_custom_target(pr2_gazebo_model_nolimit ALL
- COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/pr2/gazebo/pr2_gazebo_actuators.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_nolimit.model 1
- DEPENDS urdf2gazebo
- COMMENT "Building Gazebo model for PR2 Gazebo Actuators with no limits")
-
-#pr2_arm
-add_custom_target(pr2d2 ALL
- COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/pr2_arm/pr2d2.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2d2_xml.model
- DEPENDS urdf2gazebo
- COMMENT "Building Gazebo model for PR2D2 Arm Test")
-
-# pr2_arm_test
-add_custom_target(pr2_gazebo_model_arm ALL
- COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/pr2_arm_test/gazebo/pr2_arm_gazebo_actuators.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_arm.model
- DEPENDS urdf2gazebo
- COMMENT "Building Gazebo model for PR2 Arm Test")
-
-# single_link_test
-add_custom_target(pr2_gazebo_model_single_link ALL
- COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/single_link_test/gazebo/pr2_single_link_gazebo_actuators.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_single_link.model
- DEPENDS urdf2gazebo
- COMMENT "Building Gazebo model for Single Link Test")
-
-# dual_link_test
-add_custom_target(pr2_gazebo_model_dual_link ALL
- COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/dual_link_test/gazebo/pr2_dual_link_gazebo_actuators.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_dual_link.model
- DEPENDS urdf2gazebo
- COMMENT "Building Gazebo model for Dual Link Test")
-
-# multi_link_test
-add_custom_target(pr2_gazebo_model_multi_link ALL
- COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/multi_link_test/gazebo/pr2_multi_link_gazebo_actuators.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_multi_link.model
- DEPENDS urdf2gazebo
- COMMENT "Building Gazebo model for Multi Link Test")
-
-# pr2_prototype1
-add_custom_target(pr2_prototype1 ALL
- COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/pr2_prototype1/gazebo/pr2_prototype1_gazebo_actuators.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_prototype1.model
- DEPENDS urdf2gazebo
- COMMENT "Building Gazebo model for PR2 Prototype1 Test")
-
-
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-08 00:55:51 UTC (rev 6422)
@@ -333,6 +333,116 @@
<xi:include href="pr2_xml.model" />
</include>
+ <!-- PR2_ACTARRAY -->
+ <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <interface:audio name="gazebo_mechanism_control_dummy_iface" />
+ </controller:gazebo_mechanism_control>
+
+ <!-- battery controls -->
+ <controller:gazebo_battery name="gazebo_battery_controller" plugin="libgazebo_battery.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1.0</updateRate>
+ <timeout>5</timeout>
+ <diagnostic_rate>1.0</diagnostic_rate>
+ <battery_state_rate>1.0</battery_state_rate>
+ <full_charge_energy>120.0</full_charge_energy>
+ <diagnosticTopicName>diagnostic</diagnosticTopicName>
+ <stateTopicName>battery_state</stateTopicName>
+ <selfTestServiceName>self_test</selfTestServiceName>
+ <interface:audio name="battery_dummy_interface" />
+ </controller:gazebo_battery>
+
+ <!-- ptz camera controls -->
+ <controller:Ros_PTZ name="ptz_cam_left_controller" plugin="libRos_PTZ.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>15.0</updateRate>
+ <panJoint>ptz_pan_left_joint</panJoint>
+ <tiltJoint>ptz_tilt_left_joint</tiltJoint>
+ <commandTopicName>axis_left/ptz_cmd</commandTopicName>
+ <stateTopicName>axis_left/ptz_state</stateTopicName>
+ <interface:ptz name="ptz_left_iface" />
+ </controller:Ros_PTZ>
+
+ <controller:Ros_PTZ name="ptz_cam_right_controller" plugin="libRos_PTZ.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>15.0</updateRate>
+ <panJoint>ptz_pan_right_joint</panJoint>
+ <tiltJoint>ptz_tilt_right_joint</tiltJoint>
+ <commandTopicName>axis_right/ptz_cmd</commandTopicName>
+ <stateTopicName>axis_right/ptz_state</stateTopicName>
+ <interface:ptz name="ptz_right_iface" />
+ </controller:Ros_PTZ>
+
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_right_wrist_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>gripper_roll_right</bodyName>
+ <topicName>gripper_roll_right_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="p3d_right_wrist_position"/>
+ </controller:P3D>
+
+ <controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>gripper_roll_left</bodyName>
+ <topicName>gripper_roll_left_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="p3d_left_wrist_position"/>
+ </controller:P3D>
+
+ <controller:P3D name="p3d_base_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>base</bodyName>
+ <topicName>base_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
+ <rpyOffsets>0 0 0</rpyOffsets>
+ <interface:position name="p3d_base_position"/>
+ </controller:P3D>
+
+ <controller:F3D name="f3d_finger_tip_l_left_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>finger_tip_l_left</bodyName>
+ <topicName>finger_tip_l_left_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_l_left_position"/>
+ </controller:F3D>
+
+ <controller:F3D name="f3d_finger_tip_r_left_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>finger_tip_r_left</bodyName>
+ <topicName>finger_tip_r_left_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_r_left_position"/>
+ </controller:F3D>
+
+ <controller:F3D name="f3d_finger_tip_l_right_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>finger_tip_l_right</bodyName>
+ <topicName>finger_tip_l_right_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_l_right_position"/>
+ </controller:F3D>
+
+ <controller:F3D name="f3d_finger_tip_r_right_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>finger_tip_r_right</bodyName>
+ <topicName>finger_tip_r_right_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_r_right_position"/>
+ </controller:F3D>
+
+
+
</model:physical>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world 2008-11-08 00:55:51 UTC (rev 6422)
@@ -173,6 +173,24 @@
<xi:include href="pr2_xml_arm.model" />
</include>
+ <!-- PR2_ACTARRAY -->
+ <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+
+ <interface:audio name="gazebo_mechanism_control_dummy_iface" />
+ </controller:gazebo_mechanism_control>
+
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>gripper_roll_left</bodyName>
+ <topicName>gripper_roll_left_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="p3d_left_wrist_position"/>
+ </controller:P3D>
+
</model:physical>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world 2008-11-08 00:55:51 UTC (rev 6422)
@@ -169,6 +169,36 @@
<xi:include href="pr2_xml_dual_link.model" />
</include>
+ <!-- pr2_actarray -->
+ <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
+ <alwayson>true</alwayson>
+ <updaterate>1000.0</updaterate>
+
+ <interface:audio name="gazebo_mechanism_control_dummy_iface" />
+ </controller:gazebo_mechanism_control>
+
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_link1_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>link1</bodyName>
+ <topicName>link1_pose</topicName>
+ <frameName>link1_frame</frameName>
+ <xyzOffsets>0 0 0.5</xyzOffsets> <!-- at pivot of link2 -->
+ <rpyOffsets>0 0 0.0</rpyOffsets>
+ <interface:position name="p3d_link1_position"/>
+ </controller:P3D>
+
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_link2_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>link2</bodyName>
+ <topicName>link2_pose</topicName>
+ <frameName>link2_frame</frameName>
+ <interface:position name="p3d_link2_position"/>
+ </controller:P3D>
+
</model:physical>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_multi_link.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_multi_link.world 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_multi_link.world 2008-11-08 00:55:51 UTC (rev 6422)
@@ -169,6 +169,24 @@
<xi:include href="pr2_xml_multi_link.model" />
</include>
+ <!-- PR2_ACTARRAY -->
+ <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+
+ <interface:audio name="gazebo_mechanism_control_dummy_iface" />
+ </controller:gazebo_mechanism_control>
+
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_link3_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>link3</bodyName>
+ <topicName>link3_pose</topicName>
+ <frameName>link3_frame</frameName>
+ <interface:position name="p3d_link3_position"/>
+ </controller:P3D>
+
</model:physical>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world 2008-11-08 00:55:51 UTC (rev 6422)
@@ -111,11 +111,43 @@
<xi:include href="pr2_xml_prototype1.model" />
</include>
+ <!-- PR2_ACTARRAY -->
+ <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+
+ <interface:audio name="gazebo_mechanism_control_dummy_iface" />
+ </controller:gazebo_mechanism_control>
+
+ <!-- battery controls -->
+ <controller:gazebo_battery name="gazebo_battery_controller" plugin="libgazebo_battery.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1.0</updateRate>
+ <timeout>5</timeout>
+ <diagnostic_rate>1.0</diagnostic_rate>
+ <battery_state_rate>1.0</battery_state_rate>
+ <full_charge_energy>120.0</full_charge_energy>
+ <diagnosticTopicName>diagnostic</diagnosticTopicName>
+ <stateTopicName>battery_state</stateTopicName>
+ <selfTestServiceName>self_test</selfTestServiceName>
+ <interface:audio name="battery_dummy_interface" />
+ </controller:gazebo_battery>
+
+ <controller:P3D name="p3d_base_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <bodyName>base</bodyName>
+ <topicName>base_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
+ <rpyOffsets>0 0 0</rpyOffsets>
+ <interface:position name="p3d_base_position"/>
+ </controller:P3D>
+
</model:physical>
<!-- White Directional light -->
- <!--
<model:renderable name="directional_white">
<light>
<type>directional</type>
@@ -125,7 +157,6 @@
<attenuation>1 0.0 1.0 0.4</attenuation>
</light>
</model:renderable>
- -->
</gazebo:world>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_single_link.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_single_link.world 2008-11-08 00:51:26 UTC (rev 6421)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_single_link.world 2008-11-08 00:55:51 UTC (rev 6422)
@@ -183,6 +183,24 @@
<xi:include href="pr2_xml_single_link.model" />
</include>
+ <!-- PR2_ACTARRAY -->
+ <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+
+ <interface:audio name="gazebo_mechanism_control_dummy_iface" />
+ </controller:gazebo_mechanism_control>
+
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_link1_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <bodyName>link1</bodyName>
+ <topicName>link1_pose</topicName>
+ ...
[truncated message content] |
|
From: <hsu...@us...> - 2008-11-08 05:00:48
|
Revision: 6449
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6449&view=rev
Author: hsujohnhsu
Date: 2008-11-08 05:00:39 +0000 (Sat, 08 Nov 2008)
Log Message:
-----------
typecast ULL's to uint64_t.
Modified Paths:
--------------
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/util/tf/include/tf/transform_datatypes.h
pkg/trunk/util/tf/include/tf/transform_listener.h
pkg/trunk/util/tf/src/tf.cpp
pkg/trunk/util/tf/test/testBroadcaster.cpp
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2008-11-08 04:33:47 UTC (rev 6448)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2008-11-08 05:00:39 UTC (rev 6449)
@@ -429,7 +429,7 @@
{
try
{
- tf::Stamped<tf::Pose> robot_pose(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time(0ULL), "base");
+ tf::Stamped<tf::Pose> robot_pose(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time((uint64_t)0ULL), "base");
tf::Stamped<tf::Pose> map_pose;
tf_client_->transformPose("map", robot_pose, map_pose);
Modified: pkg/trunk/util/tf/include/tf/transform_datatypes.h
===================================================================
--- pkg/trunk/util/tf/include/tf/transform_datatypes.h 2008-11-08 04:33:47 UTC (rev 6448)
+++ pkg/trunk/util/tf/include/tf/transform_datatypes.h 2008-11-08 05:00:39 UTC (rev 6449)
@@ -66,7 +66,7 @@
std::string frame_id_;
std::string parent_id_; ///only used for transform
- Stamped() :stamp_ (0ULL),frame_id_ ("NO_ID"), parent_id_("NOT A TRANSFORM"){}; //Default constructor used only for preallocation
+ Stamped() :stamp_ ((uint64_t)0ULL),frame_id_ ("NO_ID"), parent_id_("NOT A TRANSFORM"){}; //Default constructor used only for preallocation
Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & parent_id = "NOT A TRANSFORM"):
T (input), stamp_ ( timestamp ), frame_id_ (frame_id), parent_id_(parent_id){ };
Modified: pkg/trunk/util/tf/include/tf/transform_listener.h
===================================================================
--- pkg/trunk/util/tf/include/tf/transform_listener.h 2008-11-08 04:33:47 UTC (rev 6448)
+++ pkg/trunk/util/tf/include/tf/transform_listener.h 2008-11-08 05:00:39 UTC (rev 6449)
@@ -154,7 +154,7 @@
// setWithEulers(tfArrayIn.eulers[i].header.frame_id, tfArrayIn.eulers[i].parent, tfArrayIn.eulers[i].x, tfArrayIn.eulers[i].y, tfArrayIn.eulers[i].z, tfArrayIn.eulers[i].yaw, tfArrayIn.eulers[i].pitch, tfArrayIn.eulers[i].roll, tfArrayIn.eulers[i].header.stamp.sec * 1000000000ULL + tfArrayIn.eulers[i].header.stamp.nsec);
setTransform(Stamped<Transform>(Transform(Quaternion(tfArrayIn.eulers[i].yaw, tfArrayIn.eulers[i].pitch, tfArrayIn.eulers[i].roll),
Vector3(tfArrayIn.eulers[i].x, tfArrayIn.eulers[i].y, tfArrayIn.eulers[i].z)),
- tfArrayIn.eulers[i].header.stamp.sec * 1000000000ULL + tfArrayIn.eulers[i].header.stamp.nsec,
+ tfArrayIn.eulers[i].header.stamp.sec * (uint64_t)1000000000ULL + tfArrayIn.eulers[i].header.stamp.nsec,
tfArrayIn.eulers[i].header.frame_id , tfArrayIn.eulers[i].parent) );
}
catch (tf::TransformException &ex)
@@ -175,7 +175,7 @@
// setWithQuaternion(tfArrayIn.quaternions[i].header.frame_id, tfArrayIn.quaternions[i].parent, tfArrayIn.quaternions[i].xt, tfArrayIn.quaternions[i].yt, tfArrayIn.quaternions[i].zt, tfArrayIn.quaternions[i].xr, tfArrayIn.quaternions[i].yr, tfArrayIn.quaternions[i].zr, tfArrayIn.quaternions[i].w, tfArrayIn.quaternions[i].header.stamp.sec * 1000000000ULL + tfArrayIn.quaternions[i].header.stamp.nsec);
setTransform(Stamped<Transform>(Transform(Quaternion(tfArrayIn.quaternions[i].xr, tfArrayIn.quaternions[i].yr, tfArrayIn.quaternions[i].zr, tfArrayIn.quaternions[i].w),
Vector3(tfArrayIn.quaternions[i].xt, tfArrayIn.quaternions[i].yt, tfArrayIn.quaternions[i].zt)),
- tfArrayIn.quaternions[i].header.stamp.sec * 1000000000ULL + tfArrayIn.quaternions[i].header.stamp.nsec,
+ tfArrayIn.quaternions[i].header.stamp.sec * (uint64_t)1000000000ULL + tfArrayIn.quaternions[i].header.stamp.nsec,
tfArrayIn.quaternions[i].header.frame_id , tfArrayIn.quaternions[i].parent)
);
}
Modified: pkg/trunk/util/tf/src/tf.cpp
===================================================================
--- pkg/trunk/util/tf/src/tf.cpp 2008-11-08 04:33:47 UTC (rev 6448)
+++ pkg/trunk/util/tf/src/tf.cpp 2008-11-08 05:00:39 UTC (rev 6449)
@@ -386,7 +386,7 @@
{
unsigned int parent_id;
try{
- getFrame(counter)->getData(0ULL, temp);
+ getFrame(counter)->getData((uint64_t)0ULL, temp);
parent_id = temp.parent_frame_id;
}
catch (tf::LookupException& ex)
@@ -411,7 +411,7 @@
for (unsigned int counter = 1; counter < frames_.size(); counter ++)
{
try{
- getFrame(counter)->getData(0ULL, temp);
+ getFrame(counter)->getData((uint64_t)0ULL, temp);
}
catch (tf::LookupException& ex)
{
Modified: pkg/trunk/util/tf/test/testBroadcaster.cpp
===================================================================
--- pkg/trunk/util/tf/test/testBroadcaster.cpp 2008-11-08 04:33:47 UTC (rev 6448)
+++ pkg/trunk/util/tf/test/testBroadcaster.cpp 2008-11-08 05:00:39 UTC (rev 6449)
@@ -49,7 +49,7 @@
<< 0 << 0 << 1 << 3
<< 0 << 0 << 0 << 1;
- broadcaster.sendTransform(btTransform(btQuaternion(0,0,0), btVector3(1,2,3)), 1000000000ULL, "frame1", "frame2");
+ broadcaster.sendTransform(btTransform(btQuaternion(0,0,0), btVector3(1,2,3)), (uint64_t)1000000000ULL, "frame1", "frame2");
/* pTFServer->sendEuler("count","count++",1,1,1,1,1,1,ros::Time(100000,100000));
pTFServer->sendInverseEuler("count","count++",1,1,1,1,1,1,ros::Time(100000,100000));
pTFServer->sendDH("count","count++",1,1,1,1,ros::Time(100000,100000));
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <pof...@us...> - 2008-11-08 20:24:08
|
Revision: 6456
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6456&view=rev
Author: poftwaresatent
Date: 2008-11-08 20:24:05 +0000 (Sat, 08 Nov 2008)
Log Message:
-----------
blacklist my pkgs that fail clean Hudson builds (Makefiles choke on svn up/co commands)
Added Paths:
-----------
pkg/trunk/3rdparty/libsunflower/ROS_BUILD_BLACKLIST
pkg/trunk/motion_planning/mp_benchmarks/ROS_BUILD_BLACKLIST
pkg/trunk/simulators/nepumuk/ROS_BUILD_BLACKLIST
Added: pkg/trunk/3rdparty/libsunflower/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/3rdparty/libsunflower/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/3rdparty/libsunflower/ROS_BUILD_BLACKLIST 2008-11-08 20:24:05 UTC (rev 6456)
@@ -0,0 +1 @@
+issues with sn checkout/update commands in nepumuk/libsunflower Makefiles
Added: pkg/trunk/motion_planning/mp_benchmarks/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/motion_planning/mp_benchmarks/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/motion_planning/mp_benchmarks/ROS_BUILD_BLACKLIST 2008-11-08 20:24:05 UTC (rev 6456)
@@ -0,0 +1 @@
+issues with sn checkout/update commands in nepumuk/libsunflower Makefiles
Added: pkg/trunk/simulators/nepumuk/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/simulators/nepumuk/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/simulators/nepumuk/ROS_BUILD_BLACKLIST 2008-11-08 20:24:05 UTC (rev 6456)
@@ -0,0 +1 @@
+issues with sn checkout/update commands in nepumuk/libsunflower Makefiles
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-10 19:13:12
|
Revision: 6461
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6461&view=rev
Author: hsujohnhsu
Date: 2008-11-10 19:13:05 +0000 (Mon, 10 Nov 2008)
Log Message:
-----------
updates to launch scripts for floor object world.
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
Modified: pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml 2008-11-10 18:57:15 UTC (rev 6460)
+++ pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml 2008-11-10 19:13:05 UTC (rev 6461)
@@ -3,9 +3,10 @@
<master auto="start"/>
<group name="wg">
- <include file="$(find wg_robot_description)/send.xml"/>
- <!--node pkg="gazebo" type="v.bash" args="-n $(find gazebo_robot_description)/world/robot_floorobj.world" respawn="false" output="screen"/-->
+ <!-- create model file for gazebo and launch gazebo -->
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml.model" />
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_floorobj.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
@@ -13,6 +14,13 @@
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
+ <!-- use mech.py to spawn all controllers listed in controllers.xml -->
+ <!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
+ for details of the profile, rates, see controller::LaserScannerControllerNode. -->
+ <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
+
+
+
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" />
<remap from="base_pose_gazebo_ground_truth" to="base_pose_ground_truth"/>
<param name="max_publish_frequency" value="20.0"/>
@@ -24,12 +32,6 @@
<!-- Load parameters for moving the base. -->
<param name="costmap_2d/inflation_radius" type="double" value="0.35" />
- <!-- use mech.py to spawn all controllers listed in controllers.xml -->
- <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2/controllers.xml" respawn="false" output="screen" />
- <!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
- for details of the profile, rates, see controller::LaserScannerControllerNode. -->
- <node pkg="robot_mechanism_controllers" type="control.py" args="set tilt_laser_controller 46" respawn="false" output="screen" />
-
<node pkg="nav_view" type="nav_view" args="odom:=localizedpose" respawn="false"/>
<!-- Now launch controller node required -->
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world 2008-11-10 18:57:15 UTC (rev 6460)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world 2008-11-10 19:13:05 UTC (rev 6461)
@@ -356,6 +356,114 @@
<xi:include href="pr2_xml.model" />
</include>
+ <!-- PR2_ACTARRAY -->
+ <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <interface:audio name="gazebo_mechanism_control_dummy_iface" />
+ </controller:gazebo_mechanism_control>
+
+ <!-- battery controls -->
+ <controller:gazebo_battery name="gazebo_battery_controller" plugin="libgazebo_battery.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1.0</updateRate>
+ <timeout>5</timeout>
+ <diagnostic_rate>1.0</diagnostic_rate>
+ <battery_state_rate>1.0</battery_state_rate>
+ <full_charge_energy>120.0</full_charge_energy>
+ <diagnosticTopicName>diagnostic</diagnosticTopicName>
+ <stateTopicName>battery_state</stateTopicName>
+ <selfTestServiceName>self_test</selfTestServiceName>
+ <interface:audio name="battery_dummy_interface" />
+ </controller:gazebo_battery>
+
+ <!-- ptz camera controls -->
+ <controller:Ros_PTZ name="ptz_cam_left_controller" plugin="libRos_PTZ.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>15.0</updateRate>
+ <panJoint>ptz_pan_left_joint</panJoint>
+ <tiltJoint>ptz_tilt_left_joint</tiltJoint>
+ <commandTopicName>axis_left/ptz_cmd</commandTopicName>
+ <stateTopicName>axis_left/ptz_state</stateTopicName>
+ <interface:ptz name="ptz_left_iface" />
+ </controller:Ros_PTZ>
+
+ <controller:Ros_PTZ name="ptz_cam_right_controller" plugin="libRos_PTZ.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>15.0</updateRate>
+ <panJoint>ptz_pan_right_joint</panJoint>
+ <tiltJoint>ptz_tilt_right_joint</tiltJoint>
+ <commandTopicName>axis_right/ptz_cmd</commandTopicName>
+ <stateTopicName>axis_right/ptz_state</stateTopicName>
+ <interface:ptz name="ptz_right_iface" />
+ </controller:Ros_PTZ>
+
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_right_wrist_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>gripper_roll_right</bodyName>
+ <topicName>gripper_roll_right_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="p3d_right_wrist_position"/>
+ </controller:P3D>
+
+ <controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>gripper_roll_left</bodyName>
+ <topicName>gripper_roll_left_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="p3d_left_wrist_position"/>
+ </controller:P3D>
+
+ <controller:P3D name="p3d_base_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>base</bodyName>
+ <topicName>base_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
+ <rpyOffsets>0 0 0</rpyOffsets>
+ <interface:position name="p3d_base_position"/>
+ </controller:P3D>
+
+ <controller:F3D name="f3d_finger_tip_l_left_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>finger_tip_l_left</bodyName>
+ <topicName>finger_tip_l_left_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_l_left_position"/>
+ </controller:F3D>
+
+ <controller:F3D name="f3d_finger_tip_r_left_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>finger_tip_r_left</bodyName>
+ <topicName>finger_tip_r_left_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_r_left_position"/>
+ </controller:F3D>
+
+ <controller:F3D name="f3d_finger_tip_l_right_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>finger_tip_l_right</bodyName>
+ <topicName>finger_tip_l_right_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_l_right_position"/>
+ </controller:F3D>
+
+ <controller:F3D name="f3d_finger_tip_r_right_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>finger_tip_r_right</bodyName>
+ <topicName>finger_tip_r_right_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_r_right_position"/>
+ </controller:F3D>
+
</model:physical>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-10 20:06:27
|
Revision: 6467
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6467&view=rev
Author: hsujohnhsu
Date: 2008-11-10 20:06:22 +0000 (Mon, 10 Nov 2008)
Log Message:
-----------
* added desk objects with skinny legs, so obstacle avoidance will not work properly without tilting lasers.
* put some desk obstacles in robot_wg.world
* replaced desk obstacles in robot.world and robot_floorobj.world
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
Added Paths:
-----------
pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch
pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk1.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk2.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk3.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk4.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk5.model
Added: pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch (rev 0)
+++ pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch 2008-11-10 20:06:22 UTC (rev 6467)
@@ -0,0 +1,17 @@
+<launch>
+ <group name="wg">
+ <!-- create model file for gazebo -->
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml.model" />
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml_nolimit.model 1" />
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_floorobj.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+ <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
+ </group>
+</launch>
+
Modified: pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml 2008-11-10 20:05:14 UTC (rev 6466)
+++ pkg/trunk/highlevel/highlevel_controllers/test/launch_gazebo.xml 2008-11-10 20:06:22 UTC (rev 6467)
@@ -4,23 +4,10 @@
<group name="wg">
- <!-- create model file for gazebo and launch gazebo -->
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml.model" />
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_floorobj.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
- </node>
+ <!-- start up gazebo with gazebo_robot_description/world/robot_floorobj.world -->
+ <include file="$(find pr2_gazebo)/pr2_floorobj.launch" />
- <!-- use mech.py to spawn all controllers listed in controllers.xml -->
- <!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
- for details of the profile, rates, see controller::LaserScannerControllerNode. -->
- <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
-
-
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" />
<remap from="base_pose_gazebo_ground_truth" to="base_pose_ground_truth"/>
<param name="max_publish_frequency" value="20.0"/>
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk1.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk1.model (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk1.model 2008-11-10 20:06:22 UTC (rev 6467)
@@ -0,0 +1,84 @@
+<?xml version="1.0" ?>
+<model:physical xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" name="pr2_model">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <body:box name="desk1">
+ <xyz>0 0 0.3</xyz>
+ <rpy>0 0 0</rpy>
+ <geom:box name="desk1top_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0 0 0.6</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.8 2.0 0.10</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.8 2.0 0.10</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Green</material>
+ </visual>
+ </geom:box>
+ <geom:box name="desk1leg_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0 0 .3</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.05 0.05 0.6</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.05 0.05 0.6</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Red</material>
+ </visual>
+ </geom:box>
+ <geom:box name="desk1base_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.4 1.0 0.05</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.4 1.0 0.05</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Green</material>
+ </visual>
+ </geom:box>
+ </body:box>
+</model:physical>
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk2.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk2.model (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk2.model 2008-11-10 20:06:22 UTC (rev 6467)
@@ -0,0 +1,84 @@
+<?xml version="1.0" ?>
+<model:physical xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" name="pr2_model">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <body:box name="desk2">
+ <xyz>0 0 0.4</xyz>
+ <rpy>0 0 0</rpy>
+ <geom:box name="desk1top_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0 0 0.8</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.8 0.8 0.10</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.8 0.8 0.10</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Green</material>
+ </visual>
+ </geom:box>
+ <geom:box name="desk2leg_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0 0 0.4</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.05 0.05 0.8</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.05 0.05 0.8</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Red</material>
+ </visual>
+ </geom:box>
+ <geom:box name="desk2base_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.6 0.6 0.05</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.6 0.6 0.05</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Green</material>
+ </visual>
+ </geom:box>
+ </body:box>
+</model:physical>
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk3.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk3.model (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk3.model 2008-11-10 20:06:22 UTC (rev 6467)
@@ -0,0 +1,134 @@
+<?xml version="1.0" ?>
+<model:physical xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" name="pr2_model">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <body:box name="desk3">
+ <xyz>0 0 0.3</xyz>
+ <rpy>0 0 0</rpy>
+ <geom:box name="desk3top_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0 0 0.6</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.8 2.0 0.10</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.8 2.0 0.10</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Green</material>
+ </visual>
+ </geom:box>
+ <geom:box name="desk3leg1_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>-0.3 -0.8 .3</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.05 0.05 0.6</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.05 0.05 0.6</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Red</material>
+ </visual>
+ </geom:box>
+ <geom:box name="desk3leg2_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0.3 -0.8 .3</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.05 0.05 0.6</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.05 0.05 0.6</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Red</material>
+ </visual>
+ </geom:box>
+ <geom:box name="desk3leg3_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>-0.3 0.8 .3</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.05 0.05 0.6</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.05 0.05 0.6</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Red</material>
+ </visual>
+ </geom:box>
+ <geom:box name="desk3leg4_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0.3 0.8 .3</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.05 0.05 0.6</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.05 0.05 0.6</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Red</material>
+ </visual>
+ </geom:box>
+ </body:box>
+</model:physical>
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk4.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk4.model (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk4.model 2008-11-10 20:06:22 UTC (rev 6467)
@@ -0,0 +1,84 @@
+<?xml version="1.0" ?>
+<model:physical xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" name="pr2_model">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <body:box name="desk4">
+ <xyz>0 0 0.3</xyz>
+ <rpy>0 0 0</rpy>
+ <geom:box name="desk4top_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0 0 0.6</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.75 1.5 0.10</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.75 1.5 0.10</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Green</material>
+ </visual>
+ </geom:box>
+ <geom:box name="desk4leg_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0 0 .3</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.05 0.05 0.6</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.05 0.05 0.6</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Red</material>
+ </visual>
+ </geom:box>
+ <geom:box name="desk4base_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.5 0.8 0.05</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.5 0.8 0.05</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Green</material>
+ </visual>
+ </geom:box>
+ </body:box>
+</model:physical>
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk5.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk5.model (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/desk5.model 2008-11-10 20:06:22 UTC (rev 6467)
@@ -0,0 +1,134 @@
+<?xml version="1.0" ?>
+<model:physical xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" name="pr2_model">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <body:box name="desk3">
+ <xyz>0 0 0.375</xyz>
+ <rpy>0 0 0</rpy>
+ <geom:box name="desk3top_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0 0 0.75</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.75 1.5 0.10</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.75 1.5 0.10</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Green</material>
+ </visual>
+ </geom:box>
+ <geom:box name="desk3leg1_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>-0.3 -0.7 0.375</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.05 0.05 0.75</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.05 0.05 0.75</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Red</material>
+ </visual>
+ </geom:box>
+ <geom:box name="desk3leg2_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0.3 -0.7 0.375</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.05 0.05 0.75</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.05 0.05 0.75</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Red</material>
+ </visual>
+ </geom:box>
+ <geom:box name="desk3leg3_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>-0.3 0.7 0.375</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.05 0.05 0.75</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.05 0.05 0.75</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Red</material>
+ </visual>
+ </geom:box>
+ <geom:box name="desk3leg4_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz>0.3 0.7 0.375</xyz>
+ <rpy>0 0 0</rpy>
+ <massMatrix>true</massMatrix>
+ <mass>10.0</mass>
+ <ixx>1.0</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1.0</iyy>
+ <iyz>0</iyz>
+ <izz>1.0</izz>
+ <cx>0</cx>
+ <cy>0</cy>
+ <cz>0</cz>
+ <size>0.05 0.05 0.75</size>
+ <visual>
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <scale>0.05 0.05 0.75</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Red</material>
+ </visual>
+ </geom:box>
+ </body:box>
+</model:physical>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-10 20:05:14 UTC (rev 6466)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-10 20:06:22 UTC (rev 6467)
@@ -91,76 +91,27 @@
-->
<!-- The "desk"-->
+ <!-- small desks -->
<model:physical name="desk1_model">
- <xyz> 2.28 -0.21 -0.10</xyz>
- <rpy> 0.0 0.0 0.00</rpy>
- <body:box name="desk1_legs_body">
- <geom:box name="desk1_legs_geom">
- <kp>100000000.0</kp>
- <kd>1.0</kd>
- <xyz> 0.0 0.0 0.50</xyz>
- <mesh>default</mesh>
- <size>0.5 1.0 0.75</size>
- <mass> 10.0</mass>
- <visual>
- <size> 0.5 1.0 0.75</size>
- <material>Gazebo/Rocky</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- <geom:box name="desk1_top_geom">
- <kp>100000000.0</kp>
- <kd>0.1</kd>
- <xyz> 0.0 0.0 0.90</xyz>
- <mesh>default</mesh>
- <size>0.75 1.5 0.05</size>
- <mass> 10.0</mass>
- <visual>
- <size> 0.75 1.5 0.05</size>
- <material>Gazebo/Rocky</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:box>
+ <xyz>2.28 -0.21 0</xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <include embedded="true">
+ <xi:include href="desk4.model" />
+ </include>
</model:physical>
<!-- The second "desk"-->
<model:physical name="desk2_model">
- <xyz> 2.25 -3.0 -0.10</xyz>
- <rpy> 0.0 0.0 0.00</rpy>
- <body:box name="desk2_legs_body">
- <geom:box name="desk2_legs_geom">
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <xyz> 0.0 0.0 0.50</xyz>
- <mesh>default</mesh>
- <size>0.5 1.0 0.75</size>
- <mass> 10.0</mass>
- <visual>
- <size> 0.5 1.0 0.75</size>
- <material>Gazebo/Rocky</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- <geom:box name="desk2_top_geom">
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <xyz> 0.0 0.0 0.90</xyz>
- <mesh>default</mesh>
- <size>0.75 1.5 0.05</size>
- <mass> 10.0</mass>
- <visual>
- <size> 0.75 1.5 0.05</size>
- <material>Gazebo/Rocky</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:box>
+ <xyz>2.25 -3.0 0</xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <include embedded="true">
+ <xi:include href="desk5.model" />
+ </include>
</model:physical>
<!-- The small cylinder "cup" -->
<model:physical name="cylinder1_model">
- <xyz> 2.5 0.0 0.9</xyz>
+ <xyz> 2.5 0.0 1.5</xyz>
<rpy> 0.0 0.0 0.0</rpy>
<body:cylinder name="cylinder1_body">
<geom:cylinder name="cylinder1_geom">
@@ -223,7 +174,7 @@
<!-- The small ball -->
<model:physical name="sphere1_model">
- <xyz> 2.5 -2.8 1.0</xyz>
+ <xyz> 2.5 -2.8 1.5</xyz>
<rpy> 0.0 0.0 0.0</rpy>
<body:sphere name="sphere1_body">
<geom:sphere name="sphere1_geom">
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world 2008-11-10 20:05:14 UTC (rev 6466)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world 2008-11-10 20:06:22 UTC (rev 6467)
@@ -156,72 +156,22 @@
</model:physical>
-->
- <!-- The "desk"-->
+ <!-- smal...
[truncated message content] |
|
From: <vij...@us...> - 2008-11-10 23:50:45
|
Revision: 6483
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6483&view=rev
Author: vijaypradeep
Date: 2008-11-10 23:50:40 +0000 (Mon, 10 Nov 2008)
Log Message:
-----------
Adding rigid-body jacobian libraries. Still need to add numerical tests for jacobian solver
Finished test harness for Link Parameter Jacobian calculations
Reading in URDF data and displaying result to file. Translation terms seem to be wrong.
added roslaunch script to put pr2 URDF on param server
Started phasespace/arm calibration capture code.
Added Paths:
-----------
pkg/trunk/calibration/
pkg/trunk/calibration/kinematic_calibration/
pkg/trunk/calibration/kinematic_calibration/CMakeLists.txt
pkg/trunk/calibration/kinematic_calibration/Makefile
pkg/trunk/calibration/kinematic_calibration/include/
pkg/trunk/calibration/kinematic_calibration/include/kinematic_calibration/
pkg/trunk/calibration/kinematic_calibration/include/kinematic_calibration/link_param_jacobian.h
pkg/trunk/calibration/kinematic_calibration/include/kinematic_calibration/link_param_jacobian_solver.h
pkg/trunk/calibration/kinematic_calibration/include/kinematic_calibration/verify_jacobian.h
pkg/trunk/calibration/kinematic_calibration/manifest.xml
pkg/trunk/calibration/kinematic_calibration/src/
pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
pkg/trunk/calibration/kinematic_calibration/src/link_param_jacobian.cpp
pkg/trunk/calibration/kinematic_calibration/src/link_param_jacobian_solver.cpp
pkg/trunk/calibration/kinematic_calibration/src/verify_jacobian.cpp
pkg/trunk/calibration/kinematic_calibration/test/
pkg/trunk/calibration/kinematic_calibration/test/data/
pkg/trunk/calibration/kinematic_calibration/test/data/2d_easy/
pkg/trunk/calibration/kinematic_calibration/test/data/2d_easy/jacobians.txt
pkg/trunk/calibration/kinematic_calibration/test/data/2d_easy/joint_states.txt
pkg/trunk/calibration/kinematic_calibration/test/data/2d_easy/model.txt
pkg/trunk/calibration/kinematic_calibration/test/data/2d_hard/
pkg/trunk/calibration/kinematic_calibration/test/data/2d_hard/jacobians.txt
pkg/trunk/calibration/kinematic_calibration/test/data/2d_hard/joint_states.txt
pkg/trunk/calibration/kinematic_calibration/test/data/2d_hard/model.txt
pkg/trunk/calibration/kinematic_calibration/test/data/3d_hard/
pkg/trunk/calibration/kinematic_calibration/test/data/3d_hard/jacobians.txt
pkg/trunk/calibration/kinematic_calibration/test/data/3d_hard/joint_states.txt
pkg/trunk/calibration/kinematic_calibration/test/data/3d_hard/model.txt
pkg/trunk/calibration/kinematic_calibration/test/data/edge_case_1/
pkg/trunk/calibration/kinematic_calibration/test/data/edge_case_1/jacobians.txt
pkg/trunk/calibration/kinematic_calibration/test/data/edge_case_1/joint_states.txt
pkg/trunk/calibration/kinematic_calibration/test/data/edge_case_1/model.txt
pkg/trunk/calibration/kinematic_calibration/test/jacobian_unittest.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/send_description.xml
Added: pkg/trunk/calibration/kinematic_calibration/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/CMakeLists.txt (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/CMakeLists.txt 2008-11-10 23:50:40 UTC (rev 6483)
@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(kinematic_calibration)
+genmsg()
+gensrv()
+rospack_add_library(kinematic_calibration src/link_param_jacobian.cpp
+ src/link_param_jacobian_solver.cpp
+ src/verify_jacobian.cpp
+ )
+
+rospack_add_executable(arm_phasespace_grabber src/arm_phasespace_grabber.cpp)
+target_link_libraries(arm_phasespace_grabber kinematic_calibration)
+
+rospack_add_gtest(jacobian_unittest test/jacobian_unittest.cpp)
+target_link_libraries(jacobian_unittest kinematic_calibration)
+set_target_properties(jacobian_unittest PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/test)
+
Added: pkg/trunk/calibration/kinematic_calibration/Makefile
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/Makefile (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/Makefile 2008-11-10 23:50:40 UTC (rev 6483)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/calibration/kinematic_calibration/include/kinematic_calibration/link_param_jacobian.h
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/include/kinematic_calibration/link_param_jacobian.h (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/include/kinematic_calibration/link_param_jacobian.h 2008-11-10 23:50:40 UTC (rev 6483)
@@ -0,0 +1,70 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+//! \author Vijay Pradeep
+
+#ifndef KINEMATIC_CALIBRATION_LINK_PARAM_JACOBIAN_H_
+#define KINEMATIC_CALIBRATION_LINK_PARAM_JACOBIAN_H_
+
+#include <vector>
+
+#include "kdl/frames.hpp"
+
+namespace kinematic_calibration
+{
+
+struct LinkTwists
+{
+ KDL::Twist trans_[3] ;
+ KDL::Twist rot_[3] ;
+} ;
+
+class LinkParamJacobian
+{
+public:
+ LinkParamJacobian() ;
+ ~LinkParamJacobian() ;
+
+ /**
+ * Propagate jacobian to a point specified in the current frame's coordinates
+ * \param cur_to_next vector specifying the point to propagate the jacobian to. Defined wrt the current coordinate frame.
+ */
+ void changeRefPoint(const KDL::Vector& cur_to_next) ;
+
+ std::vector<LinkTwists> twists_ ; // Should this be private?
+};
+
+}
+
+#endif /* KINEMATIC_CALIBRATION_LINK_PARAM_JACOBIAN_H_ */
Added: pkg/trunk/calibration/kinematic_calibration/include/kinematic_calibration/link_param_jacobian_solver.h
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/include/kinematic_calibration/link_param_jacobian_solver.h (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/include/kinematic_calibration/link_param_jacobian_solver.h 2008-11-10 23:50:40 UTC (rev 6483)
@@ -0,0 +1,74 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+//! \author Vijay Pradeep
+
+#ifndef KINEMATIC_CALIBRATION_LINK_PARAM_JACOBIAN_SOLVER_H_
+#define KINEMATIC_CALIBRATION_LINK_PARAM_JACOBIAN_SOLVER_H_
+
+
+#include "kinematic_calibration/link_param_jacobian.h"
+#include "kdl/chain.hpp"
+#include "kdl/jntarray.hpp"
+
+namespace kinematic_calibration
+{
+
+/**
+ * Computes the end effector jacobian wrt all the link parameters along the kinematic chain. The eef motion is calculated for 6
+ * differential perturbations: (dx, dy, dz, rx, ry, rz).
+ * dx, dy, dz are the x,y & z translational displacements (respectively) of the end effector in the link's base frame
+ * rx, ry, rz are the x y z rotations (respectively) of the end effector in the frame of the current link's tip.
+ */
+class LinkParamJacobianSolver
+{
+public:
+ LinkParamJacobianSolver() ;
+
+ ~LinkParamJacobianSolver() ;
+
+ /**
+ * Function that computes the link-parameter jacobian
+ * \param chain The KDL datatype specifying the kinematic chain
+ * \param joint_states stores the joint angles/displacements for the current configuration that we want to evaluate
+ * \param jac (output) Stores the computed jacobian
+ * \return negative on error
+ */
+ int JointsToCartesian(const KDL::Chain& chain, const KDL::JntArray& joint_states, LinkParamJacobian& jac) ;
+
+} ;
+
+}
+
+#endif /* KINEMATIC_CALIBRATION_LINK_PARAM_JACOBIAN_SOLVER_H_ */
Added: pkg/trunk/calibration/kinematic_calibration/include/kinematic_calibration/verify_jacobian.h
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/include/kinematic_calibration/verify_jacobian.h (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/include/kinematic_calibration/verify_jacobian.h 2008-11-10 23:50:40 UTC (rev 6483)
@@ -0,0 +1,111 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+//! \author Vijay Pradeep
+
+#ifndef KINEMATIC_CALIBRATION_VERIFY_JACOBIAN_H_
+#define KINEMATIC_CALIBRATION_VERIFY_JACOBIAN_H_
+
+#include <vector>
+#include <string>
+#include <fstream>
+
+#include "kdl/chain.hpp"
+
+#include "kinematic_calibration/link_param_jacobian_solver.h"
+
+
+namespace kinematic_calibration
+{
+
+class ModelGetter
+{
+public:
+ ModelGetter() { }
+ ~ModelGetter() { }
+
+ int OpenFile(const std::string& filename) ;
+
+ KDL::Chain GetModel() ;
+
+ void CloseFile() ;
+private:
+ std::ifstream infile_ ;
+
+} ;
+
+class JointStatesGetter
+{
+public:
+ JointStatesGetter() { }
+ ~JointStatesGetter() { }
+
+ int OpenFile(const std::string& filename) ;
+ int GetNextJointArray(KDL::JntArray& joint_array) ;
+ void CloseFile() ;
+private:
+ std::ifstream infile_ ;
+} ;
+
+class JacobiansGetter
+{
+public:
+ JacobiansGetter() { }
+ ~JacobiansGetter() { }
+
+ int OpenFile(const std::string& filename) ;
+ int GetNextJacobian(LinkParamJacobian& jac ) ;
+ void CloseFile() ;
+private:
+ std::ifstream infile_ ;
+} ;
+
+
+
+
+class VerifyJacobian
+{
+public:
+ VerifyJacobian() ;
+ ~VerifyJacobian() ;
+ int ComputeMaxError(const std::string& model_file, const std::string& joint_params_file, const std::string& jacobians_file, double& max_error) ;
+private:
+ ModelGetter model_getter_ ;
+ JointStatesGetter joint_params_getter_ ;
+ JacobiansGetter jacobians_getter_ ;
+} ;
+
+}
+
+#endif /* KINEMATIC_CALIBRATION_VERIFY_JACOBIAN_H_ */
Added: pkg/trunk/calibration/kinematic_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/manifest.xml (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/manifest.xml 2008-11-10 23:50:40 UTC (rev 6483)
@@ -0,0 +1,10 @@
+<package>
+ <description>Tools for calibrating kinematics chains</description>
+ <author>Vijay Pradeep</author>
+ <license>BSD</license>
+ <depend package="kdl" />
+ <depend package="robot_kinematics" />
+ <depend package="phase_space" />
+ <depend package="robot_msgs" />
+ <depend package="robot_kinematics" />
+</package>
Added: pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp 2008-11-10 23:50:40 UTC (rev 6483)
@@ -0,0 +1,318 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <stdio.h>
+
+#include "ros/node.h"
+#include "robot_msgs/MechanismState.h"
+#include "phase_space/PhaseSpaceSnapshot.h"
+#include "robot_kinematics/robot_kinematics.h"
+
+#include "kdl/chain.hpp"
+
+#include <unistd.h>
+#include <termios.h>
+
+using namespace std ;
+
+namespace kinematic_calibration
+{
+
+
+class ArmPhaseSpaceGrabber : public ros::node
+{
+
+public:
+
+
+
+ ArmPhaseSpaceGrabber() : ros::node("arm_phase_space_grabber")
+ {
+ marker_id_ = 1 ;
+
+ subscribe("phase_space_snapshot", snapshot_, &ArmPhaseSpaceGrabber::SnapshotCallback, 2) ;
+ subscribe("mechanism_state", mech_state_, &ArmPhaseSpaceGrabber::MechStateCallback, 2) ;
+ }
+
+ ~ArmPhaseSpaceGrabber()
+ {
+ unsubscribe("phase_space_snapshot") ;
+ unsubscribe("MechanismState") ;
+ }
+
+ bool spin()
+ {
+ // Setup terminal settings for getchar
+ const int fd = fileno(stdin);
+ termios prev_flags ;
+ tcgetattr(fd, &prev_flags) ;
+ termios flags ;
+ tcgetattr(fd,&flags);
+ flags.c_lflag &= ~ICANON; // set raw (unset canonical modes)
+ flags.c_cc[VMIN] = 0; // i.e. min 1 char for blocking, 0 chars for non-blocking
+ flags.c_cc[VTIME] = 0; // block if waiting for char
+ tcsetattr(fd,TCSANOW,&flags);
+
+ KDL::Chain chain ;
+
+ vector<string> joint_names ;
+
+ bool result ;
+ result = LoadJointNames("./joint_names.xml", joint_names) ;
+
+
+ const string state_data_filename = "./state_data.txt" ;
+ FILE* state_data_out ;
+ state_data_out = fopen(state_data_filename.c_str(), "w") ;
+ if (!state_data_out)
+ {
+ printf("Error opening state_data file\n") ;
+ return false ;
+ }
+ fclose(state_data_out) ;
+
+ while (ok())
+ {
+ char c = getchar() ;
+
+ switch (c)
+ {
+ case ' ':
+ {
+ printf("Capturing...\n") ;
+ phase_space::PhaseSpaceMarker cur_marker ;
+
+ GetMarker(cur_marker, marker_id_) ;
+
+ vector<double> joint_angles ;
+ GetJointAngles(joint_names, joint_angles) ;
+
+ // Print Marker Location
+ printf("% 15.10f % 15.10f % 15.10f ", cur_marker.location.x, cur_marker.location.y, cur_marker.location.z) ;
+
+ // Print Joint Angles
+ for (unsigned int i=0; i<joint_angles.size(); i++)
+ printf("% 15.10f ", joint_angles[i]) ;
+ printf("\n") ;
+
+ break ;
+ }
+ case 'c': // Build kinematic chain for arm
+ {
+ robot_kinematics::RobotKinematics robot_kinematics ;
+ string robot_desc ;
+ param("robotdesc/pr2", robot_desc, string("")) ;
+ printf("RobotDesc.length() = %u\n", robot_desc.length()) ;
+
+ robot_kinematics.loadString(robot_desc.c_str()) ;
+
+ robot_kinematics::SerialChain* serial_chain = robot_kinematics.getSerialChain("right_arm") ;
+
+ if (serial_chain == NULL)
+ {
+ printf("Got NULL Chain\n") ;
+ break ;
+ }
+
+ chain = serial_chain->chain ;
+ printf("Extracted KDL Chain with %u Joints and %u segments\n", chain.getNrOfJoints(), chain.getNrOfSegments()) ;
+ const string model_filename("./model.txt") ;
+ printf("Writing chain to file: %s\n", model_filename.c_str()) ;
+ FILE* model_out ;
+ model_out = fopen(model_filename.c_str(), "w") ;
+
+ if (!model_out)
+ {
+ printf("Error opening file\n") ;
+ break ;
+ }
+
+ for (unsigned int i=0; i < chain.getNrOfSegments(); i++)
+ {
+ printf("Segment #%u\n", i) ;
+ printf(" Translation: ") ;
+ for (unsigned int j=0; j<3; j++)
+ printf("% 15.10f ", chain.getSegment(i).getFrameToTip().p(j) ) ;
+ printf("\n") ;
+
+
+ KDL::Vector rot_axis ;
+ double rot_ang = chain.getSegment(i).getFrameToTip().M.GetRotAngle(rot_axis) ;
+
+ printf(" Rotation:\n") ;
+ printf(" Axis: ") ;
+ for (unsigned int j=0; j<3; j++)
+ printf("% 15.10f ", rot_axis(j)) ;
+ printf("\n") ;
+ printf(" Angle: % 15.10f\n", rot_ang) ;
+
+ KDL::Vector rot_vec = rot_ang * rot_axis ;
+ printf(" Product: ") ;
+ for (unsigned int j=0; j<3; j++)
+ printf("% 15.10f ", rot_vec(j)) ;
+ printf("\n\n") ;
+ }
+ fclose(model_out) ;
+
+ break ;
+ }
+ default:
+ break ;
+ }
+
+ usleep(1000) ;
+ fflush(stdout) ;
+ }
+ printf("\n") ;
+ tcsetattr(fd,TCSANOW, &prev_flags) ; // Undo any terminal changes that we made
+ return true ;
+ }
+
+ bool LoadJointNames(const string& joint_names_file, vector<string>& joint_names)
+ {
+ TiXmlDocument xml ;
+ xml.LoadFile(joint_names_file.c_str()) ;
+ TiXmlElement *config = xml.RootElement();
+
+ if (config == NULL)
+ return false ;
+
+ // Extract the joint names
+ TiXmlElement *joint_name_elem = config->FirstChildElement("joint_name") ;
+
+ while (joint_name_elem)
+ {
+
+ const char* name = joint_name_elem->Attribute("name") ;
+ printf("Found a joint_name: %s\n", name) ;
+ joint_names.push_back(name) ;
+
+ joint_name_elem = joint_name_elem->NextSiblingElement("joint_name") ;
+ }
+ return true ;
+ }
+
+ void GetMarker(phase_space::PhaseSpaceMarker& marker, int id)
+ {
+ bool marker_found = false ;
+
+ // Grab phasespace marker
+ while (!marker_found)
+ {
+ printf(" Looking for marker %u...", id) ;
+ fflush(stdout) ;
+
+ snapshot_lock_.lock() ;
+ for (unsigned int i=0; i < safe_snapshot_.get_markers_size(); i++)
+ {
+ if (safe_snapshot_.markers[i].id == id)
+ {
+ marker_found = true ;
+ marker = safe_snapshot_.markers[i] ;
+ printf("Marker Found!\n") ;
+ break ;
+ }
+ }
+ snapshot_lock_.unlock() ;
+
+ printf(".") ;
+ fflush(stdout) ;
+ usleep(100) ;
+ }
+ }
+
+ void GetJointAngles(const vector<string>& names, vector<double>& angles)
+ {
+ // Grab mechanism state and put it in a local copy
+ mech_lock_.lock() ;
+ robot_msgs::MechanismState mech_state = safe_mech_state_ ;
+ mech_lock_.unlock() ;
+
+ angles.resize(names.size()) ;
+
+ for (unsigned int i=0; i < names.size(); i++)
+ {
+ for (unsigned int j=0; j < mech_state.get_joint_states_size(); j++)
+ {
+ if (names[i] == mech_state.joint_states[j].name) // See if we found the joint we're looking for
+ {
+ angles[i] = mech_state.joint_states[j].position ;
+ break ;
+ }
+ }
+ }
+ }
+
+ void MechStateCallback()
+ {
+ mech_lock_.lock() ;
+ safe_mech_state_ = mech_state_ ;
+ mech_lock_.unlock() ;
+ }
+
+ void SnapshotCallback()
+ {
+ snapshot_lock_.lock() ;
+ safe_snapshot_ = snapshot_ ;
+ snapshot_lock_.unlock() ;
+ }
+
+private:
+ phase_space::PhaseSpaceSnapshot snapshot_ ;
+ robot_msgs::MechanismState mech_state_ ;
+
+ phase_space::PhaseSpaceSnapshot safe_snapshot_ ;
+ int marker_id_ ;
+
+ robot_msgs::MechanismState safe_mech_state_ ;
+
+ ros::thread::mutex mech_lock_ ;
+ ros::thread::mutex snapshot_lock_ ;
+} ;
+
+}
+
+using namespace kinematic_calibration ;
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv) ;
+
+ ArmPhaseSpaceGrabber grabber ;
+ grabber.spin() ;
+
+ ros::fini() ;
+
+ return 0 ;
+}
Added: pkg/trunk/calibration/kinematic_calibration/src/link_param_jacobian.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/link_param_jacobian.cpp (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/src/link_param_jacobian.cpp 2008-11-10 23:50:40 UTC (rev 6483)
@@ -0,0 +1,61 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "kinematic_calibration/link_param_jacobian.h"
+
+using namespace kinematic_calibration ;
+
+LinkParamJacobian::LinkParamJacobian()
+{
+
+
+}
+
+LinkParamJacobian::~LinkParamJacobian()
+{
+
+
+}
+void LinkParamJacobian::changeRefPoint(const KDL::Vector& cur_to_next)
+{
+ const unsigned int N = twists_.size() ;
+ for(unsigned int i=0; i<N; i++)
+ {
+ for (unsigned int j=0; j<3; j++)
+ {
+ twists_[i].trans_[j] = twists_[i].trans_[j].RefPoint(cur_to_next) ;
+ twists_[i].rot_[j] = twists_[i].rot_[j].RefPoint(cur_to_next) ;
+ }
+ }
+}
Added: pkg/trunk/calibration/kinematic_calibration/src/link_param_jacobian_solver.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/link_param_jacobian_solver.cpp (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/src/link_param_jacobian_solver.cpp 2008-11-10 23:50:40 UTC (rev 6483)
@@ -0,0 +1,136 @@
+/*********************************************************************
+* 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...
[truncated message content] |
|
From: <hsu...@us...> - 2008-11-11 01:29:22
|
Revision: 6491
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6491&view=rev
Author: hsujohnhsu
Date: 2008-11-11 01:29:19 +0000 (Tue, 11 Nov 2008)
Log Message:
-----------
update for fake IMU publishing.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-11-11 01:20:56 UTC (rev 6490)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-11-11 01:29:19 UTC (rev 6491)
@@ -53,6 +53,7 @@
<updateRate>1000.0</updateRate>
<bodyName>body_name</bodyName>
<topicName>body_pose_ground_truth</topicName>
+ <IMUTopicName>body_pose_IMU</IMUTopicName>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- option to initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
@@ -77,6 +78,7 @@
<updateRate>1000.0</updateRate>
<bodyName>body_name</bodyName>
<topicName>body_pose_ground_truth</topicName>
+ <IMUTopicName>body_pose_IMU</IMUTopicName>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- option to initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
@@ -125,6 +127,7 @@
/// \brief topic name
private: std::string topicName;
+ private: std::string IMUTopicName;
/// \brief frame transform name, should match link name
/// FIXME: extract link name directly?
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-11-11 01:20:56 UTC (rev 6490)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-11-11 01:29:19 UTC (rev 6491)
@@ -78,14 +78,15 @@
this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(bodyName));
// this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(bodyName));
- this->topicName = node->GetString("topicName", "", 1);
- this->frameName = node->GetString("frameName", "", 1);
- this->xyzOffsets = node->GetVector3("xyzOffsets", Vector3(0,0,0));
- this->rpyOffsets = node->GetVector3("rpyOffsets", Vector3(0,0,0));
+ this->topicName = node->GetString("topicName", "ground_truth", 1);
+ this->IMUTopicName = node->GetString("IMUTopicName", "imu", 1);
+ this->frameName = node->GetString("frameName", "", 1);
+ this->xyzOffsets = node->GetVector3("xyzOffsets", Vector3(0,0,0));
+ this->rpyOffsets = node->GetVector3("rpyOffsets", Vector3(0,0,0));
std::cout << "==== topic name for P3D ======== " << this->topicName << std::endl;
rosnode->advertise<std_msgs::TransformWithRateStamped>(this->topicName,10);
- rosnode->advertise<std_msgs::PoseWithRatesStamped>(this->topicName,10);
+ rosnode->advertise<std_msgs::PoseWithRatesStamped>(this->IMUTopicName,10);
}
////////////////////////////////////////////////////////////////////////////////
@@ -207,7 +208,7 @@
this->poseMsg.acc.ang_acc.az = this->aeul.z;
// publish to ros
- rosnode->publish(this->topicName,this->poseMsg);
+ rosnode->publish(this->IMUTopicName,this->poseMsg);
this->lock.unlock();
// save last time stamp
@@ -219,4 +220,5 @@
void P3D::FiniChild()
{
rosnode->unadvertise(this->topicName);
+ rosnode->unadvertise(this->IMUTopicName);
}
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-11 01:20:56 UTC (rev 6490)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-11 01:29:19 UTC (rev 6491)
@@ -333,6 +333,7 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_right</bodyName>
<topicName>gripper_roll_right_pose_ground_truth</topicName>
+ <IMUTopicName>gripper_roll_right_pose_imu</IMUTopicName>
<frameName>map</frameName>
<interface:position name="p3d_right_wrist_position"/>
</controller:P3D>
@@ -342,6 +343,7 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
<topicName>gripper_roll_left_pose_ground_truth</topicName>
+ <IMUTopicName>gripper_roll_left_pose_imu</IMUTopicName>
<frameName>map</frameName>
<interface:position name="p3d_left_wrist_position"/>
</controller:P3D>
@@ -351,6 +353,7 @@
<updateRate>1000.0</updateRate>
<bodyName>base</bodyName>
<topicName>base_pose_ground_truth</topicName>
+ <IMUTopicName>base_pose_imu</IMUTopicName>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world 2008-11-11 01:20:56 UTC (rev 6490)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world 2008-11-11 01:29:19 UTC (rev 6491)
@@ -354,6 +354,7 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_right</bodyName>
<topicName>gripper_roll_right_pose_ground_truth</topicName>
+ <IMUTopicName>gripper_roll_right_pose_imu</IMUTopicName>
<frameName>map</frameName>
<interface:position name="p3d_right_wrist_position"/>
</controller:P3D>
@@ -363,6 +364,7 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
<topicName>gripper_roll_left_pose_ground_truth</topicName>
+ <IMUTopicName>gripper_roll_left_pose_imu</IMUTopicName>
<frameName>map</frameName>
<interface:position name="p3d_left_wrist_position"/>
</controller:P3D>
@@ -372,6 +374,7 @@
<updateRate>1000.0</updateRate>
<bodyName>base</bodyName>
<topicName>base_pose_ground_truth</topicName>
+ <IMUTopicName>base_pose_imu</IMUTopicName>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world 2008-11-11 01:20:56 UTC (rev 6490)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world 2008-11-11 01:29:19 UTC (rev 6491)
@@ -138,6 +138,7 @@
<updateRate>100.0</updateRate>
<bodyName>base</bodyName>
<topicName>base_pose_ground_truth</topicName>
+ <IMUTopicName>base_pose_imu</IMUTopicName>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world 2008-11-11 01:20:56 UTC (rev 6490)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world 2008-11-11 01:29:19 UTC (rev 6491)
@@ -193,6 +193,7 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_right</bodyName>
<topicName>gripper_roll_right_pose_ground_truth</topicName>
+ <IMUTopicName>gripper_roll_right_pose_imu</IMUTopicName>
<frameName>map</frameName>
<interface:position name="p3d_right_wrist_position"/>
</controller:P3D>
@@ -202,6 +203,7 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
<topicName>gripper_roll_left_pose_ground_truth</topicName>
+ <IMUTopicName>gripper_roll_left_pose_imu</IMUTopicName>
<frameName>map</frameName>
<interface:position name="p3d_left_wrist_position"/>
</controller:P3D>
@@ -211,6 +213,7 @@
<updateRate>1000.0</updateRate>
<bodyName>base</bodyName>
<topicName>base_pose_ground_truth</topicName>
+ <IMUTopicName>base_pose_imu</IMUTopicName>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-11 03:14:00
|
Revision: 6506
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6506&view=rev
Author: hsujohnhsu
Date: 2008-11-11 03:13:50 +0000 (Tue, 11 Nov 2008)
Log Message:
-----------
update controller scan rate for tilting hokuyo to 1Hz, same as current hardware runs.
added noise for P3D imu messages.
Modified Paths:
--------------
pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
Modified: pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2008-11-11 03:01:22 UTC (rev 6505)
+++ pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch 2008-11-11 03:13:50 UTC (rev 6506)
@@ -1,6 +1,8 @@
<launch>
<param name="base_controller/odom_publish_rate" value="10" />
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2/controllers.xml" respawn="false" output="screen" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="tilt_laser_controller sine 20 0.872 0.3475" respawn="false" output="screen" />
+
+ <!--node pkg="pr2_mechanism_controllers" type="control_laser.py" args="tilt_laser_controller sine 20 0.872 0.3475" respawn="false" output="screen" /-->
+ <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="tilt_laser_controller sine 1 .45 .40" />
</launch>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-11 03:01:22 UTC (rev 6505)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-11 03:13:50 UTC (rev 6506)
@@ -334,6 +334,7 @@
<bodyName>gripper_roll_right</bodyName>
<topicName>gripper_roll_right_pose_ground_truth</topicName>
<IMUTopicName>gripper_roll_right_pose_imu</IMUTopicName>
+ <gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<interface:position name="p3d_right_wrist_position"/>
</controller:P3D>
@@ -344,6 +345,7 @@
<bodyName>gripper_roll_left</bodyName>
<topicName>gripper_roll_left_pose_ground_truth</topicName>
<IMUTopicName>gripper_roll_left_pose_imu</IMUTopicName>
+ <gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<interface:position name="p3d_left_wrist_position"/>
</controller:P3D>
@@ -354,6 +356,7 @@
<bodyName>base</bodyName>
<topicName>base_pose_ground_truth</topicName>
<IMUTopicName>base_pose_imu</IMUTopicName>
+ <gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world 2008-11-11 03:01:22 UTC (rev 6505)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world 2008-11-11 03:13:50 UTC (rev 6506)
@@ -139,6 +139,7 @@
<bodyName>base</bodyName>
<topicName>base_pose_ground_truth</topicName>
<IMUTopicName>base_pose_imu</IMUTopicName>
+ <gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world 2008-11-11 03:01:22 UTC (rev 6505)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world 2008-11-11 03:13:50 UTC (rev 6506)
@@ -194,6 +194,7 @@
<bodyName>gripper_roll_right</bodyName>
<topicName>gripper_roll_right_pose_ground_truth</topicName>
<IMUTopicName>gripper_roll_right_pose_imu</IMUTopicName>
+ <gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<interface:position name="p3d_right_wrist_position"/>
</controller:P3D>
@@ -204,6 +205,7 @@
<bodyName>gripper_roll_left</bodyName>
<topicName>gripper_roll_left_pose_ground_truth</topicName>
<IMUTopicName>gripper_roll_left_pose_imu</IMUTopicName>
+ <gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<interface:position name="p3d_left_wrist_position"/>
</controller:P3D>
@@ -214,6 +216,7 @@
<bodyName>base</bodyName>
<topicName>base_pose_ground_truth</topicName>
<IMUTopicName>base_pose_imu</IMUTopicName>
+ <gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml 2008-11-11 03:01:22 UTC (rev 6505)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml 2008-11-11 03:13:50 UTC (rev 6506)
@@ -826,7 +826,7 @@
<axis xyz="0 1 0" /> <!-- direction of the joint in a local coordinate frame -->
<anchor xyz="0 0 0" /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<limit min= "-0.785" max="1.48" effort="0.5292" velocity="1256.0" />
- <joint_properties damping="1.0" friction="0.0" />
+ <joint_properties damping="0.1" friction="0.0" />
</joint>
<joint name="tilt_laser_joint" type="fixed">
<axis xyz="0 1 0" /> <!-- direction of the joint in a local coordinate frame -->
@@ -1751,9 +1751,9 @@
<joint name="tilt_laser_mount_joint" />
<inertial>
- <mass value="1.0" />
+ <mass value="0.1" />
<com xyz="0 0 0" /> <!-- position of the center of mass with respect to the sensor's local coordinate frame -->
- <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
</inertial>
<visual>
@@ -1787,9 +1787,9 @@
<joint name="tilt_laser_joint" />
<inertial>
- <mass value="0.1" />
+ <mass value="0.01" />
<com xyz="0 0 0" /> <!-- position of the center of mass with respect to the sensor's local coordinate frame -->
- <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
</inertial>
<visual>
@@ -1823,11 +1823,11 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <updateRate>10.0</updateRate>
+ <updateRate>20.0</updateRate>
<controller:ros_laser name="ros_tilt_laser_controller" plugin="libRos_Laser.so">
<gaussianNoise>0.005</gaussianNoise>
<alwaysOn>true</alwaysOn>
- <updateRate>15.0</updateRate>
+ <updateRate>20.0</updateRate>
<topicName>tilt_scan</topicName>
<frameName>tilt_laser</frameName>
<interface:laser name="ros_tilt_laser_iface" />
@@ -1885,11 +1885,11 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <updateRate>10.0</updateRate>
+ <updateRate>20.0</updateRate>
<controller:ros_laser name="ros_base_laser_controller" plugin="libRos_Laser.so">
<gaussianNoise>0.005</gaussianNoise>
<alwaysOn>true</alwaysOn>
- <updateRate>10</updateRate>
+ <updateRate>20.0</updateRate>
<topicName>base_scan</topicName>
<frameName>base_laser</frameName>
<interface:laser name="ros_base_laser_iface" />
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-11-11 03:01:22 UTC (rev 6505)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-11-11 03:13:50 UTC (rev 6506)
@@ -209,7 +209,7 @@
<controller name="tilt_laser_controller" topic="tilt_laser_controller" type="LaserScannerControllerNode">
<joint name="tilt_laser_mount_joint" >
- <pid p="0.4" d="0" i="0" iClamp="0.1" />
+ <pid p="12" i=".1" d="1" iClamp="0.5" />
</joint>
</controller>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-11-11 03:01:22 UTC (rev 6505)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-11-11 03:13:50 UTC (rev 6506)
@@ -826,7 +826,7 @@
<axis xyz="0 1 0" /> <!-- direction of the joint in a local coordinate frame -->
<anchor xyz="0 0 0" /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<limit min= "-0.785" max="1.48" effort="0.5292" velocity="1256.0" />
- <joint_properties damping="1.0" friction="0.0" />
+ <joint_properties damping="0.1" friction="0.0" />
</joint>
<joint name="tilt_laser_joint" type="fixed">
<axis xyz="0 1 0" /> <!-- direction of the joint in a local coordinate frame -->
@@ -1751,9 +1751,9 @@
<joint name="tilt_laser_mount_joint" />
<inertial>
- <mass value="1.0" />
+ <mass value="0.1" />
<com xyz="0 0 0" /> <!-- position of the center of mass with respect to the sensor's local coordinate frame -->
- <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
</inertial>
<visual>
@@ -1787,9 +1787,9 @@
<joint name="tilt_laser_joint" />
<inertial>
- <mass value="0.1" />
+ <mass value="0.01" />
<com xyz="0 0 0" /> <!-- position of the center of mass with respect to the sensor's local coordinate frame -->
- <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
</inertial>
<visual>
@@ -1823,11 +1823,11 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <updateRate>10.0</updateRate>
+ <updateRate>20.0</updateRate>
<controller:ros_laser name="ros_tilt_laser_controller" plugin="libRos_Laser.so">
<gaussianNoise>0.005</gaussianNoise>
<alwaysOn>true</alwaysOn>
- <updateRate>15.0</updateRate>
+ <updateRate>20.0</updateRate>
<topicName>tilt_scan</topicName>
<frameName>tilt_laser</frameName>
<interface:laser name="ros_tilt_laser_iface" />
@@ -1885,11 +1885,11 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
- <updateRate>10.0</updateRate>
+ <updateRate>20.0</updateRate>
<controller:ros_laser name="ros_base_laser_controller" plugin="libRos_Laser.so">
<gaussianNoise>0.005</gaussianNoise>
<alwaysOn>true</alwaysOn>
- <updateRate>10</updateRate>
+ <updateRate>20.0</updateRate>
<topicName>base_scan</topicName>
<frameName>base_laser</frameName>
<interface:laser name="ros_base_laser_iface" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-11-11 03:42:02
|
Revision: 6508
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6508&view=rev
Author: hsujohnhsu
Date: 2008-11-11 03:41:56 +0000 (Tue, 11 Nov 2008)
Log Message:
-----------
update tilt laser controller and naming of xacroed pr2 prototype1 xml file.
Modified Paths:
--------------
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_tilt_laser_torso_gazebo.xml
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch 2008-11-11 03:34:14 UTC (rev 6507)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch 2008-11-11 03:41:56 UTC (rev 6508)
@@ -3,11 +3,11 @@
<!-- if needed, group tag allows pushing components into namespace via ns="namespace" -->
<group name="wg">
<!-- create model file for gazebo -->
- <!--node pkg="xacro" type="xacro.py" args="$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml $(find wg_robot_description)/pr2_prototype1/pr2_prototype1_xacroed.xml" /-->
- <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2_prototype1/pr2_prototype1_xacroed.xml $(find gazebo_robot_description)/world/pr2_xml_prototype1.model" />
+ <!--node pkg="xacro" type="xacro.py" args="$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml $(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml.expanded" /-->
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml.expanded $(find gazebo_robot_description)/world/pr2_xml_prototype1.model" />
<!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
(Mechanism Control, BaseControllerNode, etc...) -->
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2_prototype1/pr2_prototype1_xacroed.xml"" />
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml.expanded"" />
<!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_prototype1.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch 2008-11-11 03:34:14 UTC (rev 6507)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch 2008-11-11 03:41:56 UTC (rev 6508)
@@ -5,6 +5,7 @@
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_head_tilt_laser_torso_gazebo.xml" output="screen"/>
<!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
for details of the profile, rates, see controller::LaserScannerControllerNode. -->
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="tilt_laser_controller sine 20 0.872 0.3475" respawn="false" output="screen" />
+ <!--node pkg="pr2_mechanism_controllers" type="control_laser.py" args="tilt_laser_controller sine 20 0.872 0.3475" respawn="false" output="screen" /-->
+ <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="tilt_laser_controller sine 1 .45 .40" />
</launch>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_tilt_laser_torso_gazebo.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_tilt_laser_torso_gazebo.xml 2008-11-11 03:34:14 UTC (rev 6507)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_tilt_laser_torso_gazebo.xml 2008-11-11 03:41:56 UTC (rev 6508)
@@ -27,7 +27,7 @@
<controller name="tilt_laser_controller" topic="tilt_laser_controller" type="LaserScannerControllerNode">
<joint name="tilt_laser_mount_joint" >
- <pid p="0.4" d="0" i="0" iClamp="0.1" />
+ <pid p="12" i=".1" d="1" iClamp="0.5" />
</joint>
</controller>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jl...@us...> - 2008-11-11 03:59:46
|
Revision: 6509
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6509&view=rev
Author: jleibs
Date: 2008-11-11 03:59:42 +0000 (Tue, 11 Nov 2008)
Log Message:
-----------
ReFactoring of ImageWrapper to FillImage and reduced CvBridge.
Modified Paths:
--------------
pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp
pkg/trunk/vision/color_calib/libcolorcalib.cpp
pkg/trunk/vision/cv_view/dcam/cv_view.cpp
Added Paths:
-----------
pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
pkg/trunk/image_msgs/include/image_msgs/FillImage.h
Removed Paths:
-------------
pkg/trunk/image_msgs/include/image_msgs/ImageWrapper.h
Modified: pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp 2008-11-11 03:41:56 UTC (rev 6508)
+++ pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp 2008-11-11 03:59:42 UTC (rev 6509)
@@ -35,7 +35,8 @@
#include <cstdio>
#include "ros/node.h"
-#include "image_msgs/ImageWrapper.h"
+#include "image_msgs/Image.h"
+#include "image_msgs/FillImage.h"
#include "image_msgs/CamInfo.h"
#include "image_msgs/StereoInfo.h"
#include "dcam.h"
@@ -51,7 +52,7 @@
bool do_stereo_;
bool do_rectify_;
- image_msgs::ImageWrapper img_wrapper_;
+ image_msgs::Image img_;
image_msgs::CamInfo cam_info_;
image_msgs::StereoInfo stereo_info_;
@@ -234,11 +235,11 @@
if (stcam->stIm->imDisp)
{
- img_wrapper_.fromInterlacedData( "disparity",
- stcam->stIm->imHeight, stcam->stIm->imWidth, 1,
- "mono", "uint16",
- stcam->stIm->imDisp );
- publish("~disparity", img_wrapper_);
+ fillImage(img_, "disparity",
+ stcam->stIm->imHeight, stcam->stIm->imWidth, 1,
+ "mono", "uint16",
+ stcam->stIm->imDisp );
+ publish("~disparity", img_);
stereo_info_.has_disparity = true;
} else {
@@ -276,11 +277,11 @@
if (img->imRawType != COLOR_CODING_NONE)
{
- img_wrapper_.fromInterlacedData( "image_raw",
- img->imHeight, img->imWidth, 1,
- "mono", "byte",
- img->imRaw );
- publish(base_name + std::string("image_raw"), img_wrapper_);
+ fillImage(img_, "image_raw",
+ img->imHeight, img->imWidth, 1,
+ "mono", "byte",
+ img->imRaw );
+ publish(base_name + std::string("image_raw"), img_);
cam_info_.has_image = true;
} else {
cam_info_.has_image = false;
@@ -288,11 +289,11 @@
if (img->imType != COLOR_CODING_NONE)
{
- img_wrapper_.fromInterlacedData( "image",
- img->imHeight, img->imWidth, 1,
- "mono", "byte",
- img->im );
- publish(base_name + std::string("image"), img_wrapper_);
+ fillImage(img_, "image",
+ img->imHeight, img->imWidth, 1,
+ "mono", "byte",
+ img->im );
+ publish(base_name + std::string("image"), img_);
cam_info_.has_image = true;
} else {
cam_info_.has_image = false;
@@ -300,11 +301,11 @@
if (img->imColorType != COLOR_CODING_NONE)
{
- img_wrapper_.fromInterlacedData( "image_color",
- img->imHeight, img->imWidth, 3,
- "rgb", "byte",
- img->imColor );
- publish(base_name + std::string("image_color"), img_wrapper_);
+ fillImage(img_, "image_color",
+ img->imHeight, img->imWidth, 4,
+ "rgba", "byte",
+ img->imColor );
+ publish(base_name + std::string("image_color"), img_);
cam_info_.has_image_color = true;
} else {
cam_info_.has_image_color = false;
@@ -312,11 +313,11 @@
if (img->imRectType != COLOR_CODING_NONE)
{
- img_wrapper_.fromInterlacedData( "image_rect",
- img->imHeight, img->imWidth, 1,
- "mono", "byte",
- img->imRect );
- publish(base_name + std::string("image_rect"), img_wrapper_);
+ fillImage(img_, "image_rect",
+ img->imHeight, img->imWidth, 1,
+ "mono", "byte",
+ img->imRect );
+ publish(base_name + std::string("image_rect"), img_);
cam_info_.has_image_rect = true;
} else {
cam_info_.has_image_rect = false;
@@ -324,11 +325,11 @@
if (img->imRectColorType != COLOR_CODING_NONE)
{
- img_wrapper_.fromInterlacedData( "image_rect_color",
- img->imHeight, img->imWidth, 3,
- "rgb", "byte",
- img->imRectColor );
- publish(base_name + std::string("image_rect_color"), img_wrapper_);
+ fillImage(img_, "image_rect_color",
+ img->imHeight, img->imWidth, 4,
+ "rgba", "byte",
+ img->imRectColor );
+ publish(base_name + std::string("image_rect_color"), img_);
cam_info_.has_image_rect_color = true;
}else {
cam_info_.has_image_rect_color = false;
@@ -352,19 +353,19 @@
advertise<image_msgs::CamInfo>(base_name + std::string("cam_info"), 1);
if (img->imRawType != COLOR_CODING_NONE)
- advertise<image_msgs::ImageWrapper>(base_name + std::string("image_raw"), 1);
+ advertise<image_msgs::Image>(base_name + std::string("image_raw"), 1);
if (img->imType != COLOR_CODING_NONE)
- advertise<image_msgs::ImageWrapper>(base_name + std::string("image"), 1);
+ advertise<image_msgs::Image>(base_name + std::string("image"), 1);
if (img->imColorType != COLOR_CODING_NONE)
- advertise<image_msgs::ImageWrapper>(base_name + std::string("image_color"), 1);
+ advertise<image_msgs::Image>(base_name + std::string("image_color"), 1);
if (img->imRectType != COLOR_CODING_NONE)
- advertise<image_msgs::ImageWrapper>(base_name + std::string("image_rect"), 1);
+ advertise<image_msgs::Image>(base_name + std::string("image_rect"), 1);
if (img->imRectColorType != COLOR_CODING_NONE)
- advertise<image_msgs::ImageWrapper>(base_name + std::string("image_rect_color"), 1);
+ advertise<image_msgs::Image>(base_name + std::string("image_rect_color"), 1);
}
@@ -380,7 +381,7 @@
advertiseImages("~right/", stcam->stIm->imRight);
if (stcam->stIm->imDisp)
- advertise<image_msgs::ImageWrapper>("~disparity", 1);
+ advertise<image_msgs::Image>("~disparity", 1);
}
else
@@ -391,7 +392,7 @@
bool spin()
{
- // Start up the laser
+ // Start up the camera
while (ok())
{
serviceCam();
@@ -406,12 +407,10 @@
{
ros::init(argc, argv);
- //Keep things from dying poorly
DcamNode dc;
dc.spin();
-
ros::fini();
return 0;
}
Added: pkg/trunk/image_msgs/include/image_msgs/CvBridge.h
===================================================================
--- pkg/trunk/image_msgs/include/image_msgs/CvBridge.h (rev 0)
+++ pkg/trunk/image_msgs/include/image_msgs/CvBridge.h 2008-11-11 03:59:42 UTC (rev 6509)
@@ -0,0 +1,91 @@
+/*********************************************************************
+* 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 CVBRIDGE_HH
+#define CVBRIDGE_HH
+
+#include "image_msgs/Image.h"
+#include "opencv/cxcore.h"
+
+
+namespace image_msgs
+{
+
+ class CvBridge
+ {
+ IplImage* img_;
+ bool owns_data_;
+
+ public:
+
+ CvBridge() : img_(0), owns_data_(false)
+ {
+ img_ = cvCreateImageHeader( cvSize(0,0), IPL_DEPTH_8U, 1 );
+ }
+
+ ~CvBridge()
+ {
+ if (owns_data_)
+ cvReleaseData(&img_);
+
+ cvReleaseImageHeader(&img_);
+ }
+
+
+ inline IplImage* toIpl()
+ {
+ return img_;
+ }
+
+ bool fromImage(Image& rosimg)
+ {
+ if (rosimg.depth == "byte")
+ {
+ cvInitImageHeader(img_, cvSize(rosimg.byte_data.layout.dim[1].size, rosimg.byte_data.layout.dim[0].size), IPL_DEPTH_8U, 1);
+ cvSetData(img_, &(rosimg.byte_data.data[0]), rosimg.byte_data.layout.dim[1].stride);
+ } else if (rosimg.depth == "uint16") {
+ cvInitImageHeader(img_, cvSize(rosimg.uint16_data.layout.dim[1].size, rosimg.uint16_data.layout.dim[0].size), IPL_DEPTH_8U, 1);
+ cvSetData(img_, &(rosimg.uint16_data.data[0]), rosimg.uint16_data.layout.dim[1].stride*sizeof(uint16_t));
+ }
+
+ return true;
+
+ }
+ private:
+
+ };
+}
+
+
+#endif
Added: pkg/trunk/image_msgs/include/image_msgs/FillImage.h
===================================================================
--- pkg/trunk/image_msgs/include/image_msgs/FillImage.h (rev 0)
+++ pkg/trunk/image_msgs/include/image_msgs/FillImage.h 2008-11-11 03:59:42 UTC (rev 6509)
@@ -0,0 +1,103 @@
+/*********************************************************************
+* 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 FILLIMAGE_HH
+#define FILLIMAGE_HH
+
+#include "image_msgs/Image.h"
+
+namespace image_msgs
+{
+
+ template <class M>
+ void fillImageHelper(M &m,
+ uint32_t sz0, uint32_t st0,
+ uint32_t sz1, uint32_t st1,
+ uint32_t sz2, uint32_t st2,
+ void *d)
+ {
+ m.layout.dim.resize(3);
+ m.layout.dim[0].label = "height";
+ m.layout.dim[0].size = sz0;
+ m.layout.dim[0].stride = st0;
+ m.layout.dim[1].label = "width";
+ m.layout.dim[1].size = sz1;
+ m.layout.dim[1].stride = st1;
+ m.layout.dim[2].label = "channel";
+ m.layout.dim[2].size = sz2;
+ m.layout.dim[2].stride = st2;
+ m.data.resize(st0);
+ memcpy((char*)(&m.data[0]), (char*)(d), st0*sizeof(m.data[0]));
+ }
+
+ bool fillImage(Image& image,
+ std::string label_arg,
+ uint32_t height_arg, uint32_t width_arg, uint32_t channel_arg,
+ std::string encoding_arg, std::string depth_arg,
+ void* data_arg,
+ uint32_t channel_step = 0, uint32_t width_step = 0, uint32_t height_step = 0)
+ {
+ image.label = label_arg;
+ image.encoding = encoding_arg;
+ image.depth = depth_arg;
+
+ if (channel_step == 0)
+ channel_step = channel_arg;
+
+ if (width_step == 0)
+ width_step = width_arg * channel_step;
+
+ if (height_step == 0)
+ height_step = height_arg * width_step;
+
+ if (image.depth == "byte")
+ fillImageHelper(image.byte_data,
+ height_arg, height_step,
+ width_arg, width_step,
+ channel_arg, channel_step,
+ data_arg);
+
+ else if (image.depth == "uint16")
+ fillImageHelper(image.uint16_data,
+ height_arg, height_step,
+ width_arg, width_step,
+ channel_arg, channel_step,
+ data_arg);
+
+ return true;
+ }
+}
+
+
+#endif
Deleted: pkg/trunk/image_msgs/include/image_msgs/ImageWrapper.h
===================================================================
--- pkg/trunk/image_msgs/include/image_msgs/ImageWrapper.h 2008-11-11 03:41:56 UTC (rev 6508)
+++ pkg/trunk/image_msgs/include/image_msgs/ImageWrapper.h 2008-11-11 03:59:42 UTC (rev 6509)
@@ -1,123 +0,0 @@
-/*********************************************************************
-* 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 IMAGEWRAPPER_HH
-#define IMAGEWRAPPER_HH
-
-#include "image_msgs/Image.h"
-#include "image.h"
-#include "opencv/cxcore.h"
-
-
-namespace image_msgs
-{
-
- class ImageWrapper : public image_msgs::Image
- {
- public:
- bool fromInterlacedData(std::string label_arg,
- uint32_t height_arg, uint32_t width_arg, uint32_t channel_arg,
- std::string encoding_arg, std::string depth_arg,
- void* data_arg,
- uint32_t channel_step = 0, uint32_t width_step = 0, uint32_t height_step = 0)
- {
- label = label_arg;
- encoding = encoding_arg;
- depth = depth_arg;
-
- if (channel_step == 0)
- channel_step = channel_arg;
-
- if (width_step == 0)
- width_step = width_arg * channel_step;
-
- if (height_step == 0)
- height_step = height_arg * width_step;
-
- if (depth == "byte")
- fromDataHelper(byte_data,
- height_arg, height_step,
- width_arg, width_step,
- channel_arg, channel_step,
- data_arg);
-
- else if (depth == "uint16")
- fromDataHelper(uint16_data,
- height_arg, height_step,
- width_arg, width_step,
- channel_arg, channel_step,
- data_arg);
-
- return true;
- }
-
- IplImage* asIplImage()
- {
- IplImage* img;
- if (depth == "byte")
- {
- img = cvCreateImageHeader(cvSize(byte_data.layout.dim[1].size, byte_data.layout.dim[0].size), IPL_DEPTH_8U, 1);
- cvSetData(img, &(byte_data.data[0]), byte_data.layout.dim[1].stride);
- } else if (depth == "uint16") {
- img = cvCreateImageHeader(cvSize(uint16_data.layout.dim[1].size, uint16_data.layout.dim[0].size), IPL_DEPTH_16U, 1);
- cvSetData(img, &(uint16_data.data[0]), uint16_data.layout.dim[1].stride*sizeof(uint16_t));
- }
- return img;
- }
- private:
- template <class M>
- void fromDataHelper(M &m,
- uint32_t sz0, uint32_t st0,
- uint32_t sz1, uint32_t st1,
- uint32_t sz2, uint32_t st2,
- void *d)
- {
- m.layout.dim.resize(3);
- m.layout.dim[0].label = "height";
- m.layout.dim[0].size = sz0;
- m.layout.dim[0].stride = st0;
- m.layout.dim[1].label = "width";
- m.layout.dim[1].size = sz1;
- m.layout.dim[1].stride = st1;
- m.layout.dim[2].label = "channel";
- m.layout.dim[2].size = sz2;
- m.layout.dim[2].stride = st2;
- m.data.resize(st0);
- memcpy((char*)(&m.data[0]), (char*)(d), st0*sizeof(m.data[0]));
- }
- };
-}
-
-
-#endif
Modified: pkg/trunk/vision/color_calib/libcolorcalib.cpp
===================================================================
--- pkg/trunk/vision/color_calib/libcolorcalib.cpp 2008-11-11 03:41:56 UTC (rev 6508)
+++ pkg/trunk/vision/color_calib/libcolorcalib.cpp 2008-11-11 03:59:42 UTC (rev 6509)
@@ -158,7 +158,7 @@
for (int j = 0; j < src->width; j++)
for (int k = 0; k < channels; k++)
((uchar *)(dst->imageData + i*dst->widthStep))[j*dst->nChannels + k] =
- compandmap[((uchar *)(src->imageData + i*src->widthStep))[j*src->nChannels + k] << 4];
+ compandmap[((uchar *)(src->imageData + i*src->widthStep))[j*src->nChannels + k] << 4] >> 2;
}
}
Modified: pkg/trunk/vision/cv_view/dcam/cv_view.cpp
===================================================================
--- pkg/trunk/vision/cv_view/dcam/cv_view.cpp 2008-11-11 03:41:56 UTC (rev 6508)
+++ pkg/trunk/vision/cv_view/dcam/cv_view.cpp 2008-11-11 03:59:42 UTC (rev 6509)
@@ -4,7 +4,8 @@
#include "opencv/cv.h"
#include "opencv/highgui.h"
#include "ros/node.h"
-#include "image_msgs/ImageWrapper.h"
+#include "image_msgs/Image.h"
+#include "image_msgs/CvBridge.h"
using namespace std;
using namespace ros;
@@ -12,7 +13,8 @@
class CvView : public node
{
public:
- image_msgs::ImageWrapper img;
+ image_msgs::Image img;
+ image_msgs::CvBridge bridge;
CvView() : node("cv_view")
{
@@ -21,12 +23,11 @@
}
void image_cb()
{
- IplImage *cv_image = img.asIplImage();
+ bridge.fromImage(img);
- cvShowImage("cv_view", cv_image);
+ cvShowImage("cv_view", bridge.toIpl());
+
cvWaitKey(5);
-
- cvReleaseImageHeader(&cv_image);
}
};
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