|
From: <tjh...@us...> - 2009-02-01 10:40:19
|
Revision: 10415
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10415&view=rev
Author: tjhunter
Date: 2009-02-01 10:40:15 +0000 (Sun, 01 Feb 2009)
Log Message:
-----------
Upgraded eigen to 2.0-RC1 (required by robot_kinematics)
Modified Paths:
--------------
pkg/trunk/3rdparty/eigen/Makefile
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
Modified: pkg/trunk/3rdparty/eigen/Makefile
===================================================================
--- pkg/trunk/3rdparty/eigen/Makefile 2009-02-01 10:23:37 UTC (rev 10414)
+++ pkg/trunk/3rdparty/eigen/Makefile 2009-02-01 10:40:15 UTC (rev 10415)
@@ -1,7 +1,7 @@
all: eigen
-TARBALL = build/eigen-2.0-beta2.tar.bz2
-TARBALL_URL = http://download.tuxfamily.org/eigen/eigen-2.0-beta2.tar.bz2
+TARBALL = build/eigen-2.0-rc1.tar.bz2
+TARBALL_URL = http://download.tuxfamily.org/eigen/eigen-2.0-rc1.tar.bz2
UNPACK_CMD = tar xfj
SOURCE_DIR = build/eigen2
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-02-01 10:23:37 UTC (rev 10414)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-02-01 10:40:15 UTC (rev 10415)
@@ -29,7 +29,7 @@
*/
/*
- * Author: Melonee Wise
+ * Author: Melonee Wise
*/
#include "urdf/parser.h"
@@ -104,13 +104,13 @@
printf("Extracted KDL Chain with %u Joints and %u segments\n", num_joints_, num_segments_ );
jnt_to_jac_solver_ = new ChainJntToJacSolver(chain_);
jnt_to_pose_solver_ = new ChainFkSolverPos_recursive(chain_);
- task_jac_.set(Eigen::MatrixXf::Zero(6, num_joints_));
- identity_.set(Eigen::MatrixXf::Identity(num_joints_, num_joints_));
- constraint_null_space_.set(Eigen::MatrixXf::Zero(num_joints_, num_joints_));
- constraint_torq_.set(Eigen::MatrixXf::Zero(num_joints_, 1));
- task_torq_.set(Eigen::MatrixXf::Zero(num_joints_, 1));
-
+ task_jac_=Eigen::MatrixXf::Zero(6, num_joints_);
+ identity_=Eigen::MatrixXf::Identity(num_joints_, num_joints_);
+ constraint_null_space_=Eigen::MatrixXf::Zero(num_joints_, num_joints_);
+ constraint_torq_=Eigen::MatrixXf::Zero(num_joints_, 1);
+ task_torq_=Eigen::MatrixXf::Zero(num_joints_, 1);
+
// get chain
TiXmlElement *chain = config->FirstChildElement("chain");
if (!chain) {
@@ -167,9 +167,9 @@
void EndeffectorConstraintController::update()
{
-
-
+
+
// check if joints are calibrated
for (unsigned int i = 0; i < joints_.size(); ++i) {
if (!joints_[i]->calibrated_)
@@ -191,7 +191,7 @@
{
jnt_to_pose_solver_->JntToCart(jnt_pos, desired_frame_);
desired_frame_.M.GetRPY(desired_roll_, desired_pitch_, desired_yaw_);
- initialized_ = true;
+ initialized_ = true;
}
// get the chain jacobian
@@ -211,20 +211,20 @@
// get endeffector pose
jnt_to_pose_solver_->JntToCart(jnt_pos, endeffector_frame_);
- // compute the constraint jacobian and the constraint force
+ // compute the constraint jacobian and the constraint force
computeConstraintJacobian();
-
+
// compute the constraint wrench to apply
constraint_wrench_ = constraint_jac_ * constraint_force_;
- // compute the constraint null space to project
+ // compute the constraint null space to project
computeConstraintNullSpace();
-
+
// convert the wrench into joint torques
-
+
constraint_torq_ = task_jac_.transpose() * constraint_wrench_;
task_torq_ = constraint_null_space_ * task_jac_.transpose() * task_wrench_;
-
+
j = 0;
for (unsigned int i=0; i<num_joints_; i++)
{
@@ -244,10 +244,10 @@
double f_pitch = 0;
double f_yaw = 0;
double roll, pitch, yaw;
-
+
// Compute the end effector angle from the origin of the circle
double ee_theta = atan2(endeffector_frame_.p(2), endeffector_frame_.p(1));
-
+
// Constarint for a cylinder centered around the x axis
constraint_jac_(0,0) = 0; // this is the end of the cylinder
constraint_jac_(1,1) = 0;
@@ -291,12 +291,12 @@
{
f_r = 0;
}
-
+
endeffector_frame_.M.GetRPY(roll, pitch, yaw);
double roll_error = desired_roll_- roll;
double pitch_error = desired_pitch_- pitch;
double yaw_error = desired_yaw_- yaw;
-
+
//roll constraint
if (abs(roll_error) > 0.05)
{
@@ -308,7 +308,7 @@
{
f_roll = 0;
}
-
+
//pitch constraint
if (abs(pitch_error) > 0.05)
{
@@ -320,7 +320,7 @@
{
f_pitch = 0;
}
-
+
//yaw constraint
if (abs(yaw_error) > 0.05)
{
@@ -342,11 +342,11 @@
void EndeffectorConstraintController::computeConstraintNullSpace()
{
- // Compute generalized inverse, this is the transpose as long as the constraints are
+ // Compute generalized inverse, this is the transpose as long as the constraints are
// orthonormal to eachother. Will replace with QR method later.
constraint_null_space_ = identity_ - task_jac_.transpose()*constraint_jac_*constraint_jac_.transpose()*task_jac_;
-
-
+
+
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2009-02-02 18:39:46
|
Revision: 10426
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10426&view=rev
Author: rob_wheeler
Date: 2009-02-02 18:39:21 +0000 (Mon, 02 Feb 2009)
Log Message:
-----------
Add per-controller timing statistics
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2009-02-02 17:40:18 UTC (rev 10425)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2009-02-02 18:39:21 UTC (rev 10426)
@@ -1,6 +1,6 @@
///////////////////////////////////////////////////////////////////////////////
-// Copyright (C) 2008, Eric Berger
+// Copyright (C) 2008-2009, Willow Garage, Inc.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
@@ -41,7 +41,13 @@
#include <tinyxml/tinyxml.h>
#include <hardware_interface/hardware_interface.h>
#include <mechanism_model/robot.h>
+#include <boost/circular_buffer.hpp>
#include <boost/thread/mutex.hpp>
+#include <boost/accumulators/accumulators.hpp>
+#include <boost/accumulators/statistics/stats.hpp>
+#include <boost/accumulators/statistics/max.hpp>
+#include <boost/accumulators/statistics/mean.hpp>
+#include <boost/accumulators/statistics/variance.hpp>
#include <mechanism_model/controller.h>
#include <realtime_tools/realtime_publisher.h>
#include <misc_utils/advertised_service_guard.h>
@@ -51,9 +57,9 @@
#include <robot_srvs/SpawnController.h>
#include <robot_srvs/KillController.h>
#include <robot_msgs/MechanismState.h>
+#include <robot_msgs/DiagnosticMessage.h>
#include "tf/tfMessage.h"
-
typedef controller::Controller* (*ControllerAllocator)();
class MechanismControl {
@@ -84,6 +90,20 @@
controller::Controller* controllers_[MAX_NUM_CONTROLLERS];
std::string controller_names_[MAX_NUM_CONTROLLERS];
+ typedef boost::accumulators::accumulator_set<double, boost::accumulators::stats<boost::accumulators::tag::max, boost::accumulators::tag::mean, boost::accumulators::tag::variance> > TimeStatistics;
+ struct {
+ TimeStatistics acc_;
+ double max_;
+ boost::circular_buffer<double> max1_;
+ struct {
+ TimeStatistics acc_;
+ double max_;
+ boost::circular_buffer<double> max1_;
+ } controllers_[MAX_NUM_CONTROLLERS];
+ } diagnostics_;
+ realtime_tools::RealtimePublisher<robot_msgs::DiagnosticMessage> publisher_;
+ void publishDiagnostics();
+
// Killing a controller:
// 1. Non-realtime thread places the index of the controller into please_remove_
// 2. Realtime thread moves the controller out of the array and into removed_
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2009-02-02 17:40:18 UTC (rev 10425)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2009-02-02 18:39:21 UTC (rev 10426)
@@ -33,12 +33,17 @@
#include "ros/console.h"
using namespace mechanism;
+using namespace boost::accumulators;
MechanismControl::MechanismControl(HardwareInterface *hw) :
- state_(NULL), hw_(hw), initialized_(0), please_remove_(-1), removed_(NULL)
+ state_(NULL), hw_(hw), initialized_(0), publisher_("/diagnostics", 1), please_remove_(-1), removed_(NULL)
{
memset(controllers_, 0, MAX_NUM_CONTROLLERS * sizeof(void*));
model_.hw_ = hw;
+
+ diagnostics_.max1_.resize(60);
+ for (int i = 0; i < MAX_NUM_CONTROLLERS; ++i)
+ diagnostics_.controllers_[i].max1_.resize(60);
}
MechanismControl::~MechanismControl()
@@ -72,23 +77,95 @@
return NULL;
}
+#define ADD_STRING_FMT(lab, fmt, ...) \
+ s.label = (lab); \
+ { char buf[1024]; \
+ snprintf(buf, sizeof(buf), fmt, ##__VA_ARGS__); \
+ s.value = buf; \
+ } \
+ strings.push_back(s)
+void MechanismControl::publishDiagnostics()
+{
+ if (publisher_.trylock())
+ {
+ int active = 0;
+ TimeStatistics a;
+
+ vector<robot_msgs::DiagnosticStatus> statuses;
+ vector<robot_msgs::DiagnosticValue> values;
+ vector<robot_msgs::DiagnosticString> strings;
+ robot_msgs::DiagnosticStatus status;
+ robot_msgs::DiagnosticValue v;
+ robot_msgs::DiagnosticString s;
+
+ status.name = "Mechanism Control";
+ status.level = 0;
+ status.message = "OK";
+
+ for (int i = 0; i < MAX_NUM_CONTROLLERS; ++i)
+ {
+ if (controllers_[i] != NULL)
+ {
+ ++active;
+ double m = extract_result<tag::max>(diagnostics_.controllers_[i].acc_);
+ diagnostics_.controllers_[i].max1_.push_back(m);
+ diagnostics_.controllers_[i].max_ = std::max(m, diagnostics_.controllers_[i].max_);
+ ADD_STRING_FMT(controller_names_[i], "%.4f (avg) %.4f (stdev) %.4f (max) %.4f (1-min max) %.4f (life max)", mean(diagnostics_.controllers_[i].acc_)*1e6, sqrt(variance(diagnostics_.controllers_[i].acc_))*1e6, m*1e6, *std::max_element(diagnostics_.controllers_[i].max1_.begin(), diagnostics_.controllers_[i].max1_.end())*1e6, diagnostics_.controllers_[i].max_*1e6);
+ /* Clear accumulator */
+ diagnostics_.controllers_[i].acc_ = a;
+ }
+ }
+
+ double m = extract_result<tag::max>(diagnostics_.acc_);
+ diagnostics_.max1_.push_back(m);
+ diagnostics_.max_ = std::max(m, diagnostics_.max_);
+ ADD_STRING_FMT("Overall", "%.4f (avg) %.4f (stdev) %.4f (max) %.4f (1-min max) %.4f (life max)", mean(diagnostics_.acc_)*1e6, sqrt(variance(diagnostics_.acc_))*1e6, m*1e6, *std::max_element(diagnostics_.max1_.begin(), diagnostics_.max1_.end())*1e6, diagnostics_.max_*1e6);
+ ADD_STRING_FMT("Active controllers", "%d", active);
+
+ status.set_values_vec(values);
+ status.set_strings_vec(strings);
+ statuses.push_back(status);
+ publisher_.msg_.set_status_vec(statuses);
+ publisher_.unlockAndPublish();
+
+ /* Clear accumulator */
+ diagnostics_.acc_ = a;
+ }
+}
+
// Must be realtime safe.
void MechanismControl::update()
{
+ static int count = 0;
+
state_->propagateState();
state_->zeroCommands();
// Update all controllers
+ double start_update = realtime_gettime();
for (int i = 0; i < MAX_NUM_CONTROLLERS; ++i)
{
if (controllers_[i] != NULL)
+ {
+ double start = realtime_gettime();
controllers_[i]->update();
+ double end = realtime_gettime();
+ diagnostics_.controllers_[i].acc_(end - start);
+ }
}
+ double end_update = realtime_gettime();
+ diagnostics_.acc_(end_update - start_update);
state_->enforceSafety();
state_->propagateEffort();
+ if (++count == 1000)
+ {
+ count = 0;
+ publishDiagnostics();
+ }
+
// If there's a controller to remove, we take it out of the controllers array.
if (please_remove_ >= 0)
{
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2009-02-02 17:40:18 UTC (rev 10425)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2009-02-02 18:39:21 UTC (rev 10426)
@@ -479,7 +479,7 @@
if (!g_options.xml_)
Usage("You must specify a robot description XML file");
- ros::Node *node = new ros::Node("mechanism_control",
+ ros::Node *node = new ros::Node("pr2_etherCAT",
ros::Node::DONT_HANDLE_SIGINT);
// Catch attempts to quit
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-02-02 22:23:35
|
Revision: 10439
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10439&view=rev
Author: vijaypradeep
Date: 2009-02-02 22:23:30 +0000 (Mon, 02 Feb 2009)
Log Message:
-----------
Updating manifests. Removing dep on visual_odometry
Modified Paths:
--------------
pkg/trunk/drivers/laser/laser_scan_annotator/manifest.xml
pkg/trunk/util/point_cloud_utils/CMakeLists.txt
pkg/trunk/util/point_cloud_utils/manifest.xml
Modified: pkg/trunk/drivers/laser/laser_scan_annotator/manifest.xml
===================================================================
--- pkg/trunk/drivers/laser/laser_scan_annotator/manifest.xml 2009-02-02 21:59:15 UTC (rev 10438)
+++ pkg/trunk/drivers/laser/laser_scan_annotator/manifest.xml 2009-02-02 22:23:30 UTC (rev 10439)
@@ -4,7 +4,7 @@
</description>
<author>Vijay Pradeep</author>
<license>BSD</license>
-<review status="unreviewed" notes=""/>
+<review status="experimental" notes=""/>
<url>http://pr.willowgarage.com</url>
<depend package="std_msgs"/>
<depend package="roscpp"/>
Modified: pkg/trunk/util/point_cloud_utils/CMakeLists.txt
===================================================================
--- pkg/trunk/util/point_cloud_utils/CMakeLists.txt 2009-02-02 21:59:15 UTC (rev 10438)
+++ pkg/trunk/util/point_cloud_utils/CMakeLists.txt 2009-02-02 22:23:30 UTC (rev 10439)
@@ -3,9 +3,9 @@
rospack(point_cloud_utils)
rospack_add_library(point_cloud_utils src/blob_finder.cpp
- src/rigid_transform_finder.cpp
+ #src/rigid_transform_finder.cpp
src/scan_assembler.cpp
src/timed_scan_assembler.cpp)
#rospack_add_executable(timed_scan_tester src/timed_scan_tester.cpp)
-#target_link_libraries(timed_scan_tester point_cloud_utils)
\ No newline at end of file
+#target_link_libraries(timed_scan_tester point_cloud_utils)
Modified: pkg/trunk/util/point_cloud_utils/manifest.xml
===================================================================
--- pkg/trunk/util/point_cloud_utils/manifest.xml 2009-02-02 21:59:15 UTC (rev 10438)
+++ pkg/trunk/util/point_cloud_utils/manifest.xml 2009-02-02 22:23:30 UTC (rev 10439)
@@ -6,13 +6,13 @@
</description>
<author>Vijay Pradeep</author>
<license>BSD</license>
-<review status="unreviewed" notes=""/>
+<review status="experimental" notes=""/>
<url>http://pr.willowgarage.com</url>
<depend package="std_msgs"/>
<depend package="roscpp"/>
<depend package="tf"/>
<depend package="bullet"/>
-<depend package="visual_odometry" />
+<!-- depend package="visual_odometry" / -->
<depend package="laser_scan" />
<export>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2009-02-02 23:13:14
|
Revision: 10444
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10444&view=rev
Author: jfaustwg
Date: 2009-02-02 23:13:06 +0000 (Mon, 02 Feb 2009)
Log Message:
-----------
service request/response -> Request/Response
Modified Paths:
--------------
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_position_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_velocity_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/grasp_point_node.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_qualification.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_velocity_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_position_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_velocity_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/grasp_point_node.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_qualification.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_arm_dynamics_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_grasp_point_node.cpp
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/sine_sweep_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/hysteresis_controller.cpp
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/src/sine_sweep_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_orientation_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_velocity_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_autotuner.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_blind_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_effort_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_pd_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_smoothing_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/lqr_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_orientation_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_velocity_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_autotuner.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_blind_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_effort_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_pd_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_smoothing_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/lqr_controller.cpp
pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
pkg/trunk/drivers/cam/bumblebee_bridge/bumblebee_bridge.cpp
pkg/trunk/drivers/imu/imu_node/imu_node.cc
pkg/trunk/drivers/robot/katana/nodes/katana_client/katana_client.cpp
pkg/trunk/drivers/robot/katana/nodes/katana_server/katana_server.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/include/point_cloud_assembler/base_assembler_srv.h
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/grab_cloud_data.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
pkg/trunk/drivers/robot/pr2/pr2_power_board/include/power_node.h
pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/power_node.cpp
pkg/trunk/hardware_test/self_test/include/self_test/self_test.h
pkg/trunk/hardware_test/self_test/src/run_selftest.cpp
pkg/trunk/highlevel/highlevel_controllers/src/control_cli.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_end_effector.cpp
pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
pkg/trunk/mapping/door_handle_detector/src/door_handle_detector.cpp
pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/base/execute_plan_to_state_minimal.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_position_minimal.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state_minimal.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_replan_to_state.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/test_path_execution.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/include/robarm3d/plan_path_node.h
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/plan_path_node.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/sim_grasp_path.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/sim_path_srv.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/test_plan_path.cpp
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/deadreckon/deadreckon.cpp
pkg/trunk/nav/deadreckon/test_deadreckon_service.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/nav_view_sdl/nav_view.cpp
pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
pkg/trunk/nav/slam_gmapping/src/slam_gmapping.h
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
pkg/trunk/prcore/tf/include/tf/transform_listener.h
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
pkg/trunk/util/filter_coefficient_server/src/filter_coeff_client.cpp
pkg/trunk/util/filter_coefficient_server/test/check_function_calls.cpp
pkg/trunk/util/filter_demo/src/filtering_example.cpp
pkg/trunk/util/kinematics/libKinematics/include/libKinematics/pr2_ik_node.h
pkg/trunk/util/kinematics/libKinematics/src/pr2_ik_node.cpp
pkg/trunk/util/mux/mux.cpp
pkg/trunk/util/mux/switch.cpp
pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/bumblebee_grab_sample.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_display.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/world_models/map_saver/src/map_saver.cpp
pkg/trunk/world_models/map_server/include/map_server/image_loader.h
pkg/trunk/world_models/map_server/src/image_loader.cpp
pkg/trunk/world_models/map_server/src/main.cpp
pkg/trunk/world_models/map_server/test/rtest.cpp
pkg/trunk/world_models/map_server/test/utest.cpp
pkg/trunk/world_models/topological_map/src/ros_bottleneck_graph.cpp
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-02-02 23:10:51 UTC (rev 10443)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-02-02 23:13:06 UTC (rev 10444)
@@ -87,7 +87,7 @@
std_msgs::BaseVel _vel;
// service messages
- pr2_mechanism_controllers::WheelRadiusMultiplier::request _srv_snd, _srv_rsp;
+ pr2_mechanism_controllers::WheelRadiusMultiplier::Request _srv_snd, _srv_rsp;
// active sensors
bool _odom_active, _imu_active, _mech_active, _completed;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h 2009-02-02 23:10:51 UTC (rev 10443)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h 2009-02-02 23:13:06 UTC (rev 10444)
@@ -96,14 +96,14 @@
// * @brief Overloaded method for convenience
// * @param req
// */
-// void setJointCmd(robot_msgs::SetJointPosCmd::request &req);
+// void setJointCmd(robot_msgs::SetJointPosCmd::Request &req);
void getJointCmd(robot_msgs::JointCmd & cmd) const;
/*!
* \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
*/
-// void getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::response &resp);
+// void getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::Response &resp);
// void getCurrentConfiguration(std::vector<double> &);
@@ -114,9 +114,9 @@
boost::mutex arm_controller_lock_;
-// void setJointGains(const pr2_mechanism_controllers::SetJointGains::request &req);
+// void setJointGains(const pr2_mechanism_controllers::SetJointGains::Request &req);
-// void getJointGains(pr2_mechanism_controllers::GetJointGains::response &resp);
+// void getJointGains(pr2_mechanism_controllers::GetJointGains::Response &resp);
controller::JointEffortController* getJointEffortControllerByName(std::string name);
@@ -213,8 +213,8 @@
* @param resp The response is empty
* @return
*/
- bool setJointSrv(robot_srvs::SetJointCmd::request &req,
- robot_srvs::SetJointCmd::response &resp);
+ bool setJointSrv(robot_srvs::SetJointCmd::Request &req,
+ robot_srvs::SetJointCmd::Response &resp);
/** @brief service that returns the goal of the controller
* @note if you know the goal has been reached and you do not want to subscribe to the /mechanism_state topic, you can use it as a hack to get the position of the arm
@@ -222,8 +222,8 @@
* @param resp the response, contains a JointPosCmd message with the goal of the controller
* @return
*/
- bool getJointCmd(robot_srvs::GetJointCmd::request &req,
- robot_srvs::GetJointCmd::response &resp);
+ bool getJointCmd(robot_srvs::GetJointCmd::Request &req,
+ robot_srvs::GetJointCmd::Response &resp);
private:
robot_msgs::JointCmd msg_; //The message used by the ROS callback
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_position_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_position_controller.h 2009-02-02 23:10:51 UTC (rev 10443)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_position_controller.h 2009-02-02 23:13:06 UTC (rev 10444)
@@ -108,7 +108,7 @@
// * @brief Overloaded method for convenience
// * @param req
// */
-// void setJointPosCmd(pr2_mechanism_controllers::SetJointPosCmd::request &req);
+// void setJointPosCmd(pr2_mechanism_controllers::SetJointPosCmd::Request &req);
void setJointPosCmd(const pr2_mechanism_controllers::JointPosCmd & cmd);
@@ -117,7 +117,7 @@
/*!
* \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
*/
-// void getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::response &resp);
+// void getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::Response &resp);
// void getCurrentConfiguration(std::vector<double> &);
@@ -128,9 +128,9 @@
boost::mutex arm_controller_lock_;
-// void setJointGains(const pr2_mechanism_controllers::SetJointGains::request &req);
+// void setJointGains(const pr2_mechanism_controllers::SetJointGains::Request &req);
-// void getJointGains(pr2_mechanism_controllers::GetJointGains::response &resp);
+// void getJointGains(pr2_mechanism_controllers::GetJointGains::Response &resp);
controller::JointPositionController* getJointControllerByName(std::string name);
@@ -228,8 +228,8 @@
* @param resp The response is empty
* @return
*/
- bool setJointPosSrv(pr2_mechanism_controllers::SetJointPosCmd::request &req,
- pr2_mechanism_controllers::SetJointPosCmd::response &resp);
+ bool setJointPosSrv(pr2_mechanism_controllers::SetJointPosCmd::Request &req,
+ pr2_mechanism_controllers::SetJointPosCmd::Response &resp);
/** @brief sets a command for all the joints managed by the controller at once, by specifying an array
* This is a lightweight version of setJointPosHeadless when the caller knows the order of the joints.
@@ -247,8 +247,8 @@
* @param resp empty
* @return true if the trajectory could be followed
*/
- bool setJointPosTarget(pr2_mechanism_controllers::SetJointTarget::request &req,
- pr2_mechanism_controllers::SetJointTarget::response &resp);
+ bool setJointPosTarget(pr2_mechanism_controllers::SetJointTarget::Request &req,
+ pr2_mechanism_controllers::SetJointTarget::Response &resp);
/** @brief non blocking service to specify a position target
* Given an array of tuples (name, position, error margin), the controllers sets this as a new objective for the arm controller, for these elements only. This service retutrns immediately.
@@ -259,8 +259,8 @@
* @param resp
* @return
*/
- bool setJointPosHeadless(pr2_mechanism_controllers::SetJointTarget::request &req,
- pr2_mechanism_controllers::SetJointTarget::response &resp);
+ bool setJointPosHeadless(pr2_mechanism_controllers::SetJointTarget::Request &req,
+ pr2_mechanism_controllers::SetJointTarget::Response &resp);
/** @brief service that returns the goal of the controller
* @note if you know the goal has been reached and you do not want to subscribe to the /mechanism_state topic, you can use it as a hack to get the position of the arm
@@ -268,8 +268,8 @@
* @param resp the response, contains a JointPosCmd message with the goal of the controller
* @return
*/
- bool getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::request &req,
- pr2_mechanism_controllers::GetJointPosCmd::response &resp);
+ bool getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::Request &req,
+ pr2_mechanism_controllers::GetJointPosCmd::Response &resp);
/** @brief ROS callback hook
* Provides a ROS callback to set a new goal. The topic the controller listens to is set in the xml init file.
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-02-02 23:10:51 UTC (rev 10443)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-02-02 23:13:06 UTC (rev 10444)
@@ -208,17 +208,17 @@
* @param resp the response, contains a JointPosCmd message with the goal of the controller
* @return
*/
- bool getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::request &req,
- pr2_mechanism_controllers::GetJointPosCmd::response &resp);
+ bool getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::Request &req,
+ pr2_mechanism_controllers::GetJointPosCmd::Response &resp);
- bool setJointTrajSrv(pr2_mechanism_controllers::TrajectoryStart::request &req,
- pr2_mechanism_controllers::TrajectoryStart::response &resp);
+ bool setJointTrajSrv(pr2_mechanism_controllers::TrajectoryStart::Request &req,
+ pr2_mechanism_controllers::TrajectoryStart::Response &resp);
- bool queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::request &req,
- pr2_mechanism_controllers::TrajectoryQuery::response &resp);
+ bool queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::Request &req,
+ pr2_mechanism_controllers::TrajectoryQuery::Response &resp);
- bool cancelJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::request &req,
- pr2_mechanism_controllers::TrajectoryQuery::response &resp);
+ bool cancelJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::Request &req,
+ pr2_mechanism_controllers::TrajectoryQuery::Response &resp);
void deleteTrajectoryFromQueue(int id);
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_velocity_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_velocity_controller.h 2009-02-02 23:10:51 UTC (rev 10443)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_velocity_controller.h 2009-02-02 23:13:06 UTC (rev 10444)
@@ -85,12 +85,12 @@
*
* \param double pos Position command to issue
*/
- void setJointVelCmd(pr2_mechanism_controllers::SetJointVelCmd::request &req);
+ void setJointVelCmd(pr2_mechanism_controllers::SetJointVelCmd::Request &req);
/*!
* \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
*/
- void getJointVelCmd(pr2_mechanism_controllers::GetJointVelCmd::response &resp);
+ void getJointVelCmd(pr2_mechanism_controllers::GetJointVelCmd::Response &resp);
void getCurrentConfiguration(std::vector<double> &);
/*!
@@ -100,9 +100,9 @@
boost::mutex arm_controller_lock_;
- void setJointGains(const pr2_mechanism_controllers::SetJointGains::request &req);
+ void setJointGains(const pr2_mechanism_controllers::SetJointGains::Request &req);
- void getJointGains(pr2_mechanism_controllers::GetJointGains::response &resp);
+ void getJointGains(pr2_mechanism_controllers::GetJointGains::Response &resp);
controller::JointVelocityController* getJointControllerByName(std::string name);
@@ -139,21 +139,21 @@
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
// Services
- bool setJointVelCmd(pr2_mechanism_controllers::SetJointVelCmd::request &req,
- pr2_mechanism_controllers::SetJointVelCmd::response &resp);
+ bool setJointVelCmd(pr2_mechanism_controllers::SetJointVelCmd::Request &req,
+ pr2_mechanism_controllers::SetJointVelCmd::Response &resp);
- bool getJointVelCmd(pr2_mechanism_controllers::GetJointVelCmd::request &req,
- pr2_mechanism_controllers::GetJointVelCmd::response &resp);
+ bool getJointVelCmd(pr2_mechanism_controllers::GetJointVelCmd::Request &req,
+ pr2_mechanism_controllers::GetJointVelCmd::Response &resp);
- bool setCartesianVelCmd(pr2_mechanism_controllers::SetCartesianVelCmd::request &req,pr2_mechanism_controllers::SetCartesianVelCmd::response &resp);
+ bool setCartesianVelCmd(pr2_mechanism_controllers::SetCartesianVelCmd::Request &req,pr2_mechanism_controllers::SetCartesianVelCmd::Response &resp);
- bool getCartesianVelCmd(pr2_mechanism_controllers::GetCartesianVelCmd::request &req,pr2_mechanism_controllers::GetCartesianVelCmd::response &resp);
+ bool getCartesianVelCmd(pr2_mechanism_controllers::GetCartesianVelCmd::Request &req,pr2_mechanism_controllers::GetCartesianVelCmd::Response &resp);
- bool setJointGains(pr2_mechanism_controllers::SetJointGains::request &req,
- pr2_mechanism_controllers::SetJointGains::response &resp);
+ bool setJointGains(pr2_mechanism_controllers::SetJointGains::Request &req,
+ pr2_mechanism_controllers::SetJointGains::Response &resp);
- bool getJointGains(pr2_mechanism_controllers::GetJointGains::request &req,
- pr2_mechanism_controllers::GetJointGains::response &resp);
+ bool getJointGains(pr2_mechanism_controllers::GetJointGains::Request &req,
+ pr2_mechanism_controllers::GetJointGains::Response &resp);
private:
ArmVelocityController *c_;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h 2009-02-02 23:10:51 UTC (rev 10443)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h 2009-02-02 23:13:06 UTC (rev 10444)
@@ -469,17 +469,17 @@
void getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw);
// Services
- bool setCommand(pr2_mechanism_controllers::SetBaseCommand::request &req,
- pr2_mechanism_controllers::SetBaseCommand::response &resp);
+ bool setCommand(pr2_mechanism_controllers::SetBaseCommand::Request &req,
+ pr2_mechanism_controllers::SetBaseCommand::Response &resp);
- bool getCommand(pr2_mechanism_controllers::GetBaseCommand::request &req,
- pr2_mechanism_controllers::GetBaseCommand::response &resp);
+ bool getCommand(pr2_mechanism_controllers::GetBaseCommand::Request &req,
+ pr2_mechanism_controllers::GetBaseCommand::Response &resp);
- bool getWheelRadiusMultiplier(pr2_mechanism_controllers::WheelRadiusMultiplier::request &req,
- pr2_mechanism_controllers::WheelRadiusMultiplier::response &resp);
+ bool getWheelRadiusMultiplier(pr2_mechanism_controllers::WheelRadiusMultiplier::Request &req,
+ pr2_mechanism_controllers::WheelRadiusMultiplier::Response &resp);
- bool setWheelRadiusMultiplier(pr2_mechanism_controllers::WheelRadiusMultiplier::request &req,
- pr2_mechanism_controllers::WheelRadiusMultiplier::response &resp);
+ bool setWheelRadiusMultiplier(pr2_mechanism_controllers::WheelRadiusMultiplier::Request &req,
+ pr2_mechanism_controllers::WheelRadiusMultiplier::Response &resp);
/*
* \brief callback function for setting the desired velocity using a topic
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h 2009-02-02 23:10:51 UTC (rev 10443)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h 2009-02-02 23:13:06 UTC (rev 10444)
@@ -456,17 +456,17 @@
void getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw);
// Services
- bool setCommand(pr2_mechanism_controllers::SetBaseCommand::request &req,
- pr2_mechanism_controllers::SetBaseCommand::response &resp);
+ bool setCommand(pr2_mechanism_controllers::SetBaseCommand::Request &req,
+ pr2_mechanism_controllers::SetBaseCommand::Response &resp);
- bool getCommand(pr2_mechanism_controllers::GetBaseCommand::request &req,
- pr2_mechanism_controllers::GetBaseCommand::response &resp);
+ bool getCommand(pr2_mechanism_controllers::GetBaseCommand::Request &req,
+ pr2_mechanism_controllers::GetBaseCommand::Response &resp);
- bool getWheelRadiusMultiplier(pr2_mechanism_controllers::WheelRadiusMultiplier::request &req,
- pr2_mechanism_controllers::WheelRadiusMultiplier::response &resp);
+ bool getWheelRadiusMultiplier(pr2_mechanism_controllers::WheelRadiusMultiplier::Request &req,
+ pr2_mechanism_controllers::WheelRadiusMultiplier::Response &resp);
- bool setWheelRadiusMultiplier(pr2_mechanism_controllers::WheelRadiusMultiplier::request &req,
- pr2_mechanism_controllers::WheelRadiusMultiplier::response &resp);
+ bool setWheelRadiusMultiplier(pr2_mechanism_controllers::WheelRadiusMultiplier::Request &req,
+ pr2_mechanism_controllers::WheelRadiusMultiplier::Response &resp);
/*
* \brief callback function for setting the desired velocity using a topic
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h 2009-02-02 23:10:51 UTC (rev 10443)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h 2009-02-02 23:13:06 UTC (rev 10444)
@@ -88,8 +88,8 @@
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- bool calibrateCommand(robot_mechanism_controllers::CalibrateJoint::request &req,
- robot_mechanism_controllers::CalibrateJoint::response &resp);
+ bool calibrateCommand(robot_mechanism_controllers::CalibrateJoint::Request &req,
+ robot_mechanism_controllers::CalibrateJoint::Response &resp);
private:
mechanism::RobotState *robot_;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/grasp_point_node.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_contr...
[truncated message content] |
|
From: <jfa...@us...> - 2009-02-03 00:27:36
|
Revision: 10456
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10456&view=rev
Author: jfaustwg
Date: 2009-02-03 00:27:31 +0000 (Tue, 03 Feb 2009)
Log Message:
-----------
Remove all calls to ros::fini()
Modified Paths:
--------------
pkg/trunk/audio/audio_capture/audio_capture.cpp
pkg/trunk/audio/audio_capture/audio_clip_writer.cpp
pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/grasp_point_node.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_grasp_point_node.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
pkg/trunk/deprecated/cv_wrappers/dcam/cv_view.cpp
pkg/trunk/deprecated/cv_wrappers/minimal/cv_view.cpp
pkg/trunk/deprecated/cv_wrappers/src/cv_movie_streamer.cpp
pkg/trunk/deprecated/cv_wrappers/src/cv_view.cpp
pkg/trunk/deprecated/cv_wrappers/src/cv_view_array.cpp
pkg/trunk/deprecated/ransac_ground_plane_extraction/src/ransac_ground_plane_extraction_launch_node.cpp
pkg/trunk/deprecated/scan_utils/src/cloudToOctree/cloudToOctree.cpp
pkg/trunk/deprecated/scan_utils/src/listen_node/rosSystemCalls.cpp
pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
pkg/trunk/drivers/cam/axis_cam/src/axis_demo/axis_demo.cpp
pkg/trunk/drivers/cam/axis_cam/src/axis_ptz/axis_ptz.cpp
pkg/trunk/drivers/cam/bumblebee_bridge/bumblebee_bridge.cpp
pkg/trunk/drivers/cam/dcam/src/nodes/check_params.cpp
pkg/trunk/drivers/cam/dcam/src/nodes/dcam.cpp
pkg/trunk/drivers/cam/dcam/src/nodes/stereodcam.cpp
pkg/trunk/drivers/cam/elphel_cam/src/elphel_node/elphel_node.cpp
pkg/trunk/drivers/imu/imu_node/imu_node.cc
pkg/trunk/drivers/input/joy/joy.cpp
pkg/trunk/drivers/input/joy_annotator/joy_annotator.cpp
pkg/trunk/drivers/input/joy_annotator/joy_node.cpp
pkg/trunk/drivers/laser/hokuyo_node/hokuyo_node.cpp
pkg/trunk/drivers/laser/laser_scan_annotator/src/laser_scan_annotator_node.cpp
pkg/trunk/drivers/laser/laser_view/laser_view.cpp
pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp
pkg/trunk/drivers/phase_space/phase_space_node.cpp
pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
pkg/trunk/drivers/robot/katana/nodes/katana_client/katana_client.cpp
pkg/trunk/drivers/robot/katana/nodes/katana_server/katana_server.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/grab_cloud_data.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/laser_scan_assembler_srv.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler_srv.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/test/test_assembler.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_odometry.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pan_tilt.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pose_stamped.cpp
pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/power_node.cpp
pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
pkg/trunk/estimation/robot_pose_ekf/test/test_ekf.cpp
pkg/trunk/hardware_test/self_test/src/run_selftest.cpp
pkg/trunk/hardware_test/self_test/src/selftest_example.cpp
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/WatchDog.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/trex_watchdog.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp
pkg/trunk/highlevel/highlevel_controllers/src/control_cli.cpp
pkg/trunk/highlevel/highlevel_controllers/src/indefinite_nav.cpp
pkg/trunk/highlevel/highlevel_controllers/src/joy_batt_sender.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base_navfn.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base_topological.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_end_effector.cpp
pkg/trunk/highlevel/highlevel_controllers/src/recharge_controller.cpp
pkg/trunk/manip/spacenav_node/src/spacenav_node.cpp
pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cpp
pkg/trunk/mapping/cloud_io/src/tools/bag_pcd.cpp
pkg/trunk/mapping/cloud_io/src/tools/pcd_generator.cpp
pkg/trunk/mapping/collision_map/src/collision_map.cpp
pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
pkg/trunk/mapping/door_handle_detector/src/door_handle_detector.cpp
pkg/trunk/mapping/normal_estimation/src/normal_estimation_omp.cpp
pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp
pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp
pkg/trunk/mapping/point_cloud_downsampler/src/cloud_downsampler.cpp
pkg/trunk/mapping/sample_consensus/src/tools/sac_ground_removal.cpp
pkg/trunk/mapping/semantic_point_annotator/src/semantic_point_annotator.cpp
pkg/trunk/mapping/semantic_point_annotator/src/semantic_point_annotator_omp.cpp
pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp
pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
pkg/trunk/mechanism/mechanism_control/test/ms_publisher_test.cpp
pkg/trunk/motion_planning/kinematic_planning/test/avoid_monster.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/discrete_space_information/robarm/environment_robarm3d.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/plan_path_node.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/sim_grasp_path.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/simulate_arm_path.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/test_forward_kinematics.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/test_plan_path.cpp
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/amcl_player/cli.cpp
pkg/trunk/nav/deadreckon/deadreckon.cpp
pkg/trunk/nav/deadreckon/test_deadreckon_service.cpp
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/nav/fake_localization/ground_truth_controller.cpp
pkg/trunk/nav/nav_view/src/test/nav_view_test.cpp
pkg/trunk/nav/nav_view_sdl/nav_view.cpp
pkg/trunk/nav/slam_gmapping/src/main.cpp
pkg/trunk/nav/slam_gmapping/src/tftest.cpp
pkg/trunk/nav/teleop_base/teleop_base.cpp
pkg/trunk/nav/teleop_base/teleop_head.cpp
pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cpp
pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
pkg/trunk/nav/wavefront_player/cli.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp
pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h
pkg/trunk/prcore/tf/src/transform_sender.cpp
pkg/trunk/prcore/tf/test/testBroadcaster.cpp
pkg/trunk/prcore/tf/test/testListener.cpp
pkg/trunk/prcore/tf/test/test_notifier.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
pkg/trunk/simulators/rosstage/rosstage.cc
pkg/trunk/unported/laser_viewer/src/laser_viewer/laser_viewer.cpp
pkg/trunk/unported/simple_sdl_gui/src/key_monitor/key_monitor.cpp
pkg/trunk/unported/simple_sdl_gui/src/simple_sdl_gui_node/simple_sdl_gui.cpp
pkg/trunk/util/filter_coefficient_server/src/filter_coeff_client.cpp
pkg/trunk/util/filter_coefficient_server/test/check_function_calls.cpp
pkg/trunk/util/filter_demo/src/filtering_example.cpp
pkg/trunk/util/kinematics/libKinematics/src/pr2_ik_node.cpp
pkg/trunk/util/kinematics/robot_kinematics/test/robot_kinematics_test.cpp
pkg/trunk/util/laser_scan/src/median_filter_node.cpp
pkg/trunk/util/laser_scan/src/scan_intensity_filter.cpp
pkg/trunk/util/laser_scan/src/scan_shadows_filter.cpp
pkg/trunk/util/logsetta/carmen/carmen_logger/carmen_logger.cpp
pkg/trunk/util/message_sequencing/example/testdelay.cpp
pkg/trunk/util/message_sequencing/example/testgen.cpp
pkg/trunk/util/message_sequencing/example/testrecv.cpp
pkg/trunk/util/mux/mux.cpp
pkg/trunk/util/mux/switch.cpp
pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/bumblebee_grab_sample.cpp
pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp
pkg/trunk/vision/color_calib/calib_node.cpp
pkg/trunk/vision/color_calib/calib_node_dcam.cpp
pkg/trunk/vision/cv_mech_turk/src/cv_mturk.cpp
pkg/trunk/vision/people/src/face_detection.cpp
pkg/trunk/vision/people/src/filter/people_tracking_node.cpp
pkg/trunk/vision/people/src/leg_detector.cpp
pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
pkg/trunk/vision/people/src/timing_diagnostics_node.cpp
pkg/trunk/vision/people/src/track_starter_gui.cpp
pkg/trunk/vision/stereo_blob_tracker/src/axis_blob_tracker.cpp
pkg/trunk/vision/stereo_blob_tracker/src/blob_tracker_gui.cpp
pkg/trunk/vision/stereo_blob_tracker/src/listener.cpp
pkg/trunk/vision/stereo_blob_tracker/src/stereo_blob_tracker.cpp
pkg/trunk/vision/stereo_capture/src/project_light.cpp
pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp
pkg/trunk/vision/stereo_view/stereo_view.cpp
pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
pkg/trunk/visualization/cloud_viewer/src/cloud_node/cloud_node.cpp
pkg/trunk/visualization/multitouch_nav/skeleton.cpp
pkg/trunk/visualization/ogre_visualizer/src/test/cloud_test.cpp
pkg/trunk/visualization/ogre_visualizer/src/test/marker_test.cpp
pkg/trunk/visualization/ogre_visualizer/src/test/visualizer_test.cpp
pkg/trunk/world_models/map_saver/src/map_saver.cpp
pkg/trunk/world_models/map_server/src/main.cpp
pkg/trunk/world_models/map_server/test/rtest.cpp
Modified: pkg/trunk/audio/audio_capture/audio_capture.cpp
===================================================================
--- pkg/trunk/audio/audio_capture/audio_capture.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/audio/audio_capture/audio_capture.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -126,7 +126,7 @@
ROS_FATAL("unable to close portaudio");
ROS_BREAK();
}
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/audio/audio_capture/audio_clip_writer.cpp
===================================================================
--- pkg/trunk/audio/audio_capture/audio_clip_writer.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/audio/audio_capture/audio_clip_writer.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -168,7 +168,7 @@
AudioWriter *aw = new AudioWriter(&n);
while (n.ok())
ros::Duration(0, 500000000).sleep();
- ros::fini();
+
delete aw;
return 0;
}
Modified: pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -327,7 +327,7 @@
ArmPhaseSpaceGrabber grabber ;
grabber.spin() ;
- ros::fini() ;
+
return 0 ;
}
Modified: pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -250,6 +250,6 @@
ros::init(argc, argv) ;
SensorKinematicsGrabber grabber ;
grabber.spin() ;
- ros::fini() ;
+
return 0 ;
}
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -227,6 +227,6 @@
my_odom_calibration_node.stop();
// Clean up
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/grasp_point_node.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/grasp_point_node.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/grasp_point_node.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -309,7 +309,7 @@
GraspPointNode graspnode("grasp_point_node");
graspnode.init();
graspnode.spin();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -117,5 +117,5 @@
sleep_time.sleep();
}
*/
- ros::fini();
+
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -123,5 +123,5 @@
ROS_INFO("service call failed");
}
- ros::fini();
+
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -76,7 +76,7 @@
xfit = bc.c_->iterativeLeastSquares(A,B,"Gaussian",10);
cout << "done" << xfit << endl;
*/
- ros::fini();
+
delete robot_model;
delete robot_state;
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_grasp_point_node.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_grasp_point_node.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_grasp_point_node.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -117,6 +117,6 @@
ROS_INFO("Trajectory failed");
}
// sleep(20);
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -171,5 +171,5 @@
odom_log_file.close();
- ros::fini();
+
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -176,5 +176,5 @@
sleep_time.sleep();
}
- ros::fini();
+
}
Modified: pkg/trunk/deprecated/cv_wrappers/dcam/cv_view.cpp
===================================================================
--- pkg/trunk/deprecated/cv_wrappers/dcam/cv_view.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/deprecated/cv_wrappers/dcam/cv_view.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -36,7 +36,7 @@
ros::init(argc, argv);
CvView view;
view.spin();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/deprecated/cv_wrappers/minimal/cv_view.cpp
===================================================================
--- pkg/trunk/deprecated/cv_wrappers/minimal/cv_view.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/deprecated/cv_wrappers/minimal/cv_view.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -39,7 +39,7 @@
ros::init(argc, argv);
CvView view;
view.spin();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/deprecated/cv_wrappers/src/cv_movie_streamer.cpp
===================================================================
--- pkg/trunk/deprecated/cv_wrappers/src/cv_movie_streamer.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/deprecated/cv_wrappers/src/cv_movie_streamer.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -104,7 +104,7 @@
printf("quality (as defined by JPEG) = %d\n", quality);
CvMovieStreamer ms(argv[1], delay, loop, quality);
ms.stream_movie();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/deprecated/cv_wrappers/src/cv_view.cpp
===================================================================
--- pkg/trunk/deprecated/cv_wrappers/src/cv_view.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/deprecated/cv_wrappers/src/cv_view.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -96,7 +96,7 @@
usleep(10000);
view.check_keys();
}
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/deprecated/cv_wrappers/src/cv_view_array.cpp
===================================================================
--- pkg/trunk/deprecated/cv_wrappers/src/cv_view_array.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/deprecated/cv_wrappers/src/cv_view_array.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -155,7 +155,7 @@
usleep(10000);
view.check_keys();
}
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/deprecated/ransac_ground_plane_extraction/src/ransac_ground_plane_extraction_launch_node.cpp
===================================================================
--- pkg/trunk/deprecated/ransac_ground_plane_extraction/src/ransac_ground_plane_extraction_launch_node.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/deprecated/ransac_ground_plane_extraction/src/ransac_ground_plane_extraction_launch_node.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -13,6 +13,6 @@
catch(char const* e){
std::cout << e << std::endl;
}
- ros::fini();
+
return(0);
}
Modified: pkg/trunk/deprecated/scan_utils/src/cloudToOctree/cloudToOctree.cpp
===================================================================
--- pkg/trunk/deprecated/scan_utils/src/cloudToOctree/cloudToOctree.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/deprecated/scan_utils/src/cloudToOctree/cloudToOctree.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -126,6 +126,6 @@
if (cellSize <= 0) cellSize = 0.02;
scan_utils::CloudToOctree cto(cellSize);
cto.spin();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/deprecated/scan_utils/src/listen_node/rosSystemCalls.cpp
===================================================================
--- pkg/trunk/deprecated/scan_utils/src/listen_node/rosSystemCalls.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/deprecated/scan_utils/src/listen_node/rosSystemCalls.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -56,7 +56,7 @@
return;
}
- ros::fini();
+
fprintf(stderr,"ROS finished\n");
state = ROS_DONE;
}
Modified: pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -213,7 +213,7 @@
a.spin();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/cam/axis_cam/src/axis_demo/axis_demo.cpp
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/src/axis_demo/axis_demo.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/cam/axis_cam/src/axis_demo/axis_demo.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -116,7 +116,7 @@
a.spin();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/cam/axis_cam/src/axis_ptz/axis_ptz.cpp
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/src/axis_ptz/axis_ptz.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/cam/axis_cam/src/axis_ptz/axis_ptz.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -193,7 +193,7 @@
a.spin();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/cam/bumblebee_bridge/bumblebee_bridge.cpp
===================================================================
--- pkg/trunk/drivers/cam/bumblebee_bridge/bumblebee_bridge.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/cam/bumblebee_bridge/bumblebee_bridge.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -185,7 +185,7 @@
ros::init(argc, argv);
BumblebeeBridge b;
b.spin();
- ros::fini();
+
}
return 0;
}
Modified: pkg/trunk/drivers/cam/dcam/src/nodes/check_params.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam/src/nodes/check_params.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/cam/dcam/src/nodes/check_params.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -60,7 +60,7 @@
CheckParams cp;
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/cam/dcam/src/nodes/dcam.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam/src/nodes/dcam.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/cam/dcam/src/nodes/dcam.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -366,7 +366,7 @@
dc.spin();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/cam/dcam/src/nodes/stereodcam.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam/src/nodes/stereodcam.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/cam/dcam/src/nodes/stereodcam.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -474,7 +474,7 @@
delete g_sdc;
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/cam/elphel_cam/src/elphel_node/elphel_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/elphel_cam/src/elphel_node/elphel_node.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/cam/elphel_cam/src/elphel_node/elphel_node.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -139,6 +139,6 @@
}
}
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/imu/imu_node/imu_node.cc
===================================================================
--- pkg/trunk/drivers/imu/imu_node/imu_node.cc 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/imu/imu_node/imu_node.cc 2009-02-03 00:27:31 UTC (rev 10456)
@@ -514,7 +514,7 @@
in.spin();
- ros::fini();
+
return(0);
}
Modified: pkg/trunk/drivers/input/joy/joy.cpp
===================================================================
--- pkg/trunk/drivers/input/joy/joy.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/input/joy/joy.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -103,7 +103,7 @@
joy.start();
joy.spin();
joy.stop();
- ros::fini();
+
// exit(0);
return 0;
}
Modified: pkg/trunk/drivers/input/joy_annotator/joy_annotator.cpp
===================================================================
--- pkg/trunk/drivers/input/joy_annotator/joy_annotator.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/input/joy_annotator/joy_annotator.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -129,7 +129,7 @@
joy.start();
joy.spin();
joy.stop();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/input/joy_annotator/joy_node.cpp
===================================================================
--- pkg/trunk/drivers/input/joy_annotator/joy_node.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/input/joy_annotator/joy_node.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -123,7 +123,7 @@
joy.start();
joy.spin();
joy.stop();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/laser/hokuyo_node/hokuyo_node.cpp
===================================================================
--- pkg/trunk/drivers/laser/hokuyo_node/hokuyo_node.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/laser/hokuyo_node/hokuyo_node.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -636,7 +636,7 @@
h.spin();
- ros::fini();
+
return(0);
}
Modified: pkg/trunk/drivers/laser/laser_scan_annotator/src/laser_scan_annotator_node.cpp
===================================================================
--- pkg/trunk/drivers/laser/laser_scan_annotator/src/laser_scan_annotator_node.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/laser/laser_scan_annotator/src/laser_scan_annotator_node.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -121,7 +121,7 @@
ros::init(argc, argv);
LaserScanAnnotatorNode t ;
t.spin();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/laser/laser_view/laser_view.cpp
===================================================================
--- pkg/trunk/drivers/laser/laser_view/laser_view.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/laser/laser_view/laser_view.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -137,7 +137,7 @@
ros::init(argc, argv);
LaserView view;
view.main_loop();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp
===================================================================
--- pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -148,7 +148,7 @@
return 1;
}
printf("success.\n");
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/phase_space/phase_space_node.cpp
===================================================================
--- pkg/trunk/drivers/phase_space/phase_space_node.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/phase_space/phase_space_node.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -300,8 +300,8 @@
printf("Shutting Down OWL Client\n") ;
phase_space_node.shutdownOwlClient() ;
- ros::fini() ;
+
printf("**** Exiting ****\n") ;
return 0 ;
Modified: pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2009-02-03 00:27:31 UTC (rev 10456)
@@ -396,7 +396,7 @@
// Stop the robot
en.stop();
- ros::fini();
+
// To quote Morgan, Hooray!
return(0);
Modified: pkg/trunk/drivers/robot/katana/nodes/katana_client/katana_client.cpp
===================================================================
--- pkg/trunk/drivers/robot/katana/nodes/katana_client/katana_client.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/robot/katana/nodes/katana_client/katana_client.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -42,7 +42,7 @@
c.move_for_camera();
else if (string(argv[1]) == string("move_to_upright"))
c.move_to_upright();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/robot/katana/nodes/katana_server/katana_server.cpp
===================================================================
--- pkg/trunk/drivers/robot/katana/nodes/katana_server/katana_server.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/robot/katana/nodes/katana_server/katana_server.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -270,6 +270,6 @@
ros::init(argc, argv);
KatanaServer katanaSrv;
katanaSrv.spin();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/grab_cloud_data.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/grab_cloud_data.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/grab_cloud_data.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -107,6 +107,6 @@
ros::init(argc, argv);
GrabCloudData grabber ;
grabber.spin();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/laser_scan_assembler_srv.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/laser_scan_assembler_srv.cpp 2009-02-03 00:20:40 UTC (rev 10455)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/laser_scan_assembler_srv.cpp 2009-02-03 00:27:31 UTC (rev 10456)
@@ -104,6 +104,6 @@
ros::init(argc, argv);
LaserScanAssemblerSrv pc_assembler;
pc_assembler.spin();
- ros::fini();
+
return 0;
}
Modified: pkg/trunk/drivers/robot/pr...
[truncated message content] |
|
From: <ge...@us...> - 2009-02-03 06:24:42
|
Revision: 10469
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10469&view=rev
Author: gerkey
Date: 2009-02-03 06:24:40 +0000 (Tue, 03 Feb 2009)
Log Message:
-----------
Build fixes for OS X
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_position_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_arm_dynamics_controller.cpp
pkg/trunk/util/realtime_tools/src/realtime_tools.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp 2009-02-03 05:55:06 UTC (rev 10468)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp 2009-02-03 06:24:40 UTC (rev 10469)
@@ -110,7 +110,7 @@
void ArmDynamicsController::setJointCmd(const std::vector<double> &j_values, const std::vector<double> &j_values_dot, const std::vector<double> &j_values_dot_dot, const std::vector<std::string> & j_names)
{
assert(j_values.size() == j_names.size());
- for(uint i = 0; i < j_values.size(); ++i)
+ for(unsigned int i = 0; i < j_values.size(); ++i)
{
const std::string & name = j_names[i];
const int id = getJointControllerByName(name);
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_position_controller.cpp 2009-02-03 05:55:06 UTC (rev 10468)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_position_controller.cpp 2009-02-03 06:24:40 UTC (rev 10469)
@@ -80,7 +80,7 @@
void ArmPositionController::setJointPosCmd(const std::vector<double> &j_values, const std::vector<std::string> & j_names)
{
assert(j_values.size() == j_names.size());
- for(uint i=0;i<j_values.size();++i)
+ for(unsigned int i=0;i<j_values.size();++i)
{
const std::string & name = j_names[i];
const int id = getJointControllerPosByName(name);
@@ -97,7 +97,7 @@
void ArmPositionController::setJointPosCmd(const std::vector<double> &j_values)
{
assert(j_values.size() == joint_position_controllers_.size());
- for(uint i=0;i<j_values.size();++i)
+ for(unsigned int i=0;i<j_values.size();++i)
{
goals_[i] = j_values[i];
error_margins_[i] = -1;
@@ -125,10 +125,10 @@
{
assert(cmd.get_names_size()==cmd.get_positions_size());
- for(uint i=0;i<error_margins_.size();++i)
+ for(unsigned int i=0;i<error_margins_.size();++i)
error_margins_[i]=-1;
- for(uint i=0;i<cmd.get_names_size();++i)
+ for(unsigned int i=0;i<cmd.get_names_size();++i)
{
const std::string & name = cmd.names[i];
const int id = getJointControllerPosByName(name);
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 2009-02-03 05:55:06 UTC (rev 10468)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2009-02-03 06:24:40 UTC (rev 10469)
@@ -85,7 +85,7 @@
void HeadPanTiltController::setJointCmd(const std::vector<double> &j_values, const std::vector<std::string> & j_names)
{
assert(j_values.size() == j_names.size());
- for(uint i = 0; i < j_values.size(); ++i)
+ for(unsigned int i = 0; i < j_values.size(); ++i)
{
const std::string & name = j_names[i];
const int id = getJointControllerByName(name);
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp 2009-02-03 05:55:06 UTC (rev 10468)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp 2009-02-03 06:24:40 UTC (rev 10469)
@@ -107,7 +107,7 @@
void HeadServoingController::setJointCmd(const std::vector<double> &j_values, const std::vector<std::string> & j_names)
{
assert(j_values.size() == j_names.size());
- for(uint i = 0; i < j_values.size(); ++i)
+ for(unsigned int i = 0; i < j_values.size(); ++i)
{
const std::string & name = j_names[i];
const int id = getJointControllerByName(name);
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_arm_dynamics_controller.cpp 2009-02-03 05:55:06 UTC (rev 10468)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_arm_dynamics_controller.cpp 2009-02-03 06:24:40 UTC (rev 10469)
@@ -114,7 +114,7 @@
void PR2ArmDynamicsController::setJointCmd(const std::vector<double> &j_values, const std::vector<double> &j_values_dot, const std::vector<double> &j_values_dot_dot, const std::vector<std::string> & j_names)
{
assert(j_values.size() == j_names.size());
- for(uint i = 0; i < j_values.size(); ++i)
+ for(unsigned int i = 0; i < j_values.size(); ++i)
{
const std::string & name = j_names[i];
const int id = getJointControllerByName(name);
Modified: pkg/trunk/util/realtime_tools/src/realtime_tools.cpp
===================================================================
--- pkg/trunk/util/realtime_tools/src/realtime_tools.cpp 2009-02-03 05:55:06 UTC (rev 10468)
+++ pkg/trunk/util/realtime_tools/src/realtime_tools.cpp 2009-02-03 06:24:40 UTC (rev 10469)
@@ -28,6 +28,7 @@
*/
#if defined(__APPLE__)
+#include <sys/time.h>
#define HAS_XENOMAI 0
#else
#define HAS_XENOMAI 1
@@ -193,7 +194,14 @@
#endif
{
struct timespec n;
+#if defined(__APPLE__)
+ struct timeval t;
+ gettimeofday(&t, NULL);
+ n.tv_sec = t.tv_sec;
+ n.tv_nsec = t.tv_usec * 1000;
+#else
clock_gettime(CLOCK_REALTIME, &n);
+#endif
now = double(n.tv_nsec) / 1e9 + n.tv_sec;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jam...@us...> - 2009-02-03 17:22:24
|
Revision: 10478
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10478&view=rev
Author: jamesbowman
Date: 2009-02-03 17:22:18 +0000 (Tue, 03 Feb 2009)
Log Message:
-----------
Touch OpenCV Makefile, enable VO test
Modified Paths:
--------------
pkg/trunk/3rdparty/opencv_latest/Makefile
pkg/trunk/vision/visual_odometry/CMakeLists.txt
Modified: pkg/trunk/3rdparty/opencv_latest/Makefile
===================================================================
--- pkg/trunk/3rdparty/opencv_latest/Makefile 2009-02-03 16:41:18 UTC (rev 10477)
+++ pkg/trunk/3rdparty/opencv_latest/Makefile 2009-02-03 17:22:18 UTC (rev 10478)
@@ -29,3 +29,4 @@
rm -rf build patched
.PHONY : clean download
+
Modified: pkg/trunk/vision/visual_odometry/CMakeLists.txt
===================================================================
--- pkg/trunk/vision/visual_odometry/CMakeLists.txt 2009-02-03 16:41:18 UTC (rev 10477)
+++ pkg/trunk/vision/visual_odometry/CMakeLists.txt 2009-02-03 17:22:18 UTC (rev 10478)
@@ -27,6 +27,4 @@
#rospack_add_compile_flags(visual_odometry -O3 -DNDEBUG -Wno-missing-field-initializers )
# unit tests
-# Please don't uncomment this test until you verify that it completes,
-# rather then hanging indefinitely.
-#rospack_add_pyunit(test/directed.py)
+rospack_add_pyunit(test/directed.py)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-02-03 22:11:17
|
Revision: 10497
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10497&view=rev
Author: sfkwc
Date: 2009-02-03 22:02:50 +0000 (Tue, 03 Feb 2009)
Log Message:
-----------
rostools migrated to roslib. updated load_manifest calls and other uses of rostools
Modified Paths:
--------------
pkg/trunk/calibration/kinematic_calibration/scripts/arm_commander.py
pkg/trunk/calibration/kinematic_calibration/scripts/goto_next.py
pkg/trunk/calibration/kinematic_calibration/scripts/joy_cmd_head.py
pkg/trunk/calibration/kinematic_calibration/scripts/joy_goto_next.py
pkg/trunk/calibration/kinematic_calibration/scripts/unpack.py
pkg/trunk/calibration/laser_camera_calibration/gatherdata.py
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position.py
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position_ground_truth.py
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_laser.py
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_track_link_cmd.py
pkg/trunk/controllers/robot_mechanism_controllers/scripts/control.py
pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py
pkg/trunk/controllers/robot_mechanism_controllers/scripts/lqr.py
pkg/trunk/controllers/robot_mechanism_controllers/scripts/moveto_pose.py
pkg/trunk/controllers/robot_mechanism_controllers/scripts/moveto_trajectory.py
pkg/trunk/controllers/robot_mechanism_controllers/scripts/position.py
pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controller.py
pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py
pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/joint_controllers.py
pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/lqr_controller.py
pkg/trunk/demos/2dnav_gazebo/manifest.xml
pkg/trunk/demos/2dnav_pr2/parse_logs.py
pkg/trunk/demos/2dnav_pr2/pose_listen.py
pkg/trunk/demos/2dnav_pr2/spine_levitation.py
pkg/trunk/demos/arm_gazebo/grasp_preprogrammed.py
pkg/trunk/demos/arm_gazebo/setparam.py
pkg/trunk/demos/pr2_gazebo/tuck_arms.py
pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
pkg/trunk/drivers/network/wifi_ddwrt/nodes/ddwrt
pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/monitor
pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/view_batteries
pkg/trunk/drivers/robot/pr2/ocean_battery_driver/src/ocean_battery_driver/ibps_panel.py
pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/send_command
pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/view_power
pkg/trunk/drivers/robot/pr2/pr2_power_board/src/pr2_power_board/power_wires.py
pkg/trunk/drivers/robot/pr2/pr2_power_board/src/pr2_power_board/pr2_power_board_panel.py
pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_time.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_time.cpp
pkg/trunk/drivers/simulator/test_gazebo_plugin/test_pendulum.py
pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/test_slide.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxy_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxyw_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomy_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_vw_gt.py
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.py
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_scan.py
pkg/trunk/hardware_test/life_test/arm_life_test/random_poses.py
pkg/trunk/hardware_test/life_test/arm_life_test/random_positions_fast.py
pkg/trunk/hardware_test/life_test/arm_life_test/random_positions_slow.py
pkg/trunk/hardware_test/life_test/arm_life_test/random_wrenches.py
pkg/trunk/hardware_test/life_test/caster_life_test/periodic_drive.py
pkg/trunk/hardware_test/life_test/elbow_life_test/cycle_elbow.py
pkg/trunk/hardware_test/life_test/gripper_life_test/effort_controller.py
pkg/trunk/hardware_test/life_test/gripper_life_test/random_efforts.py
pkg/trunk/hardware_test/life_test/head_test/impact_test/impact_head_test.py
pkg/trunk/hardware_test/qualification/bin/gui
pkg/trunk/hardware_test/qualification/bin/gui_test
pkg/trunk/hardware_test/qualification/scripts/configure_mcbs.py
pkg/trunk/hardware_test/qualification/scripts/confirm_conf.py
pkg/trunk/hardware_test/qualification/scripts/hysteresis_sinesweep_plot.py
pkg/trunk/hardware_test/qualification/scripts/power_board_disable.py
pkg/trunk/hardware_test/qualification/scripts/power_board_standby.py
pkg/trunk/hardware_test/qualification/scripts/power_cycle_power_board.py
pkg/trunk/hardware_test/qualification/scripts/program_mcb.py
pkg/trunk/hardware_test/qualification/scripts/run_selftest.py
pkg/trunk/hardware_test/qualification/scripts/visual_verifier.py
pkg/trunk/hardware_test/qualification/scripts/wait_for_stationary.py
pkg/trunk/hardware_test/qualification/src/qualification/ui.py
pkg/trunk/hardware_test/qualification/tests/ethernet_test/ethernet_test.py
pkg/trunk/hardware_test/qualification/tests/simple_example/analyzer_node.py
pkg/trunk/hardware_test/qualification/tests/simple_example/test_node.py
pkg/trunk/hardware_test/runtime_monitor/scripts/log_runtime_messages
pkg/trunk/hardware_test/runtime_monitor/scripts/monitor
pkg/trunk/hardware_test/runtime_monitor/scripts/runtime_test
pkg/trunk/hardware_test/runtime_monitor/scripts/test_high_level
pkg/trunk/hardware_test/runtime_monitor/scripts/wxmonitor.py
pkg/trunk/hardware_test/runtime_monitor/src/runtime_monitor/expected.py
pkg/trunk/hardware_test/runtime_monitor/src/runtime_monitor/monitor_panel.py
pkg/trunk/hardware_test/runtime_monitor/src/runtime_monitor/test_test.py
pkg/trunk/hardware_test/runtime_monitor/testmonitor.py
pkg/trunk/hardware_test/thrash_joint/thrash_joint.py
pkg/trunk/highlevel/executive_python/bin/run_milestone_1.py
pkg/trunk/highlevel/executive_python/src/battery_monitor_adapter.py
pkg/trunk/highlevel/executive_python/src/exec_table_object_manip.py
pkg/trunk/highlevel/executive_python/src/executive.py
pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
pkg/trunk/highlevel/executive_python/src/recharge_adapter.py
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/fake_battery_controller.py
pkg/trunk/manip/teleop_joint_effort/teleop_joint_effort.py
pkg/trunk/manip/teleop_joint_effort/teleop_joint_effort_button.py
pkg/trunk/manip/teleop_spacenav/spacenav_teleop.py
pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate.py
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/scripts/detect.py
pkg/trunk/mechanism/mechanism_control/scripts/listen.py
pkg/trunk/mechanism/mechanism_control/scripts/mech.py
pkg/trunk/mechanism/mechanism_control/scripts/spawner.py
pkg/trunk/mechanism/mechanism_control/scripts/writer.py
pkg/trunk/mechanism/mechanism_control/src/mechanism_control/mechanism.py
pkg/trunk/mechanism/mechanism_control/test/listenerpublisher_ms.py
pkg/trunk/mechanism/mechanism_control/test/test_ms.py
pkg/trunk/mechanism/mechanism_control/test/test_ms_cpp.py
pkg/trunk/motion_planning/test_collision_space/src/test_cs.cpp
pkg/trunk/nav/trajectory_rollout/scripts/indefinite_nav.py
pkg/trunk/nav/trajectory_rollout/scripts/listen.py
pkg/trunk/prcore/tf/viewFrames.py
pkg/trunk/prcore/xacro/test/test_xacro.py
pkg/trunk/prcore/xacro/xacro.py
pkg/trunk/robot_control_loops/pr2_etherCAT/scripts/reset_motors.py
pkg/trunk/robot_descriptions/pr2/pr2_configurations/head_test_cart/run_head_test.py
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_prototype1/motor_monitor.py
pkg/trunk/simulators/rosstage/rosstage.cc
pkg/trunk/simulators/simulator_integration_tests/world_3d_map_gazebo_tests/testcloud.py
pkg/trunk/util/filter_coefficient_server/scripts/filter_coeff_client.py
pkg/trunk/util/filter_demo/scripts/demo_plot.py
pkg/trunk/util/ntp_monitor/ntp_monitor.py
pkg/trunk/vision/calonder_descriptor/test/directed.py
pkg/trunk/vision/people/src/leg_detector.cpp
pkg/trunk/vision/people/src/stereo_face_feature_tracker.py
pkg/trunk/vision/place_recognition/test/directed.py
pkg/trunk/vision/place_recognition/test/thrash.py
pkg/trunk/vision/star_detector/test/directed.py
pkg/trunk/vision/vision_utils/scripts/bagsort.py
pkg/trunk/vision/vision_utils/src/bag2tiff
pkg/trunk/vision/visual_odometry/nodes/camview_py.py
pkg/trunk/vision/visual_odometry/nodes/corrector.py
pkg/trunk/vision/visual_odometry/nodes/lineperf.py
pkg/trunk/vision/visual_odometry/nodes/vo.py
pkg/trunk/vision/visual_odometry/scripts/confusion.py
pkg/trunk/vision/visual_odometry/scripts/mkgeo.py
pkg/trunk/vision/visual_odometry/scripts/mkmovie.py
pkg/trunk/vision/visual_odometry/scripts/mkplot.py
pkg/trunk/vision/visual_odometry/scripts/mkplot_harris.py
pkg/trunk/vision/visual_odometry/scripts/mkrun.py
pkg/trunk/vision/visual_odometry/scripts/pruner.py
pkg/trunk/vision/visual_odometry/scripts/sos.py
pkg/trunk/vision/visual_odometry/src/marker.py
pkg/trunk/vision/visual_odometry/test/directed.py
pkg/trunk/vision/visual_odometry/test/render.py
pkg/trunk/vision/visual_odometry/test/render2.py
pkg/trunk/vision/visual_odometry/test/render3.py
pkg/trunk/vision/visual_odometry/test/render4.py
pkg/trunk/vision/vslam/scripts/mkplot.py
pkg/trunk/vision/vslam/scripts/mkplot_phsp.py
pkg/trunk/vision/vslam/scripts/plot3d.py
pkg/trunk/vision/vslam/scripts/plot_phsp.py
pkg/trunk/vision/vslam/scripts/prtest.py
pkg/trunk/vision/vslam/scripts/trajectorysynth.py
pkg/trunk/vision/vslam/scripts/transf_fit.py
pkg/trunk/vision/vslam/scripts/wobbler.py
pkg/trunk/vision/vslam/src/skeleton.py
pkg/trunk/vision/vslam/test/calonder.py
pkg/trunk/visualization/multitouch_nav/.build_version
pkg/trunk/visualization/ogre_tools/manifest.xml
pkg/trunk/visualization/ogre_visualizer/scripts/standalone_visualizer.py
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h
pkg/trunk/visualization/pr2_dashboard/manifest.xml
pkg/trunk/visualization/pr2_dashboard/scripts/pr2_dashboard
pkg/trunk/visualization/pr2_dashboard/src/pr2_dashboard/hardware_panel.py
pkg/trunk/visualization/pr2_dashboard/src/pr2_dashboard/pr2_frame.py
pkg/trunk/visualization/pr2_dashboard/src/pr2_dashboard/reset.py
pkg/trunk/visualization/pr2_gui/manifest.xml
pkg/trunk/visualization/pr2_gui/scripts/pr2_gui
pkg/trunk/visualization/pr2_gui/src/pr2_gui/hardware_panel.py
pkg/trunk/visualization/pr2_gui/src/pr2_gui/pr2_frame.py
pkg/trunk/visualization/pr2_gui/src/pr2_gui/reset.py
pkg/trunk/visualization/pr2_gui/src/pr2_gui/visualizer_panel.py
pkg/trunk/visualization/wx_camera_panel/scripts/standalone_camera.py
pkg/trunk/visualization/wxpy_ros/demos/ros_panel.py
pkg/trunk/visualization/wxpy_ros/demos/ros_simple.py
pkg/trunk/visualization/wxpy_ros/src/wxpy_ros/roscom.py
pkg/trunk/visualization/wxpy_ros/src/wxpy_ros/wxplot.py
pkg/trunk/visualization/wxpy_ros/src/wxpy_ros/wxros.py
pkg/trunk/visualization/wxpy_ros/tests/my_plot.py
pkg/trunk/visualization/wxpy_ros/tests/ros_panel.py
pkg/trunk/visualization/wxpy_ros/tests/roscom.py
pkg/trunk/visualization/wxpy_ros/tests/test_try.py
pkg/trunk/world_models/static_map_server/nodes/map_server
pkg/trunk/world_models/static_map_server/test/consumer.py
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/arm_commander.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/arm_commander.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/arm_commander.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -1,6 +1,6 @@
#!/usr/bin/env python
-import rostools
-rostools.load_manifest('kinematic_calibration')
+import roslib
+roslib.load_manifest('kinematic_calibration')
import rospy
import sys
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/goto_next.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/goto_next.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/goto_next.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -1,6 +1,6 @@
#!/usr/bin/env python
-import rostools; rostools.load_manifest('kinematic_calibration')
+import roslib; roslib.load_manifest('kinematic_calibration')
import sys
import rospy
from roscpp.msg import Empty
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/joy_cmd_head.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/joy_cmd_head.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/joy_cmd_head.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -1,6 +1,6 @@
#!/usr/bin/env python
-import rostools; rostools.load_manifest('kinematic_calibration')
+import roslib; roslib.load_manifest('kinematic_calibration')
import sys
import rospy
from std_msgs.msg import PointStamped, Point
@@ -10,7 +10,7 @@
def joy_callback(data, pub):
print 'Got joystick Comand'
if data.buttons[7] == 1:
- pub.publish(PointStamped(rostools.msg.Header(None, None, 'r_gripper_palm_link'), Point(0.17, 0, 0)))
+ pub.publish(PointStamped(roslib.msg.Header(None, None, 'r_gripper_palm_link'), Point(0.17, 0, 0)))
print 'Sent head Command'
if __name__ == '__main__':
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/joy_goto_next.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/joy_goto_next.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/joy_goto_next.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -1,6 +1,6 @@
#!/usr/bin/env python
-import rostools; rostools.load_manifest('kinematic_calibration')
+import roslib; roslib.load_manifest('kinematic_calibration')
import sys
import rospy
from roscpp.msg import Empty
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/unpack.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/unpack.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/unpack.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -32,8 +32,8 @@
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
-import rostools
-rostools.load_manifest('kinematic_calibration')
+import roslib
+roslib.load_manifest('kinematic_calibration')
import rospy
import sys
import Image as Image
Modified: pkg/trunk/calibration/laser_camera_calibration/gatherdata.py
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/gatherdata.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/calibration/laser_camera_calibration/gatherdata.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -38,14 +38,14 @@
PKG = 'laser_camera_calibration' # this package name
NAME = 'lasercamera_gatherer'
-import rostools; rostools.load_manifest(PKG)
+import roslib; roslib.load_manifest(PKG)
import sys
import thread
from numpy import *
import rospy
-from rostools import rostime
+from roslib import rostime
from std_msgs.msg import LaserScan
from robot_msgs.msg import MechanismState
from checkerboard_detector.msg import ObjectDetection
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -31,7 +31,7 @@
PKG = "pr2_mechanism_controllers"
-import rostools; rostools.load_manifest(PKG)
+import roslib; roslib.load_manifest(PKG)
import sys
import os
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position_ground_truth.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position_ground_truth.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position_ground_truth.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -31,7 +31,7 @@
PKG = "pr2_mechanism_controllers"
-import rostools; rostools.load_manifest(PKG)
+import roslib; roslib.load_manifest(PKG)
import sys
import os
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_laser.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_laser.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_laser.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -2,7 +2,7 @@
PKG = "pr2_mechanism_controllers"
-import rostools; rostools.load_manifest(PKG)
+import roslib; roslib.load_manifest(PKG)
import sys
import os
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -30,7 +30,7 @@
PKG = "pr2_mechanism_controllers"
-import rostools; rostools.load_manifest(PKG)
+import roslib; roslib.load_manifest(PKG)
import sys
import os
@@ -54,7 +54,7 @@
head_angles = rospy.Publisher('head_controller/frame_track_point', PointStamped)
rospy.init_node('head_commander', anonymous=True)
sleep(1)
- head_angles.publish(PointStamped(rostools.msg.Header(None, None, frame), Point(x, y, z)))
+ head_angles.publish(PointStamped(rospy.Header(None, None, frame), Point(x, y, z)))
sleep(1)
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -2,7 +2,7 @@
PKG = "pr2_mechanism_controllers"
-import rostools; rostools.load_manifest(PKG)
+import roslib; roslib.load_manifest(PKG)
import sys
import os
@@ -30,7 +30,7 @@
cmd = PeriodicCmd()
controller = sys.argv[1]
- cmd.header = rostools.msg.Header(None, None, None)
+ cmd.header = roslib.msg.Header(None, None, None)
cmd.profile = sys.argv[2]
cmd.period = float (sys.argv[3])
cmd.amplitude = float (sys.argv[4])
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_track_link_cmd.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_track_link_cmd.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_track_link_cmd.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -2,7 +2,7 @@
PKG = "pr2_mechanism_controllers"
-import rostools; rostools.load_manifest(PKG)
+import roslib; roslib.load_manifest(PKG)
import sys
import os
Modified: pkg/trunk/controllers/robot_mechanism_controllers/scripts/control.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/control.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/control.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -1,7 +1,7 @@
#! /usr/bin/env python
-import rostools
-rostools.load_manifest('robot_mechanism_controllers')
+import roslib
+roslib.load_manifest('robot_mechanism_controllers')
import rospy, sys
from robot_mechanism_controllers.srv import *
Modified: pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -36,9 +36,9 @@
import sys
-import rostools
-rostools.load_manifest('robot_mechanism_controllers')
-rostools.load_manifest('mechanism_control')
+import roslib
+roslib.load_manifest('robot_mechanism_controllers')
+roslib.load_manifest('mechanism_control')
import rospy
from std_msgs.msg import *
from mechanism_control import mechanism
Modified: pkg/trunk/controllers/robot_mechanism_controllers/scripts/lqr.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/lqr.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/lqr.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -1,6 +1,6 @@
# -*- coding: utf-8 -*-
-import rostools
-rostools.load_manifest('robot_mechanism_controllers')
+import roslib
+roslib.load_manifest('robot_mechanism_controllers')
from robot_mechanism_controllers.lqr_controller import *
foo=LQRProxy('right_arm_controller')
Modified: pkg/trunk/controllers/robot_mechanism_controllers/scripts/moveto_pose.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/moveto_pose.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/moveto_pose.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -27,8 +27,8 @@
# POSSIBILITY OF SUCH DAMAGE.
-import rostools; rostools.load_manifest('pr2_mechanism_controllers')
-rostools.load_manifest('rospy')
+import roslib; roslib.load_manifest('pr2_mechanism_controllers')
+roslib.load_manifest('rospy')
import random, time
import rospy
Modified: pkg/trunk/controllers/robot_mechanism_controllers/scripts/moveto_trajectory.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/moveto_trajectory.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/moveto_trajectory.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -27,8 +27,8 @@
# POSSIBILITY OF SUCH DAMAGE.
-import rostools; rostools.load_manifest('pr2_mechanism_controllers')
-rostools.load_manifest('rospy')
+import roslib; roslib.load_manifest('pr2_mechanism_controllers')
+roslib.load_manifest('rospy')
import random, time
import rospy
Modified: pkg/trunk/controllers/robot_mechanism_controllers/scripts/position.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/position.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/position.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -36,9 +36,9 @@
import sys
-import rostools
-rostools.load_manifest('robot_mechanism_controllers')
-rostools.load_manifest('mechanism_control')
+import roslib
+roslib.load_manifest('robot_mechanism_controllers')
+roslib.load_manifest('mechanism_control')
import rospy
from std_msgs.msg import *
from mechanism_control import mechanism
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controller.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controller.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controller.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -32,8 +32,8 @@
# POSSIBILITY OF SUCH DAMAGE.
#
-import rostools
-rostools.load_manifest('mechanism_control')
+import roslib
+roslib.load_manifest('mechanism_control')
import rospy, sys
from mechanism_control import mechanism
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -1,7 +1,7 @@
#!/usr/bin/env python
-import rostools
-rostools.load_manifest('robot_mechanism_controllers')
+import roslib
+roslib.load_manifest('robot_mechanism_controllers')
import rospy, sys
from robot_mechanism_controllers.srv import *
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/joint_controllers.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/joint_controllers.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/joint_controllers.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -1,5 +1,5 @@
from controller import Controller
-import rostools
+import roslib
import rospy
from srv import *
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/lqr_controller.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/lqr_controller.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/lqr_controller.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -1,7 +1,7 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
-import rostools; rostools.load_manifest('robot_mechanism_controllers')
+import roslib; roslib.load_manifest('robot_mechanism_controllers')
import rospy, sys
from robot_srvs.srv import *
Modified: pkg/trunk/demos/2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-02-03 22:02:50 UTC (rev 10497)
@@ -4,6 +4,8 @@
<license>BSD</license>
<review status="NA" notes=""/>
<url>http://pr.willowgarage.com/wiki/Simulator</url>
+ <depend package="std_msgs"/>
+ <depend package="robot_msgs"/>
<depend package="nav_view"/>
<depend package="gazebo_plugin"/>
<depend package="map_server"/>
Modified: pkg/trunk/demos/2dnav_pr2/parse_logs.py
===================================================================
--- pkg/trunk/demos/2dnav_pr2/parse_logs.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/demos/2dnav_pr2/parse_logs.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -1,8 +1,8 @@
#! /usr/bin/python
import math
-import rostools
-rostools.load_manifest('2dnav_pr2')
+import roslib
+roslib.load_manifest('2dnav_pr2')
import glob
import sys, traceback, logging, rospy
Modified: pkg/trunk/demos/2dnav_pr2/pose_listen.py
===================================================================
--- pkg/trunk/demos/2dnav_pr2/pose_listen.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/demos/2dnav_pr2/pose_listen.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -1,6 +1,6 @@
#!/usr/bin/env python
-import rostools; rostools.load_manifest('2dnav_pr2')
+import roslib; roslib.load_manifest('2dnav_pr2')
import sys, traceback, logging, rospy
from std_msgs.msg import RobotBase2DOdom
Modified: pkg/trunk/demos/2dnav_pr2/spine_levitation.py
===================================================================
--- pkg/trunk/demos/2dnav_pr2/spine_levitation.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/demos/2dnav_pr2/spine_levitation.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -1,6 +1,6 @@
#! /usr/bin/python
-import rostools
-rostools.load_manifest("2dnav_pr2")
+import roslib
+roslib.load_manifest("2dnav_pr2")
import rospy
import time
import math
Modified: pkg/trunk/demos/arm_gazebo/grasp_preprogrammed.py
===================================================================
--- pkg/trunk/demos/arm_gazebo/grasp_preprogrammed.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/demos/arm_gazebo/grasp_preprogrammed.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -38,9 +38,9 @@
NAME = 'grasp_preprogrammed'
import math
-import rostools
-rostools.load_manifest(PKG)
-rostools.load_manifest('rostest')
+import roslib
+roslib.load_manifest(PKG)
+roslib.load_manifest('rostest')
import sys, unittest
Modified: pkg/trunk/demos/arm_gazebo/setparam.py
===================================================================
--- pkg/trunk/demos/arm_gazebo/setparam.py 2009-02-03 22:01:10 UTC (rev 10496)
+++ pkg/trunk/demos/arm_gazebo/setparam.py 2009-02-03 22:02:50 UTC (rev 10497)
@@ -1,3 +1,3 @@
#!/usr/bin/env python
-import rostools.script...
[truncated message content] |
|
From: <rdi...@us...> - 2009-02-04 01:58:39
|
Revision: 10516
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10516&view=rev
Author: rdiankov
Date: 2009-02-04 01:58:35 +0000 (Wed, 04 Feb 2009)
Log Message:
-----------
removed underscores from rosoct names for matlab compat
Modified Paths:
--------------
pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_destroysession.m
pkg/trunk/util/filter_coefficient_server/scripts/filter_coeff_server.m
Modified: pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m 2009-02-04 01:48:37 UTC (rev 10515)
+++ pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m 2009-02-04 01:58:35 UTC (rev 10516)
@@ -6,7 +6,7 @@
%% by Rosen Diankov
function calibdata = startgathering(robot)
-global lastobjdet lastlaserscan lastmecstate __calibdata __iterationcount
+global lastobjdet lastlaserscan lastmecstate g_calibdata g_iterationncount
if( ~exist('robot') )
robot = [];
@@ -17,24 +17,24 @@
rosoct_add_msgs('robot_msgs');
queuesize = 1000; % need big buffer
-__calibdata = {};
+g_calibdata = {};
lastobjdet = {};
-__iterationcount = 0;
-__rosoct_msg_unsubscribe("new_ObjectDetection");
+g_iterationncount = 0;
+rosoct_unsubscribe("new_ObjectDetection");
success = rosoct_subscribe("new_ObjectDetection", @checkerboard_detector_ObjectDetection, @(msg) objdetcb(msg,robot),queuesize);
if( ~success )
error('subscribe failed');
end
lastlaserscan = {};
-__rosoct_msg_unsubscribe("new_tile_scan");
+rosoct_unsubscribe("new_tile_scan");
success = rosoct_subscribe("new_tilt_scan", @std_msgs_LaserScan, @(msg) laserscancb(msg,robot),queuesize);
if( ~success )
error('subscribe failed');
end
lastmecstate = {};
-__rosoct_msg_unsubscribe("new_mechanism_state");
+rosoct_unsubscribe("new_mechanism_state");
success = rosoct_subscribe("new_mechanism_state", @robot_msgs_MechanismState, @(msg) mecstatecb(msg,robot),queuesize);
if( ~success )
error('subscribe failed');
@@ -44,21 +44,21 @@
input('press any key after data is started streaming: ');
while(1)
pause(0.1);
- numprocessed = __rosoct_worker(1);
- if( numprocessed == 0 && __iterationcount > 0 )
+ numprocessed = rosoct_worker(1);
+ if( numprocessed == 0 && g_iterationncount > 0 )
display('stream done');
break;
end
end
-__rosoct_msg_unsubscribe("new_ObjectDetection");
-__rosoct_msg_unsubscribe("new_tile_scan");
-__rosoct_msg_unsubscribe("new_mechanism_state");
+rosoct_unsubscribe("new_ObjectDetection");
+rosoct_unsubscribe("new_tile_scan");
+rosoct_unsubscribe("new_mechanism_state");
-display(sprintf('gathering calibration done, total extracted: %d, total parsed: %d', length(__calibdata), __iterationcount));
-calibdata = __calibdata;
+display(sprintf('gathering calibration done, total extracted: %d, total parsed: %d', length(g_calibdata), g_iterationncount));
+calibdata = g_calibdata;
save calibdata.mat calibdata
-__calibdata = {};
+g_calibdata = {};
function objdetcb(msg, robot)
global lastobjdet
@@ -79,7 +79,7 @@
equal = stamp1.sec==stamp2.sec & abs(stamp1.nsec-stamp2.nsec)<0.001;
function gathermsgs(robot)
-global lastobjdet lastlaserscan lastmecstate __calibdata __iterationcount
+global lastobjdet lastlaserscan lastmecstate g_calibdata g_iterationncount
if( length(lastobjdet) == 0 || length(lastlaserscan) == 0 || length(lastmecstate) == 0 )
return;
@@ -98,7 +98,7 @@
lastlaserscan(1) = [];
lastmecstate(1) = [];
-__iterationcount = __iterationcount + 1;
+g_iterationncount = g_iterationncount + 1;
data = [];
rawjointvalues = cellfun(@(x) x.position, mecstatemsg.joint_states);
if( ~isempty(robot) )
@@ -193,5 +193,5 @@
plot(laserline([1 3]),laserline([2 4]),'r','linewidth',5); drawnow;
data.laserline = laserline;
-__calibdata{end+1} = data;
-display(sprintf('adding data %d', length(__calibdata)));
+g_calibdata{end+1} = data;
+display(sprintf('adding data %d', length(g_calibdata)));
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_destroysession.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_destroysession.m 2009-02-04 01:48:37 UTC (rev 10515)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_destroysession.m 2009-02-04 01:58:35 UTC (rev 10516)
@@ -42,7 +42,7 @@
end
%% destroys all the sessions of a session server
-__rosoct_terminate_session(session.id);
+rosoct_terminate_session(session.id);
%% if this is the global session, make sure to propagate the changes
if( ~isempty(gsession) && session.id == gsession.id )
Modified: pkg/trunk/util/filter_coefficient_server/scripts/filter_coeff_server.m
===================================================================
--- pkg/trunk/util/filter_coefficient_server/scripts/filter_coeff_server.m 2009-02-04 01:48:37 UTC (rev 10515)
+++ pkg/trunk/util/filter_coefficient_server/scripts/filter_coeff_server.m 2009-02-04 01:58:35 UTC (rev 10516)
@@ -36,7 +36,7 @@
addpath(fullfile(rosoctpath, '/scripts'));
startup;
-__rosoct_unadvertise_service('filter_coeffs');
+rosoct_unadvertise_service('filter_coeffs');
% Creates the service
suc = rosoct_advertise_service('filter_coeffs',@filter_coefficient_server_Filter,@filterserv);
@@ -46,5 +46,5 @@
% Loops to keep the octave session going
while(1)
- __rosoct_worker();
+ rosoct_worker();
end
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-02-04 05:53:43
|
Revision: 10532
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10532&view=rev
Author: tfoote
Date: 2009-02-04 05:53:40 +0000 (Wed, 04 Feb 2009)
Log Message:
-----------
changing referemces to rostime into roslib
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/manifest.xml
pkg/trunk/motion_planning/test_collision_space/manifest.xml
pkg/trunk/nav/escape_controller/manifest.xml
pkg/trunk/nav/trajectory_rollout/manifest.xml
Modified: pkg/trunk/highlevel/highlevel_controllers/manifest.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2009-02-04 05:51:24 UTC (rev 10531)
+++ pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2009-02-04 05:53:40 UTC (rev 10532)
@@ -24,7 +24,7 @@
<depend package="tf"/>
<depend package="sbpl"/>
<depend package="mpglue"/>
- <depend package="rostime"/>
+ <depend package="roslib"/>
<depend package="navfn"/>
<depend package="topological_map"/>
<depend package="angles"/>
Modified: pkg/trunk/motion_planning/test_collision_space/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/test_collision_space/manifest.xml 2009-02-04 05:51:24 UTC (rev 10531)
+++ pkg/trunk/motion_planning/test_collision_space/manifest.xml 2009-02-04 05:53:40 UTC (rev 10532)
@@ -9,7 +9,7 @@
<review status="unreviewed" notes=""/>
<depend package="roscpp"/>
- <depend package="rostime"/>
+ <depend package="roslib"/>
<depend package="tf"/>
<depend package="std_msgs"/>
<depend package="robot_msgs"/>
Modified: pkg/trunk/nav/escape_controller/manifest.xml
===================================================================
--- pkg/trunk/nav/escape_controller/manifest.xml 2009-02-04 05:51:24 UTC (rev 10531)
+++ pkg/trunk/nav/escape_controller/manifest.xml 2009-02-04 05:53:40 UTC (rev 10532)
@@ -9,7 +9,7 @@
<depend package="std_msgs" />
<depend package="rosconsole" />
<depend package="tf" />
- <depend package="rostime" />
+ <depend package="roslib" />
<depend package="rospy" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
Modified: pkg/trunk/nav/trajectory_rollout/manifest.xml
===================================================================
--- pkg/trunk/nav/trajectory_rollout/manifest.xml 2009-02-04 05:51:24 UTC (rev 10531)
+++ pkg/trunk/nav/trajectory_rollout/manifest.xml 2009-02-04 05:53:40 UTC (rev 10532)
@@ -11,7 +11,7 @@
<depend package="rosconsole" />
<depend package="roscpp" />
<depend package="tf" />
- <depend package="rostime" />
+ <depend package="roslib" />
<depend package="rospy" />
<depend package="costmap_2d" />
<url>http://pr.willowgarage.com</url>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-02-04 22:46:36
|
Revision: 10573
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10573&view=rev
Author: tfoote
Date: 2009-02-04 22:46:30 +0000 (Wed, 04 Feb 2009)
Log Message:
-----------
Filters prototypes basically operational. Merging back before I diverge too much.
r12651@raven: tfoote | 2009-01-18 21:17:12 -0800
creating a branch for offline work
r12652@raven: tfoote | 2009-01-18 21:20:02 -0800
i've got a templated factory working and am now working on making instantiation into a chain easy
r12653@raven: tfoote | 2009-01-20 21:35:12 -0800
adding robot_pose_ekf to joystick control
r12852@raven: tfoote | 2009-01-21 12:58:13 -0800
trying to create a chain and configure it
r12853@raven: tfoote | 2009-01-21 14:38:41 -0800
switching to shared pointers
r12948@raven: tfoote | 2009-01-21 22:37:14 -0800
compiling thanks to josh
r12949@raven: tfoote | 2009-01-21 22:50:10 -0800
two filters working together
r12950@raven: tfoote | 2009-01-21 23:27:00 -0800
mean and median now passing test
r12951@raven: tfoote | 2009-01-21 23:45:25 -0800
fixed macro, preceeding # was preventing replacement
r12959@raven: tfoote | 2009-01-22 01:37:38 -0800
fixing ogre_tools build on intrepid
r13029@raven: tfoote | 2009-01-22 16:19:47 -0800
starting conversion to new filters
r13533@raven: tfoote | 2009-02-03 11:08:51 -0800
making mean and median tests seperate. changing elements_per_observation to width as per API
r13534@raven: tfoote | 2009-02-03 11:44:11 -0800
switching to vectors per API review
r13775@raven: tfoote | 2009-02-03 13:26:34 -0800
templating on std::vector<data> instead of data
r13776@raven: tfoote | 2009-02-03 13:37:24 -0800
switching to new templating standard
r13777@raven: tfoote | 2009-02-03 14:20:05 -0800
commenting transferfunction for it's broken at the moment. and I don't want to convert it to parsing a string before I lay down how to do that fully.
r13825@raven: tfoote | 2009-02-03 18:01:42 -0800
factory working with complicated types, still working on chains. it seems to need the typedef.
r13826@raven: tfoote | 2009-02-03 18:13:28 -0800
chain working. the string matching was a proble again. this is going to be fun
r13827@raven: tfoote | 2009-02-04 11:46:33 -0800
changing to typeids
r13920@raven: tfoote | 2009-02-04 12:41:49 -0800
chains working with typeid
r13921@raven: tfoote | 2009-02-04 12:49:40 -0800
converting to gtests
r13922@raven: tfoote | 2009-02-04 13:30:02 -0800
converting to gtests and adding first cut at update method
r13923@raven: tfoote | 2009-02-04 13:43:05 -0800
updating of chain reporting success, hard coded so that chaining can only do vectors at the moment
r13956@raven: tfoote | 2009-02-04 14:23:25 -0800
fixing downstream build while filters are unstable
r13957@raven: tfoote | 2009-02-04 14:24:53 -0800
chains working and tested
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/deprecated/world_3d_map/CMakeLists.txt
pkg/trunk/deprecated/world_3d_map/manifest.xml
pkg/trunk/util/filter_demo/CMakeLists.txt
pkg/trunk/util/filters/CMakeLists.txt
pkg/trunk/util/filters/include/filters/filter_base.h
pkg/trunk/util/filters/include/filters/mean.h
pkg/trunk/util/filters/include/filters/median.h
pkg/trunk/util/filters/include/filters/transfer_function.h
pkg/trunk/util/filters/manifest.xml
pkg/trunk/util/filters/test/test_median.cpp
pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h
pkg/trunk/util/laser_scan/src/median_filter.cpp
Added Paths:
-----------
pkg/trunk/util/filters/include/filters/filter_chain.h
pkg/trunk/util/filters/test/test_chain.cpp
pkg/trunk/util/filters/test/test_factory.cpp
pkg/trunk/util/filters/test/test_mean.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-02-04 22:46:30 UTC (rev 10573)
@@ -12,7 +12,7 @@
src/base_controller_pos.cpp
src/base_position_controller.cpp
src/laser_scanner_controller.cpp
- src/laser_scanner_traj_controller.cpp
+#Removed while filters are unstable src/laser_scanner_traj_controller.cpp
src/laser_scanner_velocity_controller.cpp
src/arm_position_controller.cpp
src/arm_velocity_controller.cpp
Modified: pkg/trunk/deprecated/world_3d_map/CMakeLists.txt
===================================================================
--- pkg/trunk/deprecated/world_3d_map/CMakeLists.txt 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/deprecated/world_3d_map/CMakeLists.txt 2009-02-04 22:46:30 UTC (rev 10573)
@@ -2,7 +2,4 @@
set(ROS_BUILD_TYPE Release)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(world_3d_map)
-
-rospack_add_boost_directories()
-
rospack_add_executable(world_3d_map src/world_3d_map.cpp)
Modified: pkg/trunk/deprecated/world_3d_map/manifest.xml
===================================================================
--- pkg/trunk/deprecated/world_3d_map/manifest.xml 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/deprecated/world_3d_map/manifest.xml 2009-02-04 22:46:30 UTC (rev 10573)
@@ -2,8 +2,9 @@
<description>3D Map Construction</description>
<author>Ioan Sucan</author>
<license>BSD</license>
-<review status="deprecated" notes="awaiting #900 for removal"/>
+<review status="unreviewed" notes=""/>
<depend package="roscpp" />
+<depend package="boost" />
<depend package="std_msgs" />
<depend package="tf" />
<depend package="laser_scan" />
Modified: pkg/trunk/util/filter_demo/CMakeLists.txt
===================================================================
--- pkg/trunk/util/filter_demo/CMakeLists.txt 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filter_demo/CMakeLists.txt 2009-02-04 22:46:30 UTC (rev 10573)
@@ -4,9 +4,9 @@
genmsg()
gensrv()
-rospack_add_library(filter_demo
- src/filtering_example.cpp)
+#broken until transfer function filter is fixed
+#rospack_add_library(filter_demo
+# src/filtering_example.cpp)
+#rospack_add_executable(filtering_example
+# src/filtering_example.cpp)
-rospack_add_executable(filtering_example
- src/filtering_example.cpp)
-
Modified: pkg/trunk/util/filters/CMakeLists.txt
===================================================================
--- pkg/trunk/util/filters/CMakeLists.txt 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filters/CMakeLists.txt 2009-02-04 22:46:30 UTC (rev 10573)
@@ -3,10 +3,12 @@
rospack(filters)
rospack_add_gtest(median_test test/test_median.cpp)
-rospack_add_gtest(transfer_function_test test/test_transfer_function.cpp)
-target_link_libraries(median_test)
-target_link_libraries(transfer_function_test)
+# broken, needs to go to the new form factor
+#rospack_add_gtest(transfer_function_test test/test_transfer_function.cpp)
-#rospack_add_gtest(mean_test test/test_mean.cpp)
-#target_link_libraries(mean_test)
+
+rospack_add_gtest(mean_test test/test_mean.cpp)
+
+rospack_add_gtest(test_factory test/test_factory.cpp)
+rospack_add_gtest(test_chain test/test_chain.cpp)
\ No newline at end of file
Modified: pkg/trunk/util/filters/include/filters/filter_base.h
===================================================================
--- pkg/trunk/util/filters/include/filters/filter_base.h 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filters/include/filters/filter_base.h 2009-02-04 22:46:30 UTC (rev 10573)
@@ -31,9 +31,42 @@
#define FILTERS_FILTER_BASE_H_
+#include <typeinfo>
+#include <loki/Factory.h>
+#include "ros/assert.h"
+
+typedef std::vector<float> std_vector_float;
+typedef std::vector<double> std_vector_double;
+
+
namespace filters
{
+std::string getFilterID(const std::string & filter_name, const std::string & typestring)
+{
+ if (typestring == "int")
+ return filter_name + "i";
+
+ else if (typestring == "float")
+ return filter_name + "f";
+
+ else if (typestring == "double")
+ return filter_name + "d";
+
+ else if (typestring == "std_vector_float")
+ return filter_name + "St6vectorIfSaIfEE";
+
+ else if (typestring == "std_vector_double")
+ return filter_name + "St6vectorIfSaIdEE";
+
+ else
+ printf("%s\n", typestring.c_str());
+ ROS_ASSERT(typestring == "NOT A SUPPORTED TYPE");
+ return "";
+
+}
+
+
/** \brief A Base filter class to provide a standard interface for all filters
*
*/
@@ -41,14 +74,37 @@
class FilterBase
{
public:
- virtual ~FilterBase(){;};
+ FilterBase(){};
+ virtual ~FilterBase(){};
+ virtual bool configure(unsigned int number_of_elements, const std::string & arguments)=0;
+
+
/** \brief Update the filter and return the data seperately
*/
- virtual bool update(const T* const data_in, T* data_out)=0;
+ virtual bool update(const T& data_in, T& data_out)=0;
+ std::string getType() {return typeid(T).name();};
};
+
+template <typename T>
+class FilterFactory : public Loki::SingletonHolder < Loki::Factory< filters::FilterBase<T>, std::string >,
+ Loki::CreateUsingNew,
+ Loki::LongevityLifetime::DieAsSmallObjectChild >
+{
+ //empty
+};
+
+
+
+#define ROS_REGISTER_FILTER(c,t) \
+ filters::FilterBase<t> * Filters_New_##c##__##t() {return new c< t >;}; \
+ bool ROS_FILTER_## c ## _ ## t = \
+ filters::FilterFactory<t>::Instance().Register(filters::getFilterID(#c , #t ), Filters_New_##c##__##t);
+///\todo make this use templating to get the data type, the user doesn't ever set the data type at runtime
+///\todo make sure that the error messages for invalid data types are reasonable
+
}
#endif //#ifndef FILTERS_FILTER_BASE_H_
Added: pkg/trunk/util/filters/include/filters/filter_chain.h
===================================================================
--- pkg/trunk/util/filters/include/filters/filter_chain.h (rev 0)
+++ pkg/trunk/util/filters/include/filters/filter_chain.h 2009-02-04 22:46:30 UTC (rev 10573)
@@ -0,0 +1,179 @@
+/*
+ * 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 FILTERS_FILTER_CHAIN_H_
+#define FILTERS_FILTER_CHAIN_H_
+
+#include "filters/filter_base.h"
+#include <sstream>
+#include <vector>
+
+#include "boost/shared_ptr.hpp"
+
+namespace filters
+{
+
+template <typename T>
+class FilterReference
+{
+public:
+ FilterReference(const std::string& filter_name, const std::string& filter_argsuments):name_(filter_name), arguments_(filter_argsuments)
+ {
+ std::stringstream constructor_string;
+ constructor_string << filter_name << typeid(T).name();
+ filter_ = filters::FilterFactory<T>::Instance().CreateObject(constructor_string.str());
+ printf("Created filter at %p\n in reference %p\n", filter_, this);
+ };
+ ~FilterReference(){ printf("reference destructor -> deleting filter\n"); delete filter_; };
+
+ filters::FilterBase<T> * filter_;
+ std::string name_;
+ std::string arguments_;
+};
+
+
+template <typename T>
+class FilterChain
+{
+public:
+ /** \brief Create the filter chain object */
+ FilterChain(): configured_(false){};
+ /** \brief Configure the filter chain
+ * This will call configure on all filters which have been added
+ * as well as allocate the buffers*/
+ bool configure(unsigned int size)
+ {
+ buffer0_.resize(size);
+ buffer1_.resize(size);
+
+ bool result = true;
+
+ // for each element in vector configure
+ typename std::vector<boost::shared_ptr<filters::FilterReference<T> > >::iterator it;
+
+ for ( it = reference_pointers_.begin();
+ it != reference_pointers_.end(); it++) ///\todo check allignment of for
+ {
+ printf("Configured %s filter at %p\n", (*it).get()->name_.c_str(), (*it).get());
+ result = result && it->get()->filter_->configure(size, it->get()->arguments_);
+ }
+
+ if (result == true)
+ {
+ configured_ = true;
+ }
+ return result;
+ };
+
+ /** \brief Add filters to the list of filters to run on incoming data
+ * This will not configure, you must call configure before they will
+ * be useful. */
+ bool add(const std::string& filter_name, const std::string& filter_arguments)
+ {
+ configured_ = false;
+ boost::shared_ptr<filters::FilterReference<T> > p(new filters::FilterReference<T>(filter_name, filter_arguments));
+ reference_pointers_.push_back(p);
+ printf("%s filter added to vector\n", filter_name.c_str());
+ return true;
+ };
+
+ /** \brief Clear all filters from this chain */
+ bool clear()
+ {
+ configured_ = false;
+ reference_pointers_.clear();
+ buffer0_.clear();
+ buffer1_.clear();
+ return true;
+ };
+
+ /** \brief process data through each of the filters added sequentially */
+ bool update(const T& data_in, T& data_out);
+
+
+ ~FilterChain()
+ {
+ clear();
+
+ };
+
+private:
+ std::vector<boost::shared_ptr<filters::FilterReference<T> > > reference_pointers_;
+
+ T buffer0_; ///<! A temporary intermediate buffer
+ T buffer1_; ///<! A temporary intermediate buffer
+ //std::vector<T> buffer0_; ///<! A temporary intermediate buffer
+ // std::vector<T> buffer1_; ///<! A temporary intermediate buffer
+ bool configured_; ///<! whether the system is configured
+
+};
+
+template <typename T>
+bool FilterChain<T>::update (const T& data_in, T& data_out)
+{
+ unsigned int list_size = reference_pointers_.size();
+ bool result;
+ if (list_size == 0)
+ {
+ memcpy(&data_out, &data_in, sizeof(data_in));
+ result = true;
+ }
+ else if (list_size == 1)
+ result = reference_pointers_[0]->filter_->update(data_in, data_out);
+ else if (list_size == 2)
+ {
+ result = reference_pointers_[0]->filter_->update(data_in, buffer0_);
+ result = result && reference_pointers_[1]->filter_->update(buffer0_, data_out);
+ }
+ else
+ {
+ result = reference_pointers_[0]->filter_->update(data_in, buffer0_); //first copy in
+ for (unsigned int i = 1; i < reference_pointers_.size() - 1; i++) // all but first and last
+ {
+ if (i %2 == 1)
+ result = result && reference_pointers_[i]->filter_->update(buffer0_, buffer1_);
+ else
+ result = result && reference_pointers_[i]->filter_->update(buffer1_, buffer0_);
+
+ }
+ if (list_size % 2 == 1) // odd number last deposit was in buffer0
+ result = result && reference_pointers_.back()->filter_->update(buffer0_, data_out);
+ else
+ result = result && reference_pointers_.back()->filter_->update(buffer1_, data_out);
+ }
+ return result;
+
+};
+
+
+}
+
+
+
+#endif //#ifndef FILTERS_FILTER_CHAIN_H_
Modified: pkg/trunk/util/filters/include/filters/mean.h
===================================================================
--- pkg/trunk/util/filters/include/filters/mean.h 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filters/include/filters/mean.h 2009-02-04 22:46:30 UTC (rev 10573)
@@ -35,6 +35,7 @@
#include <stdio.h>
#include "filters/filter_base.h"
+#include "ros/assert.h"
namespace filters
@@ -48,49 +49,76 @@
{
public:
/** \brief Construct the filter with the expected width and height */
- MeanFilter(uint32_t number_of_observations, uint32_t elements_per_observation);
+ MeanFilter();
/** \brief Destructor to clean up
*/
~MeanFilter();
+ virtual bool configure(unsigned int width, const std::string& number_of_observations);
+
/** \brief Update the filter and return the data seperately
- * \param data_in T array with length elements_per_observation
- * \param data_out T array with length elements_per_observation
+ * \param data_in T array with length width
+ * \param data_out T array with length width
*/
- virtual bool update(T const * const data_in, T* data_out);
+ virtual bool update( const T & data_in, T& data_out);
protected:
- T * data_storage_; ///< Storage for data between updates
+ T data_storage_; ///< Storage for data between updates
uint32_t last_updated_row_; ///< The last row to have been updated by the filter
uint32_t iterations_; ///< Number of iterations up to number of observations
uint32_t number_of_observations_; ///< Number of observations over which to filter
- uint32_t elements_per_observation_; ///< Number of elements per observation
+ uint32_t width_; ///< Number of elements per observation
+ bool configured_;
};
+ROS_REGISTER_FILTER(MeanFilter, std_vector_double)
+ROS_REGISTER_FILTER(MeanFilter, std_vector_float)
+
+
template <typename T>
-MeanFilter<T>::MeanFilter(uint32_t number_of_observations, uint32_t elements_per_observation):
- last_updated_row_(number_of_observations),
+MeanFilter<T>::MeanFilter():
+ last_updated_row_(0),
iterations_(0),
- number_of_observations_(number_of_observations),
- elements_per_observation_(elements_per_observation)
+ number_of_observations_(0),
+ width_(0),
+ configured_(false)
{
- data_storage_ = new T[number_of_observations_ * elements_per_observation];
}
template <typename T>
+bool MeanFilter<T>::configure(unsigned int width, const std::string& number_of_observations)
+{
+ if (configured_)
+ return false;
+ width_ = width;
+ std::stringstream ss;
+ ss << number_of_observations;
+ ss >> number_of_observations_;
+ last_updated_row_ = number_of_observations_;
+
+ data_storage_.resize(number_of_observations_ * width_);
+ configured_ = true;
+ return true;
+}
+
+template <typename T>
MeanFilter<T>::~MeanFilter()
{
- delete [] data_storage_;
}
template <typename T>
-bool MeanFilter<T>::update(T const* const data_in, T* data_out)
+bool MeanFilter<T>::update(const T & data_in, T& data_out)
{
+ // ROS_ASSERT(data_in.size() == width_);
+ //ROS_ASSERT(data_out.size() == width_);
+ if (data_in.size() != width_ || data_out.size() != width_)
+ return false;
+
//update active row
if (last_updated_row_ >= number_of_observations_ - 1)
last_updated_row_ = 0;
@@ -98,9 +126,9 @@
last_updated_row_++;
//copy incoming data into perminant storage
- memcpy(&data_storage_[elements_per_observation_ * last_updated_row_],
- data_in,
- sizeof(T) * elements_per_observation_);
+ memcpy(&data_storage_[width_ * last_updated_row_],
+ &data_in[0],
+ sizeof(data_in[0]) * width_);
//Return values
@@ -117,12 +145,12 @@
}
//Return each value
- for (uint32_t i = 0; i < elements_per_observation_; i++)
+ for (uint32_t i = 0; i < width_; i++)
{
data_out[i] = 0;
for (uint32_t row = 0; row < length; row ++)
{
- data_out[i] += data_storage_[i + row * elements_per_observation_];
+ data_out[i] += data_storage_[i + row * width_];
}
data_out[i] /= length;
}
Modified: pkg/trunk/util/filters/include/filters/median.h
===================================================================
--- pkg/trunk/util/filters/include/filters/median.h 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filters/include/filters/median.h 2009-02-04 22:46:30 UTC (rev 10573)
@@ -31,9 +31,9 @@
#define FILTERS_MEDIAN_H_
#include <stdint.h>
-
+#include <sstream>
#include "filters/filter_base.h"
-
+#include "ros/assert.h"
/*
* Algorithm from N. Wirth's book, implementation by N. Devillard.
* This code in public domain.
@@ -85,55 +85,82 @@
*
*/
template <typename T>
-class MedianFilter: public FilterBase <T>
+class MedianFilter: public filters::FilterBase <T>
{
public:
/** \brief Construct the filter with the expected width and height */
- MedianFilter(uint32_t number_of_observations, uint32_t elements_per_observation);
+ MedianFilter();
/** \brief Destructor to clean up
*/
- ~MedianFilter()
- {
- delete [] data_storage_;
- delete [] temp_storage_;
- }
+ ~MedianFilter();
+ virtual bool configure(unsigned int elements, const std::string& arguments);
/** \brief Update the filter and return the data seperately
- * \param data_in double array with length elements_per_observation
- * \param data_out double array with length elements_per_observation
+ * \param data_in double array with length width
+ * \param data_out double array with length width
*/
- virtual bool update(T const * const data_in, T* data_out);
+ virtual bool update(const T& data_in, T& data_out);
protected:
- T * temp_storage_; ///< Preallocated storage for the list to sort
- T * data_storage_; ///< Storage for data between updates
+ ///\todo change to vector
+ T temp_storage_; ///< Preallocated storage for the list to sort
+ T data_storage_; ///< Storage for data between updates
uint32_t last_updated_row_; ///< The last row to have been updated by the filter
uint32_t iterations_; ///< Number of iterations up to number of observations
uint32_t number_of_observations_; ///< Number of observations over which to filter
- uint32_t elements_per_observation_; ///< Number of elements per observation
+ uint32_t width_; ///< Number of elements per observation
+ bool configured_; ///< Whether the filter has been configured
+
};
template <typename T>
-MedianFilter<T>::MedianFilter(uint32_t number_of_observations, uint32_t elements_per_observation):
- last_updated_row_(number_of_observations),
+MedianFilter<T>::MedianFilter():
+ last_updated_row_(0),
iterations_(0),
- number_of_observations_(number_of_observations),
- elements_per_observation_(elements_per_observation)
+ number_of_observations_(0),
+ width_(0),
+ configured_(false)
{
- data_storage_ = new T[number_of_observations_ * elements_per_observation];
- temp_storage_ = new T[elements_per_observation];
+
+};
+template <typename T>
+MedianFilter<T>::~MedianFilter()
+{
};
template <typename T>
-bool MedianFilter<T>::update(T const* const data_in, T* data_out)
+bool MedianFilter<T>::configure(uint32_t width, const std::string& number_of_observations)
{
+ if (configured_)
+ return false;
+ width_ = width;
+ std::stringstream ss;
+ ss << number_of_observations;
+ ss >> number_of_observations_;
+ data_storage_.resize(number_of_observations_ * width_);
+ temp_storage_.resize(width_);
+ last_updated_row_ = number_of_observations_;
+ configured_ = true;
+ return true;
+};
+
+template <typename T>
+bool MedianFilter<T>::update(const T& data_in, T& data_out)
+{
+ // printf("Expecting width %d, got %d and %d\n", width_, data_in.size(),data_out.size());
+ // ROS_ASSERT(data_in.size() == width_);
+ // ROS_ASSERT(data_out.size() == width_);
+ if (data_in.size() != width_ || data_out.size() != width_)
+ return false;
+ if (!configured_)
+ return false;
//update active row
if (last_updated_row_ >= number_of_observations_ - 1)
last_updated_row_ = 0;
@@ -141,9 +168,9 @@
last_updated_row_++;
//copy incoming data into perminant storage
- memcpy(&data_storage_[elements_per_observation_ * last_updated_row_],
- data_in,
- sizeof(T) * elements_per_observation_);
+ memcpy(&data_storage_[width_ * last_updated_row_],
+ &data_in[0],
+ sizeof(data_in[0]) * width_);
//Return values
@@ -160,18 +187,21 @@
}
//Return each value
- for (uint32_t i = 0; i < elements_per_observation_; i++)
+ for (uint32_t i = 0; i < width_; i++)
{
for (uint32_t row = 0; row < length; row ++)
{
- temp_storage_[row] = data_storage_[i + row * elements_per_observation_];
+ temp_storage_[row] = data_storage_[i + row * width_];
}
- data_out[i] = median(temp_storage_, length);
+ data_out[i] = median(&temp_storage_[0], length);
}
return true;
+};
+
+ROS_REGISTER_FILTER(MedianFilter, std_vector_double)
+ROS_REGISTER_FILTER(MedianFilter, std_vector_float)
}
-}
#endif //#ifndef FILTERS_MEDIAN_H_
Modified: pkg/trunk/util/filters/include/filters/transfer_function.h
===================================================================
--- pkg/trunk/util/filters/include/filters/transfer_function.h 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filters/include/filters/transfer_function.h 2009-02-04 22:46:30 UTC (rev 10573)
@@ -84,8 +84,11 @@
* \param data_in vector<T> with n elements
* \param data_out vector<T> with n elements
*/
- virtual bool update(std::vector<T> const * const data_in, std::vector<T>* data_out) ;
+ virtual bool update(const std::vector<T> & data_in, std::vector<T>& data_out) ;
+
+ //virtual bool configure(unsigned int number_of_elements, const std::string & arguments)=0;
+
protected:
unsigned int number_of_channels_;
@@ -125,29 +128,29 @@
template <typename T>
-bool TransferFunctionFilter<T>::update(std::vector<T> const* const data_in, std::vector<T>* data_out)
+bool TransferFunctionFilter<T>::update(const std::vector<T> & data_in, std::vector<T>& data_out)
{
// Ensure the correct number of inputs
- assert(data_in->size() == number_of_channels_);
+ assert(data_in.size() == number_of_channels_);
// Copy data to prevent mutation if in and out are the same ptr
- std::vector<T> current_input = *data_in;
+ std::vector<T> current_input = data_in;
for (uint32_t i = 0; i < current_input.size(); i++)
{
- (*data_out)[i]=b_[0] * current_input[i];
+ data_out[i]=b_[0] * current_input[i];
for (uint32_t row = 0; row < input_buffer_.size(); row++)
{
- (*data_out)[i] += b_[row+1] * input_buffer_[row][i];
+ (data_out)[i] += b_[row+1] * input_buffer_[row][i];
}
for (uint32_t row = 0; row < output_buffer_.size(); row++)
{
- (*data_out)[i] -= a_[row+1] * output_buffer_[row][i];
+ (data_out)[i] -= a_[row+1] * output_buffer_[row][i];
}
}
input_buffer_.push(current_input);
- output_buffer_.push(*data_out);
+ output_buffer_.push(data_out);
return true;
}
Modified: pkg/trunk/util/filters/manifest.xml
===================================================================
--- pkg/trunk/util/filters/manifest.xml 2009-02-04 22:41:58 UTC (rev 10572)
+++ pkg/trunk/util/filters/manifest.xml 2009-02-04 22:46:30 UTC (rev 10573)
@@ -7,9 +7,11 @@
</description>
<author>Tully Foote/tf...@wi..., Melonee Wise/mw...@wi...</author>
<license>BSD</license>
-<review status="unreviewed" notes=""/>
+<review status="API conditionally cleared" notes=""/>
<url>http://pr.willowgarage.com</url>
<depend package="misc_utils" />
+<depend package="loki" />
+<depend package="rosconsole" />
<export>
<cpp cflags="-I${prefix}/include"/> <!-- lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lfilter"/-->
</export>
Added: pkg/trunk/util/filters/test/test_chain.cpp
===================================================================
--- pkg/trunk/util/filters/test/test_chain.cpp (rev 0)
+++ pkg/trunk/util/filters/test/test_chain.cpp 2009-02-04 22:46:30 UTC (rev 10573)
@@ -0,0 +1,125 @@
+/*
+ * 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...
[truncated message content] |
|
From: <ge...@us...> - 2009-02-05 02:27:45
|
Revision: 10597
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10597&view=rev
Author: gerkey
Date: 2009-02-05 02:27:43 +0000 (Thu, 05 Feb 2009)
Log Message:
-----------
Switched to "new" atlas sysdeps and unblacklisted octave packages.
Modified Paths:
--------------
pkg/trunk/3rdparty/opencv_latest/manifest.xml
pkg/trunk/3rdparty/scipy/manifest.xml
pkg/trunk/vision/calonder_descriptor/manifest.xml
pkg/trunk/visualization/wxpy_ros/manifest.xml
Removed Paths:
-------------
pkg/trunk/3rdparty/octave_forge/ROS_BUILD_BLACKLIST
pkg/trunk/visualization/multitouch_nav/.build_version
Modified: pkg/trunk/3rdparty/opencv_latest/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/opencv_latest/manifest.xml 2009-02-05 02:16:42 UTC (rev 10596)
+++ pkg/trunk/3rdparty/opencv_latest/manifest.xml 2009-02-05 02:27:43 UTC (rev 10597)
@@ -19,7 +19,7 @@
<sysdepend os="ubuntu" version="8.04-hardy" package="libavformat-dev"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="libavcodec-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libavcodec-dev"/>
-<sysdepend os="ubuntu" version="7.04-feisty" package="libjasper-dev"/>
+<sysdepend os="ubuntu" version="7.04-feisty" package="libjasper-1.701-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libjasper-dev"/>
<versioncontrol type="svn" url="https://opencvlibrary.svn.sourceforge.net/svnroot/opencvlibrary/tags/latest_tested_snapshot/opencv"/>
</package>
Modified: pkg/trunk/3rdparty/scipy/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/scipy/manifest.xml 2009-02-05 02:16:42 UTC (rev 10596)
+++ pkg/trunk/3rdparty/scipy/manifest.xml 2009-02-05 02:27:43 UTC (rev 10597)
@@ -21,7 +21,8 @@
<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.04-hardy" package="atlas3-sse2-dev"/-->
+<sysdepend os="ubuntu" version="8.04-hardy" package="libatlas-sse2-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="libatlas-sse2-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="gfortran"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="fftw3-dev"/>
Modified: pkg/trunk/vision/calonder_descriptor/manifest.xml
===================================================================
--- pkg/trunk/vision/calonder_descriptor/manifest.xml 2009-02-05 02:16:42 UTC (rev 10596)
+++ pkg/trunk/vision/calonder_descriptor/manifest.xml 2009-02-05 02:27:43 UTC (rev 10597)
@@ -17,8 +17,10 @@
<sysdepend os="ubuntu" version="8.04-hardy" package="python-imaging"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="python-imaging"/>
<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.04-hardy" package="atlas3-sse2-dev"/-->
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libatlas-sse2-dev"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="atlas3-base-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="atlas3-base-dev"/>
+ <!--sysdepend os="ubuntu" version="8.04-hardy" package="atlas3-base-dev"/-->
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libatlas-base-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="libatlas-base-dev"/>
</package>
Deleted: pkg/trunk/visualization/multitouch_nav/.build_version
===================================================================
--- pkg/trunk/visualization/multitouch_nav/.build_version 2009-02-05 02:16:42 UTC (rev 10596)
+++ pkg/trunk/visualization/multitouch_nav/.build_version 2009-02-05 02:27:43 UTC (rev 10597)
@@ -1 +0,0 @@
-https://personalrobots.svn.sourceforge.net/svnroot/personalrobots/pkg/trunk/visualization/multitouch_nav:10466
Modified: pkg/trunk/visualization/wxpy_ros/manifest.xml
===================================================================
--- pkg/trunk/visualization/wxpy_ros/manifest.xml 2009-02-05 02:16:42 UTC (rev 10596)
+++ pkg/trunk/visualization/wxpy_ros/manifest.xml 2009-02-05 02:27:43 UTC (rev 10597)
@@ -15,7 +15,8 @@
<depend package="scipy"/>
<depend package="joy" />
- <sysdepend os="ubuntu" version="8.04-hardy" package="lapack3-dev" />
+ <!--sysdepend os="ubuntu" version="8.04-hardy" package="lapack3-dev" /-->
+ <sysdepend os="ubuntu" version="8.04-hardy" package="liblapack-dev" />
<sysdepend os="ubuntu" version="8.10-intrepid" package="liblapack-dev" />
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-02-05 23:26:26
|
Revision: 10618
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10618&view=rev
Author: sfkwc
Date: 2009-02-05 23:26:18 +0000 (Thu, 05 Feb 2009)
Log Message:
-----------
#882: random_utils, world_3d_map, and simulator_integration tests being removed
Removed Paths:
-------------
pkg/trunk/deprecated/random_utils/
pkg/trunk/deprecated/world_3d_map/
pkg/trunk/simulators/simulator_integration_tests/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-02-05 23:58:15
|
Revision: 10622
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10622&view=rev
Author: tfoote
Date: 2009-02-05 23:58:09 +0000 (Thu, 05 Feb 2009)
Log Message:
-----------
converting BaseVel to PoseDot tests up through 2dnave stage and gazebo integration tests pass ros#447
Modified Paths:
--------------
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
pkg/trunk/drivers/robot/segway_apox/segway.cpp
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.xml
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxy_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxyw_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomy_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_vw_gt.py
pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h
pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base_follow.cpp
pkg/trunk/mechanism/mechanism_control/scripts/detect.py
pkg/trunk/nav/deadreckon/deadreckon.cpp
pkg/trunk/nav/fake_localization/ground_truth_controller.cpp
pkg/trunk/nav/teleop_base/teleop_base.cpp
pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cpp
pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/governor_node.h
pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller_ros.h
pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
pkg/trunk/nav/trajectory_rollout/src/trajectory_controller_ros.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/prcore/pytf/manifest.xml
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch
pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/calibration_controllers/full_calibration_controller.xml
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml
pkg/trunk/simulators/rosstage/rosstage.cc
pkg/trunk/util/filters/CMakeLists.txt
pkg/trunk/util/filters/manifest.xml
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp 2009-02-05 23:58:09 UTC (rev 10622)
@@ -53,7 +53,7 @@
_completed(false)
{
// advertise the velocity commands
- advertise<std_msgs::BaseVel>("cmd_vel",10);
+ advertise<std_msgs::PoseDot>("cmd_vel",10);
// subscribe to messages
subscribe("odom", _odom, &odom_calib::odom_callback, 10);
@@ -134,7 +134,8 @@
// wait for sensor measurements from odom and imu
while (!(_odom_active && _imu_active)){
- _vel.vx = 0; _vel.vy = 0; _vel.vw = 0;
+ _vel.vel.vx = 0; _vel.vel.vy = 0; _vel.vel.vz = 0;
+ _vel.ang_vel.vx = 0; _vel.ang_vel.vy = 0; _vel.ang_vel.vz = 0;
publish("cmd_vel", _vel);
if (!_odom_active)
ROS_INFO("Waiting for wheel odometry to start");
@@ -157,14 +158,14 @@
// still moving
Duration duration = Time::now() - _time_begin;
if (duration.toSec() <= _duration){
- _vel.vx = _trans_vel;
- _vel.vw = _rot_vel;
+ _vel.vel.vx = _trans_vel;
+ _vel.ang_vel.vz = _rot_vel;
}
// finished turning
else{
_completed = true;
- _vel.vx = 0;
- _vel.vw = 0;
+ _vel.vel.vx = 0;
+ _vel.ang_vel.vz = 0;
}
publish("cmd_vel", _vel);
usleep(50000);
@@ -176,8 +177,8 @@
{
// give robot time to stop
for (unsigned int i=0; i<10; i++){
- _vel.vx = 0;
- _vel.vw = 0;
+ _vel.vel.vx = 0;
+ _vel.ang_vel.vz = 0;
publish("cmd_vel", _vel);
usleep(50000);
}
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-02-05 23:58:09 UTC (rev 10622)
@@ -43,7 +43,7 @@
// messages
#include "std_msgs/RobotBase2DOdom.h"
-#include "std_msgs/BaseVel.h"
+#include "std_msgs/PoseDot.h"
#include "std_msgs/PoseWithRatesStamped.h"
#include "robot_msgs/MechanismState.h"
@@ -84,7 +84,7 @@
robot_msgs::MechanismState _mech;
// estimated robot pose message to send
- std_msgs::BaseVel _vel;
+ std_msgs::PoseDot _vel;
// service messages
pr2_mechanism_controllers::WheelRadiusMultiplier::Request _srv_snd, _srv_rsp;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h 2009-02-05 23:58:09 UTC (rev 10622)
@@ -54,7 +54,7 @@
#include <newmat10/newmatap.h>
#include <std_msgs/RobotBase2DOdom.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
#include <pr2_msgs/Odometer.h>
#include <pr2_msgs/Covariance2D.h>
#include <pr2_msgs/BaseControllerState.h>
@@ -494,7 +494,7 @@
* \brief deal with cmd_vel command from 2dnav stack
*/
void CmdBaseVelReceived();
- std_msgs::BaseVel baseVelMsg;
+ std_msgs::PoseDot baseVelMsg;
/*!
* \brief mutex lock for setting and getting ros messages
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h 2009-02-05 23:58:09 UTC (rev 10622)
@@ -56,7 +56,7 @@
#include <newmat10/newmatap.h>
#include <std_msgs/RobotBase2DOdom.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
#include <pr2_msgs/Odometer.h>
#include <pr2_msgs/Covariance2D.h>
@@ -481,7 +481,7 @@
* \brief deal with cmd_vel command from 2dnav stack
*/
void CmdBaseVelReceived();
- std_msgs::BaseVel baseVelMsg;
+ std_msgs::PoseDot baseVelMsg;
/*!
* \brief mutex lock for setting and getting ros messages
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp 2009-02-05 23:58:09 UTC (rev 10622)
@@ -1145,7 +1145,7 @@
void BaseControllerNode::CmdBaseVelReceived()
{
this->ros_lock_.lock();
- this->setCommand(baseVelMsg.vx,baseVelMsg.vy,baseVelMsg.vw);
+ this->setCommand(baseVelMsg.vel.vx,baseVelMsg.vel.vy,baseVelMsg.ang_vel.vz);
this->ros_lock_.unlock();
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp 2009-02-05 23:58:09 UTC (rev 10622)
@@ -1187,7 +1187,7 @@
void BaseControllerPosNode::CmdBaseVelReceived()
{
this->ros_lock_.lock();
- this->setCommand(baseVelMsg.vx,baseVelMsg.vy,baseVelMsg.vw);
+ this->setCommand(baseVelMsg.vel.vx,baseVelMsg.vel.vy,baseVelMsg.vel.vz);
this->ros_lock_.unlock();
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp 2009-02-05 23:58:09 UTC (rev 10622)
@@ -35,7 +35,6 @@
#include <libTF/libTF.h>
#include <ros/node.h>
#include <robot_srvs/SetJointCmd.h>
-#include <std_msgs/BaseVel.h>
#include <std_msgs/RobotBase2DOdom.h>
static int done = 0;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp 2009-02-05 23:58:09 UTC (rev 10622)
@@ -34,7 +34,7 @@
#include <libTF/libTF.h>
#include <ros/node.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
#include <std_msgs/RobotBase2DOdom.h>
#include <std_msgs/Quaternion.h>
#include <iostream>
@@ -112,23 +112,26 @@
/*********** Start moving the robot ************/
- std_msgs::BaseVel cmd;
- cmd.vx = 0;
- cmd.vy = 0;
- cmd.vw = 0;
+ std_msgs::PoseDot cmd;
+ cmd.vel.vx = 0;
+ cmd.vel.vy = 0;
+ cmd.vel.vz = 0;
+ cmd.ang_vel.vx = 0;
+ cmd.ang_vel.vy = 0;
+ cmd.ang_vel.vz = 0;
double run_time = 0;
bool run_time_set = false;
int file_num = 0;
if(argc >= 2)
- cmd.vx = atof(argv[1]);
+ cmd.vel.vx = atof(argv[1]);
if(argc >= 3)
- cmd.vy = atof(argv[2]);
+ cmd.vel.vy = atof(argv[2]);
if(argc >= 4)
- cmd.vw = atof(argv[3]);
+ cmd.ang_vel.vz = atof(argv[3]);
if(argc >=5)
{
@@ -141,7 +144,7 @@
file_num = atoi(argv[5]);
}
- node->advertise<std_msgs::BaseVel>("cmd_vel",10);
+ node->advertise<std_msgs::PoseDot>("cmd_vel",10);
sleep(1);
libTF::Vector ang_rates;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp 2009-02-05 23:58:09 UTC (rev 10622)
@@ -35,7 +35,7 @@
#include <libTF/libTF.h>
#include <ros/node.h>
#include <std_msgs/PoseWithRatesStamped.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
#include <std_msgs/RobotBase2DOdom.h>
#include <std_msgs/Quaternion.h>
@@ -131,29 +131,33 @@
/*********** Start moving the robot ************/
- std_msgs::BaseVel cmd;
- cmd.vx = 0;
- cmd.vy = 0;
- cmd.vw = 0;
+ std_msgs::PoseDot cmd;
+ cmd.vel.vx = 0;
+ cmd.vel.vy = 0;
+ cmd.vel.vz = 0;
+ cmd.ang_vel.vx = 0;
+ cmd.ang_vel.vy = 0;
+ cmd.ang_vel.vz = 0;
double run_time = 0;
bool run_time_set = false;
+ int file_num = 0;
if(argc >= 2)
- cmd.vx = atof(argv[1]);
+ cmd.vel.vx = atof(argv[1]);
if(argc >= 3)
- cmd.vy = atof(argv[2]);
+ cmd.vel.vy = atof(argv[2]);
if(argc >= 4)
- cmd.vw = atof(argv[3]);
+ cmd.ang_vel.vz = atof(argv[3]);
if(argc ==5)
{
run_time = atof(argv[4]);
run_time_set = true;
}
- node->advertise<std_msgs::BaseVel>("cmd_vel",1);
+ node->advertise<std_msgs::PoseDot>("cmd_vel",1);
sleep(1);
node->publish("cmd_vel",cmd);
sleep(1);
@@ -164,7 +168,7 @@
while(!done)
{
ros::Duration delta_time = ros::Time::now() - start_time;
- cout << "Sending out command " << cmd.vx << " " << cmd.vy << " " << cmd.vw << endl;
+ cout << "Sending out command " << cmd.vel.vx << " " << cmd.vel.vy << " " << cmd.ang_vel.vz << endl;
if(run_time_set && delta_time.toSec() > run_time)
break;
// ang_rates = GetAsEuler(tb.ground_truth.vel.ang_vel);
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-02-05 23:58:09 UTC (rev 10622)
@@ -82,16 +82,9 @@
Eigen::Matrix<float,6,5> constraint_jac_;
Eigen::Matrix<float,6,1> constraint_wrench_;
Eigen::Matrix<float,5,1> constraint_force_;
- // joint constraint
- Eigen::MatrixXf joint_constraint_force_;
- Eigen::MatrixXf joint_constraint_jac_;
- Eigen::MatrixXf joint_constraint_null_space_;
-
Eigen::MatrixXf task_jac_;
Eigen::MatrixXf identity_;
- Eigen::MatrixXf identity_joint_;
Eigen::MatrixXf constraint_null_space_;
- Eigen::MatrixXf joint_constraint_torq_;
Eigen::MatrixXf constraint_torq_;
Eigen::MatrixXf task_torq_;
KDL::Frame endeffector_frame_;
@@ -99,14 +92,12 @@
// some parameters to define the constraint
double wall_x;
- double elbow_limit;
double threshold_x;
double wall_r;
double threshold_r;
double f_x_max;
double f_r_max;
double f_pose_max;
- double f_limit_max;
double desired_roll_;
double desired_pitch_;
double desired_yaw_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-02-05 23:58:09 UTC (rev 10622)
@@ -51,7 +51,6 @@
: jnt_to_jac_solver_(NULL),
jnt_to_pose_solver_(NULL),
initialized_(false)
-
{
constraint_jac_.setZero();
constraint_wrench_.setZero();
@@ -100,26 +99,19 @@
node->param("constraint/wall_x" , wall_x , 0.8) ; /// location of the wall
node->param("constraint/threshold_x" , threshold_x , 0.1 ) ; /// distance within the wall to apply constraint force
node->param("constraint/wall_r" , wall_r , 0.2 ) ; /// cylinder radius
- node->param("constraint/elbow_limit" , elbow_limit , -0.523 ) ; /// elbow limit
node->param("constraint/threshold_r" , threshold_r , 0.1) ; /// radius over with constraint is applied
node->param("constraint/f_x_max" , f_x_max , -1000.0) ; /// max x force
node->param("constraint/f_r_max" , f_r_max , -1000.0) ; /// max r force
node->param("constraint/f_r_max" , f_pose_max , 20.0) ; /// max r force
- node->param("constraint/f_limit_max" , f_limit_max , 20.0) ; /// max r force
-
+
// Constructs solvers and allocates matrices.
unsigned int num_joints = kdl_chain_.getNrOfJoints();
jnt_to_jac_solver_.reset(new ChainJntToJacSolver(kdl_chain_));
jnt_to_pose_solver_.reset(new ChainFkSolverPos_recursive(kdl_chain_));
task_jac_ = Eigen::MatrixXf::Zero(6, num_joints);
identity_ = Eigen::MatrixXf::Identity(6, 6);
- identity_joint_ = Eigen::MatrixXf::Identity(num_joints, num_joints);
- constraint_null_space_ = Eigen::MatrixXf::Zero(6, 6);
- joint_constraint_force_= Eigen::MatrixXf::Zero(1, 1);
- joint_constraint_jac_= Eigen::MatrixXf::Zero(num_joints, 1);
- joint_constraint_null_space_ = Eigen::MatrixXf::Zero(num_joints, num_joints);
+ constraint_null_space_ = Eigen::MatrixXf::Zero(num_joints, num_joints);
constraint_torq_ = Eigen::MatrixXf::Zero(num_joints, 1);
- joint_constraint_torq_ = Eigen::MatrixXf::Zero(num_joints, 1);
task_torq_ = Eigen::MatrixXf::Zero(num_joints, 1);
task_wrench_ = Eigen::Matrix<float,6,1>::Zero();
@@ -176,9 +168,8 @@
// convert the wrench into joint torques
- joint_constraint_torq_ = joint_constraint_jac_.transpose()*joint_constraint_force_;
- constraint_torq_ = joint_constraint_null_space_*task_jac_.transpose() * constraint_wrench_;
- task_torq_ = joint_constraint_null_space_ * task_jac_.transpose()* constraint_null_space_ * task_wrench_;
+ constraint_torq_ = task_jac_.transpose() * constraint_wrench_;
+ task_torq_ = task_jac_.transpose()* constraint_null_space_ * task_wrench_;
//ROS_ERROR("%f %f %f", task_torq_(0),task_torq_(1),task_torq_(1));
//ROS_ERROR("%.3f %.3f %.3f",constraint_null_space_(0,0), constraint_null_space_(1,1), constraint_null_space_(2,2));
@@ -187,7 +178,7 @@
JntArray jnt_eff(kdl_chain_.getNrOfJoints());
for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); ++i)
- jnt_eff(i) = joint_constraint_torq_(i)+constraint_torq_(i) + task_torq_(i);
+ jnt_eff(i) = constraint_torq_(i) + task_torq_(i);
chain_.setEfforts(jnt_eff, robot_->joint_states_);
}
@@ -287,22 +278,6 @@
{
f_yaw = 0;
}
-
- //joint constraint force - stop the elbow from going past -30 degrees (.5235 rad)
- JntArray jnt_pos(kdl_chain_.getNrOfJoints());
- chain_.getPositions(robot_->joint_states_, jnt_pos);
-
- double joint_e = elbow_limit-jnt_pos(3);
-
- if(joint_e < 0)
- {
- joint_constraint_jac_(3)=1;
- joint_constraint_force_(0)=joint_e*f_limit_max;
- }
- else
- {
- joint_constraint_force_(0)=0;
- }
constraint_force_(0) = f_x;
constraint_force_(1) = f_r;
@@ -316,7 +291,6 @@
// Compute generalized inverse, this is the transpose as long as the constraints are
// orthonormal to eachother. Will replace with QR method later.
constraint_null_space_ = identity_ - constraint_jac_*constraint_jac_.transpose();
- joint_constraint_null_space_ = identity_joint_ - joint_constraint_jac_*joint_constraint_jac_.transpose();
}
Modified: pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2009-02-05 23:58:09 UTC (rev 10622)
@@ -58,7 +58,7 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "cmd_vel"/BaseVel : velocity commands to differentially drive the robot.
+- @b "cmd_vel"/PoseDot : velocity commands to differentially drive the robot.
Publishes to (name / type):
- @b "odom"/RobotBase2DOdom : odometry data from the robot.
@@ -88,7 +88,7 @@
// Messages that I need
#include <std_msgs/RobotBase2DOdom.h>
//#include <std_msgs/RobotBase2DCmdVel.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
#include <robot_msgs/BatteryState.h>
#define PLAYER_QUEUE_LEN 32
@@ -103,7 +103,7 @@
QueuePointer q;
std_msgs::RobotBase2DOdom odom;
- std_msgs::BaseVel cmdvel;
+ std_msgs::PoseDot cmdvel;
tf::TransformBroadcaster tf;
@@ -256,9 +256,9 @@
//cmd.vel.px = this->cmdvel.vel.x;
//cmd.vel.py = this->cmdvel.vel.y;
//cmd.vel.pa = this->cmdvel.vel.th;
- cmd.vel.px = this->cmdvel.vx;
+ cmd.vel.px = this->cmdvel.vel.vx;
cmd.vel.py = 0.0;
- cmd.vel.pa = this->cmdvel.vw;
+ cmd.vel.pa = this->cmdvel.ang_vel.vz;
cmd.state = 1;
Modified: pkg/trunk/drivers/robot/segway_apox/segway.cpp
===================================================================
--- pkg/trunk/drivers/robot/segway_apox/segway.cpp 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/robot/segway_apox/segway.cpp 2009-02-05 23:58:09 UTC (rev 10622)
@@ -3,7 +3,7 @@
#include <unistd.h>
#include "ros/node.h"
#include "boost/thread/mutex.hpp"
-#include "std_msgs/BaseVel.h"
+#include "std_msgs/PoseDot.h"
#include "std_msgs/RobotBase2DOdom.h"
#include "std_msgs/String.h"
#include "rmp_frame.h"
@@ -33,7 +33,7 @@
static const int max_x_stepsize = 5, max_yaw_stepsize = 2;
- std_msgs::BaseVel cmd_vel;
+ std_msgs::PoseDot cmd_vel;
std_msgs::RobotBase2DOdom odom;
std_msgs::String op_mode;
rmp_frame_t rmp;
@@ -116,8 +116,8 @@
{
req_mutex.lock();
req_time = ros::Time::now().toSec();
- req_x_vel = cmd_vel.vx;
- req_yaw_rate = cmd_vel.vw;
+ req_x_vel = cmd_vel.vel.vx;
+ req_yaw_rate = cmd_vel.ang_vel.vz;
req_mutex.unlock();
}
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.xml 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.xml 2009-02-05 23:58:09 UTC (rev 10622)
@@ -7,8 +7,8 @@
<!-- Note how we use a different parameter namespace and node name
for this test (odom_test vs. scan_test). -->
<param name="topic" value="odom" />
- <param name="hz" value="84.128" />
- <param name="hzerror" value="50.0" />
+ <param name="hz" value="10.0" />
+ <param name="hzerror" value="1.0" />
<param name="test_duration" value="2.0" />
<param name="check_intervals" value="false" />
</test>
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py 2009-02-05 23:58:09 UTC (rev 10622)
@@ -210,13 +210,13 @@
def test_base(self):
print "LNK\n"
- pub = rospy.Publisher("cmd_vel", BaseVel)
+ pub = rospy.Publisher("cmd_vel", PoseDot)
rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
rospy.Subscriber("odom", RobotBase2DOdom, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 10.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
- pub.publish(BaseVel(TARGET_VX,TARGET_VY,TARGET_VW))
+ pub.publish(PoseDot(Velocity(TARGET_VX,TARGET_VY, 0), AngularVelocity(0,0,TARGET_VW)))
time.sleep(0.1)
# display what odom thinks
#print " odom " + " x: " + str(self.odom_e.x) + " y: " + str(self.odom_e.y) + " t: " + str(self.odom_e.t)
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py 2009-02-05 23:58:09 UTC (rev 10622)
@@ -171,13 +171,13 @@
def test_base(self):
print "LNK\n"
- pub = rospy.Publisher("cmd_vel", BaseVel)
+ pub = rospy.Publisher("cmd_vel", PoseDot)
rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
rospy.Subscriber("odom", RobotBase2DOdom, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 10.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
- pub.publish(BaseVel(TARGET_VX,TARGET_VY,TARGET_VW))
+ pub.publish(PoseDot(Velocity(TARGET_VX,TARGET_VY, 0), AngularVelocity(0,0,TARGET_VW)))
time.sleep(0.1)
# display what odom thinks
#print " odom " + " x: " + str(self.odom_x) + " y: " + str(self.odom_y) + " t: " + str(self.odom_t)
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxy_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxy_gt.py 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxy_gt.py 2009-02-05 23:58:09 UTC (rev 10622)
@@ -171,13 +171,13 @@
def test_base(self):
print "LNK\n"
- pub = rospy.Publisher("cmd_vel", BaseVel)
+ pub = rospy.Publisher("cmd_vel", PoseDot)
rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
rospy.Subscriber("odom", RobotBase2DOdom, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 10.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
- pub.publish(BaseVel(TARGET_VX,TARGET_VY,TARGET_VW))
+ pub.publish(PoseDot(Velocity(TARGET_VX,TARGET_VY, 0), AngularVelocity(0,0,TARGET_VW)))
time.sleep(0.1)
# display what odom thinks
#print " odom " + " x: " + str(self.odom_x) + " y: " + str(self.odom_y) + " t: " + str(self.odom_t)
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxyw_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxyw_gt.py 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxyw_gt.py 2009-02-05 23:58:09 UTC (rev 10622)
@@ -210,13 +210,13 @@
def test_base(self):
print "LNK\n"
- pub = rospy.Publisher("cmd_vel", BaseVel)
+ pub = rospy.Publisher("cmd_vel", PoseDot)
rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
rospy.Subscriber("odom", RobotBase2DOdom, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 10.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
- pub.publish(BaseVel(TARGET_VX,TARGET_VY,TARGET_VW))
+ pub.publish(PoseDot(Velocity(TARGET_VX,TARGET_VY, 0), AngularVelocity(0,0,TARGET_VW)))
time.sleep(0.1)
# display what odom thinks
#print " odom " + " x: " + str(self.odom_e.x) + " y: " + str(self.odom_e.y) + " t: " + str(self.odom_e.t)
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomy_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomy_gt.py 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomy_gt.py 2009-02-05 23:58:09 UTC (rev 10622)
@@ -171,13 +171,13 @@
def test_base(self):
print "LNK\n"
- pub = rospy.Publisher("cmd_vel", BaseVel)
+ pub = rospy.Publisher("cmd_vel", PoseDot)
rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
rospy.Subscriber("odom", RobotBase2DOdom, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 10.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
- pub.publish(BaseVel(TARGET_VX,TARGET_VY,TARGET_VW))
+ pub.publish(PoseDot(Velocity(TARGET_VX,TARGET_VY, 0), AngularVelocity(0,0,TARGET_VW)))
time.sleep(0.1)
# display what odom thinks
#print " odom " + " x: " + str(self.odom_x) + " y: " + str(self.odom_y) + " t: " + str(self.odom_t)
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_vw_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_vw_gt.py 2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_vw_gt.py 2009-02-05 23:58:09 UTC (rev 10622)
@@ -116,13 +116,13 @@
def test_base(self):
print "LNK\n"
- pub = rospy.Publisher("cmd_vel", BaseVel)
+ pub = rospy.Publisher("cmd_vel", PoseDot)
rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
rospy.Subscriber("odom", RobotBase2DOdom, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 60.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
- pub.publish(BaseVel(0.0,0.0,TARGET_VW))
+ pub.publish(PoseDot(Velocity(0.0,0.0,0),AngularVelocity(0,0,TARGET_VW)))
time.sleep(0.1)
self.assert_(self.success)
Modified: pkg/trunk/estimation/robot_pose_ekf/src/odom_estimati...
[truncated message content] |
|
From: <tpr...@us...> - 2009-02-06 00:43:48
|
Revision: 10634
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10634&view=rev
Author: tpratkanis
Date: 2009-02-06 00:43:44 +0000 (Fri, 06 Feb 2009)
Log Message:
-----------
Rename controller from left_arm_controller to left_arm_trajectory_controller. Change arm_trajectory_command to /left_arm_trajectory_controller/arm_trajectory_command and /right_arm_trajectory_controller/arm_trajectory_command. Also make testTraj work with new change.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp
pkg/trunk/demos/arm_gazebo/l_arm_trajectory_controller.xml
pkg/trunk/demos/arm_gazebo/r_arm_trajectory_controller.xml
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp 2009-02-06 00:37:50 UTC (rev 10633)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp 2009-02-06 00:43:44 UTC (rev 10634)
@@ -104,8 +104,12 @@
cmd.points[2].time = 0.0;
node->advertise<robot_msgs::JointTraj>("arm_trajectory_command",1);
+ node->advertise<robot_msgs::JointTraj>("left_arm_trajectory_controller/arm_trajectory_command",1);
+ node->advertise<robot_msgs::JointTraj>("right_arm_trajectory_controller/arm_trajectory_command",1);
sleep(1);
node->publish("arm_trajectory_command",cmd);
+ node->publish("left_arm_trajectory_controller/arm_trajectory_command",cmd);
+ node->publish("right_arm_trajectory_controller/arm_trajectory_command",cmd);
sleep(4);
ros::Time start_time = ros::Time::now();
Modified: pkg/trunk/demos/arm_gazebo/l_arm_trajectory_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_arm_trajectory_controller.xml 2009-02-06 00:37:50 UTC (rev 10633)
+++ pkg/trunk/demos/arm_gazebo/l_arm_trajectory_controller.xml 2009-02-06 00:43:44 UTC (rev 10634)
@@ -3,8 +3,8 @@
<controllers>
<!-- ========================================= -->
<!-- left arm array -->
- <controller name="left_arm_controller" type="ArmTrajectoryControllerNode">
- <listen_topic name="arm_trajectory_command" />
+ <controller name="left_arm_trajectory_controller" type="ArmTrajectoryControllerNode">
+ <listen_topic name="left_arm_trajectory_controller/arm_trajectory_command" />
<kinematics>
<elem key="kdl_chain_name">left_arm</elem>
</kinematics>
Modified: pkg/trunk/demos/arm_gazebo/r_arm_trajectory_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_trajectory_controller.xml 2009-02-06 00:37:50 UTC (rev 10633)
+++ pkg/trunk/demos/arm_gazebo/r_arm_trajectory_controller.xml 2009-02-06 00:43:44 UTC (rev 10634)
@@ -4,7 +4,7 @@
<!-- ========================================= -->
<!-- right arm array -->
<controller name="right_arm_trajectory_controller" type="ArmTrajectoryControllerNode">
- <listen_topic name="arm_trajectory_command" />
+ <listen_topic name="right_arm_trajectory_controller/arm_trajectory_command" />
<kinematics>
<elem key="kdl_chain_name">right_arm</elem>
</kinematics>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tpr...@us...> - 2009-02-06 01:03:16
|
Revision: 10644
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10644&view=rev
Author: tpratkanis
Date: 2009-02-06 01:03:13 +0000 (Fri, 06 Feb 2009)
Log Message:
-----------
PARALLEL_JOBS -> ROS_PARALLEL_JOBS
Modified Paths:
--------------
pkg/trunk/3rdparty/trex/make_trex
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/run-jam
pkg/trunk/highlevel/test_executive_trex_pr2/run-jam
Modified: pkg/trunk/3rdparty/trex/make_trex
===================================================================
--- pkg/trunk/3rdparty/trex/make_trex 2009-02-06 00:54:00 UTC (rev 10643)
+++ pkg/trunk/3rdparty/trex/make_trex 2009-02-06 01:03:13 UTC (rev 10644)
@@ -10,7 +10,7 @@
cd TREX
# Build the nddl jar file used to parse input files
-jam $PARALLEL_JOBS -q nddl.jar
+jam $ROS_PARALLEL_JOBS -q nddl.jar
if [ $? -ne 0 ] ; then
echo "Jam of nddl.jar failed."
@@ -19,9 +19,9 @@
# Build the debug version
if [ `uname` = Darwin ]; then
- jam $PARALLEL_JOBS -q libTREX_g.dylib
+ jam $ROS_PARALLEL_JOBS -q libTREX_g.dylib
else
- jam $PARALLEL_JOBS -q libTREX_g.so
+ jam $ROS_PARALLEL_JOBS -q libTREX_g.so
fi
if [ $? -ne 0 ] ; then
@@ -31,9 +31,9 @@
# Build the optimized version
if [ `uname` = Darwin ]; then
- jam $PARALLEL_JOBS -sVARIANTS=OPTIMIZED -q libTREX_o.dylib
+ jam $ROS_PARALLEL_JOBS -sVARIANTS=OPTIMIZED -q libTREX_o.dylib
else
- jam $PARALLEL_JOBS -sVARIANTS=OPTIMIZED -q libTREX_o.so
+ jam $ROS_PARALLEL_JOBS -sVARIANTS=OPTIMIZED -q libTREX_o.so
fi
if [ $? -ne 0 ] ; then
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/run-jam
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/run-jam 2009-02-06 00:54:00 UTC (rev 10643)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/run-jam 2009-02-06 01:03:13 UTC (rev 10644)
@@ -4,5 +4,5 @@
cd `rospack find executive_trex_pr2`
PLASMA_HOME=`rospack find trex`/PLASMA/
-jam $PARALLEL_JOBS exec_inputs
+jam $ROS_PARALLEL_JOBS exec_inputs
Modified: pkg/trunk/highlevel/test_executive_trex_pr2/run-jam
===================================================================
--- pkg/trunk/highlevel/test_executive_trex_pr2/run-jam 2009-02-06 00:54:00 UTC (rev 10643)
+++ pkg/trunk/highlevel/test_executive_trex_pr2/run-jam 2009-02-06 01:03:13 UTC (rev 10644)
@@ -4,5 +4,5 @@
cd `rospack find test_executive_trex_pr2`
PLASMA_HOME=`rospack find trex`/PLASMA/
-jam $PARALLEL_JOBS exec_test_inputs
+jam $ROS_PARALLEL_JOBS exec_test_inputs
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-02-06 03:55:26
|
Revision: 10676
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10676&view=rev
Author: rdiankov
Date: 2009-02-06 03:55:23 +0000 (Fri, 06 Feb 2009)
Log Message:
-----------
orEnvTriangulate should return one-indexed indices
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m
pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-02-06 03:37:33 UTC (rev 10675)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-02-06 03:55:23 UTC (rev 10676)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 645
+SVN_REVISION = -r 652
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
#SVN_PATCH = fini_patch.patch
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m 2009-02-06 03:37:33 UTC (rev 10675)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m 2009-02-06 03:55:23 UTC (rev 10676)
@@ -30,7 +30,7 @@
tripoints = reshape(res.points,[3 numpoints/3]);
numinds = length(res.indices);
- triindices = reshape(res.indices,[3 numinds/3]);
+ triindices = reshape(res.indices,[3 numinds/3])+1;
else
tripoints = [];
triindices = [];
Modified: pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp
===================================================================
--- pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp 2009-02-06 03:37:33 UTC (rev 10675)
+++ pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp 2009-02-06 03:55:23 UTC (rev 10676)
@@ -32,7 +32,6 @@
#include "opencv/cv.h"
#include "opencv/highgui.h"
#include "image_msgs/CvBridge.h"
-#include "image_msgs/StereoInfo.h"
#include "image_msgs/CamInfo.h"
#include "image_msgs/Image.h"
#include "checkerboard_detector/ObjectDetection.h"
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-02-06 14:13:38
|
Revision: 10679
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10679&view=rev
Author: rdiankov
Date: 2009-02-06 14:13:35 +0000 (Fri, 06 Feb 2009)
Log Message:
-----------
added image support for openraveros, added octave decoding of the Image message
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/manifest.xml
pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotSensorGetData.m
pkg/trunk/openrave_planning/openraveros/src/openraveros.h
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/openraveros/srv/robot_sensorgetdata.srv
Added Paths:
-----------
pkg/trunk/image_msgs/octave/
pkg/trunk/image_msgs/octave/image_msgs_processImage.m
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-02-06 07:25:58 UTC (rev 10678)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-02-06 14:13:35 UTC (rev 10679)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 652
+SVN_REVISION = -r 653
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
#SVN_PATCH = fini_patch.patch
include $(shell rospack find mk)/svn_checkout.mk
Added: pkg/trunk/image_msgs/octave/image_msgs_processImage.m
===================================================================
--- pkg/trunk/image_msgs/octave/image_msgs_processImage.m (rev 0)
+++ pkg/trunk/image_msgs/octave/image_msgs_processImage.m 2009-02-06 14:13:35 UTC (rev 10679)
@@ -0,0 +1,14 @@
+%% I = image_msgs_processImage(image)
+%%
+%% returns an image matrix given a image_msgs/Image
+function I = image_msgs_processImage(image)
+%res.camimage.layout
+%I = zeros([data.height data.width 3]);
+layout = eval(sprintf('image.%s_data.layout',image.depth));
+dimsizes = [];
+for i = 1:length(layout.dim)
+ dimsizes(i) = layout.dim{i}.size;
+end
+I = reshape(eval(sprintf('image.%s_data.data',image.depth)),dimsizes(end:-1:1));
+%% reverse the dimension order
+I = permute(I,length(dimsizes):-1:1);
Modified: pkg/trunk/openrave_planning/openraveros/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/openraveros/manifest.xml 2009-02-06 07:25:58 UTC (rev 10678)
+++ pkg/trunk/openrave_planning/openraveros/manifest.xml 2009-02-06 14:13:35 UTC (rev 10679)
@@ -8,6 +8,7 @@
<depend package="roscpp_sessions"/>
<depend package="openrave"/>
<depend package="std_msgs"/>
+ <depend package="image_msgs"/>
<depend package="rosoct"/>
<url>http://pr.willowgarage.com/wiki/openrave</url>
</package>
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2009-02-06 07:25:58 UTC (rev 10678)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2009-02-06 14:13:35 UTC (rev 10679)
@@ -48,6 +48,8 @@
rosoct_add_msgs('openraveros');
rosoct_add_srvs('openraveros');
+ addpath(fullfile(rosoct_findpackage('image_msgs'),'octave'));
+
rosoct('shutdown'); % restart the client
openraveros_initialized = 1;
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotSensorGetData.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotSensorGetData.m 2009-02-06 07:25:58 UTC (rev 10678)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotSensorGetData.m 2009-02-06 14:13:35 UTC (rev 10679)
@@ -12,6 +12,7 @@
% For image data
% data.KK - 3x3 intrinsic matrix
% data.T - 3x4 camera matrix (to project a point multiply by KK*inv(T))
+% data.P - 3x4 camera projection matrix
% data.I - the rgb image size(I) = [height width 3]
function data = orRobotSensorGetData(robotid, sensorindex)
@@ -37,11 +38,11 @@
numint = length(res.laserint);
data.laserint = reshape(res.laserint,[3 numint/3]);
case 'camera'
- error('camera not supported yet');
- data.KK = reshape(res.KK,[3 3]);
- data.T = reshape(res.T.m,[3 4]);
- data.rawimage = res.image;
- display('image decoding not implemented yet');
+ data.KK = reshape(res.caminfo.K,[3 3]);
+ data.P = reshape(res.caminfo.P,[3 4]);
+ data.T = inv([inv(data.KK)*data.P; 0 0 0 1]);
+ data.T = data.T(1:3,:);
+ data.I = image_msgs_processImage(res.camimage);
otherwise
error('unknown type')
end
Modified: pkg/trunk/openrave_planning/openraveros/src/openraveros.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/openraveros.h 2009-02-06 07:25:58 UTC (rev 10678)
+++ pkg/trunk/openrave_planning/openraveros/src/openraveros.h 2009-02-06 14:13:35 UTC (rev 10679)
@@ -74,7 +74,7 @@
#define stricmp strcasecmp
#endif
-inline std::wstring _ravembstowcs(const char* pstr)
+inline std::wstring __stdmbstowcs(const char* pstr)
{
size_t len = mbstowcs(NULL, pstr, 0);
std::wstring w; w.resize(len);
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2009-02-06 07:25:58 UTC (rev 10678)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2009-02-06 14:13:35 UTC (rev 10679)
@@ -26,6 +26,9 @@
#include "openraveros.h"
+#include "image_msgs/Image.h"
+#include "image_msgs/CamInfo.h"
+
class ROSServer : public RaveServerBase
{
class LockEnvironment
@@ -138,7 +141,7 @@
public:
SetControllerWorker(RobotBase* probot, const string& type, const string& cmd, bool& retval) : _probot(probot), _type(type), _cmd(cmd), _retval(retval) {}
virtual void work() {
- _retval = _probot->SetController(_ravembstowcs(_type.c_str()).c_str(), _cmd.c_str(), true);
+ _retval = _probot->SetController(__stdmbstowcs(_type.c_str()).c_str(), _cmd.c_str(), true);
}
private:
RobotBase* _probot;
@@ -766,7 +769,7 @@
bool env_getbody_srv(env_getbody::Request& req, env_getbody::Response& res)
{
- KinBody* pbody = GetEnv()->GetKinBody(_ravembstowcs(req.name.c_str()).c_str());
+ KinBody* pbody = GetEnv()->GetKinBody(__stdmbstowcs(req.name.c_str()).c_str());
if( pbody == NULL )
return false;
res.bodyid = pbody->GetNetworkId();
@@ -1279,8 +1282,8 @@
if( pbody == NULL || !pbody->IsRobot() )
return false;
+ LockEnvironment envlock(this);
RobotBase* probot = (RobotBase*)pbody;
-
if( req.sensorindex >= probot->GetSensors().size() )
return false;
@@ -1335,50 +1338,69 @@
}
case SensorBase::ST_Camera:
res.type = "camera";
- RAVELOG_ERRORA("camera data serialization not implemented yet!\n");
- ROS_ASSERT(0);
- break;
-// SensorBase::CameraSensorData* pcameradata = (SensorBase::CameraSensorData*)pdata.get();
-//
-// if( psensor->GetSensorGeometry()->GetType() != SensorBase::ST_Camera ) {
-// RAVELOG(L"sensor geometry not a camera type\n");
-// return false;
-// }
-//
-// SensorBase::CameraGeomData* pgeom = (SensorBase::CameraGeomData*)psensor->GetSensorGeometry();
-//
-// if( (int)pcameradata->vimagedata.size() != pgeom->width*pgeom->height*3 ) {
-// RAVELOG(L"image data wrong size %d != %d\n", pcameradata->vimagedata.size(), pgeom->width*pgeom->height*3);
-// return false;
-// }
-//
-// sout << pgeom->width << " " << pgeom->height << " ";
-// for(int i = 0; i < 4; ++i)
-// sout << pgeom->KK[i] << " ";
-// sout << TransformMatrix(pcameradata->t) << " ";
-// //FOREACH(it, pcameradata->vimagedata) sout << (int)*it << " ";
-//
-// // RLE encoding (about 3x faster than sending raw images)
-// int curvalue = 0, lastdiff = 0, lastvalue = 0xffffff&*(int*)&pcameradata->vimagedata[0];
-// list<int> difs, values;
-// for(int i = 1; i < (int)pcameradata->vimagedata.size()/3; ++i) {
-// curvalue = 0xffffff&*(int*)&pcameradata->vimagedata[3*i];
-// if( curvalue != lastvalue ) {
-// values.push_back(lastvalue);
-// difs.push_back(i-lastdiff);
-// lastdiff = i;
-// lastvalue = curvalue;
-// }
-// }
-// difs.push_back(pcameradata->vimagedata.size()/3-lastdiff);
-// values.push_back(curvalue);
-//
-// sout << values.size() << " ";
-// FOREACH(it, values)
-// sout << *it << " ";
-// sout << difs.size() << " ";
-// FOREACH(it, difs)
-// sout << *it << " ";
+ SensorBase::CameraSensorData* pcameradata = (SensorBase::CameraSensorData*)pdata.get();
+
+ if( psensor->GetSensorGeometry()->GetType() != SensorBase::ST_Camera ) {
+ RAVELOG(L"sensor geometry not a camera type\n");
+ return false;
+ }
+
+ SensorBase::CameraGeomData* pgeom = (SensorBase::CameraGeomData*)psensor->GetSensorGeometry();
+
+ if( (int)pcameradata->vimagedata.size() != pgeom->width*pgeom->height*3 ) {
+ RAVELOG(L"image data wrong size %d != %d\n", pcameradata->vimagedata.size(), pgeom->width*pgeom->height*3);
+ return false;
+ }
+
+ res.caminfo.width = pgeom->width;
+ res.caminfo.height = pgeom->height;
+ for(int i = 0; i < 5; ++i)
+ res.caminfo.D[i] = 0;
+ res.caminfo.K[0] = pgeom->KK[0]; res.caminfo.K[1] = 0; res.caminfo.K[2] = pgeom->KK[2];
+ res.caminfo.K[3] = 0; res.caminfo.K[4] = pgeom->KK[1]; res.caminfo.K[5] = pgeom->KK[3];
+ res.caminfo.K[6] = 0; res.caminfo.K[7] = 0; res.caminfo.K[8] = 1;
+
+ TransformMatrix tKK;
+ for(int i = 0; i < 3; ++i) {
+ tKK.m[4*i+0] = res.caminfo.K[3*i+0];
+ tKK.m[4*i+1] = res.caminfo.K[3*i+1];
+ tKK.m[4*i+2] = res.caminfo.K[3*i+2];
+ }
+
+ TransformMatrix tP = tKK * pcameradata->t.inverse();
+
+ res.caminfo.R[0] = 1; res.caminfo.R[1] = 0; res.caminfo.R[2] = 0;
+ res.caminfo.R[3] = 0; res.caminfo.R[4] = 1; res.caminfo.R[5] = 0;
+ res.caminfo.R[6] = 0; res.caminfo.R[7] = 0; res.caminfo.R[8] = 1;
+
+ for(int i = 0; i < 3; ++i) {
+ res.caminfo.P[4*i+0] = tP.m[4*i+0];
+ res.caminfo.P[4*i+1] = tP.m[4*i+1];
+ res.caminfo.P[4*i+2] = tP.m[4*i+2];
+ res.caminfo.P[4*i+3] = tP.trans[i];
+ }
+
+ res.camimage.header.stamp = ros::Time::now();
+ res.camimage.header.seq = pcameradata->id;
+ res.camimage.label = _stdwcstombs(probot->GetSensors()[req.sensorindex].GetName());
+ res.camimage.encoding = "rgb";
+ res.camimage.depth = "uint8";
+ std_msgs::MultiArrayLayout& layout = res.camimage.uint8_data.layout;
+
+ int nchannels = 3;
+ layout.dim.resize(3);
+ layout.dim.resize(3);
+ layout.dim[0].label = "height";
+ layout.dim[0].size = pgeom->height;
+ layout.dim[0].stride = pgeom->width*pgeom->height*nchannels;
+ layout.dim[1].label = "width";
+ layout.dim[1].size = pgeom->width;
+ layout.dim[1].stride = pgeom->width*nchannels;
+ layout.dim[2].label = "channel";
+ layout.dim[2].size = nchannels;
+ layout.dim[2].stride = nchannels;
+ res.camimage.uint8_data.data.resize(layout.dim[0].stride);
+ memcpy(&res.camimage.uint8_data.data[0], &pcameradata->vimagedata[0], res.camimage.uint8_data.data.size());
}
return true;
@@ -1405,7 +1427,7 @@
return false;
}
- stringstream ss(req.args);
+ stringstream ss; ss << req.cmd << " " << req.args;
stringstream sout;
bool bsuccess = false;
AddWorker(new SendCmdSensorWorker(sensor.GetSensor(),ss,sout,bsuccess), true);
Modified: pkg/trunk/openrave_planning/openraveros/srv/robot_sensorgetdata.srv
===================================================================
--- pkg/trunk/openrave_planning/openraveros/srv/robot_sensorgetdata.srv 2009-02-06 07:25:58 UTC (rev 10678)
+++ pkg/trunk/openrave_planning/openraveros/srv/robot_sensorgetdata.srv 2009-02-06 14:13:35 UTC (rev 10679)
@@ -19,12 +19,5 @@
float32[] laserint
## camera
-
-# data.KK - 3x3 intrinsic matrix
-float32[9] KK
-
-# data.T - 3x4 camera matrix (to project a point multiply by KK*inv(T))
-AffineTransformMatrix T
-
-# data.I - the rgb image size(I) = [height width 3]
-std_msgs/Image image
+image_msgs/CamInfo caminfo # camera calibration parameters and where camera is
+image_msgs/Image camimage # camera image
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-02-06 19:03:50
|
Revision: 10682
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10682&view=rev
Author: gerkey
Date: 2009-02-06 19:03:46 +0000 (Fri, 06 Feb 2009)
Log Message:
-----------
Modified rospack invocation to match new syntax, #861 in ros.
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/visualization/ogre_tools/CMakeLists.txt
pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt
pkg/trunk/visualization/wx_camera_panel/CMakeLists.txt
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-02-06 18:35:56 UTC (rev 10681)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-02-06 19:03:46 UTC (rev 10682)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 653
+SVN_REVISION = -r 654
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
#SVN_PATCH = fini_patch.patch
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/visualization/ogre_tools/CMakeLists.txt
===================================================================
--- pkg/trunk/visualization/ogre_tools/CMakeLists.txt 2009-02-06 18:35:56 UTC (rev 10681)
+++ pkg/trunk/visualization/ogre_tools/CMakeLists.txt 2009-02-06 19:03:46 UTC (rev 10682)
@@ -49,7 +49,7 @@
include_directories( ${OPENGL_INCLUDE_DIR} )
# Find the combined swig flags for this project
-_rospack_invoke(${PROJECT_NAME} ${PROJECT_NAME} SWIG_FLAGS "--lang=swig" "--attrib=flags" "export")
+_rospack_invoke(${PROJECT_NAME} ${PROJECT_NAME} SWIG_FLAGS "export" "--lang=swig" "--attrib=flags")
set(SWIG_FLAGS ${${PROJECT_NAME}_SWIG_FLAGS})
# Find the wxswig executable
Modified: pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt 2009-02-06 18:35:56 UTC (rev 10681)
+++ pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt 2009-02-06 19:03:46 UTC (rev 10682)
@@ -27,7 +27,7 @@
include_directories( src/ogre_visualizer )
# Find the combined swig flags for this project
-_rospack_invoke(${PROJECT_NAME} ${PROJECT_NAME} SWIG_FLAGS "--lang=swig" "--attrib=flags" "export")
+_rospack_invoke(${PROJECT_NAME} ${PROJECT_NAME} SWIG_FLAGS "export" "--lang=swig" "--attrib=flags")
set(SWIG_FLAGS ${${PROJECT_NAME}_SWIG_FLAGS})
# Find the wxswig executable
Modified: pkg/trunk/visualization/wx_camera_panel/CMakeLists.txt
===================================================================
--- pkg/trunk/visualization/wx_camera_panel/CMakeLists.txt 2009-02-06 18:35:56 UTC (rev 10681)
+++ pkg/trunk/visualization/wx_camera_panel/CMakeLists.txt 2009-02-06 19:03:46 UTC (rev 10682)
@@ -15,7 +15,7 @@
include_directories(${PYTHON_INCLUDE_PATH})
# Find the combined swig flags for this project
-_rospack_invoke(${PROJECT_NAME} ${PROJECT_NAME} SWIG_FLAGS "--lang=swig" "--attrib=flags" "export")
+_rospack_invoke(${PROJECT_NAME} ${PROJECT_NAME} SWIG_FLAGS "export" "--lang=swig" "--attrib=flags")
set(SWIG_FLAGS ${${PROJECT_NAME}_SWIG_FLAGS})
# Find the wxswig executable
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2009-02-07 00:40:42
|
Revision: 10729
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10729&view=rev
Author: rob_wheeler
Date: 2009-02-07 00:40:40 +0000 (Sat, 07 Feb 2009)
Log Message:
-----------
Add launch script that switches to realtime interface prior to launching
pr2_etherCAT.
Modified Paths:
--------------
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch
Added Paths:
-----------
pkg/trunk/robot_control_loops/pr2_etherCAT/scripts/launch
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2009-02-07 00:38:39 UTC (rev 10728)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2009-02-07 00:40:40 UTC (rev 10729)
@@ -81,7 +81,7 @@
ROS_BREAK();
}
-#if 0
+#if 1
if (set_socket_timeout(ni_, 1000*500))
{
ROS_FATAL("Unable to change socket timeout");
Added: pkg/trunk/robot_control_loops/pr2_etherCAT/scripts/launch
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/scripts/launch (rev 0)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/scripts/launch 2009-02-07 00:40:40 UTC (rev 10729)
@@ -0,0 +1,14 @@
+#!/bin/sh
+
+ifconfig eth0 > /dev/null 2>&1
+if [ "$?" = "0" ]; then
+ echo "In non-realtime mode"
+ echo "Switching to realtime"
+ /usr/local/rtnet/sbin/rtup
+ sleep 5
+else
+ echo "In realtime mode"
+fi
+
+echo "Launching pr2_etherCAT [$PWD] with options $*"
+../pr2_etherCAT $*
Property changes on: pkg/trunk/robot_control_loops/pr2_etherCAT/scripts/launch
___________________________________________________________________
Added: svn:executable
+ *
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch 2009-02-07 00:38:39 UTC (rev 10728)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch 2009-02-07 00:40:40 UTC (rev 10729)
@@ -6,7 +6,7 @@
<include file="pre.machine" />
<!-- pr2_etherCAT -->
- <node machine="xenomai_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i rteth0 -x /robotdesc/pr2"/>
+ <node machine="xenomai_root" pkg="pr2_etherCAT" type="launch" args="-i rteth0 -x /robotdesc/pr2"/>
<!-- PR2 Calibration -->
<include file="$(find pr2_prototype1)/calibrate.launch" />
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-02-07 00:38:39 UTC (rev 10728)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-02-07 00:40:40 UTC (rev 10729)
@@ -6,7 +6,7 @@
<include file="prf.machine" />
<!-- pr2_etherCAT -->
- <node machine="xenomai_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i rteth0 -x /robotdesc/pr2"/>
+ <node machine="xenomai_root" pkg="pr2_etherCAT" type="launch" args="-i rteth0 -x /robotdesc/pr2"/>
<!-- PR2 Calibration -->
<include file="$(find pr2_full)/calibrate.launch" />
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch 2009-02-07 00:38:39 UTC (rev 10728)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch 2009-02-07 00:40:40 UTC (rev 10729)
@@ -6,7 +6,7 @@
<include file="prg.machine" />
<!-- pr2_etherCAT -->
- <node machine="xenomai_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i rteth0 -x /robotdesc/pr2"/>
+ <node machine="xenomai_root" pkg="pr2_etherCAT" type="launch" args="-i rteth0 -x /robotdesc/pr2"/>
<!-- PR2 Calibration -->
<include file="$(find pr2_prototype1)/calibrate.launch" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-02-07 01:24:35
|
Revision: 10733
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10733&view=rev
Author: isucan
Date: 2009-02-07 01:24:30 +0000 (Sat, 07 Feb 2009)
Log Message:
-----------
cleaned up planning_models (probably not sufficient, yet; added tests)
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPSpaceInformation.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_replan_to_state.cpp
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp
pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
Added Paths:
-----------
pkg/trunk/world_models/robot_models/planning_models/test/
pkg/trunk/world_models/robot_models/planning_models/test/test_kinematic.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h 2009-02-07 01:22:56 UTC (rev 10732)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequest.h 2009-02-07 01:24:30 UTC (rev 10733)
@@ -152,7 +152,7 @@
/* extract the components needed for the start state of the desired group */
for (unsigned int i = 0 ; i < dim ; ++i)
- start->values[i] = start_state.vals[model->kmodel->groupStateIndexList[model->groupID][i]];
+ start->values[i] = start_state.vals[model->kmodel->getModelInfo().groupStateIndexList[model->groupID][i]];
}
else
{
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-02-07 01:22:56 UTC (rev 10732)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-02-07 01:24:30 UTC (rev 10733)
@@ -147,9 +147,9 @@
RKPModel *m = models[req.params.model_id];
- if (m->kmodel->stateDimension != req.start_state.get_vals_size())
+ if (m->kmodel->getModelInfo().stateDimension != req.start_state.get_vals_size())
{
- ROS_ERROR("Dimension of start state expected to be %d but was received as %d", m->kmodel->stateDimension, req.start_state.get_vals_size());
+ ROS_ERROR("Dimension of start state expected to be %d but was received as %d", m->kmodel->getModelInfo().stateDimension, req.start_state.get_vals_size());
return false;
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h 2009-02-07 01:22:56 UTC (rev 10732)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestState.h 2009-02-07 01:24:30 UTC (rev 10733)
@@ -62,9 +62,9 @@
RKPModel *m = models[req.params.model_id];
RKPPlannerSetup *psetup = m->planners[req.params.planner_id];
- if (m->kmodel->stateDimension != req.start_state.get_vals_size())
+ if (m->kmodel->getModelInfo().stateDimension != req.start_state.get_vals_size())
{
- ROS_ERROR("Dimension of start state expected to be %d but was received as %d", m->kmodel->stateDimension, req.start_state.get_vals_size());
+ ROS_ERROR("Dimension of start state expected to be %d but was received as %d", m->kmodel->getModelInfo().stateDimension, req.start_state.get_vals_size());
return false;
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPSpaceInformation.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPSpaceInformation.h 2009-02-07 01:22:56 UTC (rev 10732)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPSpaceInformation.h 2009-02-07 01:24:30 UTC (rev 10733)
@@ -56,28 +56,28 @@
m_divisions = divisions;
/* compute the state space for this group */
- m_stateDimension = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID].size() : m_kmodel->stateDimension;
+ m_stateDimension = m_groupID >= 0 ? m_kmodel->getModelInfo().groupStateIndexList[m_groupID].size() : m_kmodel->getModelInfo().stateDimension;
m_stateComponent.resize(m_stateDimension);
for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
{
if (m_stateComponent[i].type == StateComponent::UNKNOWN)
m_stateComponent[i].type = StateComponent::NORMAL;
- int p = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID][i] * 2 : i * 2;
- m_stateComponent[i].minValue = m_kmodel->stateBounds[p ];
- m_stateComponent[i].maxValue = m_kmodel->stateBounds[p + 1];
+ int p = m_groupID >= 0 ? m_kmodel->getModelInfo().groupStateIndexList[m_groupID][i] * 2 : i * 2;
+ m_stateComponent[i].minValue = m_kmodel->getModelInfo().stateBounds[p ];
+ m_stateComponent[i].maxValue = m_kmodel->getModelInfo().stateBounds[p + 1];
m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / m_divisions;
- for (unsigned int j = 0 ; j < m_kmodel->floatingJoints.size() ; ++j)
- if (m_kmodel->floatingJoints[j] == p)
+ for (unsigned int j = 0 ; j < m_kmodel->getModelInfo().floatingJoints.size() ; ++j)
+ if (m_kmodel->getModelInfo().floatingJoints[j] == p)
{
m_floatingJoints.push_back(i);
m_stateComponent[i + 3].type = StateComponent::QUATERNION;
break;
}
- for (unsigned int j = 0 ; j < m_kmodel->planarJoints.size() ; ++j)
- if (m_kmodel->planarJoints[j] == p)
+ for (unsigned int j = 0 ; j < m_kmodel->getModelInfo().planarJoints.size() ; ++j)
+ if (m_kmodel->getModelInfo().planarJoints[j] == p)
{
m_planarJoints.push_back(i);
break;
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-07 01:22:56 UTC (rev 10732)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-07 01:24:30 UTC (rev 10733)
@@ -231,14 +231,6 @@
delete i->second;
}
- void currentState(robot_msgs::KinematicState &state)
- {
- state.set_vals_size(m_kmodel->stateDimension);
- const double *params = m_robotState->getParams();
- for (unsigned int i = 0 ; i < state.get_vals_size() ; ++i)
- state.vals[i] = params[i];
- }
-
bool isSafeToPlan(void)
{
if (!isMapUpdated(m_intervalCollisionMap))
@@ -317,6 +309,141 @@
}
}
+ bool replanToPosition(robot_srvs::KinematicReplanLinkPosition::Request &req, robot_srvs::KinematicReplanLinkPosition::Response &res)
+ {
+ ROS_INFO("Request for replanning to a position");
+ bool st = false;
+ res.id = -1;
+
+ stopReplanning();
+
+ if (m_robotState)
+ {
+ // back up the request
+ m_currentPlanToPositionRequest = req.value;
+
+ // start planning thread
+ m_replanningLock.lock();
+ m_currentRequestType = R_POSITION;
+
+ m_currentPlanStatus.id = ++m_replanID;
+ m_currentPlanStatus.valid = 1;
+ m_currentPlanStatus.path.set_states_size(0);
+ m_currentPlanStatus.done = 0;
+ m_currentPlanStatus.distance = -1.0;
+ res.id = m_currentPlanStatus.id;
+ m_statusLock.unlock();
+
+ ROS_INFO("Start replanning with plan id %d", res.id);
+ m_replanningThread = new boost::thread(boost::bind(&KinematicPlanning::replanToPositionThread, this));
+ m_replanningLock.unlock();
+
+ st = true;
+ }
+ else
+ ROS_ERROR("Current robot state is unknown. Cannot start replanning.");
+
+ return st;
+ }
+
+ bool planToState(robot_srvs::KinematicPlanState::Request &req, robot_srvs::KinematicPlanState::Response &res)
+ {
+ ROS_INFO("Request for planning to a state");
+ bool trivial = false;
+ if (req.value.start_state.get_vals_size() == 0)
+ {
+ currentState(req.value.start_state);
+ ROS_INFO("Using current state as starting point");
+ }
+
+ bool result = false;
+
+ res.value.unsafe = isSafeToPlan() ? 0 : 1;
+ result = m_requestState.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
+ res.value.id = -1;
+ res.value.done = trivial ? 1 : 0;
+ res.value.valid = res.value.path.get_states_size() > 0;
+
+ return result;
+ }
+
+ bool planToPosition(robot_srvs::KinematicPlanLinkPosition::Request &req, robot_srvs::KinematicPlanLinkPosition::Response &res)
+ {
+ ROS_INFO("Request for planning to a position");
+ bool trivial = false;
+ if (req.value.start_state.get_vals_size() == 0)
+ {
+ currentState(req.value.start_state);
+ ROS_INFO("Using current state as starting point");
+ }
+
+ bool result = false;
+
+ res.value.unsafe = isSafeToPlan() ? 0 : 1;
+ result = m_requestLinkPosition.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
+
+ res.value.id = -1;
+ res.value.done = trivial ? 1 : 0;
+ res.value.valid = res.value.path.get_states_size() > 0;
+
+ return result;
+ }
+
+ virtual void setRobotDescription(robot_desc::URDF *file)
+ {
+ CollisionSpaceMonitor::setRobotDescription(file);
+ defaultPosition();
+
+ ROS_INFO("=======================================");
+ std::stringstream ss;
+ m_kmodel->printModelInfo(ss);
+ ROS_INFO("%s", ss.str().c_str());
+ ROS_INFO("=======================================");
+
+ /* set the data for the model */
+ RKPModel *model = new RKPModel();
+ model->collisionSpaceID = 0;
+ model->collisionSpace = m_collisionSpace;
+ model->kmodel = m_kmodel;
+ model->groupName = m_kmodel->getModelName();
+ createMotionPlanningInstances(model);
+
+ /* remember the model by the robot's name */
+ m_models[model->groupName] = model;
+
+ /* create a model for each group */
+ std::vector<std::string> groups;
+ m_kmodel->getGroups(groups);
+
+ for (unsigned int i = 0 ; i < groups.size() ; ++i)
+ {
+ RKPModel *model = new RKPModel();
+ model->collisionSpaceID = 0;
+ model->collisionSpace = m_collisionSpace;
+ model->kmodel = m_kmodel;
+ model->groupID = m_kmodel->getGroupID(groups[i]);
+ model->groupName = groups[i];
+ createMotionPlanningInstances(model);
+ m_models[model->groupName] = model;
+ }
+ }
+
+ void currentState(robot_msgs::KinematicState &state)
+ {
+ state.set_vals_size(m_kmodel->getModelInfo().stateDimension);
+ const double *params = m_robotState->getParams();
+ for (unsigned int i = 0 ; i < state.get_vals_size() ; ++i)
+ state.vals[i] = params[i];
+ }
+
+ void knownModels(std::vector<std::string> &model_ids)
+ {
+ for (std::map<std::string, RKPModel*>::const_iterator i = m_models.begin() ; i != m_models.end() ; ++i)
+ model_ids.push_back(i->first);
+ }
+
+protected:
+
void publishStatus(void)
{
double seconds;
@@ -438,64 +565,6 @@
return st;
}
- bool replanToPosition(robot_srvs::KinematicReplanLinkPosition::Request &req, robot_srvs::KinematicReplanLinkPosition::Response &res)
- {
- ROS_INFO("Request for replanning to a position");
- bool st = false;
- res.id = -1;
-
- stopReplanning();
-
- if (m_robotState)
- {
- // back up the request
- m_currentPlanToPositionRequest = req.value;
-
- // start planning thread
- m_replanningLock.lock();
- m_currentRequestType = R_POSITION;
-
- m_currentPlanStatus.id = ++m_replanID;
- m_currentPlanStatus.valid = 1;
- m_currentPlanStatus.path.set_states_size(0);
- m_currentPlanStatus.done = 0;
- m_currentPlanStatus.distance = -1.0;
- res.id = m_currentPlanStatus.id;
- m_statusLock.unlock();
-
- ROS_INFO("Start replanning with plan id %d", res.id);
- m_replanningThread = new boost::thread(boost::bind(&KinematicPlanning::replanToPositionThread, this));
- m_replanningLock.unlock();
-
- st = true;
- }
- else
- ROS_ERROR("Current robot state is unknown. Cannot start replanning.");
-
- return st;
- }
-
- bool planToState(robot_srvs::KinematicPlanState::Request &req, robot_srvs::KinematicPlanState::Response &res)
- {
- ROS_INFO("Request for planning to a state");
- bool trivial = false;
- if (req.value.start_state.get_vals_size() == 0)
- {
- currentState(req.value.start_state);
- ROS_INFO("Using current state as starting point");
- }
-
- bool result = false;
-
- res.value.unsafe = isSafeToPlan() ? 0 : 1;
- result = m_requestState.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
- res.value.id = -1;
- res.value.done = trivial ? 1 : 0;
- res.value.valid = res.value.path.get_states_size() > 0;
-
- return result;
- }
-
/** Wait for a change in the environment and recompute the motion plan */
void replanToStateThread(void)
{
@@ -531,28 +600,6 @@
}
}
- bool planToPosition(robot_srvs::KinematicPlanLinkPosition::Request &req, robot_srvs::KinematicPlanLinkPosition::Response &res)
- {
- ROS_INFO("Request for planning to a position");
- bool trivial = false;
- if (req.value.start_state.get_vals_size() == 0)
- {
- currentState(req.value.start_state);
- ROS_INFO("Using current state as starting point");
- }
-
- bool result = false;
-
- res.value.unsafe = isSafeToPlan() ? 0 : 1;
- result = m_requestLinkPosition.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
-
- res.value.id = -1;
- res.value.done = trivial ? 1 : 0;
- res.value.valid = res.value.path.get_states_size() > 0;
-
- return result;
- }
-
/** Wait for a change in the environment and recompute the motion plan */
void replanToPositionThread(void)
{
@@ -635,51 +682,6 @@
m_collisionMonitorCondition.notify_all();
}
- virtual void setRobotDescription(robot_desc::URDF *file)
- {
- CollisionSpaceMonitor::setRobotDescription(file);
- defaultPosition();
-
- ROS_INFO("=======================================");
- std::stringstream ss;
- m_kmodel->printModelInfo(ss);
- ROS_INFO("%s", ss.str().c_str());
- ROS_INFO("=======================================");
-
- /* set the data for the model */
- RKPModel *model = new RKPModel();
- model->collisionSpaceID = 0;
- model->collisionSpace = m_collisionSpace;
- model->kmodel = m_kmodel;
- model->groupName = m_kmodel->name;
- createMotionPlanningInstances(model);
-
- /* remember the model by the robot's name */
- m_models[model->groupName] = model;
-
- /* create a model for each group */
- std::vector<std::string> groups;
- m_kmodel->getGroups(groups);
-
- for (unsigned int i = 0 ; i < groups.size() ; ++i)
- {
- RKPModel *model = new RKPModel();
- model->collisionSpaceID = 0;
- model->collisionSpace = m_collisionSpace;
- model->kmodel = m_kmodel;
- model->groupID = m_kmodel->getGroupID(groups[i]);
- model->groupName = groups[i];
- createMotionPlanningInstances(model);
- m_models[model->groupName] = model;
- }
- }
-
- void knownModels(std::vector<std::string> &model_ids)
- {
- for (std::map<std::string, RKPModel*>::const_iterator i = m_models.begin() ; i != m_models.end() ; ++i)
- model_ids.push_back(i->first);
- }
-
private:
/* instantiate the planners that can be used */
Modified: pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-02-07 01:22:56 UTC (rev 10732)
+++ pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-02-07 01:24:30 UTC (rev 10733)
@@ -142,9 +142,9 @@
myModel *model = m_models[req.model_id];
if (model)
{
- if (model->kmodel->stateDimension != req.start_state.get_vals_size())
+ if (model->kmodel->getModelInfo().stateDimension != req.start_state.get_vals_size())
{
- ROS_ERROR("Dimension of start state expected to be %d but was received as %d", model->kmodel->stateDimension, req.start_state.get_vals_size());
+ ROS_ERROR("Dimension of start state expected to be %d but was received as %d", model->kmodel->getModelInfo().stateDimension, req.start_state.get_vals_size());
return false;
}
@@ -167,7 +167,7 @@
/* extract the components needed for the start state of the desired group */
for (unsigned int i = 0 ; i < dim ; ++i)
- start->values[i] = req.start_state.vals[model->kmodel->groupStateIndexList[model->groupID][i]];
+ start->values[i] = req.start_state.vals[model->kmodel->getModelInfo().groupStateIndexList[model->groupID][i]];
}
else
{
@@ -215,7 +215,7 @@
model->collisionSpaceID = 0;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
- model->groupName = m_kmodel->name;
+ model->groupName = m_kmodel->getModelName();
setupModel(model);
/* remember the model by the robot's name */
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-02-07 01:22:56 UTC (rev 10732)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-02-07 01:24:30 UTC (rev 10733)
@@ -217,7 +217,7 @@
// get the current state from the StateParams instance monitored by the KinematicStateMonitor
void currentState(robot_msgs::KinematicState &state)
{
- state.set_vals_size(m_kmodel->stateDimension);
+ state.set_vals_size(m_kmodel->getModelInfo().stateDimension);
for (unsigned int i = 0 ; i < state.get_vals_size() ; ++i)
state.vals[i] = m_robotState->getParams()[i];
}
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp 2009-02-07 01:22:56 UTC (rev 10732)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp 2009-02-07 01:24:30 UTC (rev 10733)
@@ -159,7 +159,7 @@
// get the current state from the StateParams instance monitored by the KinematicStateMonitor
void currentState(robot_msgs::KinematicState &state)
{
- state.set_vals_size(m_kmodel->stateDimension);
+ state.set_vals_size(m_kmodel->getModelInfo().stateDimension);
for (unsigned int i = 0 ; i < state.get_vals_size() ; ++i)
state.vals[i] = m_robotState->getParams()[i];
}
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_replan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_replan_to_state.cpp 2009-02-07 01:22:56 UTC (rev 10732)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_replan_to_state.cpp 2009-02-07 01:24:30 UTC (rev 10733)
@@ -151,7 +151,7 @@
// get the current state from the StateParams instance monitored by the KinematicStateMonitor
void currentState(robot_msgs::KinematicState &state)
{
- state.set_vals_size(m_kmodel->stateDimension);
+ state.set_vals_size(m_kmodel->getModelInfo().stateDimension);
for (unsigned int i = 0 ; i < state.get_vals_size() ; ++i)
state.vals[i] = m_robotState->getParams()[i];
}
Modified: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-02-07 01:22:56 UTC (rev 10732)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-02-07 01:24:30 UTC (rev 10733)
@@ -77,7 +77,7 @@
int collision_space::EnvironmentModel::getModelID(const std::string& robot_name) const
{
for (unsigned int i = 0 ; i < m_models.size() ; ++i)
- if (m_models[i]->name == robot_name)
+ if (m_models[i]->getModelName() == robot_name)
return i;
return -1;
}
Modified: pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt 2009-02-07 01:22:56 UTC (rev 10732)
+++ pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt 2009-02-07 01:24:30 UTC (rev 10733)
@@ -2,3 +2,7 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(planning_models)
rospack_add_library(planning_models src/kinematic.cpp)
+
+# Unit tests
+rospack_add_gtest(test_kinematic test/test_kinematic.cpp)
+target_link_libraries(test_kinematic planning_models)
Modified: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-07 01:22:56 UTC (rev 10732)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-07 01:24:30 UTC (rev 10733)
@@ -150,8 +150,8 @@
/** Name of the joint */
std::string name;
- /** The model that owns this joint */
- KinematicModel *owner;
+ /** The robot that owns this joint */
+ Robot *owner;
/** the links that this joint connects */
Link *before;
@@ -338,7 +338,7 @@
std::string name;
/** The model that owns this link */
- KinematicModel *owner;
+ Robot *owner;
/** Joint that connects this link to the parent link */
Joint *before;
@@ -378,15 +378,14 @@
};
/** A robot structure */
- class Robot
+ struct Robot
{
- public:
-
Robot(KinematicModel *model)
{
owner = model;
chain = NULL;
stateDimension = 0;
+ rootTransform.setIdentity();
}
virtual ~Robot(void)
@@ -395,10 +394,14 @@
delete chain;
}
- void computeTransforms(const double *params, int groupID = -1);
-
+ /** Add transforms to the rootTransform such that the robot is in its planar/floating link frame */
+ bool reduceToRobotFrame(void);
+
/** The model that owns this robot */
KinematicModel *owner;
+
+ /** A transform that is applied to the entire robot */
+ btTransform rootTransform;
/** List of links in the robot */
std::vector<Link*> links;
@@ -440,20 +443,42 @@
std::vector<std::vector<Joint*> > groupChainStart;
};
+
+ /** State information */
+ struct ModelInfo
+ {
+ /** Cumulative list of floating joints */
+ std::vector<int> floatingJoints;
+
+ /** Cumulative list of planar joints */
+ std::vector<int> planarJoints;
+
+ /** Cumulative state dimension */
+ unsigned int stateDimension;
+
+ /** Cumulative state bounds */
+ std::vector<double> stateBounds;
+
+ /** A map defining the parameter names in the complete state */
+ std::map<std::string, unsigned int> parameterIndex;
+ std::map<unsigned int, std::string> parameterName;
+
+ /** Cumulative index list */
+ std::vector< std::vector<unsigned int> > groupStateIndexList;
+
+ /** Cumulative list of group roots */
+ std::vector< std::vector<Joint*> > groupChainStart;
+ };
/** A class that can hold the named parameters of this planning model */
class StateParams
{
public:
- StateParams(KinematicModel *model)
+ StateParams(KinematicModel *model) : m_owner(model), m_mi(model->getModelInfo())
{
assert(model->isBuilt());
- m_owner = model;
- m_dim = m_owner->stateDimension > 0 ? m_owner->stateDimension : 0;
- m_params = m_dim > 0 ? new double[m_dim] : NULL;
- m_pos = m_owner->parameterNames;
- m_name = m_owner->parameterValues;
+ m_params = m_mi.stateDimension > 0 ? new double[m_mi.stateDimension] : NULL;
setAll(0);
reset();
}
@@ -511,18 +536,15 @@
protected:
KinematicModel *m_owner;
- unsigned int m_dim;
+ ModelInfo &m_mi;
double *m_params;
- std::map<std::string, unsigned int> m_pos;
- std::map<unsigned int, std::string> m_name;
std::map<unsigned int, bool> m_seen;
};
KinematicModel(void)
{
- rootTransform.setIdentity();
- stateDimension = 0;
+ m_mi.stateDimension = 0;
m_ignoreSensors = false;
m_verbose = false;
m_built = false;
@@ -534,13 +556,15 @@
delete m_robots[i];
}
+ void build(const std::string &description, bool ignoreSensors = false);
virtual void build(const robot_desc::URDF &model, bool ignoreSensors = false);
bool isBuilt(void) const;
StateParams* newStateParams(void);
void setVerbose(bool verbose);
-
+ const std::string& getModelName(void) const;
+
unsigned int getRobotCount(void) const;
Robot* getRobot(unsigned int index) const;
@@ -571,36 +595,15 @@
void printModelInfo(std::ostream &out = std::cout);
void printLinkPoses(std::ostream &out = std::cout) const;
- /** The name of the model */
- std::string name;
-
- /** A transform that is applied to the entire model */
- btTransform rootTransform;
+ ModelInfo& getModelInfo(void);
+ const ModelInfo& getModelInfo(void) const;
- /** Cumulative list of floating joints */
- std::vector<int> floatingJoints;
-
- /** Cumulative list of planar joints */
- std::vector<int> planarJoints;
+ protected:
- /** Cumulative state dimension */
- unsigned int stateDimension;
+ /** The name of the model */
+ std::string m_name;
+ ModelInfo m_mi;
- /** Cumulative state bounds */
- std::vector<double> stateBounds;
-
- /** A map defining the parameter names in the complete state */
- std::map<std::string, unsigned int> parameterNames;
- std::map<unsigned int, std::string> parameterValues;
-
- /** Cumulative index list */
- std::vector< std::vector<unsigned int> > groupStateIndexList;
-
- /** Cumulative list of group roots */
- std::vector< std::vector<Joint*> > groupChainStart;
-
- protected:
-
std::vector<Robot*> m_robots;
std::vector<std::string> m_groups;
std::map<std::string, int> m_groupsMap;
Modified: pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-02-07 01:22:56 UTC (rev 10732)
+++ pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-02-07 01:24:30 UTC (rev 10733)
@@ -48,17 +48,31 @@
}
};
+const std::string& planning_models::KinematicModel::getModelName(void) const
+{
+ return m_name;
+}
+const planning_models::KinematicModel::ModelInfo& planning_models::KinematicModel::getModelInfo(void) const
+{
+ return m_mi;
+}
+
+planning_models::KinematicModel::ModelInfo& planning_models::KinematicModel::getModelInfo(void)
+{
+ return m_mi;
+}
+
void planning_models::KinematicModel::defaultState(void)
{
/* The default state of the robot. Place each value at 0.0, if
within bounds. Otherwise, select middle point. */
- double params[stateDimension];
- for (unsigned int i = 0 ; i < stateDimension ; ++i)
- if (stateBounds[2 * i] <= 0.0 && stateBounds[2 * i + 1] >= 0.0)
+ double params[m_mi.stateDimension];
+ for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+ if (m_mi.stateBounds[2 * i] <= 0.0 && m_mi.stateBounds[2 * i + 1] >= 0.0)
params[i] = 0.0;
else
- params[i] = (stateBounds[2 * i] + stateBounds[2 * i + 1]) / 2.0;
+ params[i] = (m_mi.stateBounds[2 * i] + m_mi.stateBounds[2 * i + 1]) / 2.0;
computeTransforms(params);
}
@@ -69,9 +83,9 @@
if (groupID >= 0)
{
- for (unsigned int i = 0 ; i < groupChainStart[groupID].size() ; ++i)
+ for (unsigned int i = 0 ; i < m_mi.groupChainStart[groupID].size() ; ++i)
{
- Joint *start = groupChainStart[groupID][i];
+ Joint *start = m_mi.groupChainStart[groupID][i];
params = start->computeTransform(params, groupID);
}
}
@@ -85,19 +99,15 @@
}
}
-void planning_models::KinematicModel::Robot::computeTransforms(const double *params, int groupID)
-{
- chain->computeTransform(params, groupID);
-}
-
void planning_models::KinematicModel::computeParameterNames(void)
{
- parameterNames.clear();
-
+ m_mi.parameterIndex.clear();
+ unsigned int pos = 0;
for (unsigned int i = 0 ; i < m_robots.size(); ++i)
{
Joint *start = m_robots[i]->chain;
- start->computeParameterNames(0);
+ start->computeParameterNames(pos);
+ pos += m_robots[i]->stateDimension;
}
}
@@ -115,7 +125,7 @@
}
const double* planning_models::KinematicModel::Link::computeTransform(const double *params, int groupID)
-{
+{
...
[truncated message content] |
|
From: <rdi...@us...> - 2009-02-07 04:11:20
|
Revision: 10746
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10746&view=rev
Author: rdiankov
Date: 2009-02-07 04:11:16 +0000 (Sat, 07 Feb 2009)
Log Message:
-----------
fixed openrave_robot_description parsing
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-02-07 03:35:24 UTC (rev 10745)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-02-07 04:11:16 UTC (rev 10746)
@@ -13,7 +13,7 @@
BOOST_LIBRARY_DIRS=$(shell rosboost-cfg --lib_dirs)
installed: $(SVN_DIR) patched
- @cd $(SVN_DIR) && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && export CPATH="$(shell rospack find bullet)/include:$(CPATH)" && export LD_LIBRARY_PATH=$(shell rospack find bullet)/lib:$(LD_LIBRARY_PATH) && mkdir -p build && cd build && BOOST_INCLUDEDIR=$(BOOST_INCLUDE_DIRS) BOOST_LIBRARYDIR=$(BOOST_LIBRARY_DIRS) cmake -DCMAKE_INSTALL_PREFIX=$(PWD) -DCMAKE_BUILD_TYPE=Release -DBoost_INCLUDE_DIR=$(BOOST_INCLUDE_DIRS) -DBoost_LIBRARY_DIR=$(BOOST_LIBRARY_DIRS) .. && make $(ROS_PARALLEL_JOBS) install
+ @cd $(SVN_DIR) && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && export CPATH="$(shell rospack find bullet)/include:$(CPATH)" && export LD_LIBRARY_PATH=$(shell rospack find bullet)/lib:$(LD_LIBRARY_PATH) && mkdir -p build && cd build && BOOST_INCLUDEDIR=$(BOOST_INCLUDE_DIRS) BOOST_LIBRARYDIR=$(BOOST_LIBRARY_DIRS) cmake -DCMAKE_INSTALL_PREFIX=$(PWD) -DCMAKE_BUILD_TYPE=Release -DBoost_INCLUDE_DIR=$(BOOST_INCLUDE_DIRS) -DBoost_LIBRARY_DIR=$(BOOST_LIBRARY_DIRS) .. && export PARALLEL_JOBS=ROS_PARALLEL_JOBS && make $(ROS_PARALLEL_JOBS) install
touch installed
clean:
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2009-02-07 03:35:24 UTC (rev 10745)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2009-02-07 04:11:16 UTC (rev 10746)
@@ -143,92 +143,98 @@
TiXmlElement *geom = new TiXmlElement("geom");
string type;
+ btTransform vis;
- // I'm not at all sure that this is the right thing to do.
- if(!link->visual)
- {
- printf("skipping link without visual tag: %s\n", link->name.c_str());
- return;
+ if(!link->visual) {
+ printf("link without visual tag: %s\n", link->name.c_str());
+ type = "box";
+ double extents[3] = {0.001,0.001,0.001};
+ addKeyValue(geom, "extents", values2str(3,extents));
}
- switch(link->visual->geometry->type) {
- case robot_desc::URDF::Link::Geometry::MESH: {
- robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(link->visual->geometry->shape);
+ else {
+ switch(link->visual->geometry->type) {
+ case robot_desc::URDF::Link::Geometry::MESH: {
+ robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(link->visual->geometry->shape);
- // Trim Both leading and trailing spaces
- size_t startpos = mesh->filename.find_first_not_of(" \t");
- size_t endpos = mesh->filename.find_last_not_of(" \t");
+ // Trim Both leading and trailing spaces
+ size_t startpos = mesh->filename.find_first_not_of(" \t");
+ size_t endpos = mesh->filename.find_last_not_of(" \t");
- if(( string::npos == startpos ) || ( string::npos == endpos))
- mesh->filename = "";
- else
- mesh->filename = mesh->filename.substr( startpos, endpos-startpos+1 );
+ if(( string::npos == startpos ) || ( string::npos == endpos))
+ mesh->filename = "";
+ else
+ mesh->filename = mesh->filename.substr( startpos, endpos-startpos+1 );
- if( mesh->filename.empty() ) {
- cerr << "mesh file is empty for link " << link->name << ", adding box" << endl;
- type = "box";
- double extents[3] = {0.01,0.01,0.01};
- addKeyValue(geom, "extents", values2str(3,extents));
- }
- else {
- type = "trimesh";
- stringstream ss;
+ if( mesh->filename.empty() ) {
+ cerr << "mesh file is empty for link " << link->name << ", adding box" << endl;
+ type = "box";
+ double extents[3] = {0.01,0.01,0.01};
+ addKeyValue(geom, "extents", values2str(3,extents));
+ }
+ else {
+ type = "trimesh";
+ stringstream ss;
- s_listResourceNames.push_back(mesh->filename + ".iv");
+ s_listResourceNames.push_back(mesh->filename + ".iv");
- string collisionfilename;
+ string collisionfilename;
- map<string, string> mcolinfo = link->collision->data.getMapTagValues("collision", "mesh");
- if( mcolinfo["type"] == string("visual"))
- collisionfilename = mesh->filename + string(".iv");
- else
- collisionfilename = string("convex/") + mesh->filename + string("_convex.iv");
+ map<string, string> mcolinfo = link->collision->data.getMapTagValues("collision", "mesh");
+ if( mcolinfo["type"] == string("visual"))
+ collisionfilename = mesh->filename + string(".iv");
+ else
+ collisionfilename = string("convex/") + mesh->filename + string("_convex.iv");
- // check for convex meshes
- ifstream ifile((s_inresdir + collisionfilename).c_str(), ios_base::binary);
- if( !ifile ) {
- cerr << "failed to find convex decomposition file: " << s_inresdir << collisionfilename << endl;
- collisionfilename = mesh->filename + string(".iv");
- }
- else {
- s_listResourceNames.push_back(collisionfilename);
- }
+ // check for convex meshes
+ ifstream ifile((s_inresdir + collisionfilename).c_str(), ios_base::binary);
+ if( !ifile ) {
+ cerr << "failed to find convex decomposition file: " << s_inresdir << collisionfilename << endl;
+ collisionfilename = mesh->filename + string(".iv");
+ }
+ else {
+ s_listResourceNames.push_back(collisionfilename);
+ }
- //s_listResourceNames.push_back(mesh->filename + "_low.iv");
+ //s_listResourceNames.push_back(mesh->filename + "_low.iv");
- ss << mesh->filename << ".iv " << mesh->scale[0];
- addKeyValue(geom, "render", ss.str());
+ ss << mesh->filename << ".iv " << mesh->scale[0];
+ addKeyValue(geom, "render", ss.str());
- ss.str("");
- ss << collisionfilename << " " << mesh->scale[0]; // don't use low!
- addKeyValue(geom, "data", ss.str());
+ ss.str("");
+ ss << collisionfilename << " " << mesh->scale[0]; // don't use low!
+ addKeyValue(geom, "data", ss.str());
+ }
+ break;
}
- break;
+ case robot_desc::URDF::Link::Geometry::BOX: {
+ type = "box";
+ robot_desc::URDF::Link::Geometry::Box* box = static_cast<robot_desc::URDF::Link::Geometry::Box*>(link->visual->geometry->shape);
+ addKeyValue(geom, "extents", values2str(3,box->size));
+ break;
+ }
+ case robot_desc::URDF::Link::Geometry::CYLINDER: {
+ type = "cylinder";
+ robot_desc::URDF::Link::Geometry::Cylinder* cylinder = static_cast<robot_desc::URDF::Link::Geometry::Cylinder*>(link->visual->geometry->shape);
+ addKeyValue(geom,"radius",values2str(1,&cylinder->radius));
+ addKeyValue(geom,"height",values2str(1,&cylinder->length));
+ break;
+ }
+ case robot_desc::URDF::Link::Geometry::SPHERE: {
+ type = "sphere";
+ double radius = static_cast<robot_desc::URDF::Link::Geometry::Sphere*>(link->visual->geometry->shape)->radius;
+ addKeyValue(geom,"radius",values2str(1,&radius));
+ break;
+ }
+ default: {
+ fprintf(stderr,"Unknown body type: %d in geometry '%s'\n", link->visual->geometry->type, link->visual->geometry->name.c_str());
+ exit(-1);
+ }
+ }
+
+ setupTransform(vis, link->visual->xyz, link->visual->rpy);
+ copyOpenraveMap(link->visual->data, geom);
}
- case robot_desc::URDF::Link::Geometry::BOX: {
- type = "box";
- robot_desc::URDF::Link::Geometry::Box* box = static_cast<robot_desc::URDF::Link::Geometry::Box*>(link->visual->geometry->shape);
- addKeyValue(geom, "extents", values2str(3,box->size));
- break;
- }
- case robot_desc::URDF::Link::Geometry::CYLINDER: {
- type = "cylinder";
- robot_desc::URDF::Link::Geometry::Cylinder* cylinder = static_cast<robot_desc::URDF::Link::Geometry::Cylinder*>(link->visual->geometry->shape);
- addKeyValue(geom,"radius",values2str(1,&cylinder->radius));
- addKeyValue(geom,"height",values2str(1,&cylinder->length));
- break;
- }
- case robot_desc::URDF::Link::Geometry::SPHERE: {
- type = "sphere";
- double radius = static_cast<robot_desc::URDF::Link::Geometry::Sphere*>(link->visual->geometry->shape)->radius;
- addKeyValue(geom,"radius",values2str(1,&radius));
- break;
- }
- default: {
- fprintf(stderr,"Unknown body type: %d in geometry '%s'\n", link->visual->geometry->type, link->visual->geometry->name.c_str());
- exit(-1);
- }
- }
-
+
geom->SetAttribute("type", type);
// compute the visualisation transfrom
@@ -236,13 +242,9 @@
// setupTransform(coll, link->collision->xyz, link->collision->rpy);
// coll.inverse();
- btTransform vis;
- setupTransform(vis, link->visual->xyz, link->visual->rpy);
// coll = coll.inverseTimes(vis);
addTransform(geom, vis);
-
- copyOpenraveMap(link->visual->data, geom);
body->LinkEndChild(geom); // end geom
// mass
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-02-08 23:36:50
|
Revision: 10797
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10797&view=rev
Author: rdiankov
Date: 2009-02-08 23:36:45 +0000 (Sun, 08 Feb 2009)
Log Message:
-----------
changed urdf2openrave to ignore empty links until proper fix can be determined, fixed working directories for roslaunch scripts, fixed but in openraveros that can make showing the viewer slow, added grasp table generation to build process
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/3rdparty/soqt/Makefile
pkg/trunk/calibration/laser_camera_calibration/startcalibration.ros.xml
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/ormanipulation/maketables.ros.xml
pkg/trunk/openrave_planning/ormanipulation/manifest.xml
pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml
pkg/trunk/openrave_planning/ormanipulation/startmanip.ros.xml
pkg/trunk/openrave_planning/ormanipulation/startmanip_sim.ros.xml
pkg/trunk/openrave_planning/ormanipulation/viewtables.ros.xml
pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
Added Paths:
-----------
pkg/trunk/openrave_planning/ormanipulation/CMakeLists.txt
pkg/trunk/openrave_planning/ormanipulation/Makefile
pkg/trunk/openrave_planning/ormanipulation/maketables_noviewer.ros.xml
Removed Paths:
-------------
pkg/trunk/openrave_planning/ormanipulation/octave/grasp_pr2_ricebox.mat
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-02-08 22:11:25 UTC (rev 10796)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-02-08 23:36:45 UTC (rev 10797)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 654
+SVN_REVISION = -r 655
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
#SVN_PATCH = fini_patch.patch
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/3rdparty/soqt/Makefile
===================================================================
--- pkg/trunk/3rdparty/soqt/Makefile 2009-02-08 22:11:25 UTC (rev 10796)
+++ pkg/trunk/3rdparty/soqt/Makefile 2009-02-08 23:36:45 UTC (rev 10797)
@@ -26,3 +26,4 @@
wipe: clean
rm -rf build
+
Modified: pkg/trunk/calibration/laser_camera_calibration/startcalibration.ros.xml
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/startcalibration.ros.xml 2009-02-08 22:11:25 UTC (rev 10796)
+++ pkg/trunk/calibration/laser_camera_calibration/startcalibration.ros.xml 2009-02-08 23:36:45 UTC (rev 10797)
@@ -6,5 +6,5 @@
<env name="OPENRAVE_PLUGINS" value="$(env OPENRAVE_PLUGINS):$(find openrave)/share/openrave/plugins:$(find orplugins)"/>
</node>
-<!-- <node pkg="laser_camera_calibration" type="runcalibration.m" output="screen"/> -->
+ <node pkg="laser_camera_calibration" cwd="node" type="runcalibration.m" output="screen"/>
</launch>
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2009-02-08 22:11:25 UTC (rev 10796)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2009-02-08 23:36:45 UTC (rev 10797)
@@ -281,9 +281,9 @@
while(_ok) {
{
+ usleep(1000);
boost::mutex::scoped_lock lockcreate(_mutexViewer);
- if( _strviewer.size() == 0 || !_penvViewer ) {
- usleep(1000);
+ if( _strviewer.size() == 0 || !_penvViewer ) {
continue;
}
Added: pkg/trunk/openrave_planning/ormanipulation/CMakeLists.txt
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/CMakeLists.txt (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/CMakeLists.txt 2009-02-08 23:36:45 UTC (rev 10797)
@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+add_definitions(-Wall)
+rospack(ormanipulation)
+
+# process all urdf files
+set(GRASP_TABLE_OUTPUT ${CMAKE_SOURCE_DIR}/octave/grasp_pr2_ricebox.mat)
+
+add_custom_command(
+ OUTPUT ${GRASP_TABLE_OUTPUT}
+ COMMAND roslaunch "${CMAKE_SOURCE_DIR}/maketables_noviewer.ros.xml"
+ WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/octave
+ DEPENDS ${CMAKE_SOURCE_DIR}/octave/runmaketables.m ${CMAKE_SOURCE_DIR}/octave/MakePR2GraspTables.m)
+add_custom_target(grasp_tables ALL DEPENDS ${GRASP_TABLE_OUTPUT})
Added: pkg/trunk/openrave_planning/ormanipulation/Makefile
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/Makefile (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/Makefile 2009-02-08 23:36:45 UTC (rev 10797)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Modified: pkg/trunk/openrave_planning/ormanipulation/maketables.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/maketables.ros.xml 2009-02-08 22:11:25 UTC (rev 10796)
+++ pkg/trunk/openrave_planning/ormanipulation/maketables.ros.xml 2009-02-08 23:36:45 UTC (rev 10797)
@@ -2,5 +2,5 @@
<launch>
<machine name="localhost" address="localhost" default="true"/>
<include file="$(find ormanipulation)/startopenrave.ros.xml"/>
- <node pkg="ormanipulation" type="runmaketables.m" output="screen"/>
+ <node pkg="ormanipulation" cwd="node" type="runmaketables.m" output="screen"/>
</launch>
Added: pkg/trunk/openrave_planning/ormanipulation/maketables_noviewer.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/maketables_noviewer.ros.xml (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/maketables_noviewer.ros.xml 2009-02-08 23:36:45 UTC (rev 10797)
@@ -0,0 +1,8 @@
+<!-- Make various grasp tables for the PR2 gripper -->
+<launch>
+ <machine name="localhost" address="localhost" default="true"/>
+ <include file="$(find ormanipulation)/startopenrave.ros.xml"/>
+ <node pkg="ormanipulation" cwd="node" type="runmaketables.m" output="screen">
+ <env name="ORMANIPULATION_NOVIEWER" value="1"/>
+ </node>
+</launch>
Modified: pkg/trunk/openrave_planning/ormanipulation/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/manifest.xml 2009-02-08 22:11:25 UTC (rev 10796)
+++ pkg/trunk/openrave_planning/ormanipulation/manifest.xml 2009-02-08 23:36:45 UTC (rev 10797)
@@ -6,6 +6,7 @@
<license>BSD</license>
<depend package="openraveros"/>
<depend package="orplugins"/>
+ <depend package="roslaunch"/>
<depend package="laser_scan"/>
<depend package="robot_self_filter"/>
<depend package="collision_map"/>
Deleted: pkg/trunk/openrave_planning/ormanipulation/octave/grasp_pr2_ricebox.mat
===================================================================
(Binary files differ)
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/startup.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-02-08 22:11:25 UTC (rev 10796)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/startup.m 2009-02-08 23:36:45 UTC (rev 10797)
@@ -12,7 +12,12 @@
end
setrealsession();
-openraveros_restart('openrave_session','qtcoin',1);
+viewer = '';
+if( ~strcmp(getenv('ORMANIPULATION_NOVIEWER'),'1') )
+ viewer = 'qtcoin';
+end
+
+openraveros_restart('openrave_session',viewer,1);
orEnvSetOptions('wdims 800 600');
orEnvSetOptions('simulation timestep 0.001');
%orEnvSetOptions('collision bullet');
Modified: pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml 2009-02-08 22:11:25 UTC (rev 10796)
+++ pkg/trunk/openrave_planning/ormanipulation/openrave_sensing.ros.xml 2009-02-08 23:36:45 UTC (rev 10797)
@@ -9,5 +9,5 @@
<node pkg="phase_space" type="phase_space_node"/>
<!-- launch octave scripts to start the openrave client -->
- <node pkg="ormanipulation" type="runperception.m" output="screen"/>
+ <node pkg="ormanipulation" cwd="node" type="runperception.m" output="screen"/>
</launch>
Modified: pkg/trunk/openrave_planning/ormanipulation/startmanip.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startmanip.ros.xml 2009-02-08 22:11:25 UTC (rev 10796)
+++ pkg/trunk/openrave_planning/ormanipulation/startmanip.ros.xml 2009-02-08 23:36:45 UTC (rev 10797)
@@ -8,5 +8,5 @@
<node pkg="phase_space" type="phase_space_node"/>
<!-- launch octave scripts to start the openrave client -->
- <node pkg="ormanipulation" type="runmanip.m" output="screen"/>
+ <node pkg="ormanipulation" cwd="node" type="runmanip.m" output="screen"/>
</launch>
Modified: pkg/trunk/openrave_planning/ormanipulation/startmanip_sim.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startmanip_sim.ros.xml 2009-02-08 22:11:25 UTC (rev 10796)
+++ pkg/trunk/openrave_planning/ormanipulation/startmanip_sim.ros.xml 2009-02-08 23:36:45 UTC (rev 10797)
@@ -2,5 +2,5 @@
<launch>
<machine name="localhost" address="localhost" default="true"/>
<include file="$(find ormanipulation)/startopenrave.ros.xml"/>
- <node pkg="ormanipulation" type="runmanip_sim.m" output="screen"/>
+ <node pkg="ormanipulation" cwd="node" type="runmanip_sim.m" output="screen"/>
</launch>
Modified: pkg/trunk/openrave_planning/ormanipulation/viewtables.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/viewtables.ros.xml 2009-02-08 22:11:25 UTC (rev 10796)
+++ pkg/trunk/openrave_planning/ormanipulation/viewtables.ros.xml 2009-02-08 23:36:45 UTC (rev 10797)
@@ -2,5 +2,5 @@
<launch>
<machine name="localhost" address="localhost" default="true"/>
<include file="$(find ormanipulation)/startopenrave.ros.xml"/>
- <node pkg="ormanipulation" type="rungrasptables.m" output="screen"/>
+ <node pkg="ormanipulation" cwd="node" type="rungrasptables.m" output="screen"/>
</launch>
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2009-02-08 22:11:25 UTC (rev 10796)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2009-02-08 23:36:45 UTC (rev 10797)
@@ -147,6 +147,7 @@
if(!link->visual) {
printf("link without visual tag: %s\n", link->name.c_str());
+ return; // return for now until openrave supports these bodies better
type = "box";
double extents[3] = {0.001,0.001,0.001};
addKeyValue(geom, "extents", values2str(3,extents));
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-02-09 18:38:51
|
Revision: 10833
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10833&view=rev
Author: sfkwc
Date: 2009-02-09 18:38:47 +0000 (Mon, 09 Feb 2009)
Log Message:
-----------
point_cloud_utils is being replaced by point_cloud_mapping. Will be removed as soon as vpradeep ports some code into the new pcm package.
Added Paths:
-----------
pkg/trunk/deprecated/point_cloud_utils/
Removed Paths:
-------------
pkg/trunk/util/point_cloud_utils/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-02-09 19:31:41
|
Revision: 10838
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10838&view=rev
Author: isucan
Date: 2009-02-09 19:31:32 +0000 (Mon, 09 Feb 2009)
Log Message:
-----------
copied the URDF parser part of wg_robot_description_parser to planning_models; large chunks of the parsing code will be removed, since we only need the data for planning (kinematic structure + dynamic properties)
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/util/self_watch/manifest.xml
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
pkg/trunk/world_models/robot_models/planning_models/manifest.xml
pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
Added Paths:
-----------
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h
pkg/trunk/world_models/robot_models/planning_models/src/urdf.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-02-09 19:31:32 UTC (rev 10838)
@@ -185,13 +185,13 @@
return true;
}
- virtual void setRobotDescription(robot_desc::URDF *file)
+ virtual void setRobotDescription(planning_models::URDF *file)
{
KinematicStateMonitor::setRobotDescription(file);
if (m_kmodel)
{
std::vector<std::string> links;
- robot_desc::URDF::Group *g = file->getGroup("collision_check");
+ planning_models::URDF::Group *g = file->getGroup("collision_check");
if (g && g->hasFlag("collision"))
links = g->linkNames;
m_collisionSpace->lock();
@@ -218,9 +218,9 @@
protected:
- void addSelfCollisionGroups(unsigned int cid, robot_desc::URDF *model)
+ void addSelfCollisionGroups(unsigned int cid, planning_models::URDF *model)
{
- std::vector<robot_desc::URDF::Group*> groups;
+ std::vector<planning_models::URDF::Group*> groups;
model->getGroups(groups);
m_collisionSpace->lock();
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-02-09 19:31:32 UTC (rev 10838)
@@ -43,7 +43,7 @@
#include <ros/time.h>
#include <ros/console.h>
-#include <urdf/URDF.h>
+#include <planning_models/urdf.h>
#include <planning_models/kinematic.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
@@ -130,7 +130,7 @@
void setRobotDescriptionFromData(const char *data)
{
- robot_desc::URDF *file = new robot_desc::URDF();
+ planning_models::URDF *file = new planning_models::URDF();
if (file->loadString(data))
setRobotDescription(file);
else
@@ -139,14 +139,14 @@
void setRobotDescriptionFromFile(const char *filename)
{
- robot_desc::URDF *file = new robot_desc::URDF();
+ planning_models::URDF *file = new planning_models::URDF();
if (file->loadFile(filename))
setRobotDescription(file);
else
delete file;
}
- virtual void setRobotDescription(robot_desc::URDF *file)
+ virtual void setRobotDescription(planning_models::URDF *file)
{
if (m_urdf)
delete m_urdf;
@@ -296,7 +296,7 @@
ros::Node *m_node;
tf::TransformListener m_tf;
- robot_desc::URDF *m_urdf;
+ planning_models::URDF *m_urdf;
planning_models::KinematicModel *m_kmodel;
// info about the pose; this is not placed in the robot's kinematic state
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-02-09 19:31:32 UTC (rev 10838)
@@ -13,7 +13,6 @@
<depend package="robot_msgs" />
<depend package="robot_srvs" />
- <depend package="wg_robot_description_parser"/>
<depend package="planning_models"/>
<depend package="collision_space"/>
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-09 19:31:32 UTC (rev 10838)
@@ -389,7 +389,7 @@
return result;
}
- virtual void setRobotDescription(robot_desc::URDF *file)
+ virtual void setRobotDescription(planning_models::URDF *file)
{
CollisionSpaceMonitor::setRobotDescription(file);
defaultPosition();
@@ -688,12 +688,12 @@
void createMotionPlanningInstances(RKPModel* model)
{
std::map<std::string, std::string> options;
- robot_desc::URDF::Group *group = m_urdf->getGroup(model->kmodel->getURDFGroup(model->groupName));
+ planning_models::URDF::Group *group = m_urdf->getGroup(model->kmodel->getURDFGroup(model->groupName));
options.clear();
if (group)
{
- const robot_desc::URDF::Map &data = group->data;
+ const planning_models::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "RRT");
}
@@ -703,7 +703,7 @@
options.clear();
if (group)
{
- const robot_desc::URDF::Map &data = group->data;
+ const planning_models::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "LazyRRT");
}
model->addLazyRRT(options);
@@ -711,7 +711,7 @@
options.clear();
if (group)
{
- const robot_desc::URDF::Map &data = group->data;
+ const planning_models::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "EST");
}
model->addEST(options);
@@ -719,7 +719,7 @@
options.clear();
if (group)
{
- const robot_desc::URDF::Map &data = group->data;
+ const planning_models::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "SBL");
}
model->addSBL(options);
@@ -727,7 +727,7 @@
options.clear();
if (group)
{
- const robot_desc::URDF::Map &data = group->data;
+ const planning_models::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "IKSBL");
}
model->addIKSBL(options);
Modified: pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-02-09 19:31:32 UTC (rev 10838)
@@ -200,7 +200,7 @@
}
}
- virtual void setRobotDescription(robot_desc::URDF *file)
+ virtual void setRobotDescription(planning_models::URDF *file)
{
CollisionSpaceMonitor::setRobotDescription(file);
Modified: pkg/trunk/util/self_watch/manifest.xml
===================================================================
--- pkg/trunk/util/self_watch/manifest.xml 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/util/self_watch/manifest.xml 2009-02-09 19:31:32 UTC (rev 10838)
@@ -7,7 +7,7 @@
<depend package="rosconsole" />
<depend package="robot_msgs" />
- <depend package="wg_robot_description_parser" />
+
<depend package="planning_models" />
<depend package="collision_space" />
Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-02-09 19:31:32 UTC (rev 10838)
@@ -66,7 +66,7 @@
if (getParam("robot_description", content))
{
// parse the description
- robot_desc::URDF *file = new robot_desc::URDF();
+ planning_models::URDF *file = new planning_models::URDF();
if (file->loadString(content.c_str()))
{
// create a kinematic model out of the parsed description
@@ -97,7 +97,7 @@
// get the list of links that are enabled for collision checking
std::vector<std::string> links;
- robot_desc::URDF::Group *g = file->getGroup("collision_check");
+ planning_models::URDF::Group *g = file->getGroup("collision_check");
if (g && g->hasFlag("collision"))
links = g->linkNames;
@@ -117,7 +117,7 @@
// get the self collision groups and add them to the collision space
int nscgroups = 0;
- std::vector<robot_desc::URDF::Group*> groups;
+ std::vector<planning_models::URDF::Group*> groups;
file->getGroups(groups);
for (unsigned int i = 0 ; i < groups.size() ; ++i)
if (groups[i]->hasFlag("self_collision"))
Modified: pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt 2009-02-09 19:31:32 UTC (rev 10838)
@@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(planning_models)
-rospack_add_library(planning_models src/kinematic.cpp)
+rospack_add_library(planning_models src/urdf.cpp src/kinematic.cpp)
# Unit tests
rospack_add_gtest(test_kinematic test/test_kinematic.cpp)
Modified: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-09 19:16:05 UTC (rev 10837)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-09 19:31:32 UTC (rev 10838)
@@ -37,7 +37,7 @@
#ifndef KINEMATIC_ROBOT_MODEL_
#define KINEMATIC_ROBOT_MODEL_
-#include <urdf/URDF.h>
+#include <planning_models/urdf.h>
#include <LinearMath/btTransform.h>
#include <iostream>
@@ -183,7 +183,7 @@
virtual void updateVariableTransform(const double *params) = 0;
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot) = 0;
+ virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot) = 0;
};
/** A fixed joint */
@@ -195,7 +195,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
};
/** A planar joint */
@@ -212,7 +212,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
};
/** A floating joint */
@@ -229,7 +229,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
};
/** A prismatic joint */
@@ -247,7 +247,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
btVector3 axis;
double limit[2];
@@ -268,7 +268,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
btVector3 axis;
btVector3 anchor;
@@ -374,7 +374,7 @@
const double* computeTransform(const double *params, int groupID = -1);
/** Extract the information needed by the joint given the URDF description */
- void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ void extractInformation(const URDF::Link *urdfLink, Robot *robot);
};
@@ -558,7 +558,7 @@
}
void build(const std::string &description, bool ignoreSensors = false);
- virtual void build(const robot_desc::URDF &model, bool ignoreSensors = false);
+ virtual void build(const URDF &model, bool ignoreSensors = false);
bool isBuilt(void) const;
StateParams* newStateParams(void);
@@ -617,19 +617,19 @@
private:
/** Build the needed datastructure for a joint */
- void buildChainJ(Robot *robot, Link *parent, Joint *joint, const robot_desc::URDF::Link *urdfLink, const robot_desc::URDF &model);
+ void buildChainJ(Robot *robot, Link *parent, Joint *joint, const URDF::Link *urdfLink, const URDF &model);
/** Build the needed datastructure for a link */
- void buildChainL(Robot *robot, Joint *parent, Link *link, const robot_desc::URDF::Link *urdfLink, const robot_desc::URDF &model);
+ void buildChainL(Robot *robot, Joint *parent, Link *link, const URDF::Link *urdfLink, const URDF &model);
/** Construct the list of groups the model knows about (the ones marked with the 'plan' attribute) */
- void constructGroupList(const robot_desc::URDF &model);
+ void constructGroupList(const URDF &model);
/* compute the parameter names */
void computeParameterNames(void);
/** Allocate a joint of appropriate type, depending on the loaded link */
- Joint* createJoint(const robot_desc::URDF::Link* urdfLink);
+ Joint* createJoint(const URDF::Link* urdfLink);
};
Added: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h (rev 0)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h 2009-02-09 19:31:32 UTC (rev 10838)
@@ -0,0 +1,851 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/** \author Ioan Sucan */
+
+#ifndef URDF_PARSER_
+#define URDF_PARSER_
+
+#include <tinyxml/tinyxml.h>
+#include <iostream>
+#include <string>
+#include <vector>
+#include <map>
+
+/**
+ @mainpage
+
+ @htmlinclude ../../manifest.html
+
+ Universal Robot Description Format (URDF) parser. The URDF class
+ is able to load a robot description from multiple files, string
+ buffers, streams and produce a datastructure similar to the XML
+ structure. Expression evaluation and file inclusion are performed
+ if needed. Checks are performed to make sure the document is
+ correct: values that are inconsistend are reported, typos in the
+ names, unknown tags, etc. For more documentation, see the URDF
+ description. */
+
+/** Namespace that contains the URDF parser */
+namespace planning_models
+{
+ /** This class contains a complete parser for URDF documents.
+ Datastructures that resemble the ones specified in the XML
+ document are instantiated, easy-access pointers are computed
+ (to access a datastructure from another). Checks are made to
+ make sure the parsed data is correct. Since actuators,
+ transmissions and controllers are potentially robot specific,
+ their data is kept as XML elements.
+ */
+ class URDF
+ {
+ public:
+
+ /** This class encapsulates data that can be attached to various tags in the format */
+ class Map
+ {
+ public:
+
+ Map(void)
+ {
+ }
+
+ virtual ~Map(void)
+ {
+ }
+
+ /** Get the set of flags used for the processed <map> tags. Each <map> tag has one flag. Multiple tags may
+ have the same flag. */
+ void getMapTagFlags(std::vector<std::string> &flags) const;
+
+ /** Given a specific flag, retrieve the names of the <map> tags that have the given flag */
+ void getMapTagNames(const std::string &flag, std::vector<std::string> &names) const;
+
+ /** Given a name and a flag, retrieve the defined map (string, string) */
+ std::map<std::string, std::string> getMapTagValues(const std::string &flag, const std::string &name) const;
+
+ /** Given a name and a flag, retrieve the defined map (string, XML) */
+ std::map<std::string, const TiXmlElement*> getMapTagXML(const std::string &flag, const std::string &name) const;
+
+ /** Check whether a key under empty flag and empty name has been defined */
+ bool hasDefault(const std::string &key) const;
+
+ /** Retrieve the value of a key (string) under empty flag and empty name has been defined */
+ std::string getDefaultValue(const std::string &key) const;
+
+ /** Retrieve the value of a key (XML) under empty flag and empty name has been defined */
+ const TiXmlElement* getDefaultXML(const std::string &key) const;
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ /** Add a string element to the map */
+ void add(const std::string &flag, const std::string &name, const std::string &key, const std::string &value);
+
+ /** Add an XML element to the map */
+ void add(const std::string &flag, const std::string &name, const std::string &key, const TiXmlElement *value);
+
+ Map& operator=(const Map &rhs)
+ {
+ m_data = rhs.m_data;
+ return *this;
+ }
+ protected:
+
+ struct Element
+ {
+ Element(void)
+ {
+ xml = NULL;
+ str = NULL;
+ }
+
+ ~Element(void)
+ {
+ if (str)
+ delete str;
+ }
+
+ std::string *str; // allocated locally
+ const TiXmlElement *xml; // allocated in the XML document
+ };
+
+ std::map < std::string, std::map < std::string, std::map < std::string, Element > > > m_data;
+ };
+
+
+ /** Forward declaration of groups */
+ struct Group;
+
+ /** This class defines link instances. It contains instances of collision, visual, inertial and joint descriptions. */
+ struct Link
+ {
+
+ /** Class for link geometry instances */
+ struct Geometry
+ {
+ struct Shape
+ {
+ virtual ~Shape(void)
+ {
+ }
+ };
+
+ struct Sphere : public Shape
+ {
+ Sphere(void) : Shape()
+ {
+ radius = 0.0;
+ }
+
+ double radius;
+ };
+
+ struct Box : public Shape
+ {
+ Box(void) : Shape()
+ {
+ size[0] = size[1] = size[2] = 0.0;
+ }
+
+ double size[3];
+ };
+
+ struct Cylinder : public Shape
+ {
+ Cylinder(void) : Shape()
+ {
+ length = radius = 0.0;
+ }
+
+ double length, radius;
+ };
+
+ struct Mesh : public Shape
+ {
+ Mesh(void) : Shape()
+ {
+ scale[0] = scale[1] = scale[2] = 1.0;
+ }
+
+ std::string filename;
+ double scale[3];
+ };
+
+ Geometry(void)
+ {
+ type = UNKNOWN;
+ shape = NULL;
+ isSet["name"] = false;
+ isSet["size"] = false;
+ isSet["length"] = false;
+ isSet["radius"] = false;
+ isSet["filename"] = false;
+ isSet["scale"] = false;
+ }
+
+ virtual ~Geometry(void)
+ {
+ if (shape)
+ delete shape;
+ }
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ enum
+ {
+ UNKNOWN, SPHERE, BOX, CYLINDER, MESH
+ } type;
+ std::string name;
+ Shape *shape;
+ Map data;
+ std::map<std::string, bool> isSet;
+ };
+
+
+ /** Class for link joint instances (connects a link to its parent) */
+ struct Joint
+ {
+ Joint(void)
+ {
+ axis[0] = axis[1] = axis[2] = 0.0;
+ anchor[0] = anchor[1] = anchor[2] = 0.0;
+ damping = 0.0;
+ friction = 0.0;
+ limit[0] = limit[1] = 0.0;
+ safetyLength[0] = safetyLength[1] = 0.0;
+ velocityLimit = 0.0;
+ effortLimit = 0.0;
+ type = UNKNOWN;
+ pjointMimic = NULL;
+ fMimicMult = 1.0; fMimicOffset = 0.0;
+ isSet["name"] = false;
+ isSet["type"] = false;
+ isSet["axis"] = false;
+ isSet["anchor"] = false;
+ isSet["damping"] = false;
+ isSet["friction"] = false;
+ isSet["limit"] = false;
+ isSet["safetyLengthMin"] = false;
+ isSet["safetyLengthMax"] = false;
+ isSet["effortLimit"] = false;
+ isSet["pjointMimic"] = false;
+ isSet["fMimicMult"] = false;
+ isSet["fMimicOffset"] = false;
+ isSet["velocityLimit"] = false;
+ isSet["calibration"] = false;
+ }
+
+ virtual ~Joint(void)
+ {
+ }
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ enum
+ {
+ UNKNOWN, FIXED, REVOLUTE, PRISMATIC, PLANAR, FLOATING
+ } type;
+ std::string name;
+ double axis[3]; // vector describing the axis of rotation: (x,y,z)
+ double anchor[3]; // point about which the axis defines the rotation: (x,y,z)
+ double damping; // damping coefficient
+ double friction; // friction coefficient
+ double limit[2]; // the joint limits: (min, max)
+ double safetyLength[2]; // the joint limits: (min, max)
+ double effortLimit;
+ double velocityLimit;
+ std::string calibration;
+ Map data;
+ std::map<std::string, bool> isSet;
+
+ // only valid for joints that mimic other joint's values
+ Joint* pjointMimic; // if not NULL, this joint mimics pjointMimic
+ double fMimicMult, fMimicOffset; // the multiplication and offset coeffs. Ie, X = mult*Y+offset
+ // where Y is pjointMimic's joint value, X is this joint's value.
+ };
+
+ /** Class for link collision component instances */
+ struct Collision
+ {
+ Collision(void)
+ {
+ xyz[0] = xyz[1] = xyz[2] = 0.0;
+ rpy[0] = rpy[1] = rpy[2] = 0.0;
+ verbose = false;
+ geometry = NULL;
+ isSet["name"] = false;
+ isSet["verbose"] = false;
+ isSet["xyz"] = false;
+ isSet["rpy"] = false;
+ isSet["geometry"] = false;
+ }
+
+ virtual ~Collision(void)
+ {
+ if (geometry)
+ delete geometry;
+ }
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ std::string name;
+ bool verbose;
+ double xyz[3];
+ double rpy[3];
+ Geometry *geometry;
+ Map data;
+ std::map<std::string, bool> isSet;
+ };
+
+ /** Class for link inertial component instances */
+ struct Inertial
+ {
+ Inertial(void)
+ {
+ mass = 0.0;
+ com[0] = com[1] = com[2] = 0.0;
+ inertia[0] = inertia[1] = inertia[2] = inertia[3] = inertia[4] = inertia[5] = 0.0;
+ isSet["name"] = false;
+ isSet["mass"] = false;
+ isSet["inertia"] = false;
+ isSet["com"] = false;
+ }
+
+ virtual ~Inertial(void)
+ {
+ }
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ std::string name;
+ double mass;
+ /** Ixx Ixy Ixz Iyy Iyz Izz */
+ double inertia[6];
+ double com[3];
+ Map data;
+ std::map<std::string, bool> isSet;
+ };
+
+ /** Class for link visual component instances */
+ struct Visual
+ {
+ Visual(void)
+ {
+ xyz[0] = xyz[1] = xyz[2] = 0.0;
+ rpy[0] = rpy[1] = rpy[2] = 0.0;
+ geometry = NULL;
+ isSet["name"] = false;
+ isSet["xyz"] = false;
+ isSet["rpy"] = false;
+ isSet["geometry"] = false;
+ }
+
+ virtual ~Visual(void)
+ {
+ if (geometry)
+ delete geometry;
+ }
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ std::string name;
+ double xyz[3];
+ double rpy[3];
+ Geometry *geometry;
+ Map data;
+ std::map<std::string, bool> isSet;
+ };
+
+ Link(void)
+ {
+ parent = NULL;
+ xyz[0] = xyz[1] = xyz[2] = 0.0;
+ rpy[0] = rpy[1] = rpy[2] = 0.0;
+ inertial = NULL;
+ visual = NULL;
+ collision = NULL;
+ joint = NULL;
+ isSet["name"] = false;
+ isSet["parent"] = false;
+ isSet["inertial"] = false;
+ isSet["visual"] = false;
+ isSet["collision"] = false;
+ isSet["joint"] = false;
+ isSet["xyz"] = false;
+ isSet["rpy"] = false;
+ }
+
+ virtual ~Link(void)
+ {
+ if (inertial)
+ delete inertial;
+ if (visual)
+ delete visual;
+ if (collision)
+ delete collision;
+ if (joint)
+ delete joint;
+ }
+
+ /** Check if the link instance is inside some group */
+ bool insideGroup(Group *group) const;
+
+ /** Check if the link instance is inside some group */
+ bool insideGroup(const std::string &group) const;
+
+ /** Check if the link instance is also a sensor */
+ virtual bool canSense(void) const;
+
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
+
+ Link *parent;
+ std::string parentName;
+ std::string name;
+ std::vector<Link*> children;
+
+ Inertial *inertial;
+ Visual *visual;
+ Collision *collision;
+ Joint *joint;
+
+ double rpy[3];
+ double xyz[3];
+ Map data;
+
+ std::vector<Group*> groups;
+ std::vector<bool> inGroup;
+
+ std::map<std::string, bool> isSet;
+ };
+
+ /** Class for defining sensors. This is basically a link with a few extra parameters */
+ struct Sensor : public Link
+ {
+ Sensor(void)
+ {
+ type = UNKNOWN;
+ isSet["type"] = false;
+ isSet["calibration"] = false;
+ }
+
+ virtual ~Sensor(void)
+ {
+ }
+
+ virtual bool canSense(void) const;
+ virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
...
[truncated message content] |