Menu
▾
▴
personalrobots-commit
|
From: <mee...@us...> - 2009-08-24 23:54:44
|
Revision: 22790
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22790&view=rev
Author: meeussen
Date: 2009-08-24 23:54:35 +0000 (Mon, 24 Aug 2009)
Log Message:
-----------
Port mechanism model, kdl parser, robot_state_publisher to new urdf format. Update api of mechanism model. Door test works
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
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_velocity_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pr2_gripper_controller.h
pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_pose_twist_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_spline_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_tff_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/cartesian_twist_controller_ik.cpp
pkg/trunk/sandbox/experimental_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_autotuner.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_calibration_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_chain_constraint_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_chain_sine_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_inverse_dynamics_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_limit_calibration_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_pd_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_position_smoothing_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/pid_position_velocity_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/pr2_gripper_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/probe.cpp
pkg/trunk/sandbox/experimental_controllers/src/trajectory_controller.cpp
pkg/trunk/stacks/mechanism/kdl_parser/CMakeLists.txt
pkg/trunk/stacks/mechanism/kdl_parser/manifest.xml
pkg/trunk/stacks/mechanism/kdl_parser/test/test_kdl_parser.cpp
pkg/trunk/stacks/mechanism/mechanism_control/manifest.xml
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/stacks/mechanism/mechanism_control/src/recorder.cpp
pkg/trunk/stacks/mechanism/mechanism_model/CMakeLists.txt
pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/gripper_transmission.h
pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/joint.h
pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/robot.h
pkg/trunk/stacks/mechanism/mechanism_model/manifest.xml
pkg/trunk/stacks/mechanism/mechanism_model/src/chain.cpp
pkg/trunk/stacks/mechanism/mechanism_model/src/gripper_transmission.cpp
pkg/trunk/stacks/mechanism/mechanism_model/src/joint.cpp
pkg/trunk/stacks/mechanism/mechanism_model/src/pr2_gripper_transmission.cpp
pkg/trunk/stacks/mechanism/mechanism_model/src/robot.cpp
pkg/trunk/stacks/mechanism/mechanism_model/src/simple_transmission.cpp
pkg/trunk/stacks/mechanism/mechanism_model/test/test_chain.cpp
pkg/trunk/stacks/mechanism/mechanism_model/test/test_helpers.h
pkg/trunk/stacks/mechanism/robot_state_publisher/include/robot_state_publisher/joint_state_listener.h
pkg/trunk/stacks/mechanism/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h
pkg/trunk/stacks/mechanism/robot_state_publisher/manifest.xml
pkg/trunk/stacks/mechanism/robot_state_publisher/src/joint_state_listener.cpp
pkg/trunk/stacks/mechanism/robot_state_publisher/src/state_publisher.cpp
pkg/trunk/stacks/mechanism/robot_state_publisher/test/test_publisher.cpp
pkg/trunk/stacks/pr2/pr2_alpha/pre_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_alpha/prf_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_alpha/prg_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_etherCAT/src/main.cpp
pkg/trunk/stacks/pr2_common/pr2_defs/defs/arm/arm_defs.urdf.xacro
pkg/trunk/stacks/pr2_simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
pkg/trunk/stacks/robot_model/urdf/include/urdf/joint.h
pkg/trunk/stacks/robot_model/urdf/include/urdf/model.h
pkg/trunk/stacks/robot_model/urdf/src/joint.cpp
pkg/trunk/stacks/simulator_gazebo/gazebo/launch/empty_world.launch
Added Paths:
-----------
pkg/trunk/stacks/mechanism/kdl_parser/include/kdl_parser/dom_parser.hpp
pkg/trunk/stacks/mechanism/kdl_parser/include/kdl_parser/xml_parser.hpp
pkg/trunk/stacks/mechanism/kdl_parser/pr2.urdf
pkg/trunk/stacks/mechanism/kdl_parser/src/dom_parser.cpp
pkg/trunk/stacks/mechanism/kdl_parser/src/xml_parser.cpp
pkg/trunk/stacks/mechanism/kdl_parser/test/example_dom.cpp
pkg/trunk/stacks/mechanism/kdl_parser/test/example_xml.cpp
Removed Paths:
-------------
pkg/trunk/stacks/mechanism/kdl_parser/include/kdl_parser/tree_parser.hpp
pkg/trunk/stacks/mechanism/kdl_parser/src/tree_parser.cpp
pkg/trunk/stacks/mechanism/kdl_parser/test/example.cpp
pkg/trunk/stacks/mechanism/mechanism_model/pr2_desc.xml
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2009-08-24 23:54:35 UTC (rev 22790)
@@ -41,8 +41,8 @@
//#include <robot_mechanism_controllers/joint_position_smoothing_controller.h>
#include <realtime_tools/realtime_publisher.h>
+#include <tf/tf.h>
-
#include "filters/transfer_function.h"
// Messages
@@ -102,7 +102,7 @@
//boost::mutex track_link_lock_ ;
//bool track_link_enabled_ ;
//mechanism::LinkState* target_link_ ;
- mechanism::LinkState* mount_link_ ;
+ //mechanism::LinkState* mount_link_ ;
tf::Vector3 track_point_ ;
//JointPositionSmoothController joint_position_controller_ ; // The PID position controller that is doing all the under-the-hood controls stuff
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h 2009-08-24 23:54:35 UTC (rev 22790)
@@ -37,6 +37,7 @@
#include <nav_msgs/Odometry.h>
#include <realtime_tools/realtime_publisher.h>
#include <tf/tfMessage.h>
+#include <tf/tf.h>
#include <pr2_mechanism_controllers/base_kinematics.h>
#include <angles/angles.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-08-24 23:54:35 UTC (rev 22790)
@@ -28,6 +28,7 @@
<depend package="pluginlib" />
<depend package="rosconsole" />
<depend package="eigen" />
+ <depend package="tf" />
<depend package="angles" />
<depend package="control_toolbox" />
<depend package="filters" />
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -49,21 +49,23 @@
wheel_speed_cmd_ = 0;
wheel_speed_actual_ = 0;
- mechanism::Link *link = robot_state->model_->getLink(link_name);
- if(!link)
- {
+ KDL::SegmentMap::const_iterator link;
+ try{
+ link = robot_state->model_->robot_model_.getSegment(link_name);
+ }
+ catch (KDL::Tree::segment_name_exception& ex){
ROS_ERROR("Could not find link with name %s",link_name.c_str());
return false;
}
ROS_DEBUG("wheel name: %s",link_name.c_str());
link_name_ = link_name;
- joint_name_ = link->joint_name_;
+ joint_name_ = link->second.segment.getJoint().getName();
joint_ = robot_state->getJointState(joint_name_);
- tf::Transform offset = link->getOffset();
- offset_.x = offset.getOrigin().x();
- offset_.y = offset.getOrigin().y();
- offset_.z = offset.getOrigin().z();
+ KDL::Vector offset = link->second.segment.pose(0.0).p;
+ offset_.x = offset.x();
+ offset_.y = offset.y();
+ offset_.z = offset.z();
node.param<double> ("wheel_radius_scaler", wheel_radius_scaler_, 1.0);
ROS_DEBUG("Loading wheel: %s",link_name_.c_str());
ROS_DEBUG("offset_.x: %f, offset_.y: %f, offset_.z: %f", offset_.x, offset_.y, offset_.z);
@@ -82,26 +84,27 @@
steer_angle_actual_ = 0;
num_children_ = 0;
- ROS_DEBUG("caster name: %s",link_name.c_str());
- link_name_ = link_name;
- mechanism::Link *link = robot_state->model_->getLink(link_name);
- if(!link)
- {
+ KDL::SegmentMap::const_iterator link;
+ try{
+ link = robot_state->model_->robot_model_.getSegment(link_name);
+ }
+ catch (KDL::Tree::segment_name_exception& ex){
ROS_ERROR("Could not find link with name %s",link_name.c_str());
return false;
}
- joint_name_ = link->joint_name_;
+ ROS_DEBUG("caster name: %s",link_name.c_str());
+ link_name_ = link_name;
+
+ joint_name_ = link->second.segment.getJoint().getName();
joint_ = robot_state->getJointState(joint_name_);
- tf::Transform offset = link->getOffset();
- offset_.x = offset.getOrigin().x();
- offset_.y = offset.getOrigin().y();
- offset_.z = offset.getOrigin().z();
+ KDL::Vector offset = link->second.segment.pose(0.0).p;
+ offset_.x = offset.x();
+ offset_.y = offset.y();
+ offset_.z = offset.z();
- KDL::SegmentMap::const_iterator root = robot_state->model_->tree_.getSegment(link_name);
-
- for(unsigned int i=0; i < root->second.children.size(); i++)
+ for(unsigned int i=0; i < link->second.children.size(); i++)
{
- KDL::SegmentMap::const_iterator child = root->second.children[i];
+ KDL::SegmentMap::const_iterator child = link->second.children[i];
Wheel tmp;
parent_->wheel_.push_back(tmp);
if(!parent_->wheel_[parent_->num_wheels_].init(robot_state, node.getNamespace(), child->second.segment.getName()))
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -248,11 +248,11 @@
if (pub_calibrated_)
{
- if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ if (last_publish_time_ + 0.5 < robot_->getTime())
{
if (pub_calibrated_->trylock())
{
- last_publish_time_ = robot_->hw_->current_time_;
+ last_publish_time_ = robot_->getTime();
pub_calibrated_->unlockAndPublish();
}
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -190,11 +190,11 @@
case CALIBRATED:
if (pub_calibrated_)
{
- if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ if (last_publish_time_ + 0.5 < robot_->getTime())
{
if (pub_calibrated_->trylock())
{
- last_publish_time_ = robot_->hw_->current_time_;
+ last_publish_time_ = robot_->getTime();
pub_calibrated_->unlockAndPublish();
}
}
@@ -224,12 +224,12 @@
if (c_.calibrated())
{
- if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ if (last_publish_time_ + 0.5 < robot_->getTime())
{
assert(pub_calibrated_);
if (pub_calibrated_->trylock())
{
- last_publish_time_ = robot_->hw_->current_time_;
+ last_publish_time_ = robot_->getTime();
pub_calibrated_->unlockAndPublish();
}
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -104,7 +104,7 @@
ROS_ERROR("%s:: Error initializing pid element", name_.c_str()) ;
return false ;
}
- last_time_ = robot->hw_->current_time_ ;
+ last_time_ = robot->getTime() ;
last_error_ = 0.0 ;
// ***** Derivative Error Filter Element *****
@@ -216,7 +216,7 @@
// ***** Run the position control loop *****
double cmd = traj_command_ + tracking_offset_ ;
- double time = robot_->hw_->current_time_ ;
+ double time = robot_->getTime() ;
double error(0.0) ;
angles::shortest_angular_distance_with_limits(cmd, joint_state_->position_,
joint_state_->joint_->joint_limit_min_,
@@ -237,7 +237,7 @@
double LaserScannerTrajController::getCurProfileTime()
{
- double time = robot_->hw_->current_time_ ;
+ double time = robot_->getTime() ;
double time_from_start = time - traj_start_time_ ;
double mod_time = time_from_start - floor(time_from_start/traj_.getTotalTime())*traj_.getTotalTime() ;
return mod_time ;
@@ -273,7 +273,7 @@
traj_.setTrajectory(traj_points) ;
- traj_start_time_ = robot_->hw_->current_time_ ;
+ traj_start_time_ = robot_->getTime() ;
traj_duration_ = traj_.getTotalTime() ;
@@ -291,8 +291,8 @@
double low_pt = -cmd.amplitude + cmd.offset ;
- double soft_limit_low = joint_state_->joint_->joint_limit_min_ + joint_state_->joint_->safety_length_min_ ;
- double soft_limit_high = joint_state_->joint_->joint_limit_max_ - joint_state_->joint_->safety_length_max_ ;
+ double soft_limit_low = joint_state_->joint_->safety_limit_min_;
+ double soft_limit_high = joint_state_->joint_->safety_limit_max_;
if (low_pt < soft_limit_low)
{
@@ -461,7 +461,7 @@
{
// Should we be populating header.stamp here? Or, we can simply let ros take care of the timestamp
ros::Time cur_time ;
- cur_time.fromSec(robot_->hw_->current_time_) ;
+ cur_time.fromSec(robot_->getTime()) ;
m_scanner_signal_.header.stamp = cur_time ;
m_scanner_signal_.signal = cur_profile_segment ;
need_to_send_msg_ = true ;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -171,7 +171,7 @@
cmd_vel_t_.linear.y = 0.0;
}
cmd_vel_t_.angular.z = filters::clamp(cmd_vel.angular.z, -max_vel_.angular.z, max_vel_.angular.z);
- cmd_received_timestamp_ = base_kin_.robot_state_->hw_->current_time_;
+ cmd_received_timestamp_ = base_kin_.robot_state_->getTime();
ROS_DEBUG("BaseController:: command received: %f %f %f",cmd_vel.linear.x,cmd_vel.linear.y,cmd_vel.angular.z);
ROS_DEBUG("BaseController:: command current: %f %f %f", cmd_vel_.linear.x,cmd_vel_.linear.y,cmd_vel_.angular.z);
@@ -250,8 +250,8 @@
}
}
- last_time_ = base_kin_.robot_state_->hw_->current_time_;
- cmd_received_timestamp_ = base_kin_.robot_state_->hw_->current_time_;
+ last_time_ = base_kin_.robot_state_->getTime();
+ cmd_received_timestamp_ = base_kin_.robot_state_->getTime();
for(int i = 0; i < base_kin_.num_casters_; i++)
{
caster_controller_[i]->starting();
@@ -266,7 +266,7 @@
void Pr2BaseController::update()
{
- double current_time = base_kin_.robot_state_->hw_->current_time_;
+ double current_time = base_kin_.robot_state_->getTime();
double dT = std::min<double>(current_time - last_time_, base_kin_.MAX_DT_);
if(new_cmd_available_)
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -105,15 +105,15 @@
bool Pr2Odometry::starting()
{
- current_time_ = base_kin_.robot_state_->hw_->current_time_;
- last_time_ = base_kin_.robot_state_->hw_->current_time_;
- last_publish_time_ = base_kin_.robot_state_->hw_->current_time_;
+ current_time_ = base_kin_.robot_state_->getTime();
+ last_time_ = base_kin_.robot_state_->getTime();
+ last_publish_time_ = base_kin_.robot_state_->getTime();
return true;
}
void Pr2Odometry::update()
{
- current_time_ = base_kin_.robot_state_->hw_->current_time_;
+ current_time_ = base_kin_.robot_state_->getTime();
updateOdometry();
publish();
last_time_ = current_time_;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -360,12 +360,12 @@
if (pub_calibrated_)
{
- if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ if (last_publish_time_ + 0.5 < robot_->getTime())
{
assert(pub_calibrated_);
if (pub_calibrated_->trylock())
{
- last_publish_time_ = robot_->hw_->current_time_;
+ last_publish_time_ = robot_->getTime();
pub_calibrated_->unlockAndPublish();
}
}
@@ -403,12 +403,12 @@
if (c_.calibrated())
{
- if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ if (last_publish_time_ + 0.5 < robot_->getTime())
{
assert(pub_calibrated_);
if (pub_calibrated_->trylock())
{
- last_publish_time_ = robot_->hw_->current_time_;
+ last_publish_time_ = robot_->getTime();
pub_calibrated_->unlockAndPublish();
}
}
Modified: 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_controller.h 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_position_controller.h 2009-08-24 23:54:35 UTC (rev 22790)
@@ -63,8 +63,8 @@
#include <controller_interface/controller.h>
#include <control_toolbox/pid.h>
#include "control_toolbox/pid_gains_setter.h"
+#include <boost/scoped_ptr.hpp>
#include <realtime_tools/realtime_publisher.h>
-
#include <std_msgs/Float64.h>
#include <robot_mechanism_controllers/JointControllerState.h>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_velocity_controller.h 2009-08-24 23:54:35 UTC (rev 22790)
@@ -61,6 +61,7 @@
#include <ros/node.h>
#include <ros/node_handle.h>
+#include <boost/scoped_ptr.hpp>
#include "controller_interface/controller.h"
#include "control_toolbox/pid.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -127,7 +127,7 @@
// initialize desired pose/twist
twist_ff_ = Twist::Zero();
pose_desi_ = getPose();
- last_time_ = robot_state_->hw_->current_time_;
+ last_time_ = robot_state_->getTime();
loop_count_ = 0;
return true;
@@ -138,7 +138,7 @@
void CartesianPoseController::update()
{
// get time
- double time = robot_state_->hw_->current_time_;
+ double time = robot_state_->getTime();
double dt = time - last_time_;
last_time_ = time;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -142,7 +142,7 @@
fb_pid_controller_[i].reset();
// time
- last_time_ = robot_state_->hw_->current_time_;
+ last_time_ = robot_state_->getTime();
// set disired twist to 0
twist_desi_ = Twist::Zero();
@@ -159,7 +159,7 @@
return;
// get time
- double time = robot_state_->hw_->current_time_;
+ double time = robot_state_->getTime();
double dt = time - last_time_;
last_time_ = time;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -57,7 +57,7 @@
{
assert(robot);
robot_ = robot;
- last_time_ = robot->hw_->current_time_;
+ last_time_ = robot->getTime();
joint_state_ = robot_->getJointState(joint_name);
if (!joint_state_)
@@ -158,7 +158,7 @@
assert(robot_ != NULL);
double error(0);
- double time = robot_->hw_->current_time_;
+ double time = robot_->getTime();
assert(joint_state_->joint_);
dt_= time - last_time_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -244,12 +244,12 @@
case CALIBRATED:
if (pub_calibrated_)
{
- if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ if (last_publish_time_ + 0.5 < robot_->getTime())
{
assert(pub_calibrated_);
if (pub_calibrated_->trylock())
{
- last_publish_time_ = robot_->hw_->current_time_;
+ last_publish_time_ = robot_->getTime();
pub_calibrated_->unlockAndPublish();
}
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -55,7 +55,7 @@
{
assert(robot);
robot_ = robot;
- last_time_ = robot->hw_->current_time_;
+ last_time_ = robot->getTime();
joint_state_ = robot_->getJointState(joint_name);
if (!joint_state_)
@@ -159,7 +159,7 @@
void JointVelocityController::update()
{
assert(robot_ != NULL);
- double time = robot_->hw_->current_time_;
+ double time = robot_->getTime();
double error = joint_state_->velocity_ - command_;
dt_ = time - last_time_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
@@ -76,7 +76,7 @@
double TriggerController::getTick()
{
- return getTick(robot_->hw_->current_time_, config_);
+ return getTick(robot_->getTime(), config_);
}
void TriggerController::update()
@@ -101,7 +101,7 @@
}
//if (actuator_command_->digital_out_ && !(active ^ config_.active_low))
- // ROS_DEBUG("digital out falling at time %f", robot_->hw_->current_time_);
+ // ROS_DEBUG("digital out falling at time %f", robot_->getTime());
actuator_command_->digital_out_ = active ^ config_.active_low;
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h 2009-08-24 23:54:35 UTC (rev 22790)
@@ -126,6 +126,7 @@
std::vector<int> joint_type_;
mechanism::Robot* robot_;
+ mechanism::RobotState* robot_state_;
void updateJointControllers(void);
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pid_position_velocity_controller.h 2009-08-24 23:54:35 UTC (rev 22790)
@@ -62,6 +62,7 @@
#include <ros/node.h>
#include <ros/node_handle.h>
+#include <boost/scoped_ptr.hpp>
#include "controller_interface/controller.h"
#include "control_toolbox/pid.h"
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pr2_gripper_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pr2_gripper_controller.h 2009-08-24 23:54:00 UTC (rev 22789)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/pr2_gripper_controller.h 2009-08-24 23:54:35 UTC (rev 22790)
@@ -49,6 +49,7 @@
#include "ros/callback_queue.h"
#include <boost/shared_ptr.hpp>
+#include <boost/scoped_ptr.hpp>
namespace controller
{
Modified: pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp 2009-08-24 23:54:0...
[truncated message content] |
|
From: <tf...@us...> - 2009-08-24 23:57:17
|
Revision: 22791
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22791&view=rev
Author: tfoote
Date: 2009-08-24 23:57:09 +0000 (Mon, 24 Aug 2009)
Log Message:
-----------
merging back filters change, door_demos_gazebo passes and 2dnav_gazebo runs
r31481@agn (orig r21824): tfoote | 2009-08-13 14:02:29 -0700
creating branch to make filters dynamically load
r31696@agn (orig r21981): tfoote | 2009-08-16 18:37:27 -0700
compiling example plugin
r31697@agn (orig r21982): tfoote | 2009-08-16 19:47:40 -0700
passing test
r31698@agn (orig r21983): tfoote | 2009-08-16 19:48:15 -0700
adding export
r31860@agn (orig r22114): tfoote | 2009-08-17 23:19:14 -0700
functioning dynamically loaded filters from offline work
r31862@agn (orig r22116): tfoote | 2009-08-17 23:22:14 -0700
adding multi library support to plugin xml parser with plugin_libraries top level tag
r31895@agn (orig r22147): tfoote | 2009-08-18 10:32:24 -0700
removing unused package
r31904@agn (orig r22153): tfoote | 2009-08-18 11:20:48 -0700
cleaning up debugging messages
r31919@agn (orig r22161): tfoote | 2009-08-18 13:50:36 -0700
new MultiChannelFilterBase
r31920@agn (orig r22162): tfoote | 2009-08-18 14:00:11 -0700
transfer function test recovered
r32021@agn (orig r22256): tfoote | 2009-08-18 22:22:21 -0700
tests passing
r32028@agn (orig r22263): tfoote | 2009-08-18 23:14:34 -0700
compiling under new architecture
r32067@agn (orig r22296): tfoote | 2009-08-19 11:09:09 -0700
catching XML parse failure
r32071@agn (orig r22300): tfoote | 2009-08-19 11:18:15 -0700
scan_to_cloud running
r32107@agn (orig r22330): tfoote | 2009-08-19 14:27:17 -0700
adding two filter test to make sure we can load from different manifests
r32119@agn (orig r22340): tfoote | 2009-08-19 15:34:06 -0700
lots of debugging output
r32155@agn (orig r22375): tfoote | 2009-08-19 18:03:04 -0700
fixing types
r32165@agn (orig r22383): tfoote | 2009-08-19 18:21:24 -0700
removing redundant boost flag now and making multi channel actually configure with multichannel
r32178@agn (orig r22393): tfoote | 2009-08-19 19:33:42 -0700
explicit MultiChannel vs single for FilterChain
r32186@agn (orig r22401): tfoote | 2009-08-19 20:06:20 -0700
single channel filter working too
r32189@agn (orig r22402): tfoote | 2009-08-19 20:16:57 -0700
more tests of single channel mean filter
r32192@agn (orig r22404): tfoote | 2009-08-19 20:34:46 -0700
float precision for median filter added
r32193@agn (orig r22405): tfoote | 2009-08-19 20:35:19 -0700
forgot xml
r32199@agn (orig r22410): tfoote | 2009-08-19 21:13:40 -0700
shadows.launch running
r32232@agn (orig r22429): tfoote | 2009-08-20 09:53:38 -0700
exporting median filters with float type
r32233@agn (orig r22430): tfoote | 2009-08-20 09:57:11 -0700
all launch files running successfully in laser_scan package
r32235@agn (orig r22432): tfoote | 2009-08-20 10:18:55 -0700
fixing for filters API change
r32237@agn (orig r22434): tfoote | 2009-08-20 10:33:55 -0700
fixing for filters API change
r32242@agn (orig r22438): tfoote | 2009-08-20 10:48:53 -0700
fixing for filters API change
r32246@agn (orig r22441): tfoote | 2009-08-20 11:05:01 -0700
doesn't actually need filters
r32249@agn (orig r22443): tfoote | 2009-08-20 11:22:38 -0700
blacklisting intead of porting for it's out of scope for M3
r32250@agn (orig r22444): tfoote | 2009-08-20 11:28:56 -0700
fixing for new filters API
r32253@agn (orig r22447): tfoote | 2009-08-20 12:00:55 -0700
switching to new filters API
r32270@agn (orig r22461): tfoote | 2009-08-20 13:31:51 -0700
blacklisting packages the haven't bee converted #2516
r32271@agn (orig r22462): tfoote | 2009-08-20 13:38:41 -0700
fixing blacklist location
r32273@agn (orig r22463): tfoote | 2009-08-20 13:41:55 -0700
makeing makefile more robust
r32302@agn (orig r22487): eitanme | 2009-08-20 16:28:21 -0700
Adding parsing from the parameter server to filter base
r32307@agn (orig r22492): tfoote | 2009-08-20 16:52:21 -0700
switching to simpler constructor
r32323@agn (orig r22508): eitanme | 2009-08-20 18:11:45 -0700
Working towards re-writing things. FilterBase now parses XmlRpc values as discussed
r32329@agn (orig r22514): tfoote | 2009-08-20 18:23:44 -0700
one more filter to migrate
r32339@agn (orig r22521): eitanme | 2009-08-20 18:45:13 -0700
Compiling version of the new filter chain configure implementation
r32346@agn (orig r22528): eitanme | 2009-08-20 19:39:40 -0700
Not sure if this compiles or not, but this should be a complete impelementation at least
r32347@agn (orig r22529): eitanme | 2009-08-20 19:46:40 -0700
Compiling version of new filter chain and base
r32348@agn (orig r22530): tfoote | 2009-08-20 19:48:39 -0700
switching first to rostest
r32351@agn (orig r22533): tfoote | 2009-08-20 20:01:15 -0700
fixing filters to use new param syntax, not compiling
r32372@agn (orig r22553): tfoote | 2009-08-21 10:21:41 -0700
only xmlrpc errors now
r32394@agn (orig r22574): tfoote | 2009-08-21 12:47:24 -0700
Now passing single filter tests, still need to work a bit on the chains
r32395@agn (orig r22575): tfoote | 2009-08-21 13:01:19 -0700
chain tests passing
r32403@agn (orig r22582): tfoote | 2009-08-21 13:19:42 -0700
transfer function tests passing
r32404@agn (orig r22583): tfoote | 2009-08-21 13:21:17 -0700
better debugging output
r32414@agn (orig r22592): eitanme | 2009-08-21 13:52:13 -0700
Adding a call to getParam for an XmlRpc value
r32416@agn (orig r22593): tfoote | 2009-08-21 14:15:11 -0700
making chain configure from rpc public
r32418@agn (orig r22595): tfoote | 2009-08-21 14:19:18 -0700
compiling with filters from params
r32433@agn (orig r22604): tfoote | 2009-08-21 15:27:37 -0700
fixing unittest which erroneously passed before
r32437@agn (orig r22607): eitanme | 2009-08-21 15:49:14 -0700
Allowing for empty filter chains
r32440@agn (orig r22609): eitanme | 2009-08-21 15:51:46 -0700
Checking in FILTER_LIST
r32441@agn (orig r22610): eitanme | 2009-08-21 15:52:43 -0700
new task
r32442@agn (orig r22611): tfoote | 2009-08-21 15:55:52 -0700
switched default_scan_shadows to use yaml
r32444@agn (orig r22613): eitanme | 2009-08-21 16:00:34 -0700
Removing filter list now that its a spreadsheet
r32448@agn (orig r22616): tfoote | 2009-08-21 16:11:30 -0700
xml -> yaml but can't rename grrrr
r32453@agn (orig r22621): tfoote | 2009-08-21 16:35:07 -0700
filter_chain into yaml
r32454@agn (orig r22622): eitanme | 2009-08-21 16:35:16 -0700
Moving code and tests to filter_chain naming convention
r32460@agn (orig r22628): eitanme | 2009-08-21 16:54:39 -0700
Moving launch files to work with the new filters
r32466@agn (orig r22633): tfoote | 2009-08-21 17:29:38 -0700
fixed all xml -> yaml and launch files
r32467@agn (orig r22634): eitanme | 2009-08-21 17:34:07 -0700
moving to work with new filters
r32470@agn (orig r22637): eitanme | 2009-08-21 19:56:46 -0700
Checking in a half compiling effort at turning xml into an xml rpc value... sorry Tully... I'll work on this more later
r32479@agn (orig r22643): tfoote | 2009-08-22 17:26:44 -0700
fixing compile and changing from xml to yaml
r32483@agn (orig r22647): tfoote | 2009-08-22 17:57:11 -0700
fixing namespace
r32484@agn (orig r22648): tfoote | 2009-08-22 17:58:55 -0700
switching from xml to yaml filters
r32487@agn (orig r22651): tfoote | 2009-08-22 19:32:57 -0700
new filters format tests passing if migrated
r32495@agn (orig r22655): tfoote | 2009-08-22 22:24:17 -0700
compiling. . . passing all zero tests
r32496@agn (orig r22656): tfoote | 2009-08-22 22:46:39 -0700
switching to reading from param server directly instead of xml off the parameter server
r32497@agn (orig r22657): tfoote | 2009-08-22 22:57:57 -0700
fixing controller file for use with xmlrpc
r32498@agn (orig r22658): tfoote | 2009-08-23 00:20:41 -0700
switching xml to yaml filter calls
r32499@agn (orig r22659): tfoote | 2009-08-23 00:28:18 -0700
converting to new filters and yaml
r32501@agn (orig r22661): tfoote | 2009-08-23 00:45:10 -0700
fixing typo
r32502@agn (orig r22662): tfoote | 2009-08-23 00:48:15 -0700
xml->yaml filters
r32503@agn (orig r22663): tfoote | 2009-08-23 00:59:23 -0700
xml->yaml filters
r32504@agn (orig r22664): tfoote | 2009-08-23 00:59:39 -0700
xml->yaml filters
r32508@agn (orig r22668): tfoote | 2009-08-23 01:24:47 -0700
changing xml syntax for xmlrpc format
r32591@agn (orig r22736): tfoote | 2009-08-24 12:24:01 -0700
robusitfying makefiles
r32594@agn (orig r22739): tfoote | 2009-08-24 12:49:25 -0700
makeing Makefiles more robust
r32634@agn (orig r22777): eitanme | 2009-08-24 15:48:02 -0700
Fixing XML filter specification... not all that pretty though, should be changed eventually to read from the param server
r32636@agn (orig r22778): tfoote | 2009-08-24 16:01:35 -0700
changing to debug
r32646@agn (orig r22788): tfoote | 2009-08-24 16:50:56 -0700
fixing script and usage
Modified Paths:
--------------
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/lasers_and_filters.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/demos/door_demos_gazebo/launch/perception_planner.xml
pkg/trunk/demos/milestone2/milestone2_actions/config/perception.xml
pkg/trunk/demos/plug_in/make_lowpass.m
pkg/trunk/highlevel/plugs/plugs_core/scripts/make_lowpass.m
pkg/trunk/mapping/door_tracker/launch/door_tracker.launch
pkg/trunk/mapping/door_tracker/src/door_tracker.cpp
pkg/trunk/mapping/hallway_tracker/launch/hallway_tracker.launch
pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp
pkg/trunk/pr2/life_test/laser_tilt_test/life_test/controllers.xml
pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml
pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser+stereo-perception.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
pkg/trunk/sandbox/annotated_map_builder/config/perception.xml
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/cartesian_hybrid_controller.h
pkg/trunk/sandbox/experimental_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/stacks/common/filters/CMakeLists.txt
pkg/trunk/stacks/common/filters/include/filters/filter_base.h
pkg/trunk/stacks/common/filters/include/filters/filter_chain.h
pkg/trunk/stacks/common/filters/include/filters/mean.h
pkg/trunk/stacks/common/filters/include/filters/median.h
pkg/trunk/stacks/common/filters/include/filters/transfer_function.h
pkg/trunk/stacks/common/filters/manifest.xml
pkg/trunk/stacks/common/filters/src/transfer_function.cpp
pkg/trunk/stacks/common/filters/test/test_chain.cpp
pkg/trunk/stacks/common/filters/test/test_mean.cpp
pkg/trunk/stacks/common/filters/test/test_median.cpp
pkg/trunk/stacks/common/filters/test/test_transfer_function.cpp
pkg/trunk/stacks/common/laser_scan/CMakeLists.txt
pkg/trunk/stacks/common/laser_scan/footprint.launch
pkg/trunk/stacks/common/laser_scan/include/laser_scan/footprint_filter.h
pkg/trunk/stacks/common/laser_scan/include/laser_scan/intensity_filter.h
pkg/trunk/stacks/common/laser_scan/include/laser_scan/laser_scan.h
pkg/trunk/stacks/common/laser_scan/include/laser_scan/median_filter.h
pkg/trunk/stacks/common/laser_scan/include/laser_scan/point_cloud_footprint_filter.h
pkg/trunk/stacks/common/laser_scan/include/laser_scan/scan_shadows_filter.h
pkg/trunk/stacks/common/laser_scan/intensity_demo.launch
pkg/trunk/stacks/common/laser_scan/manifest.xml
pkg/trunk/stacks/common/laser_scan/median_filter.launch
pkg/trunk/stacks/common/laser_scan/multi.launch
pkg/trunk/stacks/common/laser_scan/shadows.launch
pkg/trunk/stacks/common/laser_scan/src/generic_laser_filter_node.cpp
pkg/trunk/stacks/common/laser_scan/src/laser_scan.cc
pkg/trunk/stacks/common/laser_scan/src/scan_to_cloud.cpp
pkg/trunk/stacks/common/laser_scan/test/projection_test.cpp
pkg/trunk/stacks/geometry/bullet/Makefile.bullet
pkg/trunk/stacks/geometry/kdl/Makefile.orig
pkg/trunk/stacks/laser_drivers/point_cloud_assembler/manifest.xml
pkg/trunk/stacks/laser_drivers/point_cloud_assembler/src/laser_scan_assembler_srv.cpp
pkg/trunk/stacks/semantic_mapping/door_handle_detector/launch/perception.xml
pkg/trunk/stacks/semantic_mapping/door_handle_detector/launch/setup_for_recording.launch
pkg/trunk/stacks/trex/trex_pr2_writing_test/launch/head_cart/perception.launch
pkg/trunk/stacks/trex/trex_pr2_writing_test/launch/perception.launch
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/launch/outlet_coarse_detection/perception.launch
Added Paths:
-----------
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/ROS_BUILD_BLACKLIST
pkg/trunk/motion_planning/planning_research/pm_wrapper/include/sbpl_pm_wrapper/
pkg/trunk/motion_planning/planning_research/pm_wrapper/include/sbpl_pm_wrapper/pm_wrapper.h
pkg/trunk/sandbox/3dnav_pr2/ROS_BUILD_BLACKLIST
pkg/trunk/sandbox/joint_waypoint_controller/ROS_BUILD_BLACKLIST
pkg/trunk/sandbox/spline_smoother/ROS_BUILD_BLACKLIST
pkg/trunk/sandbox/trajectory_controllers/ROS_BUILD_BLACKLIST
pkg/trunk/stacks/common/filters/default_plugins.xml
pkg/trunk/stacks/common/filters/src/mean.cpp
pkg/trunk/stacks/common/filters/src/median.cpp
pkg/trunk/stacks/common/filters/test/test_chain.launch
pkg/trunk/stacks/common/filters/test/test_chain.yaml
pkg/trunk/stacks/common/filters/test/test_mean.launch
pkg/trunk/stacks/common/filters/test/test_mean.yaml
pkg/trunk/stacks/common/filters/test/test_median.launch
pkg/trunk/stacks/common/filters/test/test_median.yaml
pkg/trunk/stacks/common/filters/test/test_transfer_function.launch
pkg/trunk/stacks/common/filters/test/test_transfer_function.yaml
pkg/trunk/stacks/common/laser_scan/default_scan_shadows.yaml
pkg/trunk/stacks/common/laser_scan/footprint.filters.yaml
pkg/trunk/stacks/common/laser_scan/intensity_demo.filters.yaml
pkg/trunk/stacks/common/laser_scan/laser_scan_plugins.xml
pkg/trunk/stacks/common/laser_scan/median_filter_5.yaml
pkg/trunk/stacks/common/laser_scan/multi.yaml
pkg/trunk/stacks/common/laser_scan/point_cloud_footprint.filters.yaml
pkg/trunk/stacks/common/laser_scan/src/laser_scan_filters.cpp
pkg/trunk/stacks/common/laser_scan/src/median_filter.cpp
pkg/trunk/stacks/common/laser_scan/src/pointcloud_filters.cpp
pkg/trunk/util/iir_filters/ROS_BUILD_BLACKLIST
Removed Paths:
-------------
pkg/trunk/stacks/common/filters/test/test_factory.cpp
pkg/trunk/stacks/common/laser_scan/default_scan_shadows.xml
pkg/trunk/stacks/common/laser_scan/footprint.filters.xml
pkg/trunk/stacks/common/laser_scan/intensity_demo.filters.xml
pkg/trunk/stacks/common/laser_scan/median_filter_5.xml
pkg/trunk/stacks/common/laser_scan/multi.xml
pkg/trunk/stacks/common/laser_scan/point_cloud_footprint.filters.xml
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/nav_rework:21482
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/action_branch:20116
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/common_msgs-0.1-v2:20514
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/common_msgs-0.1-v3:20628
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/common_msgs_0.1:20375
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/milestone2:16602
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/nav_rework:17337
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/plugin_branch:20980
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/pluginlib_rework:22236
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/tags/milestone2_tags/milestone2_final:16577
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/nav_rework:21482
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/action_branch:20116
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/common_msgs-0.1-v2:20514
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/common_msgs-0.1-v3:20628
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/common_msgs_0.1:20375
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/filters_dynamic_loading:22788
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/milestone2:16602
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/nav_rework:17337
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/plugin_branch:20980
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/pluginlib_rework:22236
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/tags/milestone2_tags/milestone2_final:16577
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/lasers_and_filters.xml
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/lasers_and_filters.xml 2009-08-24 23:54:35 UTC (rev 22790)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/lasers_and_filters.xml 2009-08-24 23:57:09 UTC (rev 22791)
@@ -9,8 +9,8 @@
<!-- Filter for tilt laser shadowing/veiling -->
<node pkg="laser_scan" type="scan_to_cloud" respawn="true" machine="three" name="tilt_shadow_filter">
- <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
- <param name="cloud_filters" textfile="$(find laser_scan)/point_cloud_footprint.filters.xml" />
+ <rosparam command="load" ns="scan_filters" file="$(find laser_scan)/default_scan_shadows.yaml" />
+ <rosparam command="load" ns="cloud_filters" file="$(find laser_scan)/point_cloud_footprint.filters.yaml" />
<param name="scan_topic" value="tilt_scan" />
<param name="cloud_topic" value="tilt_scan_filtered" />
<param name="high_fidelity" value="true" />
@@ -18,8 +18,8 @@
<!-- Filter for base laser shadowing/veiling -->
<node pkg="laser_scan" type="scan_to_cloud" respawn="true" machine="three" name="base_shadow_filter" >
- <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
- <param name="cloud_filters" textfile="$(find laser_scan)/point_cloud_footprint.filters.xml" />
+ <rosparam command="load" ns="scan_filters" file="$(find laser_scan)/default_scan_shadows.yaml" />
+ <rosparam command="load" ns="cloud_filters" file="$(find laser_scan)/point_cloud_footprint.filters.yaml" />
<param name="scan_topic" value="base_scan" />
<param name="cloud_topic" value="base_scan_marking" />
</node>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-08-24 23:54:35 UTC (rev 22790)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-08-24 23:57:09 UTC (rev 22791)
@@ -115,8 +115,25 @@
return false ;
}
- d_error_filter.FilterBase<double>::configure((unsigned int)1, filter_elem) ;
+ std::string xml_string;
+ TiXmlElement *struct_elem = filter_elem->FirstChildElement("value");
+ if(!struct_elem)
+ {
+ ROS_ERROR("Xml is missing a <value> tag inside filter spec, cannot parse!");
+ return false;
+ }
+ TiXmlPrinter printer;
+ printer.SetIndent(" ");
+ struct_elem->Accept(&printer);
+ std::string filter_str = printer.Str();
+ ROS_DEBUG("Constructing filter with XML: %s", filter_str.c_str());
+ //xmlrpc_elem->Print(xml_string, 10);
+ int offset = 0;
+ XmlRpc::XmlRpcValue rpc_config(filter_str, &offset);
+ ROS_DEBUG("XmlRpc parsed xml: %s type: %d", rpc_config.toXml().c_str(), rpc_config.getType());
+ d_error_filter.MultiChannelFilterBase<double>::configure((unsigned int)1, rpc_config) ;
+
// ***** Max Rate and Acceleration Elements *****
TiXmlElement *max_rate_elem = config->FirstChildElement("max_rate") ;
if (!max_rate_elem)
@@ -222,11 +239,17 @@
joint_state_->joint_->joint_limit_min_,
joint_state_->joint_->joint_limit_max_,
error) ;
+
+
double dt = time - last_time_ ;
double d_error = (error-last_error_)/dt ;
- double filtered_d_error ;
+ std::vector<double> vin;
+ vin.push_back(d_error);
+ std::vector<double> vout = vin;
+
+ d_error_filter.update(vin, vout) ;
- d_error_filter.update(d_error, filtered_d_error) ;
+ double filtered_d_error = vout[0];
// Add filtering step
// Update pid with d_error added
Modified: pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-08-24 23:54:35 UTC (rev 22790)
+++ pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-08-24 23:57:09 UTC (rev 22791)
@@ -18,7 +18,6 @@
<depend package="tf_conversions" />
<depend package="kdl" />
<depend package="angles" />
- <depend package="filters" />
<depend package="diagnostic_msgs" />
<depend package="pluginlib" />
<url>http://pr.willowgarage.com</url>
Modified: pkg/trunk/demos/door_demos_gazebo/launch/perception_planner.xml
===================================================================
--- pkg/trunk/demos/door_demos_gazebo/launch/perception_planner.xml 2009-08-24 23:54:35 UTC (rev 22790)
+++ pkg/trunk/demos/door_demos_gazebo/launch/perception_planner.xml 2009-08-24 23:57:09 UTC (rev 22791)
@@ -19,8 +19,8 @@
<!-- Filter for tilt laser shadowing/veiling -->
<node pkg="laser_scan" type="scan_to_cloud" respawn="true" machine="three" name="tilt_shadow_filter">
- <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
- <param name="cloud_filters" textfile="$(find laser_scan)/point_cloud_footprint.filters.xml" />
+ <rosparam command="load" ns="scan_filters" file="$(find laser_scan)/default_scan_shadows.yaml" />
+ <rosparam command="load" ns="cloud_filters" file="$(find laser_scan)/point_cloud_footprint.filters.yaml" />
<param name="scan_topic" value="tilt_scan" />
<param name="cloud_topic" value="tilt_scan_filtered" />
<param name="high_fidelity" value="true" />
@@ -28,8 +28,8 @@
<!-- Filter for base laser shadowing/veiling -->
<node pkg="laser_scan" type="scan_to_cloud" respawn="true" machine="three" name="base_shadow_filter" >
- <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
- <param name="cloud_filters" textfile="$(find laser_scan)/point_cloud_footprint.filters.xml" />
+ <rosparam command="load" ns="scan_filters" file="$(find laser_scan)/default_scan_shadows.yaml" />
+ <rosparam command="load" ns="cloud_filters" file="$(find laser_scan)/point_cloud_footprint.filters.yaml" />
<param name="scan_topic" value="base_scan" />
<param name="cloud_topic" value="base_scan_marking" />
</node>
@@ -37,7 +37,7 @@
<!-- Laser scan assembler for tilt laser -->
<node pkg="point_cloud_assembler" type="laser_scan_assembler_srv" output="screen" name="laser_scan_assembler" respawn="true">
<remap from="scan_in" to="tilt_scan"/>
- <param name="filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
+ <rosparam command="load" ns="filters" file="$(find laser_scan)/default_scan_shadows.yaml" />
<param name="tf_cache_time_secs" type="double" value="10.0" />
<param name="max_scans" type="int" value="1000" />
<param name="ignore_laser_skew" type="bool" value="true" />
Modified: pkg/trunk/demos/milestone2/milestone2_actions/config/perception.xml
===================================================================
--- pkg/trunk/demos/milestone2/milestone2_actions/config/perception.xml 2009-08-24 23:54:35 UTC (rev 22790)
+++ pkg/trunk/demos/milestone2/milestone2_actions/config/perception.xml 2009-08-24 23:57:09 UTC (rev 22791)
@@ -23,8 +23,8 @@
<!-- Filter for tilt laser shadowing/veiling -->
<node pkg="laser_scan" type="scan_to_cloud" respawn="true" machine="three" name="tilt_shadow_filter">
- <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
- <param name="cloud_filters" textfile="$(find laser_scan)/point_cloud_footprint.filters.xml" />
+ <rosparam command="load" ns="scan_filters" file="$(find laser_scan)/default_scan_shadows.yaml" />
+ <rosparam command="load" ns="cloud_filters" file="$(find laser_scan)/point_cloud_footprint.filters.yaml" />
<param name="scan_topic" value="tilt_scan" />
<param name="cloud_topic" value="tilt_scan_filtered" />
<param name="high_fidelity" value="true" />
@@ -32,8 +32,8 @@
<!-- Filter for base laser shadowing/veiling -->
<node pkg="laser_scan" type="scan_to_cloud" respawn="true" machine="three" name="base_shadow_filter" >
- <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
- <param name="cloud_filters" textfile="$(find laser_scan)/point_cloud_footprint.filters.xml" />
+ <rosparam command="load" ns="scan_filters" file="$(find laser_scan)/default_scan_shadows.yaml" />
+ <rosparam command="load" ns="cloud_filters" file="$(find laser_scan)/point_cloud_footprint.filters.yaml" />
<param name="scan_topic" value="base_scan" />
<param name="cloud_topic" value="base_scan_marking" />
</node>
@@ -41,7 +41,7 @@
<!-- Laser scan assembler for tilt laser -->
<node pkg="point_cloud_assembler" type="laser_scan_assembler_srv" name="laser_scan_assembler" respawn="true">
<remap from="scan_in" to="tilt_scan"/>
- <param name="filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
+ <rosparam command="load" ns="filters" file="$(find laser_scan)/default_scan_shadows.yaml" />
<param name="tf_cache_time_secs" type="double" value="10.0" />
<param name="max_scans" type="int" value="1000" />
<param name="ignore_laser_skew" type="bool" value="true" />
Modified: pkg/trunk/demos/plug_in/make_lowpass.m
===================================================================
--- pkg/trunk/demos/plug_in/make_lowpass.m 2009-08-24 23:54:35 UTC (rev 22790)
+++ pkg/trunk/demos/plug_in/make_lowpass.m 2009-08-24 23:57:09 UTC (rev 22791)
@@ -9,6 +9,54 @@
a = [1 (-(1 - alpha))];
b = [alpha 0];
-printf('<filter name="%s" type="TransferFunctionFilter">\n', filter_name);
-printf(' <params a="%f %f" b="%f %f" />\n', a(1), a(2), b(1), b(2));
-printf('</filter>\n');
+#printf('<filter name="%s" type="TransferFunctionFilter">\n', filter_name);
+#printf(' <params a="%f %f" b="%f %f" />\n', a(1), a(2), b(1), b(2));
+#printf('</filter>\n');
+
+printf('<filter>');
+printf('<value>');
+printf('<array>');
+printf('<data>');
+printf('<struct>');
+printf('<member>');
+printf('<name>%s</name>', filter_name);
+printf('<value>d_error_filter</value>');
+printf('</member>');
+printf('<member>');
+printf('<name>type</name>');
+printf('<value>TransferFunctionFilter</value>');
+printf('</member>');
+printf('<member>');
+printf('<name>params</name>');
+printf('<value>');
+printf('<struct>');
+printf('<member>');
+printf('<name>a</name>');
+printf('<value>');
+printf('<array>');
+printf('<data>');
+printf('<value><double>%f</double></value>', a(1));
+printf('<value><double>%f</double></value>', a(2));
+printf('</data>');
+printf('</array>');
+printf('</value>');
+printf('</member>');
+printf('<member>');
+printf('<name>b</name>');
+printf('<value>');
+printf('<array>');
+printf('<data>');
+printf('<value><double>%f</double></value>', b(1));
+printf('<value><double>%f</double></value>', b(2));
+printf('</data>');
+printf('</array>');
+printf('</value>');
+printf('</member>');
+printf('</struct>');
+printf('</value>');
+printf('</member>');
+printf('</struct>');
+printf('</data>');
+printf('</array>');
+printf('</value>');
+printf('</filter>');
Added: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/ROS_BUILD_BLACKLIST 2009-08-24 23:57:09 UTC (rev 22791)
@@ -0,0 +1 @@
+broken see ticket https://prdev.willowgarage.com/trac/personalrobots/ticket/2516
Modified: pkg/trunk/highlevel/plugs/plugs_core/scripts/make_lowpass.m
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/scripts/make_lowpass.m 2009-08-24 23:54:35 UTC (rev 22790)
+++ pkg/trunk/highlevel/plugs/plugs_core/scripts/make_lowpass.m 2009-08-24 23:57:09 UTC (rev 22791)
@@ -9,6 +9,54 @@
a = [1 (-(1 - alpha))];
b = [alpha 0];
-printf('<filter name="%s" type="TransferFunctionFilter">\n', filter_name);
-printf(' <params a="%f %f" b="%f %f" />\n', a(1), a(2), b(1), b(2));
-printf('</filter>\n');
+#printf('<filter name="%s" type="TransferFunctionFilter">\n', filter_name);
+#printf(' <params a="%f %f" b="%f %f" />\n', a(1), a(2), b(1), b(2));
+#printf('</filter>\n');
+
+printf('<f...
[truncated message content] |
|
From: <vij...@us...> - 2009-08-25 00:33:26
|
Revision: 22794
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22794&view=rev
Author: vijaypradeep
Date: 2009-08-25 00:33:17 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
Blacklisting calibration related packages, since interfaces are changing
Added Paths:
-----------
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/ROS_BUILD_BLACKLIST
pkg/trunk/vision/led_detection/ROS_BUILD_BLACKLIST
pkg/trunk/vision/stereo_checkerboard_detector/ROS_BUILD_BLACKLIST
Added: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/ROS_BUILD_BLACKLIST 2009-08-25 00:33:17 UTC (rev 22794)
@@ -0,0 +1 @@
+
Added: pkg/trunk/vision/led_detection/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/led_detection/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/vision/led_detection/ROS_BUILD_BLACKLIST 2009-08-25 00:33:17 UTC (rev 22794)
@@ -0,0 +1 @@
+
Added: pkg/trunk/vision/stereo_checkerboard_detector/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/stereo_checkerboard_detector/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/vision/stereo_checkerboard_detector/ROS_BUILD_BLACKLIST 2009-08-25 00:33:17 UTC (rev 22794)
@@ -0,0 +1 @@
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-08-25 01:38:42
|
Revision: 22804
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22804&view=rev
Author: tfoote
Date: 2009-08-25 01:38:34 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
switching to new pluginlib api as per review #2531
Modified Paths:
--------------
pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_user/example.cpp
pkg/trunk/stacks/common/pluginlib/include/pluginlib/class_loader.h
pkg/trunk/stacks/common/pluginlib/include/pluginlib/class_loader_imp.h
Modified: pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_user/example.cpp
===================================================================
--- pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_user/example.cpp 2009-08-25 01:36:49 UTC (rev 22803)
+++ pkg/trunk/sandbox/pluginlib_tutorials/pluginlib_tutorial_user/example.cpp 2009-08-25 01:38:34 UTC (rev 22804)
@@ -48,7 +48,7 @@
}
- if (cl.loadClass("square"))
+ if (cl.loadLibraryForClass("square"))
{
ROS_INFO("Loaded library with plugin square inside");
}
@@ -79,12 +79,12 @@
else ROS_INFO("Triangle Class not loaded");
- if (!cl.loadClass("line"))
+ if (!cl.loadLibraryForClass("line"))
ROS_ERROR("Correctly failed to load line in polygon loader");
pluginlib::ClassLoader<shape> ph("pluginlib_tutorial_interfaces", "shape");
- if (!ph.loadClass("line"))
+ if (!ph.loadLibraryForClass("line"))
ROS_ERROR("Failed to load line");
if (ph.isClassLoaded("line"))
Modified: pkg/trunk/stacks/common/pluginlib/include/pluginlib/class_loader.h
===================================================================
--- pkg/trunk/stacks/common/pluginlib/include/pluginlib/class_loader.h 2009-08-25 01:36:49 UTC (rev 22803)
+++ pkg/trunk/stacks/common/pluginlib/include/pluginlib/class_loader.h 2009-08-25 01:38:34 UTC (rev 22804)
@@ -97,10 +97,9 @@
/**
* @brief Given the lookup name of a class, returns the type of the associated base class
- * @param lookup_name The lookup name of the class
* @return The type of the associated base class
*/
- std::string getBaseClassType(const std::string& lookup_name);
+ std::string getBaseClassType() const;
/**
* @brief Given the name of a class, returns name of the containing package
@@ -130,7 +129,7 @@
* @param lookup_name The lookup name of the class to load
* @return True if the class and its associated library were successfully loaded, false otherwise
*/
- bool loadClass(const std::string & lookup_name);
+ bool loadLibraryForClass(const std::string & lookup_name);
private:
/**
Modified: pkg/trunk/stacks/common/pluginlib/include/pluginlib/class_loader_imp.h
===================================================================
--- pkg/trunk/stacks/common/pluginlib/include/pluginlib/class_loader_imp.h 2009-08-25 01:36:49 UTC (rev 22803)
+++ pkg/trunk/stacks/common/pluginlib/include/pluginlib/class_loader_imp.h 2009-08-25 01:38:34 UTC (rev 22804)
@@ -148,7 +148,7 @@
}
template <class T>
- bool ClassLoader<T>::loadClass(const std::string & lookup_name)
+ bool ClassLoader<T>::loadLibraryForClass(const std::string & lookup_name)
{
std::string library_path;
ClassMapIterator it = classes_available_.find(lookup_name);
@@ -235,12 +235,9 @@
}
template <class T>
- std::string ClassLoader<T>::getBaseClassType(const std::string& lookup_name)
+ std::string ClassLoader<T>::getBaseClassType() const
{
- ClassMapIterator it = classes_available_.find(lookup_name);
- if (it != classes_available_.end())
- return it->second.base_class_;
- return "";
+ return base_class_;
}
template <class T>
@@ -265,7 +262,7 @@
T* ClassLoader<T>::createClassInstance(const std::string& lookup_name, bool auto_load)
{
if ( auto_load && !isClassLoaded(lookup_name))
- if(!loadClass(lookup_name))
+ if(!loadLibraryForClass(lookup_name))
{
ROS_ERROR("Failed to auto load library");
throw std::runtime_error("Failed to auto load library for class " + lookup_name + ".");
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ehb...@us...> - 2009-08-25 03:33:07
|
Revision: 22815
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22815&view=rev
Author: ehberger
Date: 2009-08-25 03:32:57 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
removing circular dependencies
Modified Paths:
--------------
pkg/trunk/controllers/stack.xml
pkg/trunk/drivers/stack.xml
pkg/trunk/openrave_planning/stack.xml
pkg/trunk/stacks/pr2/stack.xml
Modified: pkg/trunk/controllers/stack.xml
===================================================================
--- pkg/trunk/controllers/stack.xml 2009-08-25 03:01:12 UTC (rev 22814)
+++ pkg/trunk/controllers/stack.xml 2009-08-25 03:32:57 UTC (rev 22815)
@@ -10,7 +10,7 @@
<depend stack="geometry"/> <!-- tf, kdl, angles, eigen, eigen, tf, angles, angles -->
<depend stack="mechanism"/> <!-- mechanism_model, mechanism_model, kdl_parser, mechanism_model, mechanism_control, hardware_interface, mechanism_control, mechanism_model, mechanism_msgs -->
- <depend stack="util"/> <!-- realtime_tools, realtime_tools, realtime_tools, realtime_tools, trajectory -->
+ <!--depend stack="util"/CAUSING CIRCULAR DEPENDENCY--> <!-- realtime_tools, realtime_tools, realtime_tools, realtime_tools, trajectory -->
<depend stack="common"/> <!-- tinyxml, manipulation_msgs, filters, tinyxml, robot_actions, filters, manipulation_msgs -->
<depend stack="ros"/> <!-- rospy, rospy, rospy, roscpp, std_msgs, rosconsole, roscpp, roscpp, roscpp, rospy, std_msgs, rosconsole -->
<depend stack="visualization_common"/> <!-- visualization_msgs, visualization_msgs -->
Modified: pkg/trunk/drivers/stack.xml
===================================================================
--- pkg/trunk/drivers/stack.xml 2009-08-25 03:01:12 UTC (rev 22814)
+++ pkg/trunk/drivers/stack.xml 2009-08-25 03:32:57 UTC (rev 22815)
@@ -12,12 +12,12 @@
<depend stack="camera_drivers"/> <!-- prosilica_cam, dcam -->
<depend stack="calibration"/> <!-- calibration_msgs, mocap_msgs, mocap_msgs -->
<depend stack="geometry"/> <!-- tf, tf, angles, bullet, tf, tf -->
- <depend stack="hardware_test"/> <!-- diagnostic_updater, diagnostic_updater, diagnostic_updater, diagnostic_updater, self_test, self_test -->
+ <depend stack="diagnostics"/> <!-- diagnostic_updater, diagnostic_updater, diagnostic_updater, diagnostic_updater, self_test, self_test -->
<depend stack="3rdparty"/> <!-- player -->
<depend stack="mechanism"/> <!-- mechanism_msgs, hardware_interface, mechanism_control, mechanism_model, mechanism_msgs, mechanism_msgs -->
<depend stack="controllers"/> <!-- pr2_mechanism_controllers, robot_mechanism_controllers, pr2_mechanism_controllers, dynamic_verification_controllers, pr2_mechanism_controllers, robot_mechanism_controllers, robot_mechanism_controllers -->
<depend stack="opencv"/> <!-- opencv_latest, opencv_latest, opencv_latest -->
- <depend stack="simulators"/> <!-- gazebo, opende -->
+ <depend stack="simulator_gazebo"/> <!-- gazebo, opende -->
<depend stack="common"/> <!-- laser_scan, tinyxml -->
<depend stack="driver_common"/> <!-- driver_base, dynamic_reconfigure -->
<depend stack="ros"/> <!-- std_msgs, roscpp, rospy, std_msgs, rostest, std_msgs, roscpp, rospy, std_msgs, std_msgs, roscpp, roscpp, std_msgs, message_filters, rospy, roscpp, std_msgs, roscpp, std_msgs, roscpp, std_msgs, rospy, rospy, roscpp, roscpp, std_msgs, roscpp, rospy, std_msgs, roscpp, std_msgs, roscpp, std_msgs -->
Modified: pkg/trunk/openrave_planning/stack.xml
===================================================================
--- pkg/trunk/openrave_planning/stack.xml 2009-08-25 03:01:12 UTC (rev 22814)
+++ pkg/trunk/openrave_planning/stack.xml 2009-08-25 03:32:57 UTC (rev 22815)
@@ -12,7 +12,7 @@
<depend stack="geometry"/> <!-- bullet, tf -->
<depend stack="mechanism"/> <!-- mechanism_msgs -->
<depend stack="util"/> <!-- or_robot_self_filter -->
- <depend stack="simulators"/> <!-- opende -->
+ <depend stack="simulator_gazebo"/> <!-- opende -->
<depend stack="common"/> <!-- manipulation_msgs, laser_scan -->
<depend stack="ros"/> <!-- std_msgs, roscpp, roscpp, roscpp, roscpp_sessions, std_msgs, rosoct, roscpp, roslaunch -->
<depend stack="common_msgs"/> <!-- sensor_msgs, mapping_msgs -->
Modified: pkg/trunk/stacks/pr2/stack.xml
===================================================================
--- pkg/trunk/stacks/pr2/stack.xml 2009-08-25 03:01:12 UTC (rev 22814)
+++ pkg/trunk/stacks/pr2/stack.xml 2009-08-25 03:32:57 UTC (rev 22815)
@@ -17,7 +17,7 @@
<depend stack="mechanism"/> <!-- mechanism_control, hardware_interface, mechanism_control, mechanism_model, convex_decomposition, ivcon, robot_state_publisher, mechanism_bringup, convex_decomposition, ivcon -->
<depend stack="controllers"/> <!-- robot_mechanism_controllers, pr2_mechanism_controllers, joint_qualification_controllers, dynamic_verification_controllers, pr2_mechanism_controllers -->
- <depend stack="calibration"/> <!-- joint_calibration_monitor -->
+ <!--depend stack="calibration"/--> <!--Causing circular dependencies-->
<depend stack="pr2_common"/>
<depend stack="sound_drivers"/> <!-- sound_play -->
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-08-25 05:41:44
|
Revision: 22820
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22820&view=rev
Author: vijaypradeep
Date: 2009-08-25 05:41:33 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
Moving actionlib messages into common_msgs/actionlib_msgs. Trac #2504
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/manifest.xml
pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionFeedback.msg
pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionGoal.msg
pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionResult.msg
pkg/trunk/highlevel/move_arm/msg/ActuateGripperFeedback.msg
pkg/trunk/highlevel/move_arm/msg/ActuateGripperResult.msg
pkg/trunk/highlevel/move_arm/msg/MoveArmActionFeedback.msg
pkg/trunk/highlevel/move_arm/msg/MoveArmActionGoal.msg
pkg/trunk/highlevel/move_arm/msg/MoveArmActionResult.msg
pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp
pkg/trunk/stacks/common/actionlib/genaction.py
pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h
pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_goal_handle_imp.h
pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_helpers.h
pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine_imp.h
pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_manager_imp.h
pkg/trunk/stacks/common/actionlib/include/actionlib/goal_id_generator.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server_imp.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/server_goal_handle.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/server_goal_handle_imp.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server_imp.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/status_tracker.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/status_tracker_imp.h
pkg/trunk/stacks/common/actionlib/manifest.xml
pkg/trunk/stacks/common/actionlib_tutorials/manifest.xml
pkg/trunk/stacks/common/actionlib_tutorials/msg/AveragingActionFeedback.msg
pkg/trunk/stacks/common/actionlib_tutorials/msg/AveragingActionGoal.msg
pkg/trunk/stacks/common/actionlib_tutorials/msg/AveragingActionResult.msg
pkg/trunk/stacks/common/actionlib_tutorials/msg/FibonacciActionFeedback.msg
pkg/trunk/stacks/common/actionlib_tutorials/msg/FibonacciActionGoal.msg
pkg/trunk/stacks/common/actionlib_tutorials/msg/FibonacciActionResult.msg
pkg/trunk/stacks/mechanism/mechanism_msgs/manifest.xml
pkg/trunk/stacks/mechanism/mechanism_msgs/msg/SwitchControllerActionFeedback.msg
pkg/trunk/stacks/mechanism/mechanism_msgs/msg/SwitchControllerActionGoal.msg
pkg/trunk/stacks/mechanism/mechanism_msgs/msg/SwitchControllerActionResult.msg
pkg/trunk/stacks/navigation/move_base_msgs/manifest.xml
pkg/trunk/stacks/navigation/move_base_msgs/msg/MoveBaseActionFeedback.msg
pkg/trunk/stacks/navigation/move_base_msgs/msg/MoveBaseActionGoal.msg
pkg/trunk/stacks/navigation/move_base_msgs/msg/MoveBaseActionResult.msg
pkg/trunk/stacks/navigation/move_base_msgs/msg/MoveBaseFeedback.msg
pkg/trunk/stacks/navigation/move_base_msgs/msg/MoveBaseResult.msg
Added Paths:
-----------
pkg/trunk/stacks/common_msgs/actionlib_msgs/
pkg/trunk/stacks/common_msgs/actionlib_msgs/msg/
pkg/trunk/stacks/common_msgs/actionlib_msgs/msg/GoalID.msg
pkg/trunk/stacks/common_msgs/actionlib_msgs/msg/GoalStatus.msg
pkg/trunk/stacks/common_msgs/actionlib_msgs/msg/GoalStatusArray.msg
Removed Paths:
-------------
pkg/trunk/stacks/common/actionlib/msg/GoalID.msg
pkg/trunk/stacks/common/actionlib/msg/GoalStatus.msg
pkg/trunk/stacks/common/actionlib/msg/GoalStatusArray.msg
pkg/trunk/stacks/common/actionlib/msg/RequestType.msg
Modified: pkg/trunk/highlevel/move_arm/manifest.xml
===================================================================
--- pkg/trunk/highlevel/move_arm/manifest.xml 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/highlevel/move_arm/manifest.xml 2009-08-25 05:41:33 UTC (rev 22820)
@@ -22,6 +22,7 @@
<depend package="planning_models" />
<depend package="planning_environment" />
<depend package="actionlib"/>
+ <depend package="actionlib_msgs"/>
<depend package="experimental_controllers"/>
<depend package="tf_conversions"/>
Modified: pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionFeedback.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionFeedback.msg 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionFeedback.msg 2009-08-25 05:41:33 UTC (rev 22820)
@@ -1,4 +1,4 @@
Header header
-actionlib/GoalStatus status
+actionlib_msgs/GoalStatus status
ActuateGripperFeedback feedback
Modified: pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionGoal.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionGoal.msg 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionGoal.msg 2009-08-25 05:41:33 UTC (rev 22820)
@@ -1,4 +1,4 @@
Header header
-actionlib/GoalID goal_id
+actionlib_msgs/GoalID goal_id
ActuateGripperGoal goal
Modified: pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionResult.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionResult.msg 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionResult.msg 2009-08-25 05:41:33 UTC (rev 22820)
@@ -1,4 +1,4 @@
Header header
-actionlib/GoalStatus status
+actionlib_msgs/GoalStatus status
ActuateGripperResult result
Modified: pkg/trunk/highlevel/move_arm/msg/ActuateGripperFeedback.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/ActuateGripperFeedback.msg 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/highlevel/move_arm/msg/ActuateGripperFeedback.msg 2009-08-25 05:41:33 UTC (rev 22820)
@@ -1,2 +1 @@
-float64 crap
Modified: pkg/trunk/highlevel/move_arm/msg/ActuateGripperResult.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/ActuateGripperResult.msg 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/highlevel/move_arm/msg/ActuateGripperResult.msg 2009-08-25 05:41:33 UTC (rev 22820)
@@ -1 +0,0 @@
-float64 crap
Modified: pkg/trunk/highlevel/move_arm/msg/MoveArmActionFeedback.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/MoveArmActionFeedback.msg 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/highlevel/move_arm/msg/MoveArmActionFeedback.msg 2009-08-25 05:41:33 UTC (rev 22820)
@@ -1,4 +1,4 @@
Header header
-actionlib/GoalStatus status
+actionlib_msgs/GoalStatus status
MoveArmFeedback feedback
Modified: pkg/trunk/highlevel/move_arm/msg/MoveArmActionGoal.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/MoveArmActionGoal.msg 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/highlevel/move_arm/msg/MoveArmActionGoal.msg 2009-08-25 05:41:33 UTC (rev 22820)
@@ -1,4 +1,4 @@
Header header
-actionlib/GoalID goal_id
+actionlib_msgs/GoalID goal_id
MoveArmGoal goal
Modified: pkg/trunk/highlevel/move_arm/msg/MoveArmActionResult.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/MoveArmActionResult.msg 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/highlevel/move_arm/msg/MoveArmActionResult.msg 2009-08-25 05:41:33 UTC (rev 22820)
@@ -1,4 +1,4 @@
Header header
-actionlib/GoalStatus status
+actionlib_msgs/GoalStatus status
MoveArmResult result
Modified: pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp
===================================================================
--- pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp 2009-08-25 05:41:33 UTC (rev 22820)
@@ -57,10 +57,10 @@
return action_feedback.cur_pose;
}*/
-PoseStamped fromActionResult(const move_base_msgs::MoveBaseResult& action_result)
+/*PoseStamped fromActionResult(const move_base_msgs::MoveBaseResult& action_result)
{
return action_result.final_pose;
-}
+}*/
int main(int argc, char** argv)
{
@@ -69,7 +69,7 @@
ros::NodeHandle n;
action_translator::ActionTranslator<move_base_msgs::MoveBaseAction, PoseStamped, PoseStamped>
- translator("move_base", &fromOldGoal, NULL, &fromActionResult);
+ translator("move_base", &fromOldGoal, NULL, NULL);
robot_actions::ActionRunner runner(10.0);
runner.connect<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped>(translator);
Modified: pkg/trunk/stacks/common/actionlib/genaction.py
===================================================================
--- pkg/trunk/stacks/common/actionlib/genaction.py 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/stacks/common/actionlib/genaction.py 2009-08-25 05:41:33 UTC (rev 22820)
@@ -94,21 +94,21 @@
goal_msg = goal
action_goal_msg = """
Header header
-actionlib/GoalID goal_id
+actionlib_msgs/GoalID goal_id
%sGoal goal
""" % name
result_msg = result
action_result_msg = """
Header header
-actionlib/GoalStatus status
+actionlib_msgs/GoalStatus status
%sResult result
""" % name
feedback_msg = feedback
action_feedback_msg = """
Header header
-actionlib/GoalStatus status
+actionlib_msgs/GoalStatus status
%sFeedback feedback
""" % name
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h 2009-08-25 05:41:33 UTC (rev 22820)
@@ -117,7 +117,7 @@
*/
void cancelAllGoals()
{
- GoalID cancel_msg;
+ actionlib_msgs::GoalID cancel_msg;
// CancelAll policy encoded by stamp=0, id=0
cancel_msg.stamp = ros::Time(0,0);
cancel_msg.id = ros::Time(0,0);
@@ -196,7 +196,7 @@
goal_pub_.publish(action_goal);
}
- void sendCancelFunc(const GoalID& cancel_msg)
+ void sendCancelFunc(const actionlib_msgs::GoalID& cancel_msg)
{
cancel_pub_.publish(cancel_msg);
}
@@ -206,7 +206,7 @@
// Start publishers and subscribers
server_connected_ = false;
goal_pub_ = queue_advertise<ActionGoal>("goal", 1, boost::bind(&ActionClient::serverConnectionCb, this, _1), queue);
- cancel_pub_ = n_.advertise<GoalID>("cancel", 1, true);
+ cancel_pub_ = n_.advertise<actionlib_msgs::GoalID>("cancel", 1, true);
manager_.registerSendGoalFunc(boost::bind(&ActionClientT::sendGoalFunc, this, _1));
manager_.registerCancelFunc(boost::bind(&ActionClientT::sendCancelFunc, this, _1));
@@ -238,7 +238,7 @@
return n_.subscribe(ops);
}
- void statusCb(const GoalStatusArrayConstPtr& status_array)
+ void statusCb(const actionlib_msgs::GoalStatusArrayConstPtr& status_array)
{
manager_.updateStatuses(status_array);
}
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_goal_handle_imp.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_goal_handle_imp.h 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_goal_handle_imp.h 2009-08-25 05:41:33 UTC (rev 22820)
@@ -94,21 +94,21 @@
if (comm_state_ != CommState::DONE)
ROS_WARN("Asking for the terminal state when we're in [%s]", comm_state_.toString().c_str());
- GoalStatus goal_status = list_handle_.getElem()->getGoalStatus();
+ actionlib_msgs::GoalStatus goal_status = list_handle_.getElem()->getGoalStatus();
switch (goal_status.status)
{
- case GoalStatus::PENDING:
- case GoalStatus::ACTIVE:
- case GoalStatus::PREEMPTING:
- case GoalStatus::RECALLING:
+ case actionlib_msgs::GoalStatus::PENDING:
+ case actionlib_msgs::GoalStatus::ACTIVE:
+ case actionlib_msgs::GoalStatus::PREEMPTING:
+ case actionlib_msgs::GoalStatus::RECALLING:
ROS_ERROR("Asking for terminal state, but latest goal status is %u", goal_status.status); return TerminalState::LOST;
- case GoalStatus::PREEMPTED: return TerminalState::PREEMPTED;
- case GoalStatus::SUCCEEDED: return TerminalState::SUCCEEDED;
- case GoalStatus::ABORTED: return TerminalState::ABORTED;
- case GoalStatus::REJECTED: return TerminalState::REJECTED;
- case GoalStatus::RECALLED: return TerminalState::RECALLED;
- case GoalStatus::LOST: return TerminalState::LOST;
+ case actionlib_msgs::GoalStatus::PREEMPTED: return TerminalState::PREEMPTED;
+ case actionlib_msgs::GoalStatus::SUCCEEDED: return TerminalState::SUCCEEDED;
+ case actionlib_msgs::GoalStatus::ABORTED: return TerminalState::ABORTED;
+ case actionlib_msgs::GoalStatus::REJECTED: return TerminalState::REJECTED;
+ case actionlib_msgs::GoalStatus::RECALLED: return TerminalState::RECALLED;
+ case actionlib_msgs::GoalStatus::LOST: return TerminalState::LOST;
default:
ROS_ERROR("Unknown goal status: %u", goal_status.status); break;
}
@@ -154,7 +154,7 @@
ActionGoalConstPtr action_goal = list_handle_.getElem()->getActionGoal();
- GoalID cancel_msg;
+ actionlib_msgs::GoalID cancel_msg;
cancel_msg.stamp = ros::Time(0,0);
cancel_msg.id = list_handle_.getElem()->getActionGoal()->goal_id.id;
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_helpers.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_helpers.h 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/client_helpers.h 2009-08-25 05:41:33 UTC (rev 22820)
@@ -51,9 +51,8 @@
#include "actionlib/client/terminal_state.h"
// msgs
-#include "actionlib/GoalID.h"
-#include "actionlib/GoalStatusArray.h"
-#include "actionlib/RequestType.h"
+#include "actionlib_msgs/GoalID.h"
+#include "actionlib_msgs/GoalStatusArray.h"
namespace actionlib
{
@@ -74,7 +73,7 @@
typedef boost::function<void (GoalHandleT) > TransitionCallback;
typedef boost::function<void (GoalHandleT, const FeedbackConstPtr&) > FeedbackCallback;
typedef boost::function<void (const ActionGoalConstPtr)> SendGoalFunc;
- typedef boost::function<void (const GoalID&)> CancelFunc;
+ typedef boost::function<void (const actionlib_msgs::GoalID&)> CancelFunc;
GoalManager() { }
@@ -85,7 +84,7 @@
TransitionCallback transition_cb = TransitionCallback(),
FeedbackCallback feedback_cb = FeedbackCallback() );
- void updateStatuses(const GoalStatusArrayConstPtr& status_array);
+ void updateStatuses(const actionlib_msgs::GoalStatusArrayConstPtr& status_array);
void updateFeedbacks(const ActionFeedbackConstPtr& action_feedback);
void updateResults(const ActionResultConstPtr& action_result);
@@ -224,11 +223,11 @@
ActionGoalConstPtr getActionGoal() const;
CommState getCommState() const;
- GoalStatus getGoalStatus() const;
+ actionlib_msgs::GoalStatus getGoalStatus() const;
ResultConstPtr getResult() const;
// Transitions caused by messages
- void updateStatus(GoalHandleT& gh, const GoalStatusArrayConstPtr& status_array);
+ void updateStatus(GoalHandleT& gh, const actionlib_msgs::GoalStatusArrayConstPtr& status_array);
void updateFeedback(GoalHandleT& gh, const ActionFeedbackConstPtr& feedback);
void updateResult(GoalHandleT& gh, const ActionResultConstPtr& result);
@@ -242,7 +241,7 @@
// State
CommState state_;
ActionGoalConstPtr action_goal_;
- GoalStatus latest_goal_status_;
+ actionlib_msgs::GoalStatus latest_goal_status_;
ActionResultConstPtr latest_result_;
// Callbacks
@@ -253,7 +252,7 @@
//! Change the state, as well as print out ROS_DEBUG info
void setCommState(const CommState& state);
void setCommState(const CommState::StateEnum& state);
- const GoalStatus* findGoalStatus(const std::vector<GoalStatus>& status_vec) const;
+ const actionlib_msgs::GoalStatus* findGoalStatus(const std::vector<actionlib_msgs::GoalStatus>& status_vec) const;
};
}
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine_imp.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine_imp.h 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/comm_state_machine_imp.h 2009-08-25 05:41:33 UTC (rev 22820)
@@ -64,7 +64,7 @@
}
template <class ActionSpec>
-GoalStatus CommStateMachine<ActionSpec>::getGoalStatus() const
+actionlib_msgs::GoalStatus CommStateMachine<ActionSpec>::getGoalStatus() const
{
return latest_goal_status_;
}
@@ -96,7 +96,7 @@
}
template <class ActionSpec>
-const GoalStatus* CommStateMachine<ActionSpec>::findGoalStatus(const std::vector<GoalStatus>& status_vec) const
+const actionlib_msgs::GoalStatus* CommStateMachine<ActionSpec>::findGoalStatus(const std::vector<actionlib_msgs::GoalStatus>& status_vec) const
{
for (unsigned int i=0; i<status_vec.size(); i++)
if (status_vec[i].goal_id.id == action_goal_->goal_id.id)
@@ -146,9 +146,9 @@
}
template <class ActionSpec>
-void CommStateMachine<ActionSpec>::updateStatus(GoalHandleT& gh, const GoalStatusArrayConstPtr& status_array)
+void CommStateMachine<ActionSpec>::updateStatus(GoalHandleT& gh, const actionlib_msgs::GoalStatusArrayConstPtr& status_array)
{
- const GoalStatus* goal_status = findGoalStatus(status_array->status_list);
+ const actionlib_msgs::GoalStatus* goal_status = findGoalStatus(status_array->status_list);
if (goal_status)
latest_goal_status_ = *goal_status;
@@ -171,19 +171,19 @@
{
switch (goal_status->status)
{
- case GoalStatus::PENDING:
+ case actionlib_msgs::GoalStatus::PENDING:
transitionToState(gh, CommState::PENDING); break;
- case GoalStatus::ACTIVE:
+ case actionlib_msgs::GoalStatus::ACTIVE:
transitionToState(gh, CommState::ACTIVE); break;
- case GoalStatus::PREEMPTED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
- case GoalStatus::REJECTED:
- case GoalStatus::RECALLED:
+ case actionlib_msgs::GoalStatus::PREEMPTED:
+ case actionlib_msgs::GoalStatus::SUCCEEDED:
+ case actionlib_msgs::GoalStatus::ABORTED:
+ case actionlib_msgs::GoalStatus::REJECTED:
+ case actionlib_msgs::GoalStatus::RECALLED:
transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
- case GoalStatus::PREEMPTING:
+ case actionlib_msgs::GoalStatus::PREEMPTING:
transitionToState(gh, CommState::PREEMPTING);
- case GoalStatus::RECALLING:
+ case actionlib_msgs::GoalStatus::RECALLING:
transitionToState(gh, CommState::RECALLING);
default:
ROS_ERROR("BUG: Got an unknown status from the ActionServer. status = %u", goal_status->status);
@@ -196,22 +196,22 @@
{
switch (goal_status->status)
{
- case GoalStatus::PENDING:
+ case actionlib_msgs::GoalStatus::PENDING:
break;
- case GoalStatus::ACTIVE:
+ case actionlib_msgs::GoalStatus::ACTIVE:
transitionToState(gh, CommState::ACTIVE);
break;
- case GoalStatus::PREEMPTED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
- case GoalStatus::REJECTED:
- case GoalStatus::RECALLED:
+ case actionlib_msgs::GoalStatus::PREEMPTED:
+ case actionlib_msgs::GoalStatus::SUCCEEDED:
+ case actionlib_msgs::GoalStatus::ABORTED:
+ case actionlib_msgs::GoalStatus::REJECTED:
+ case actionlib_msgs::GoalStatus::RECALLED:
transitionToState(gh, CommState::WAITING_FOR_RESULT);
break;
- case GoalStatus::PREEMPTING:
+ case actionlib_msgs::GoalStatus::PREEMPTING:
transitionToState(gh, CommState::PREEMPTING);
break;
- case GoalStatus::RECALLING:
+ case actionlib_msgs::GoalStatus::RECALLING:
transitionToState(gh, CommState::RECALLING);
break;
default:
@@ -224,21 +224,21 @@
{
switch (goal_status->status)
{
- case GoalStatus::PENDING:
+ case actionlib_msgs::GoalStatus::PENDING:
ROS_ERROR("Invalid transition from ACTIVE to PENDING"); break;
- case GoalStatus::ACTIVE:
+ case actionlib_msgs::GoalStatus::ACTIVE:
break;
- case GoalStatus::REJECTED:
+ case actionlib_msgs::GoalStatus::REJECTED:
ROS_ERROR("Invalid transition from ACTIVE to REJECTED"); break;
- case GoalStatus::RECALLING:
+ case actionlib_msgs::GoalStatus::RECALLING:
ROS_ERROR("Invalid transition from ACTIVE to RECALLING"); break;
- case GoalStatus::RECALLED:
+ case actionlib_msgs::GoalStatus::RECALLED:
ROS_ERROR("Invalid transition from ACTIVE to RECALLED"); break;
- case GoalStatus::PREEMPTED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
+ case actionlib_msgs::GoalStatus::PREEMPTED:
+ case actionlib_msgs::GoalStatus::SUCCEEDED:
+ case actionlib_msgs::GoalStatus::ABORTED:
transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
- case GoalStatus::PREEMPTING:
+ case actionlib_msgs::GoalStatus::PREEMPTING:
transitionToState(gh, CommState::PREEMPTING); break;
default:
ROS_ERROR("BUG: Got an unknown goal status from the ActionServer. status = %u", goal_status->status);
@@ -250,18 +250,18 @@
{
switch (goal_status->status)
{
- case GoalStatus::PENDING :
+ case actionlib_msgs::GoalStatus::PENDING :
ROS_ERROR("Invalid Transition from WAITING_FOR_RESUT to PENDING"); break;
- case GoalStatus::PREEMPTING:
+ case actionlib_msgs::GoalStatus::PREEMPTING:
ROS_ERROR("Invalid Transition from WAITING_FOR_RESUT to PREEMPTING"); break;
- case GoalStatus::RECALLING:
+ case actionlib_msgs::GoalStatus::RECALLING:
ROS_ERROR("Invalid Transition from WAITING_FOR_RESUT to RECALLING"); break;
- case GoalStatus::ACTIVE:
- case GoalStatus::PREEMPTED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
- case GoalStatus::REJECTED:
- case GoalStatus::RECALLED:
+ case actionlib_msgs::GoalStatus::ACTIVE:
+ case actionlib_msgs::GoalStatus::PREEMPTED:
+ case actionlib_msgs::GoalStatus::SUCCEEDED:
+ case actionlib_msgs::GoalStatus::ABORTED:
+ case actionlib_msgs::GoalStatus::REJECTED:
+ case actionlib_msgs::GoalStatus::RECALLED:
break;
default:
ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
@@ -273,19 +273,19 @@
{
switch (goal_status->status)
{
- case GoalStatus::PENDING:
+ case actionlib_msgs::GoalStatus::PENDING:
break;
- case GoalStatus::ACTIVE:
+ case actionlib_msgs::GoalStatus::ACTIVE:
break;
- case GoalStatus::PREEMPTED:
- case GoalStatus::RECALLED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
- case GoalStatus::REJECTED:
+ case actionlib_msgs::GoalStatus::PREEMPTED:
+ case actionlib_msgs::GoalStatus::RECALLED:
+ case actionlib_msgs::GoalStatus::SUCCEEDED:
+ case actionlib_msgs::GoalStatus::ABORTED:
+ case actionlib_msgs::GoalStatus::REJECTED:
transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
- case GoalStatus::PREEMPTING:
+ case actionlib_msgs::GoalStatus::PREEMPTING:
transitionToState(gh, CommState::PREEMPTING); break;
- case GoalStatus::RECALLING:
+ case actionlib_msgs::GoalStatus::RECALLING:
transitionToState(gh, CommState::RECALLING); break;
default:
ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
@@ -297,19 +297,19 @@
{
switch (goal_status->status)
{
- case GoalStatus::PENDING:
+ case actionlib_msgs::GoalStatus::PENDING:
ROS_ERROR("Invalid Transition from RECALLING to PENDING"); break;
- case GoalStatus::ACTIVE:
+ case actionlib_msgs::GoalStatus::ACTIVE:
ROS_ERROR("Invalid Transition from RECALLING to ACTIVE"); break;
- case GoalStatus::PREEMPTED:
- case GoalStatus::RECALLED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
- case GoalStatus::REJECTED:
+ case actionlib_msgs::GoalStatus::PREEMPTED:
+ case actionlib_msgs::GoalStatus::RECALLED:
+ case actionlib_msgs::GoalStatus::SUCCEEDED:
+ case actionlib_msgs::GoalStatus::ABORTED:
+ case actionlib_msgs::GoalStatus::REJECTED:
transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
- case GoalStatus::PREEMPTING:
+ case actionlib_msgs::GoalStatus::PREEMPTING:
transitionToState(gh, CommState::PREEMPTING); break;
- case GoalStatus::RECALLING:
+ case actionlib_msgs::GoalStatus::RECALLING:
break;
default:
ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
@@ -321,22 +321,22 @@
{
switch (goal_status->status)
{
- case GoalStatus::PENDING:
+ case actionlib_msgs::GoalStatus::PENDING:
ROS_ERROR("Invalid Transition from PREEMPTING to PENDING"); break;
- case GoalStatus::ACTIVE:
+ case actionlib_msgs::GoalStatus::ACTIVE:
ROS_ERROR("Invalid Transition from PREEMPTING to ACTIVE"); break;
- case GoalStatus::REJECTED:
+ case actionlib_msgs::GoalStatus::REJECTED:
ROS_ERROR("Invalid Transition from PREEMPTING to REJECTED"); break;
- case GoalStatus::RECALLING:
+ case actionlib_msgs::GoalStatus::RECALLING:
ROS_ERROR("Invalid Transition from PREEMPTING to RECALLING"); break;
- case GoalStatus::RECALLED:
+ case actionlib_msgs::GoalStatus::RECALLED:
ROS_ERROR("Invalid Transition from PREEMPTING to RECALLED"); break;
break;
- case GoalStatus::PREEMPTED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
+ case actionlib_msgs::GoalStatus::PREEMPTED:
+ case actionlib_msgs::GoalStatus::SUCCEEDED:
+ case actionlib_msgs::GoalStatus::ABORTED:
transitionToState(gh, CommState::WAITING_FOR_RESULT); break;
- case GoalStatus::PREEMPTING:
+ case actionlib_msgs::GoalStatus::PREEMPTING:
break;
default:
ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
@@ -348,19 +348,19 @@
{
switch (goal_status->status)
{
- case GoalStatus::PENDING:
+ case actionlib_msgs::GoalStatus::PENDING:
ROS_ERROR("Invalid Transition from DONE to PENDING"); break;
- case GoalStatus::ACTIVE:
+ case actionlib_msgs::GoalStatus::ACTIVE:
ROS_ERROR("Invalid Transition from DONE to ACTIVE"); break;
- case GoalStatus::RECALLING:
+ case actionlib_msgs::GoalStatus::RECALLING:
ROS_ERROR("Invalid Transition from DONE to RECALLING"); break;
- case GoalStatus::PREEMPTING:
+ case actionlib_msgs::GoalStatus::PREEMPTING:
ROS_ERROR("Invalid Transition from DONE to PREEMPTING"); break;
- case GoalStatus::PREEMPTED:
- case GoalStatus::SUCCEEDED:
- case GoalStatus::ABORTED:
- case GoalStatus::RECALLED:
- case GoalStatus::REJECTED:
+ case actionlib_msgs::GoalStatus::PREEMPTED:
+ case actionlib_msgs::GoalStatus::SUCCEEDED:
+ case actionlib_msgs::GoalStatus::ABORTED:
+ case actionlib_msgs::GoalStatus::RECALLED:
+ case actionlib_msgs::GoalStatus::REJECTED:
break;
default:
ROS_ERROR("BUG: Got an unknown state from the ActionServer. status = %u", goal_status->status);
@@ -379,7 +379,7 @@
void CommStateMachine<ActionSpec>::processLost(GoalHandleT& gh)
{
ROS_DEBUG("Processing LOST");
- latest_goal_status_.status = GoalStatus::LOST;
+ latest_goal_status_.status = actionlib_msgs::GoalStatus::LOST;
transitionToState(gh, CommState::DONE);
}
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_manager_imp.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_manager_imp.h 2009-08-25 05:40:49 UTC (rev 22819)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/goal_manager_imp.h 2009-08-25 05:41:33 UTC (rev 22820)
@@ -85,7 +85,7 @@
}
template<class ActionSpec>
-void GoalManager<ActionSpec>::updateStatuses(const GoalStatusArrayConstPtr& status_array)
+void GoalManager<ActionSpec>::updateStatuses(const actionlib_msgs::GoalStatusArrayConstPtr& status_array)
{
boost::recursive_mutex::scoped_lock lock(list_mutex_);
typename ManagedListT::iterator it = list_.begin();
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/goal_id_generator.h
===================================================================
--...
[truncated message content] |
|
From: <mee...@us...> - 2009-08-25 17:25:24
|
Revision: 22850
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22850&view=rev
Author: meeussen
Date: 2009-08-25 17:25:12 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
kdl parser supports parsing form file, string, xml and robot model. Put in new namespace to avoid conflicts with old parser
Modified Paths:
--------------
pkg/trunk/stacks/mechanism/kdl_parser/include/kdl_parser/dom_parser.hpp
pkg/trunk/stacks/mechanism/kdl_parser/src/dom_parser.cpp
pkg/trunk/stacks/mechanism/kdl_parser/test/example_dom.cpp
pkg/trunk/stacks/mechanism/mechanism_model/src/robot.cpp
pkg/trunk/stacks/mechanism/robot_state_publisher/src/state_publisher.cpp
pkg/trunk/stacks/mechanism/robot_state_publisher/test/test_publisher.cpp
pkg/trunk/util/pr2_ik/src/pr2_ik_solver.cpp
Modified: pkg/trunk/stacks/mechanism/kdl_parser/include/kdl_parser/dom_parser.hpp
===================================================================
--- pkg/trunk/stacks/mechanism/kdl_parser/include/kdl_parser/dom_parser.hpp 2009-08-25 17:18:49 UTC (rev 22849)
+++ pkg/trunk/stacks/mechanism/kdl_parser/include/kdl_parser/dom_parser.hpp 2009-08-25 17:25:12 UTC (rev 22850)
@@ -43,9 +43,12 @@
using namespace std;
-namespace KDL{
+namespace kdl_parser{
-bool treeFromRobotModel(urdf::Model& robot_model, Tree& tree);
+bool treeFromFile(const string& file, KDL::Tree& tree);
+bool treeFromString(const string& xml, KDL::Tree& tree);
+bool treeFromXml(TiXmlElement *root, KDL::Tree& tree);
+bool treeFromRobotModel(urdf::Model& robot_model, KDL::Tree& tree);
}
#endif
Modified: pkg/trunk/stacks/mechanism/kdl_parser/src/dom_parser.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/kdl_parser/src/dom_parser.cpp 2009-08-25 17:18:49 UTC (rev 22849)
+++ pkg/trunk/stacks/mechanism/kdl_parser/src/dom_parser.cpp 2009-08-25 17:25:12 UTC (rev 22850)
@@ -36,9 +36,12 @@
#include "kdl_parser/dom_parser.hpp"
#include <kdl/frames_io.hpp>
+
+
using namespace std;
+using namespace KDL;
-namespace KDL{
+namespace kdl_parser{
// construct vector
@@ -125,12 +128,47 @@
}
+bool treeFromFile(const string& file, Tree& tree)
+{
+ TiXmlDocument urdf_xml;
+ urdf_xml.LoadFile(file);
+ TiXmlElement *root = urdf_xml.FirstChildElement("robot");
+ if (!root){
+ cerr << "Could not parse the xml" << endl;
+ return false;
+ }
+ return treeFromXml(root, tree);
+}
+
+bool treeFromString(const string& xml, Tree& tree)
+{
+ TiXmlDocument urdf_xml;
+ urdf_xml.Parse(xml.c_str());
+ TiXmlElement *root = urdf_xml.FirstChildElement("robot");
+ if (!root){
+ cerr << "Could not parse the xml" << endl;
+ return false;
+ }
+ return treeFromXml(root, tree);
+}
+
+bool treeFromXml(TiXmlElement *root, Tree& tree)
+{
+ urdf::Model robot_model;
+ if (!robot_model.initFile("pr2.urdf")){
+ cerr << "Could not generate robot model" << endl;
+ return false;
+ }
+ return treeFromRobotModel(robot_model, tree);
+}
+
+
bool treeFromRobotModel(urdf::Model& robot_model, Tree& tree)
{
tree = Tree(robot_model.getRoot()->name);
- // add all children
+ // add all children
for (size_t i=0; i<robot_model.getRoot()->child_links.size(); i++)
if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree))
return false;
Modified: pkg/trunk/stacks/mechanism/kdl_parser/test/example_dom.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/kdl_parser/test/example_dom.cpp 2009-08-25 17:18:49 UTC (rev 22849)
+++ pkg/trunk/stacks/mechanism/kdl_parser/test/example_dom.cpp 2009-08-25 17:25:12 UTC (rev 22850)
@@ -50,7 +50,7 @@
{cerr << "Could not generate robot model" << endl; return false;}
Tree my_tree;
- if (!treeFromRobotModel(robot_model, my_tree))
+ if (!kdl_parser::treeFromRobotModel(robot_model, my_tree))
{cerr << "Could not extract kdl tree" << endl; return false;}
// walk through tree
Modified: pkg/trunk/stacks/mechanism/mechanism_model/src/robot.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_model/src/robot.cpp 2009-08-25 17:18:49 UTC (rev 22849)
+++ pkg/trunk/stacks/mechanism/mechanism_model/src/robot.cpp 2009-08-25 17:25:12 UTC (rev 22850)
@@ -58,7 +58,7 @@
}
// Constructs a kdl robot model from the robot description.
- if (!treeFromRobotModel(robot_description, robot_model_)){
+ if (!kdl_parser::treeFromRobotModel(robot_description, robot_model_)){
ROS_ERROR("Mechanism Model failed to construct a kdl tree from the robot model");
return false;
}
Modified: pkg/trunk/stacks/mechanism/robot_state_publisher/src/state_publisher.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/robot_state_publisher/src/state_publisher.cpp 2009-08-25 17:18:49 UTC (rev 22849)
+++ pkg/trunk/stacks/mechanism/robot_state_publisher/src/state_publisher.cpp 2009-08-25 17:25:12 UTC (rev 22850)
@@ -73,7 +73,7 @@
// constructs a kdl tree from the robot model
Tree tree;
- if (!treeFromRobotModel(robot_model, tree)){
+ if (!kdl_parser::treeFromRobotModel(robot_model, tree)){
ROS_ERROR("Failed to extract kdl tree from robot model");
return -1;
}
Modified: pkg/trunk/stacks/mechanism/robot_state_publisher/test/test_publisher.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/robot_state_publisher/test/test_publisher.cpp 2009-08-25 17:18:49 UTC (rev 22849)
+++ pkg/trunk/stacks/mechanism/robot_state_publisher/test/test_publisher.cpp 2009-08-25 17:25:12 UTC (rev 22850)
@@ -73,7 +73,7 @@
// constructs a kdl tree from the robot model
Tree tree;
- if (!treeFromRobotModel(robot_model, tree))
+ if (!kdl_parser::treeFromRobotModel(robot_model, tree))
ROS_ERROR("Failed to extract kdl tree from robot model");
publisher = new JointStateListener(tree);
Modified: pkg/trunk/util/pr2_ik/src/pr2_ik_solver.cpp
===================================================================
--- pkg/trunk/util/pr2_ik/src/pr2_ik_solver.cpp 2009-08-25 17:18:49 UTC (rev 22849)
+++ pkg/trunk/util/pr2_ik/src/pr2_ik_solver.cpp 2009-08-25 17:25:12 UTC (rev 22850)
@@ -178,7 +178,7 @@
// create robot chain from root to tip
KDL::Tree tree;
- if (!KDL::treeFromRobotModel(robot_model_, tree))
+ if (!kdl_parser::treeFromRobotModel(robot_model_, tree))
ROS_ERROR("Could not initialize tree object");
if (!tree.getChain(root_name, tip_name, chain_))
ROS_ERROR("Could not initialize chain object");
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-08-25 17:42:16
|
Revision: 22859
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22859&view=rev
Author: vijaypradeep
Date: 2009-08-25 17:41:58 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
Moving calibration stack into stacks
Added Paths:
-----------
pkg/trunk/stacks/calibration/
pkg/trunk/stacks/calibration/calibration_msgs/
pkg/trunk/stacks/calibration/calibration_msgs/CMakeLists.txt
pkg/trunk/stacks/calibration/calibration_msgs/Makefile
pkg/trunk/stacks/calibration/calibration_msgs/manifest.xml
pkg/trunk/stacks/calibration/calibration_msgs/msg/
pkg/trunk/stacks/calibration/calibration_msgs/msg/Interval.msg
pkg/trunk/stacks/calibration/joint_states_settler/
pkg/trunk/stacks/calibration/joint_states_settler/CMakeLists.txt
pkg/trunk/stacks/calibration/joint_states_settler/Makefile
pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml
pkg/trunk/stacks/calibration/settlerlib/
pkg/trunk/stacks/calibration/settlerlib/CMakeLists.txt
pkg/trunk/stacks/calibration/settlerlib/Makefile
pkg/trunk/stacks/calibration/settlerlib/include/
pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/
pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/deflated.h
pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/sorted_deque.h
pkg/trunk/stacks/calibration/settlerlib/manifest.xml
pkg/trunk/stacks/calibration/settlerlib/test/
pkg/trunk/stacks/calibration/settlerlib/test/CMakeLists.txt
pkg/trunk/stacks/calibration/settlerlib/test/sorted_deque_unittest.cpp
pkg/trunk/stacks/calibration/stack.xml
Removed Paths:
-------------
pkg/trunk/calibration/calibration_msgs/CMakeLists.txt
pkg/trunk/calibration/calibration_msgs/Makefile
pkg/trunk/calibration/calibration_msgs/manifest.xml
pkg/trunk/calibration/calibration_msgs/msg/Interval.msg
pkg/trunk/calibration/joint_states_settler/CMakeLists.txt
pkg/trunk/calibration/joint_states_settler/Makefile
pkg/trunk/calibration/joint_states_settler/manifest.xml
pkg/trunk/calibration/settlerlib/CMakeLists.txt
pkg/trunk/calibration/settlerlib/Makefile
pkg/trunk/calibration/settlerlib/include/settlerlib/deflated.h
pkg/trunk/calibration/settlerlib/include/settlerlib/sorted_deque.h
pkg/trunk/calibration/settlerlib/manifest.xml
pkg/trunk/calibration/settlerlib/test/CMakeLists.txt
pkg/trunk/calibration/settlerlib/test/sorted_deque_unittest.cpp
pkg/trunk/calibration/stack.xml
Deleted: pkg/trunk/calibration/calibration_msgs/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/calibration_msgs/CMakeLists.txt 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/calibration_msgs/CMakeLists.txt 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,22 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Set the build type. Options are:
-# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
-# Debug : w/ debug symbols, w/o optimization
-# Release : w/o debug symbols, w/ optimization
-# RelWithDebInfo : w/ debug symbols, w/ optimization
-# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rosbuild_init()
-
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-#uncomment if you have defined messages
-rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
Deleted: pkg/trunk/calibration/calibration_msgs/Makefile
===================================================================
--- pkg/trunk/calibration/calibration_msgs/Makefile 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/calibration_msgs/Makefile 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Deleted: pkg/trunk/calibration/calibration_msgs/manifest.xml
===================================================================
--- pkg/trunk/calibration/calibration_msgs/manifest.xml 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/calibration_msgs/manifest.xml 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,14 +0,0 @@
-<package>
- <description brief="calibration_msgs">
-
- calibration_msgs
-
- </description>
- <author>Vijay Pradeep</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://pr.willowgarage.com/wiki/calibration_msgs</url>
-
-</package>
-
-
Deleted: pkg/trunk/calibration/calibration_msgs/msg/Interval.msg
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/Interval.msg 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/calibration_msgs/msg/Interval.msg 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,2 +0,0 @@
-time start
-time end
Deleted: pkg/trunk/calibration/joint_states_settler/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/joint_states_settler/CMakeLists.txt 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/joint_states_settler/CMakeLists.txt 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,30 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Set the build type. Options are:
-# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
-# Debug : w/ debug symbols, w/o optimization
-# Release : w/o debug symbols, w/ optimization
-# RelWithDebInfo : w/ debug symbols, w/ optimization
-# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rosbuild_init()
-
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-#uncomment if you have defined messages
-#rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
-
-#common commands for building c++ executables and libraries
-#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-#rosbuild_add_executable(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
Deleted: pkg/trunk/calibration/joint_states_settler/Makefile
===================================================================
--- pkg/trunk/calibration/joint_states_settler/Makefile 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/joint_states_settler/Makefile 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Deleted: pkg/trunk/calibration/joint_states_settler/manifest.xml
===================================================================
--- pkg/trunk/calibration/joint_states_settler/manifest.xml 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/joint_states_settler/manifest.xml 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,17 +0,0 @@
-<package>
- <description brief="joint_states_settler">
-
- joint_states_settler
-
- </description>
- <author>Vijay Pradeep</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://pr.willowgarage.com/wiki/joint_states_settler</url>
- <depend package="roscpp"/>
- <depend package="actionlib"/>
- <depend package="mechanism_msgs"/>
-
-</package>
-
-
Deleted: pkg/trunk/calibration/settlerlib/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/settlerlib/CMakeLists.txt 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/settlerlib/CMakeLists.txt 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,31 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Set the build type. Options are:
-# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
-# Debug : w/ debug symbols, w/o optimization
-# Release : w/o debug symbols, w/ optimization
-# RelWithDebInfo : w/ debug symbols, w/ optimization
-# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rosbuild_init()
-
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-#uncomment if you have defined messages
-#rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
-
-#common commands for building c++ executables and libraries
-#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-#rosbuild_add_executable(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
-add_subdirectory(test EXCLUDE_FROM_ALL)
Deleted: pkg/trunk/calibration/settlerlib/Makefile
===================================================================
--- pkg/trunk/calibration/settlerlib/Makefile 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/settlerlib/Makefile 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Deleted: pkg/trunk/calibration/settlerlib/include/settlerlib/deflated.h
===================================================================
--- pkg/trunk/calibration/settlerlib/include/settlerlib/deflated.h 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/settlerlib/include/settlerlib/deflated.h 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,56 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#ifndef SETTLERLIB_DEFLATED_H_
-#define SETTLERLIB_DEFLATED_H_
-
-#include <boost/shared_ptr.hpp>
-#include "roslib/Header.h"
-
-namespace settlerlib
-{
-
-class Deflated
-{
-public:
- roslib::Header header; // Need header to put in cache
- std::vector<double> channel_;
-};
-
-}
-
-#endif
-
-
-
Deleted: pkg/trunk/calibration/settlerlib/include/settlerlib/sorted_deque.h
===================================================================
--- pkg/trunk/calibration/settlerlib/include/settlerlib/sorted_deque.h 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/settlerlib/include/settlerlib/sorted_deque.h 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,337 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2009, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#include <deque>
-#include <boost/shared_ptr.hpp>
-#include <boost/bind.hpp>
-#include <boost/function.hpp>
-#include "ros/time.h"
-#include "ros/console.h"
-
-#ifndef SETTLERLIB_SORTED_DEQUE_H_
-#define SETTLERLIB_SORTED_DEQUE_H_
-
-#define DEQUE_DEBUG(fmt, ...) \
- ROS_DEBUG_NAMED(logger_.c_str(), fmt,##__VA_ARGS__)
-
-namespace settlerlib
-{
-
-/**
- * \brief Adds helper routines to the STL Deque for holding messages with headers
- *
- * Provides functionality to store a deque of messages that are sorted by timestamp. Users can
- * then extract specific intervals of messages. Old messages fall off the back of the deque
- */
-template <class M>
-class SortedDeque : public std::deque<M>
-{
-public:
-
- using std::deque<M>::size;
- using std::deque<M>::front;
- using std::deque<M>::pop_front;
- using std::deque<M>::begin;
- using std::deque<M>::end;
- using std::deque<M>::rbegin;
- using std::deque<M>::rend;
- using std::deque<M>::insert;
- using std::deque<M>::at;
- using std::deque<M>::erase;
-
- /**
- * \brief Assumes that '.header.stamp' can be used to get the stamp used for sorting
- * \param logger name of rosconsole logger to display debugging output from this deque
- */
- SortedDeque(std::string logger = "deque") : std::deque<M>(), logger_(logger)
- {
- getStamp = &SortedDeque<M>::getStructStamp;
- }
-
- /**
- * \brief Advanced constructor, allowing user to specific the timestamp getter method
- * \param getStampFunc Function pointer specific how to get timestamp from the contained datatype
- * \param logger name of rosconsole logger to display debugging output from this deque
- */
- SortedDeque(boost::function<const ros::Time&(const M&)> getStampFunc,
- std::string logger = "deque") : std::deque<M>(), logger_(logger)
- {
- getStamp = getStampFunc;
- }
-
- /**
- * \brief Set the maximum # of elements this deque can hold. Older elems are popped once the length is exeeded
- * \param max_size The maximum # of elems to hold. Passing in 0 implies that the queue can grow indefinitely (which is a recipe for a memory leak)
- */
- void setMaxSize(unsigned int max_size)
- {
- max_size_ = max_size ;
- }
-
- /**
- * \brief Add a new element to the deque, correctly sorted by timestamp
- * \param msg the element to add
- */
- void add(const M& msg)
- {
- DEQUE_DEBUG("Called add()");
- DEQUE_DEBUG_STATS(" ");
- if (max_size_ != 0)
- {
- while (size() >= max_size_) // Keep popping off old data until we have space for a new msg
- {
- pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
- DEQUE_DEBUG(" Popping element");
- DEQUE_DEBUG_STATS(" ");
- }
- }
-
- typename std::deque<M>::reverse_iterator rev_it = rbegin();
-
- // Keep walking backwards along deque until we hit the beginning,
- // or until we find a timestamp that's smaller than (or equal to) msg's timestamp
- while(rev_it != rend() && getStamp(*rev_it) > getStamp(msg))
- rev_it++;
-
- // Add msg to the cache
- insert(rev_it.base(), msg);
- DEQUE_DEBUG(" Done inserting");
- DEQUE_DEBUG_STATS(" ");
- }
-
- /**
- * \brief Extract all the elements that occur in the interval between the start and end times
- * \param start The start of the interval
- * \param end The end of the interval
- */
- std::vector<M> getInterval(const ros::Time& start, const ros::Time& end)
- {
- // Find the starting index. (Find the first index after [or at] the start of the interval)
- unsigned int start_index = 0 ;
- while(start_index < size() &&
- getStamp(at(start_index)) < start)
- {
- start_index++ ;
- }
-
- // Find the ending index. (Find the first index after the end of interval)
- unsigned int end_index = start_index ;
- while(end_index < size() &&
- getStamp(at(end_index)) <= end)
- {
- end_index++ ;
- }
-
- std::vector<M> interval_elems ;
- interval_elems.reserve(end_index - start_index) ;
- for (unsigned int i=start_index; i<end_index; i++)
- {
- interval_elems.push_back(at(i)) ;
- }
-
- return interval_elems ;
- }
-
- /**
- * Retrieve the smallest interval of messages that surrounds an interval from start to end.
- * If the messages in the cache do not surround (start,end), then this will return the interval
- * that gets closest to surrounding (start,end)
- */
- std::vector<M> getSurroundingInterval(const ros::Time& start, const ros::Time& end)
- {
- // Find the starting index. (Find the first index after [or at] the start of the interval)
- unsigned int start_index = size()-1;
- while(start_index > 0 &&
- getStamp(at(start_index)) > start)
- {
- start_index--;
- }
- unsigned int end_index = start_index;
- while(end_index < size()-1 &&
- getStamp(at(end_index)) < end)
- {
- end_index++;
- }
-
- std::vector<M> interval_elems;
- interval_elems.reserve(end_index - start_index + 1) ;
- for (unsigned int i=start_index; i<=end_index; i++)
- {
- interval_elems.push_back(at(i)) ;
- }
-
- return interval_elems;
- }
-
- /**
- * \brief Grab the oldest element that occurs right before the specified time.
- * \param time The time that must occur after the elem
- * \param out Output: Stores the extracted elem
- * \return False there are no elems before the specified time
- **/
- bool getElemBeforeTime(const ros::Time& time, M& out)
- {
- unsigned int i=0 ;
- int elem_index = -1 ;
- while (i<size() &&
- getStamp(at(i)) < time)
- {
- elem_index = i ;
- i++ ;
- }
-
- if (elem_index >= 0)
- {
- out = at(elem_index);
- return true;
- }
- //out = M();
- return false;
- }
-
- /**
- * \brief Grab the oldest element that occurs right after the specified time.
- * \param time The time that must occur before the elem
- * \param out Output: Stores the extracted elem
- * \return False there are no elems after the specified time
- */
- bool getElemAfterTime(const ros::Time& time, M& out)
- {
- int i=size()-1 ;
- int elem_index = -1 ;
- while (i>=0 &&
- getStamp(at(i)) > time)
- {
- elem_index = i ;
- i-- ;
- }
-
- if (elem_index >= 0)
- {
- out = at(elem_index);
- return true;
- }
- //out = M();
- return false;
- }
-
- /**
- * \brief Get the elem that occurs closest to the specified time
- * \param time The time that we want to closest elem to
- * \param out Output: Stores the extracted elem
- * \return False if the queue is empty
- */
- bool getClosestElem(const ros::Time& time, M& out)
- {
- if (size() == 0)
- return false;
-
- typename std::deque<M>::iterator it = begin();
- typename std::deque<M>::iterator best = it;
-
- double best_diff = fabs( (time - getStamp(*best)).toSec());
-
- while (it != end())
- {
- double cur_diff = fabs( (time - getStamp(*it)).toSec());
- if (cur_diff < best_diff)
- {
- best_diff = cur_diff;
- best = it;
- }
- ++it;
- }
- out = *best;
- return true;
- }
-
-
- /**
- * \brief Removes all elements that occur before the specified time
- * \param time All elems that occur before this are removed from the deque
- */
- void removeAllBeforeTime(const ros::Time& time)
- {
- DEQUE_DEBUG("Called removeAllBeforeTime()");
- DEQUE_DEBUG(" Erasing all elems before time: %u %u", time.sec, time.nsec);
- typename std::deque<M>::iterator it = begin();
-
- while (size() > 0 && getStamp(front()) < time)
- {
- DEQUE_DEBUG(" Erasing elem at time: %u, %u", getStamp(front()).sec, getStamp(front()).nsec);
- pop_front();
- DEQUE_DEBUG(" Erased an elem");
- DEQUE_DEBUG_STATS(" ");
- }
- DEQUE_DEBUG(" Done erasing elems");
- }
-
- static const ros::Time& getPtrStamp(const M& m)
- {
- return m->header.stamp;
- }
-
- static const ros::Time& getStructStamp(const M& m)
- {
- return m.header.stamp;
- }
-
- static const ros::Time& getHeaderStamp(const M& m)
- {
- return m.stamp;
- }
-private:
- unsigned int max_size_;
- std::string logger_;
-
-
- /*const ros::Time& getStamp(const M& m)
- {
- return getStructStamp(m);
- }*/
-
- boost::function<const ros::Time&(const M&)> getStamp;
-
- inline void DEQUE_DEBUG_STATS(const std::string& prefix)
- {
- DEQUE_DEBUG("%sdeque.size(): %u max_size: %u", prefix.c_str(), (unsigned int) size(), max_size_);
- }
-};
-
-}
-
-#undef DEQUE_DEBUG
-#undef DEQUE_WARN
-
-#endif
Deleted: pkg/trunk/calibration/settlerlib/manifest.xml
===================================================================
--- pkg/trunk/calibration/settlerlib/manifest.xml 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/settlerlib/manifest.xml 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,14 +0,0 @@
-<package>
- <description brief="A set of routines to help with creating a settler">
- Defines helper functions and routines that greatly help when trying to create a settler
- for a specific sensor channel.
- </description>
- <author>Vijay Pradeep</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://pr.willowgarage.com/wiki/settlerlib</url>
- <depend package="roscpp"/>
-
-</package>
-
-
Deleted: pkg/trunk/calibration/settlerlib/test/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/settlerlib/test/CMakeLists.txt 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/settlerlib/test/CMakeLists.txt 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,5 +0,0 @@
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
-
-# ********** Tests **********
-
-rospack_add_gtest(test/sorted_deque_unittest sorted_deque_unittest.cpp)
Deleted: pkg/trunk/calibration/settlerlib/test/sorted_deque_unittest.cpp
===================================================================
--- pkg/trunk/calibration/settlerlib/test/sorted_deque_unittest.cpp 2009-08-25 17:40:40 UTC (rev 22858)
+++ pkg/trunk/calibration/settlerlib/test/sorted_deque_unittest.cpp 2009-08-25 17:41:58 UTC (rev 22859)
@@ -1,222 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-
-#include <gtest/gtest.h>
-
-#include "settlerlib/sorted_deque.h"
-
-using namespace std;
-using namespace settlerlib;
-
-struct Header
-{
- ros::Time stamp ;
-} ;
-
-
-struct Msg
-{
- Header header ;
- int data ;
-} ;
-
-void fillEasy(SortedDeque<Msg>& sd, unsigned int start, unsigned int end)
-{
- for (unsigned int i=start; i < end; i++)
- {
- Msg msg ;
- msg.data = i ;
- msg.header.stamp.fromSec(i*10) ;
-
- sd.add(msg) ;
- }
-}
-
-TEST(SortedDeque, easyInterval)
-{
- SortedDeque<Msg> sd;
- sd.setMaxSize(10);
-
- fillEasy(sd, 0, 5);
-
- vector<Msg> interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
-
- EXPECT_EQ(interval_data.size(), (unsigned int) 3) ;
- EXPECT_EQ(interval_data[0].data, 1) ;
- EXPECT_EQ(interval_data[1].data, 2) ;
- EXPECT_EQ(interval_data[2].data, 3) ;
-
- // Look for an interval past the end of the cache
- interval_data = sd.getInterval(ros::Time().fromSec(55), ros::Time().fromSec(65)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
-
- // Look for an interval that fell off the back of the cache
- fillEasy(sd, 5, 20) ;
- interval_data = sd.getInterval(ros::Time().fromSec(5), ros::Time().fromSec(35)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 0) ;
-}
-
-TEST(SortedDeque, easySurroundingInterval)
-{
- SortedDeque<Msg> cache;
- cache.setMaxSize(10);
- fillEasy(cache, 1, 6);
-
- vector<Msg> interval_data;
- interval_data = cache.getSurroundingInterval(ros::Time(15,0), ros::Time(35,0)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 4);
- EXPECT_EQ(interval_data[0].data, 1);
- EXPECT_EQ(interval_data[1].data, 2);
- EXPECT_EQ(interval_data[2].data, 3);
- EXPECT_EQ(interval_data[3].data, 4);
-
- interval_data = cache.getSurroundingInterval(ros::Time(0,0), ros::Time(35,0)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 4);
- EXPECT_EQ(interval_data[0].data, 1);
-
- interval_data = cache.getSurroundingInterval(ros::Time(35,0), ros::Time(35,0)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 2);
- EXPECT_EQ(interval_data[0].data, 3);
- EXPECT_EQ(interval_data[1].data, 4);
-
- interval_data = cache.getSurroundingInterval(ros::Time(55,0), ros::Time(55,0)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 1);
- EXPECT_EQ(interval_data[0].data, 5);
-}
-
-Msg buildMsg(double time, int data)
-{
- Msg msg;
- msg.data = data;
- msg.header.stamp.fromSec(time);
- return msg;
-}
-
-TEST(SortedDeque, easyUnsorted)
-{
- SortedDeque<Msg> cache;
- cache.setMaxSize(10);
-
- cache.add(buildMsg(10.0, 1)) ;
- cache.add(buildMsg(30.0, 3)) ;
- cache.add(buildMsg(70.0, 7)) ;
- cache.add(buildMsg( 5.0, 0)) ;
- cache.add(buildMsg(20.0, 2)) ;
-
- vector<Msg> interval_data = cache.getInterval(ros::Time().fromSec(3), ros::Time().fromSec(15)) ;
-
- EXPECT_EQ(interval_data.size(), (unsigned int) 2) ;
- EXPECT_EQ(interval_data[0].data, 0) ;
- EXPECT_EQ(interval_data[1].data, 1) ;
-
- // Grab all the data
- interval_data = cache.getInterval(ros::Time().fromSec(0), ros::Time().fromSec(80)) ;
- EXPECT_EQ(interval_data.size(), (unsigned int) 5) ;
- EXPECT_EQ(interval_data[0].data, 0) ;
- EXPECT_EQ(interval_data[1].data, 1) ;
- EXPECT_EQ(interval_data[2].data, 2) ;
- EXPECT_EQ(interval_data[3].data, 3) ;
- EXPECT_EQ(interval_data[4].data, 7) ;
-}
-
-TEST(SortedDeque, easyElemBeforeAfter)
-{
- SortedDeque<Msg> cache;
- cache.setMaxSize(10);
- Msg elem;
-
- fillEasy(cache, 5, 10) ;
-
- bool result;
- result = cache.getElemAfterTime( ros::Time().fromSec(85.0), elem);
-
- ASSERT_TRUE(result);
- EXPECT_EQ(elem.data, 9);
-
- result = cache.getElemBeforeTime( ros::Time().fromSec(85.0), elem);
- ASSERT_TRUE(result) ;
- EXPECT_EQ(elem.data, 8) ;
-
- result = cache.getElemBeforeTime( ros::Time().fromSec(45.0), elem);
- EXPECT_FAL...
[truncated message content] |
|
From: <ei...@us...> - 2009-08-25 18:22:47
|
Revision: 22863
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22863&view=rev
Author: eitanme
Date: 2009-08-25 18:22:41 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
Fixing filter chains that are pushed to the parameter server using xml. Also, adding a test for a transfer_function chain to the filters package
Modified Paths:
--------------
pkg/trunk/demos/plug_in/make_lowpass.m
pkg/trunk/highlevel/plugs/plugs_core/scripts/make_lowpass.m
pkg/trunk/stacks/common/filters/default_plugins.xml
pkg/trunk/stacks/common/filters/include/filters/filter_chain.h
pkg/trunk/stacks/common/filters/src/transfer_function.cpp
pkg/trunk/stacks/common/filters/test/test_chain.cpp
pkg/trunk/stacks/common/filters/test/test_chain.yaml
Modified: pkg/trunk/demos/plug_in/make_lowpass.m
===================================================================
--- pkg/trunk/demos/plug_in/make_lowpass.m 2009-08-25 17:58:14 UTC (rev 22862)
+++ pkg/trunk/demos/plug_in/make_lowpass.m 2009-08-25 18:22:41 UTC (rev 22863)
@@ -13,20 +13,16 @@
#printf(' <params a="%f %f" b="%f %f" />\n', a(1), a(2), b(1), b(2));
#printf('</filter>\n');
-printf('<filter>');
printf('<value>');
printf('<array>');
printf('<data>');
+printf('<value>');
printf('<struct>');
printf('<member>');
-printf('<name>%s</name>', filter_name);
-printf('<value>d_error_filter</value>');
+printf('<name>name</name>');
+printf('<value>%s</value>', filter_name);
printf('</member>');
printf('<member>');
-printf('<name>type</name>');
-printf('<value>TransferFunctionFilter</value>');
-printf('</member>');
-printf('<member>');
printf('<name>params</name>');
printf('<value>');
printf('<struct>');
@@ -55,8 +51,12 @@
printf('</struct>');
printf('</value>');
printf('</member>');
+printf('<member>');
+printf('<name>type</name>');
+printf('<value>MultiChannelTransferFunctionFilterDouble</value>');
+printf('</member>');
printf('</struct>');
+printf('</value>');
printf('</data>');
printf('</array>');
printf('</value>');
-printf('</filter>');
Modified: pkg/trunk/highlevel/plugs/plugs_core/scripts/make_lowpass.m
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/scripts/make_lowpass.m 2009-08-25 17:58:14 UTC (rev 22862)
+++ pkg/trunk/highlevel/plugs/plugs_core/scripts/make_lowpass.m 2009-08-25 18:22:41 UTC (rev 22863)
@@ -13,20 +13,16 @@
#printf(' <params a="%f %f" b="%f %f" />\n', a(1), a(2), b(1), b(2));
#printf('</filter>\n');
-printf('<filter>');
printf('<value>');
printf('<array>');
printf('<data>');
+printf('<value>');
printf('<struct>');
printf('<member>');
-printf('<name>%s</name>', filter_name);
-printf('<value>d_error_filter</value>');
+printf('<name>name</name>');
+printf('<value>%s</value>', filter_name);
printf('</member>');
printf('<member>');
-printf('<name>type</name>');
-printf('<value>TransferFunctionFilter</value>');
-printf('</member>');
-printf('<member>');
printf('<name>params</name>');
printf('<value>');
printf('<struct>');
@@ -55,8 +51,12 @@
printf('</struct>');
printf('</value>');
printf('</member>');
+printf('<member>');
+printf('<name>type</name>');
+printf('<value>MultiChannelTransferFunctionFilterDouble</value>');
+printf('</member>');
printf('</struct>');
+printf('</value>');
printf('</data>');
printf('</array>');
printf('</value>');
-printf('</filter>');
Modified: pkg/trunk/stacks/common/filters/default_plugins.xml
===================================================================
--- pkg/trunk/stacks/common/filters/default_plugins.xml 2009-08-25 17:58:14 UTC (rev 22862)
+++ pkg/trunk/stacks/common/filters/default_plugins.xml 2009-08-25 18:22:41 UTC (rev 22863)
@@ -52,7 +52,7 @@
</class>
</library>
<library path="lib/libtransfer_function">
- <class name="transferFunctionMultiDouble" type="filters::TransferFunctionDoubleFilter"
+ <class name="MultiChannelTransferFunctionFilterDouble" type="filters::TransferFunctionFilter<double>"
base_class_type="filters::MultiChannelFilterBase<double>">
<description>
THis is a transfer filter which works on a stream of doubles.
Modified: pkg/trunk/stacks/common/filters/include/filters/filter_chain.h
===================================================================
--- pkg/trunk/stacks/common/filters/include/filters/filter_chain.h 2009-08-25 17:58:14 UTC (rev 22862)
+++ pkg/trunk/stacks/common/filters/include/filters/filter_chain.h 2009-08-25 18:22:41 UTC (rev 22863)
@@ -138,11 +138,12 @@
bool configure(XmlRpc::XmlRpcValue& config)
{
/*************************** Parse the XmlRpcValue ***********************************/
-
//Verify proper naming and structure
if (config.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("The filter chain specification must be a list. but is of of XmlRpcType %d", config.getType());
+ ROS_ERROR("The xml passed in is formatted as follows:\n %s", config.toXml().c_str());
+
return false;
}
@@ -333,11 +334,12 @@
bool configure(unsigned int size, XmlRpc::XmlRpcValue& config)
{
/*************************** Parse the XmlRpcValue ***********************************/
-
//Verify proper naming and structure
if (config.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("The filter chain specification must be a list. but is of of XmlRpcType %d", config.getType());
+ ROS_ERROR("The xml passed in is formatted as follows:\n %s", config.toXml().c_str());
+
return false;
}
Modified: pkg/trunk/stacks/common/filters/src/transfer_function.cpp
===================================================================
--- pkg/trunk/stacks/common/filters/src/transfer_function.cpp 2009-08-25 17:58:14 UTC (rev 22862)
+++ pkg/trunk/stacks/common/filters/src/transfer_function.cpp 2009-08-25 18:22:41 UTC (rev 22863)
@@ -30,4 +30,4 @@
#include "filters/transfer_function.h"
#include "pluginlib/class_list_macros.h"
-PLUGINLIB_REGISTER_CLASS(TransferFunctionMultiDouble, filters::TransferFunctionFilter<double>, filters::MultiChannelFilterBase<double>)
+PLUGINLIB_REGISTER_CLASS(MultiChannelTransferFunctionFilterDouble, filters::TransferFunctionFilter<double>, filters::MultiChannelFilterBase<double>)
Modified: pkg/trunk/stacks/common/filters/test/test_chain.cpp
===================================================================
--- pkg/trunk/stacks/common/filters/test/test_chain.cpp 2009-08-25 17:58:14 UTC (rev 22862)
+++ pkg/trunk/stacks/common/filters/test/test_chain.cpp 2009-08-25 18:22:41 UTC (rev 22863)
@@ -142,6 +142,63 @@
EXPECT_NEAR(input1[i], v1a[i], epsilon);
}
}
+
+TEST(MultiChannelFilterChain, TransferFunction){
+ double epsilon = 1e-4;
+
+ filters::MultiChannelFilterChain<double> chain("double");
+ EXPECT_TRUE(chain.configure(3, "TransferFunction" ));
+
+ std::vector<double> in1,in2,in3,in4,in5,in6,in7;
+ std::vector<double> out1;
+
+ in1.push_back(10.0);
+ in1.push_back(10.0);
+ in1.push_back(10.0);
+ //
+ in2.push_back(70.0);
+ in2.push_back(30.0);
+ in2.push_back(8.0);
+ //
+ in3.push_back(-1.0);
+ in3.push_back(5.0);
+ in3.push_back(22.0);
+ //
+ in4.push_back(44.0);
+ in4.push_back(23.0);
+ in4.push_back(8.0);
+ //
+ in5.push_back(10.0);
+ in5.push_back(10.0);
+ in5.push_back(10.0);
+ //
+ in6.push_back(5.0);
+ in6.push_back(-1.0);
+ in6.push_back(5.0);
+ //
+ in7.push_back(6.0);
+ in7.push_back(-30.0);
+ in7.push_back(2.0);
+ //
+ out1.push_back(17.1112);
+ out1.push_back(9.0285);
+ out1.push_back(8.3102);
+ EXPECT_TRUE(chain.update(in1, in1));
+ EXPECT_TRUE(chain.update(in2, in2));
+ EXPECT_TRUE(chain.update(in3, in3));
+ EXPECT_TRUE(chain.update(in4, in4));
+ EXPECT_TRUE(chain.update(in5, in5));
+ EXPECT_TRUE(chain.update(in6, in6));
+ EXPECT_TRUE(chain.update(in7, in7));
+
+ chain.clear();
+
+ for(unsigned int i=0; i<out1.size(); i++)
+ {
+ EXPECT_NEAR(out1[i], in7[i], epsilon);
+ }
+}
+
/*
TEST(MultiChannelFilterChain, OverlappingNames){
filters::MultiChannelFilterChain<double> chain("double");
Modified: pkg/trunk/stacks/common/filters/test/test_chain.yaml
===================================================================
--- pkg/trunk/stacks/common/filters/test/test_chain.yaml 2009-08-25 17:58:14 UTC (rev 22862)
+++ pkg/trunk/stacks/common/filters/test/test_chain.yaml 2009-08-25 18:22:41 UTC (rev 22863)
@@ -29,6 +29,15 @@
name: median_test2
type: MultiChannelMedianFilterDouble
params: {number_of_observations: 5}
+
+TransferFunction:
+ filter_chain:
+ -
+ name: transfer_function
+ type: MultiChannelTransferFunctionFilterDouble
+ params:
+ a: [1.0, -1.760041880343169, 1.182893262037831]
+ b: [0.018098933007514, 0.054296799022543, 0.054296799022543, 0.018098933007514]
MeanFilterFloat5:
filter_chain:
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <bha...@us...> - 2009-08-25 18:39:19
|
Revision: 22867
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22867&view=rev
Author: bhaskarama
Date: 2009-08-25 18:39:10 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
De-anonymize clear known objects
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
Modified: pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp 2009-08-25 18:36:51 UTC (rev 22866)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp 2009-08-25 18:39:10 UTC (rev 22867)
@@ -378,7 +378,8 @@
int main(int argc, char **argv)
{
- ros::init(argc, argv, "clear_known_objects", ros::init_options::AnonymousName);
+ // Took out anonymous - bmm
+ ros::init(argc, argv, "clear_known_objects");
ClearKnownObjects cko;
cko.run();
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch 2009-08-25 18:36:51 UTC (rev 22866)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch 2009-08-25 18:39:10 UTC (rev 22867)
@@ -17,7 +17,7 @@
</node>
<!-- remove points corresponding to known objects -->
- <node machine="three" pkg="planning_environment" type="clear_known_objects" respawn="true" output="screen">
+ <node machine="three" pkg="planning_environment" type="clear_known_objects" name="clear_known_objects" respawn="true" output="screen">
<remap from="robot_description" to="robot_description" />
<!-- define a frame that stays fixed for the known objects -->
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-08-25 19:06:21
|
Revision: 22876
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22876&view=rev
Author: eitanme
Date: 2009-08-25 19:06:06 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
Making some changes as per the base_local_planner api review
Modified Paths:
--------------
pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp
pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h
pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp
pkg/trunk/stacks/navigation/nav_core/include/nav_core/base_local_planner.h
Modified: pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
===================================================================
--- pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-08-25 19:06:06 UTC (rev 22876)
@@ -246,7 +246,7 @@
ROS_INFO ("Passing plan to controller");
if(new_plan_){
new_plan_ = false;
- if(!tc_->updatePlan(global_plan_)){
+ if(!tc_->setPlan(global_plan_)){
resetState();
ROS_WARN("move_base aborted because it failed to pass the plan from the planner to the controller");
return robot_actions::ABORTED;
@@ -260,7 +260,7 @@
last_valid_control_ = ros::Time::now();
//check for success
- if(tc_->goalReached()){
+ if(tc_->isGoalReached()){
if(attempted_rotation_){
valid_control = false;
if(done_half_rotation_){
Modified: pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/sandbox/teleop_anti_collision/src/safe_teleop.cpp 2009-08-25 19:06:06 UTC (rev 22876)
@@ -273,7 +273,7 @@
rotate_plan.push_back(rotate_goal_msg);
//pass the rotation goal to the controller
- if(!tc_->updatePlan(rotate_plan)){
+ if(!tc_->setPlan(rotate_plan)){
ROS_ERROR("Failed to pass global plan to the controller, aborting in place rotation attempt.");
return false;
}
@@ -432,7 +432,7 @@
case PLANNING:
ROS_DEBUG("In planning state");
if(makePlan(goal, global_plan)){
- if(!tc_->updatePlan(global_plan)){
+ if(!tc_->setPlan(global_plan)){
//ABORT and SHUTDOWN COSTMAPS
ROS_ERROR("Failed to pass global plan to the controller, aborting.");
resetState();
@@ -466,7 +466,7 @@
ROS_DEBUG("In controlling state");
//check to see if we've reached our goal
- if(tc_->goalReached()){
+ if(tc_->isGoalReached()){
ROS_DEBUG("Goal reached!");
resetState();
as_.setSucceeded();
@@ -520,7 +520,7 @@
break;
case EXECUTE_ROTATE_1:
ROS_DEBUG("In in-place rotation state 1");
- if(tc_->goalReached()){
+ if(tc_->isGoalReached()){
clearing_state_ = IN_PLACE_ROTATION_2;
}
else if(!rotateRobot()){
@@ -540,7 +540,7 @@
break;
case EXECUTE_ROTATE_2:
ROS_DEBUG("In in-place rotation state 2");
- if(tc_->goalReached() || !rotateRobot()){
+ if(tc_->isGoalReached() || !rotateRobot()){
clearing_state_ = AGGRESSIVE_RESET;
state_ = PLANNING;
}
Modified: pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/stacks/navigation/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-08-25 19:06:06 UTC (rev 22876)
@@ -103,15 +103,6 @@
~TrajectoryPlannerROS();
/**
- * @brief Used for display purposes, allows the footprint of the robot to be drawn in visualization tools
- * @param x_i The x position of the robot
- * @param y_i The y position of the robot
- * @param theta_i The orientation of the robot
- * @return A vector of points in world coordinates that correspond to the verticies of the robot's footprint
- */
- std::vector<geometry_msgs::Point> drawFootprint(double x_i, double y_i, double theta_i);
-
- /**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid trajectory was found, false otherwise
@@ -119,28 +110,29 @@
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
/**
- * @brief Update the plan that the controller is following
+ * @brief Set the plan that the controller is following
* @param orig_global_plan The plan to pass to the controller
* @return True if the plan was updated successfully, false otherwise
*/
- bool updatePlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
+ bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
/**
- * @brief Returns the local goal the robot is pursuing
- * @param x Will be set to the x position of the goal in world coordinates
- * @param y Will be set to the y position of the goal in world coordinates
- * @return
- */
- void getLocalGoal(double& x, double& y);
-
- /**
* @brief Check if the goal pose has been achieved
* @return True if achieved, false otherwise
*/
- bool goalReached();
+ bool isGoalReached();
private:
/**
+ * @brief Used for display purposes, allows the footprint of the robot to be drawn in visualization tools
+ * @param x_i The x position of the robot
+ * @param y_i The y position of the robot
+ * @param theta_i The orientation of the robot
+ * @return A vector of points in world coordinates that correspond to the verticies of the robot's footprint
+ */
+ std::vector<geometry_msgs::Point> drawFootprint(double x_i, double y_i, double theta_i);
+
+ /**
* @brief Check whether the robot is stopped or not
* @return True if the robot is stopped, false otherwise
*/
Modified: pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp 2009-08-25 19:06:06 UTC (rev 22876)
@@ -236,7 +236,7 @@
}
}
- bool TrajectoryPlannerROS::goalReached(){
+ bool TrajectoryPlannerROS::isGoalReached(){
if(!initialized_){
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
@@ -305,7 +305,7 @@
return false;
}
- bool TrajectoryPlannerROS::updatePlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan){
+ bool TrajectoryPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan){
if(!initialized_){
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
return false;
@@ -514,16 +514,6 @@
return true;
}
- void TrajectoryPlannerROS::getLocalGoal(double& x, double& y){
- if(!initialized_){
- ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
- return;
- }
-
- tc_->getLocalGoal(x, y);
- return;
- }
-
void TrajectoryPlannerROS::prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
ROS_ASSERT(global_plan.size() >= plan.size());
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
Modified: pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h
===================================================================
--- pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h 2009-08-25 19:06:06 UTC (rev 22876)
@@ -49,8 +49,8 @@
#include <vector>
#include <string>
#include <nav_msgs/GetPlan.h>
+#include <geometry_msgs/Twist.h>
#include <visualization_msgs/Marker.h>
-#include <geometry_msgs/Twist.h>
#include <pluginlib/class_loader.h>
Modified: pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/stacks/navigation/move_base/src/move_base.cpp 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/stacks/navigation/move_base/src/move_base.cpp 2009-08-25 19:06:06 UTC (rev 22876)
@@ -327,7 +327,7 @@
rotate_plan.push_back(rotate_goal_msg);
//pass the rotation goal to the controller
- if(!tc_->updatePlan(rotate_plan)){
+ if(!tc_->setPlan(rotate_plan)){
ROS_ERROR("Failed to pass global plan to the controller, aborting in place rotation attempt.");
return false;
}
@@ -482,7 +482,7 @@
case PLANNING:
ROS_DEBUG("In planning state");
if(makePlan(goal, global_plan)){
- if(!tc_->updatePlan(global_plan)){
+ if(!tc_->setPlan(global_plan)){
//ABORT and SHUTDOWN COSTMAPS
ROS_ERROR("Failed to pass global plan to the controller, aborting.");
resetState();
@@ -516,7 +516,7 @@
ROS_DEBUG("In controlling state");
//check to see if we've reached our goal
- if(tc_->goalReached()){
+ if(tc_->isGoalReached()){
ROS_DEBUG("Goal reached!");
resetState();
as_.setSucceeded();
@@ -570,7 +570,7 @@
break;
case EXECUTE_ROTATE_1:
ROS_DEBUG("In in-place rotation state 1");
- if(tc_->goalReached()){
+ if(tc_->isGoalReached()){
clearing_state_ = IN_PLACE_ROTATION_2;
}
else if(!rotateRobot()){
@@ -590,7 +590,7 @@
break;
case EXECUTE_ROTATE_2:
ROS_DEBUG("In in-place rotation state 2");
- if(tc_->goalReached() || !rotateRobot()){
+ if(tc_->isGoalReached() || !rotateRobot()){
clearing_state_ = AGGRESSIVE_RESET;
state_ = PLANNING;
}
Modified: pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp
===================================================================
--- pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp 2009-08-25 19:06:06 UTC (rev 22876)
@@ -318,13 +318,13 @@
rotate_plan.push_back(rotate_goal_msg);
//pass the rotation goal to the controller
- if(!tc_->updatePlan(rotate_plan)){
+ if(!tc_->setPlan(rotate_plan)){
ROS_ERROR("Failed to pass global plan to the controller, aborting in place rotation attempt.");
return;
}
geometry_msgs::Twist cmd_vel;
- while(!isPreemptRequested() && ros_node_.ok() && !tc_->goalReached()){
+ while(!isPreemptRequested() && ros_node_.ok() && !tc_->isGoalReached()){
if(tc_->computeVelocityCommands(cmd_vel)){
//make sure that we send the velocity command to the base
vel_pub_.publish(cmd_vel);
@@ -403,7 +403,7 @@
case PLANNING:
ROS_DEBUG("In planning state");
if(makePlan(goal, global_plan)){
- if(!tc_->updatePlan(global_plan)){
+ if(!tc_->setPlan(global_plan)){
//ABORT and SHUTDOWN COSTMAPS
ROS_ERROR("Failed to pass global plan to the controller, aborting.");
resetState();
@@ -434,7 +434,7 @@
ROS_DEBUG("In controlling state");
//check to see if we've reached our goal
- if(tc_->goalReached()){
+ if(tc_->isGoalReached()){
ROS_DEBUG("Goal reached!");
resetState();
return robot_actions::SUCCESS;
Modified: pkg/trunk/stacks/navigation/nav_core/include/nav_core/base_local_planner.h
===================================================================
--- pkg/trunk/stacks/navigation/nav_core/include/nav_core/base_local_planner.h 2009-08-25 18:59:31 UTC (rev 22875)
+++ pkg/trunk/stacks/navigation/nav_core/include/nav_core/base_local_planner.h 2009-08-25 19:06:06 UTC (rev 22876)
@@ -45,8 +45,8 @@
class BaseLocalPlanner{
public:
virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
- virtual bool goalReached() = 0;
- virtual bool updatePlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
+ virtual bool isGoalReached() = 0;
+ virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-08-25 21:12:34
|
Revision: 22891
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22891&view=rev
Author: vijaypradeep
Date: 2009-08-25 21:12:26 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
JointStatesDeflater passes unit tests
Modified Paths:
--------------
pkg/trunk/stacks/calibration/joint_states_settler/CMakeLists.txt
pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml
pkg/trunk/stacks/calibration/settlerlib/include/settlerlib/deflated.h
pkg/trunk/stacks/calibration/settlerlib/manifest.xml
Added Paths:
-----------
pkg/trunk/stacks/calibration/joint_states_settler/include/
pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/
pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/deflated_joint_states.h
pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_deflater.h
pkg/trunk/stacks/calibration/joint_states_settler/src/
pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_deflater.cpp
pkg/trunk/stacks/calibration/joint_states_settler/test/
pkg/trunk/stacks/calibration/joint_states_settler/test/CMakeLists.txt
pkg/trunk/stacks/calibration/joint_states_settler/test/joint_states_deflater_unittest.cpp
Removed Paths:
-------------
pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h
pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/src/joint_states_deflater.cpp
pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/test/joint_states_deflater_unittest.cpp
Deleted: pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h 2009-08-25 20:57:28 UTC (rev 22890)
+++ pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h 2009-08-25 21:12:26 UTC (rev 22891)
@@ -1,67 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#ifndef CALIBRATION_MESSAGE_FILTERS_JOINT_STATES_DEFLATER_H_
-#define CALIBRATION_MESSAGE_FILTERS_JOINT_STATES_DEFLATER_H_
-
-#include "calibration_message_filters/deflated.h"
-#include "mechanism_msgs/JointStates.h"
-
-namespace calibration_message_filters
-{
-
-
-class JointStatesDeflater
-{
-public:
- typedef DeflatedMsg<mechanism_msgs::JointStates> DeflatedJointStates;
-
- JointStatesDeflater();
-
- void setDeflationJointNames(std::vector<std::string> joint_names);
- void deflate(const mechanism_msgs::JointStatesConstPtr& joint_states, DeflatedJointStates& deflated_elem);
-
-private:
- std::vector<unsigned int> mapping_;
- std::vector<std::string> joint_names_;
-
- void updateMapping(const mechanism_msgs::JointStates& joint_states);
-
-};
-
-}
-
-
-
-#endif
Deleted: pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/src/joint_states_deflater.cpp
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/src/joint_states_deflater.cpp 2009-08-25 20:57:28 UTC (rev 22890)
+++ pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/src/joint_states_deflater.cpp 2009-08-25 21:12:26 UTC (rev 22891)
@@ -1,96 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#include "calibration_message_filters/joint_states_deflater.h"
-
-using namespace std;
-using namespace calibration_message_filters;
-using namespace mechanism_msgs;
-
-JointStatesDeflater::JointStatesDeflater()
-{
- mapping_.clear();
-}
-
-void JointStatesDeflater::setDeflationJointNames(std::vector<std::string> joint_names)
-{
- joint_names_ = joint_names;
- mapping_.resize(joint_names_.size());
-}
-
-void JointStatesDeflater::deflate(const JointStatesConstPtr& joint_states, DeflatedJointStates& deflated_elem)
-{
- if (mapping_.size() != joint_names_.size())
- updateMapping(*joint_states);
-
- const unsigned int N = joint_names_.size();
-
- deflated_elem.deflated_.resize(N);
-
- for (unsigned int i=0; i<N; i++)
- {
- if ( mapping_[i] >= joint_states->joints.size() )
- updateMapping(*joint_states);
-
- if ( joint_states->joints[mapping_[i]].name != joint_names_[i])
- updateMapping(*joint_states);
-
- deflated_elem.header = joint_states->header;
- deflated_elem.deflated_[i] = joint_states->joints[mapping_[i]].position;
- deflated_elem.msg = joint_states;
- }
-}
-
-void JointStatesDeflater::updateMapping(const JointStates& joint_states)
-{
- ROS_DEBUG("Updating the JointStates mapping");
-
- const unsigned int N = joint_names_.size();
-
- mapping_.resize(N);
-
- for (unsigned int i=0; i<N; i++)
- {
- bool mapping_found = false;
- for (unsigned int j=0; j<joint_states.joints.size(); j++)
- {
- if ( joint_names_[i] == joint_states.joints[j].name)
- {
- mapping_[i] = j;
- mapping_found = true;
- }
- }
- ROS_ERROR_COND(!mapping_found, "Couldn't find mapping for [%s]", joint_names_[i].c_str());
- }
-}
Deleted: pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/test/joint_states_deflater_unittest.cpp
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/test/joint_states_deflater_unittest.cpp 2009-08-25 20:57:28 UTC (rev 22890)
+++ pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/test/joint_states_deflater_unittest.cpp 2009-08-25 21:12:26 UTC (rev 22891)
@@ -1,119 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-
-#include <gtest/gtest.h>
-
-#include "calibration_message_filters/joint_states_deflater.h"
-
-using namespace std;
-using namespace calibration_message_filters;
-using namespace mechanism_msgs;
-
-
-static const double eps = 1e-10;
-
-TEST(JointStatesDeflator, easy1)
-{
- JointStatesDeflater deflater;
-
- vector<string> joint_names;
- joint_names.resize(2);
- joint_names[0] = "A";
- joint_names[1] = "C";
- deflater.setDeflationJointNames(joint_names);
-
- JointStatesPtr joint_states;
- DeflatedMsg<JointStates> deflated_msg;
-
- // ***********************************************
-
- joint_states.reset(new JointStates);
- joint_states->joints.resize(3);
- joint_states->joints[0].name = "A";
- joint_states->joints[1].name = "B";
- joint_states->joints[2].name = "C";
- joint_states->joints[0].position = 1.0;
- joint_states->joints[1].position = 2.0;
- joint_states->joints[2].position = 3.0;
-
- deflater.deflate(joint_states, deflated_msg);
-
- EXPECT_EQ(deflated_msg.deflated_.size(), (unsigned int) 2);
- EXPECT_NEAR(deflated_msg.deflated_[0], 1.0, eps);
- EXPECT_NEAR(deflated_msg.deflated_[1], 3.0, eps);
-
- // ***********************************************
-
- joint_states.reset(new JointStates);
- joint_states->joints.resize(3);
- joint_states->joints[0].name = "C";
- joint_states->joints[1].name = "A";
- joint_states->joints[2].name = "B";
- joint_states->joints[0].position = 4.0;
- joint_states->joints[1].position = 5.0;
- joint_states->joints[2].position = 6.0;
-
- deflater.deflate(joint_states, deflated_msg);
-
- EXPECT_EQ(deflated_msg.deflated_.size(), (unsigned int) 2);
- EXPECT_NEAR(deflated_msg.deflated_[0], 5.0, eps);
- EXPECT_NEAR(deflated_msg.deflated_[1], 4.0, eps);
-
- // ***********************************************
-
- joint_states.reset(new JointStates);
- joint_states->joints.resize(4);
- joint_states->joints[0].name = "D";
- joint_states->joints[1].name = "C";
- joint_states->joints[2].name = "B";
- joint_states->joints[3].name = "A";
- joint_states->joints[0].position = 7.0;
- joint_states->joints[1].position = 8.0;
- joint_states->joints[2].position = 9.0;
- joint_states->joints[3].position = 10.0;
-
- deflater.deflate(joint_states, deflated_msg);
-
- EXPECT_EQ(deflated_msg.deflated_.size(), (unsigned int) 2);
- EXPECT_NEAR(deflated_msg.deflated_[0], 10.0, eps);
- EXPECT_NEAR(deflated_msg.deflated_[1], 8.0, eps);
-
-}
-
-
-int main(int argc, char **argv){
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
Modified: pkg/trunk/stacks/calibration/joint_states_settler/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/CMakeLists.txt 2009-08-25 20:57:28 UTC (rev 22890)
+++ pkg/trunk/stacks/calibration/joint_states_settler/CMakeLists.txt 2009-08-25 21:12:26 UTC (rev 22891)
@@ -11,20 +11,18 @@
rosbuild_init()
-#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-#uncomment if you have defined messages
-#rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
+# rosbuild_genmsg()
+# rosbuild_gensrv()
-#common commands for building c++ executables and libraries
-#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-#rosbuild_add_executable(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
+rosbuild_add_library(${PROJECT_NAME} src/joint_states_deflater.cpp)
+
+# target_link_libraries(${PROJECT_NAME} another_library)
+# rosbuild_add_boost_directories()
+# rosbuild_link_boost(${PROJECT_NAME} thread)
+# rosbuild_add_executable(example examples/example.cpp)
+# target_link_libraries(example ${PROJECT_NAME})
+
+add_subdirectory(test EXCLUDE_FROM_ALL)
\ No newline at end of file
Copied: pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/deflated_joint_states.h (from rev 22890, pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h)
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/deflated_joint_states.h (rev 0)
+++ pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/deflated_joint_states.h 2009-08-25 21:12:26 UTC (rev 22891)
@@ -0,0 +1,52 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef JOINT_STATES_SETTLER_DEFLATED_JOINT_STATES_H_
+#define JOINT_STATES_SETTLER_DEFLATED_JOINT_STATES_H_
+
+#include <mechanism_msgs/JointStates.h>
+#include <settlerlib/deflated.h>
+
+namespace joint_states_settler
+{
+
+class DeflatedJointStates : public settlerlib::Deflated
+{
+public:
+ mechanism_msgs::JointStatesConstPtr msg_;
+};
+
+}
+
+#endif
Copied: pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_deflater.h (from rev 22890, pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/include/calibration_message_filters/joint_states_deflater.h)
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_deflater.h (rev 0)
+++ pkg/trunk/stacks/calibration/joint_states_settler/include/joint_states_settler/joint_states_deflater.h 2009-08-25 21:12:26 UTC (rev 22891)
@@ -0,0 +1,87 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef JOINT_STATES_SETTLER_JOINT_STATES_DEFLATER_H_
+#define JOINT_STATES_SETTLER_JOINT_STATES_DEFLATER_H_
+
+#include <mechanism_msgs/JointStates.h>
+#include <settlerlib/deflated.h>
+#include "deflated_joint_states.h"
+
+namespace joint_states_settler
+{
+
+/**
+ * \brief Given a set a joint names, efficiently extracts a subset of joint positions
+ *
+ * This class is generally more efficient than other methods, because it caches the the mapping
+ * from the incoming joint_states message to the deflated vector. It also updates the mapping
+ * whenever the ordering in joint_states changes
+ **/
+class JointStatesDeflater
+{
+public:
+
+ JointStatesDeflater();
+
+ /**
+ * \brief Specify which joints to extract
+ * \param joint_names Ordered list of joint names to extract
+ */
+ void setDeflationJointNames(std::vector<std::string> joint_names);
+
+ /**
+ * \brief Perform the deflation on a joint_states message
+ * \param joint_states Incoming JointStates message
+ * \param Ouput datatype. Stores the deflated data, along with the original joint states message
+ */
+ void deflate(const mechanism_msgs::JointStatesConstPtr& joint_states, DeflatedJointStates& deflated_elem);
+
+private:
+ std::vector<unsigned int> mapping_;
+ std::vector<std::string> joint_names_;
+
+ /**
+ * \brief Given a stereotypical JointStates message, computes the mapping
+ * from JointStates to the deflated data
+ */
+ void updateMapping(const mechanism_msgs::JointStates& joint_states);
+
+};
+
+}
+
+
+
+#endif
Modified: pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml 2009-08-25 20:57:28 UTC (rev 22890)
+++ pkg/trunk/stacks/calibration/joint_states_settler/manifest.xml 2009-08-25 21:12:26 UTC (rev 22891)
@@ -11,6 +11,7 @@
<depend package="roscpp"/>
<depend package="actionlib"/>
<depend package="mechanism_msgs"/>
+ <depend package="settlerlib"/>
</package>
Copied: pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_deflater.cpp (from rev 22890, pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/src/joint_states_deflater.cpp)
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_deflater.cpp (rev 0)
+++ pkg/trunk/stacks/calibration/joint_states_settler/src/joint_states_deflater.cpp 2009-08-25 21:12:26 UTC (rev 22891)
@@ -0,0 +1,95 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "joint_states_settler/joint_states_deflater.h"
+
+using namespace std;
+using namespace joint_states_settler;
+
+JointStatesDeflater::JointStatesDeflater()
+{
+ mapping_.clear();
+}
+
+void JointStatesDeflater::setDeflationJointNames(std::vector<std::string> joint_names)
+{
+ joint_names_ = joint_names;
+ mapping_.resize(joint_names_.size());
+}
+
+void JointStatesDeflater::deflate(const mechanism_msgs::JointStatesConstPtr& joint_states, DeflatedJointStates& deflated_elem)
+{
+ if (mapping_.size() != joint_names_.size())
+ updateMapping(*joint_states);
+
+ const unsigned int N = joint_names_.size();
+
+ deflated_elem.channels_.resize(N);
+
+ for (unsigned int i=0; i<N; i++)
+ {
+ if ( mapping_[i] >= joint_states->joints.size() )
+ updateMapping(*joint_states);
+
+ if ( joint_states->joints[mapping_[i]].name != joint_names_[i])
+ updateMapping(*joint_states);
+
+ deflated_elem.header = joint_states->header;
+ deflated_elem.channels_[i] = joint_states->joints[mapping_[i]].position;
+ deflated_elem.msg_ = joint_states;
+ }
+}
+
+void JointStatesDeflater::updateMapping(const mechanism_msgs::JointStates& joint_states)
+{
+ ROS_DEBUG("Updating the JointStates mapping");
+
+ const unsigned int N = joint_names_.size();
+
+ mapping_.resize(N);
+
+ for (unsigned int i=0; i<N; i++)
+ {
+ bool mapping_found = false;
+ for (unsigned int j=0; j<joint_states.joints.size(); j++)
+ {
+ if ( joint_names_[i] == joint_states.joints[j].name)
+ {
+ mapping_[i] = j;
+ mapping_found = true;
+ }
+ }
+ ROS_ERROR_COND(!mapping_found, "Couldn't find mapping for [%s]", joint_names_[i].c_str());
+ }
+}
Added: pkg/trunk/stacks/calibration/joint_states_settler/test/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/test/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/calibration/joint_states_settler/test/CMakeLists.txt 2009-08-25 21:12:26 UTC (rev 22891)
@@ -0,0 +1,6 @@
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR})
+
+# ********** Tests **********
+rospack_add_gtest(test/joint_states_deflater_unittest joint_states_deflater_unittest.cpp)
+target_link_libraries(test/joint_states_deflater_unittest joint_states_settler)
+
Copied: pkg/trunk/stacks/calibration/joint_states_settler/test/joint_states_deflater_unittest.cpp (from rev 22890, pkg/trunk/calibration_experimental/sandbox/calibration_message_filters/test/joint_states_deflater_unittest.cpp)
===================================================================
--- pkg/trunk/stacks/calibration/joint_states_settler/test/joint_states_deflater_unittest.cpp (rev 0)
+++ pkg/trunk/stacks/calibration/joint_states_settler/test/joint_states_deflater_unittest.cpp 2009-08-25 21:12:26 UTC (rev 22891)
@@ -0,0 +1,115 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <gtest/gtest.h>
+#include "joint_states_settler/joint_states_deflater.h"
+
+using namespace std;
+using namespace joint_states_settler;
+using namespace mechanism_msgs;
+
+static const double eps = 1e-10;
+
+TEST(JointStatesDeflator, easy1)
+{
+ JointStatesDeflater deflater;
+
+ vector<string> joint_names;
+ joint_names.resize(2);
+ joint_names[0] = "A";
+ joint_names[1] = "C";
+ deflater.setDeflationJointNames(joint_names);
+
+ JointStatesPtr joint_states;
+ DeflatedJointStates deflated_msg;
+
+ // ***********************************************
+
+ joint_states.reset(new JointStates);
+ joint_states->joints.resize(3);
+ joint_states->joints[0].name = "A";
+ joint_states->joints[1].name = "B";
+ joint_states->joints[2].name = "C";
+ joint_states->joints[0].p...
[truncated message content] |
|
From: <sf...@us...> - 2009-08-25 22:02:57
|
Revision: 22900
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22900&view=rev
Author: sfkwc
Date: 2009-08-25 22:02:49 +0000 (Tue, 25 Aug 2009)
Log Message:
-----------
moving dense_laser_assembler to laser_pipeline stack
Added Paths:
-----------
pkg/trunk/stacks/laser_pipeline/dense_laser_assembler/
Removed Paths:
-------------
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mr...@us...> - 2009-08-26 00:05:10
|
Revision: 22923
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22923&view=rev
Author: mrinal
Date: 2009-08-26 00:05:01 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
Un-blacklisting a few packages which depended on filters. (ticket #2516)
Removed Paths:
-------------
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/ROS_BUILD_BLACKLIST
pkg/trunk/sandbox/3dnav_pr2/ROS_BUILD_BLACKLIST
pkg/trunk/sandbox/trajectory_controllers/ROS_BUILD_BLACKLIST
Deleted: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/ROS_BUILD_BLACKLIST 2009-08-26 00:04:00 UTC (rev 22922)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/ROS_BUILD_BLACKLIST 2009-08-26 00:05:01 UTC (rev 22923)
@@ -1 +0,0 @@
-broken see ticket https://prdev.willowgarage.com/trac/personalrobots/ticket/2516
Deleted: pkg/trunk/sandbox/3dnav_pr2/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/ROS_BUILD_BLACKLIST 2009-08-26 00:04:00 UTC (rev 22922)
+++ pkg/trunk/sandbox/3dnav_pr2/ROS_BUILD_BLACKLIST 2009-08-26 00:05:01 UTC (rev 22923)
@@ -1 +0,0 @@
-broken see ticket https://prdev.willowgarage.com/trac/personalrobots/ticket/2516
Deleted: pkg/trunk/sandbox/trajectory_controllers/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/sandbox/trajectory_controllers/ROS_BUILD_BLACKLIST 2009-08-26 00:04:00 UTC (rev 22922)
+++ pkg/trunk/sandbox/trajectory_controllers/ROS_BUILD_BLACKLIST 2009-08-26 00:05:01 UTC (rev 22923)
@@ -1 +0,0 @@
-broken see ticket https://prdev.willowgarage.com/trac/personalrobots/ticket/2516
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-08-26 01:38:43
|
Revision: 22930
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22930&view=rev
Author: eitanme
Date: 2009-08-26 01:38:16 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
Removing the old move_base executable, updating all launch files to include a move_base_translator so that things work with the old action interface. Also, updating tests so that they'll pass using the _old suffix to use the old action_client interface. Tried to get everything, but its possible I didn't... launch files are hard to test... cross your fingers
Modified Paths:
--------------
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base/move_base.xml
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base_local/move_base_local.xml
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/rviz_move_base.launch
pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch
pkg/trunk/demos/erratic_gazebo/2dnav-gazebo-simple-fake_localization.xml
pkg/trunk/demos/milestone2/milestone2_tests/test/move_base.launch
pkg/trunk/demos/test_2dnav_gazebo/manifest.xml
pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
pkg/trunk/highlevel/doors/doors_core/test/test_planner.cpp
pkg/trunk/highlevel/move_base_stage/move_base/move_base.xml
pkg/trunk/highlevel/move_base_stage/move_base_multi_robot.launch
pkg/trunk/highlevel/plugs/plugs_core/test/test_approach.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_executive.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_outlet_coarse.cpp
pkg/trunk/nav/visual_nav/launch/move_base.launch
pkg/trunk/nav/visual_nav/launch/move_base_robot.launch
pkg/trunk/sandbox/action_translator/include/action_translator/action_translator.h
pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp
pkg/trunk/sandbox/teleop_anti_collision/launch/run_teleop_anti_collision.xml
pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server_imp.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server_imp.h
pkg/trunk/stacks/navigation/move_base/CMakeLists.txt
pkg/trunk/stacks/navigation/move_base/manifest.xml
pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
pkg/trunk/stacks/navigation/stack.xml
pkg/trunk/stacks/trex/trex_pr2_writing_test/src/test_move_to_helper.cpp
Removed Paths:
-------------
pkg/trunk/stacks/navigation/move_base/include/move_base/move_base_old.h
pkg/trunk/stacks/navigation/move_base/src/move_base_old.cpp
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base/move_base.xml
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base/move_base.xml 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base/move_base.xml 2009-08-26 01:38:16 UTC (rev 22930)
@@ -2,7 +2,13 @@
<include file="$(find pr2_alpha)/$(env ROBOT).machine" />
<node pkg="mux" type="throttle" args="3.0 /move_base/local_costmap/voxel_grid /move_base/local_costmap/voxel_grid_throttled" />
-<node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen" machine="four">
+
+<!-- Run the move base translator -->
+<node pkg="move_base_client" type="move_base_translator" name="move_base_translator" machine="four">
+ <param name="action_name" value="move_base" />
+</node>
+
+<node pkg="move_base" type="move_base" name="move_base" output="screen" machine="four">
<remap from="odom" to="pr2_odometry/odom" />
<param name="controller_frequency" value="10.0" />
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base_local/move_base_local.xml
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base_local/move_base_local.xml 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/move_base_local/move_base_local.xml 2009-08-26 01:38:16 UTC (rev 22930)
@@ -2,6 +2,12 @@
<include file="$(find pr2_alpha)/$(env ROBOT).machine" />
<node pkg="mux" type="throttle" args="3.0 /move_base_local/local_costmap/voxel_grid /move_base_local/local_costmap/voxel_grid_throttled" />
+
+<!-- Run the move base translator -->
+<node pkg="move_base_client" type="move_base_translator" name="move_base_local_translator" machine="four">
+ <param name="action_name" value="move_base_local" />
+</node>
+
<node pkg="move_base" type="move_base" respawn="false" name="move_base_local" output="screen" machine="three">
<remap from="odom" to="base/odom" />
<param name="controller_frequency" value="10.0" />
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/rviz_move_base.launch
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/rviz_move_base.launch 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/rviz/rviz_move_base.launch 2009-08-26 01:38:16 UTC (rev 22930)
@@ -1,6 +1,6 @@
<launch>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find 2dnav_pr2)/rviz/move_base.vcg">
- <remap from="goal" to="/move_base/activate" />
+ <remap from="goal" to="/move_base_old/activate" />
</node>
<node pkg="costmap_2d" type="costmap_2d_markers" name="voxel_visualizer">
<remap from="voxel_grid" to="/move_base/local_costmap/voxel_grid_throttled"/>
Modified: pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch
===================================================================
--- pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch 2009-08-26 01:38:16 UTC (rev 22930)
@@ -1,5 +1,11 @@
<launch>
<master auto="start"/>
+
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator">
+ <param name="action_name" value="move_base" />
+ </node>
+
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="footprint_padding" value="0.01" />
Modified: pkg/trunk/demos/erratic_gazebo/2dnav-gazebo-simple-fake_localization.xml
===================================================================
--- pkg/trunk/demos/erratic_gazebo/2dnav-gazebo-simple-fake_localization.xml 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/demos/erratic_gazebo/2dnav-gazebo-simple-fake_localization.xml 2009-08-26 01:38:16 UTC (rev 22930)
@@ -26,6 +26,12 @@
<param name="laser_max_range" value="30.0" />
</node>
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator">
+ <param name="action_name" value="move_base" />
+ </node>
+
+
<!-- for moving -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="controller_frequency" value="10.0" />
Modified: pkg/trunk/demos/milestone2/milestone2_tests/test/move_base.launch
===================================================================
--- pkg/trunk/demos/milestone2/milestone2_tests/test/move_base.launch 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/demos/milestone2/milestone2_tests/test/move_base.launch 2009-08-26 01:38:16 UTC (rev 22930)
@@ -2,6 +2,12 @@
<master auto="start"/>
<group name="wg">
<param name="/use_sim_time" value="true"/>
+
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator">
+ <param name="action_name" value="move_base" />
+ </node>
+
<node pkg="move_base" type="move_base" respawn="false" name="move_base" >
<param name="global_frame" value="map" />
Modified: pkg/trunk/demos/test_2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/manifest.xml 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/demos/test_2dnav_gazebo/manifest.xml 2009-08-26 01:38:16 UTC (rev 22930)
@@ -10,7 +10,6 @@
<depend package="map_server"/>
<depend package="fake_localization"/>
<depend package="teleop_arm_keyboard"/>
- <depend package="trex_pr2"/>
<depend package="robot_pose_ekf"/>
<depend package="pr2_gazebo"/>
<depend package="tf"/>
Modified: pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-08-26 01:38:16 UTC (rev 22930)
@@ -183,7 +183,7 @@
self.bumped = True
def stateInput(self, state):
- if self.publish_goal:
+ if self.publish_goal and state.status.value == state.status.ACTIVE:
state_eul = tft.euler_from_quaternion([state.goal.pose.orientation.x,state.goal.pose.orientation.y,state.goal.pose.orientation.z,state.goal.pose.orientation.w])
print "target: ", self.target_x, ",", self.target_y, ",", self.target_t
print "state.goal: (", state.goal.pose.position.x, ",", state.goal.pose.position.y, ",", state.goal.pose.position.z \
@@ -219,13 +219,13 @@
print "LNK\n"
#pub_base = rospy.Publisher("cmd_vel", BaseVel)
- pub_goal = rospy.Publisher("/move_base/activate", PoseStamped)
+ pub_goal = rospy.Publisher("/move_base_old/activate", PoseStamped)
pub_pose = rospy.Publisher("initialpose" , PoseWithCovarianceStamped)
rospy.Subscriber("base_pose_ground_truth", Odometry , self.p3dInput)
rospy.Subscriber("pr2_odometry/odom" , Odometry , self.odomInput)
rospy.Subscriber("base_bumper/info" , String , self.bumpedInput)
rospy.Subscriber("torso_lift_bumper/info", String , self.bumpedInput)
- rospy.Subscriber("/move_base/feedback" , MoveBaseState , self.stateInput)
+ rospy.Subscriber("/move_base_old/feedback" , MoveBaseState , self.stateInput)
rospy.Subscriber("/amcl_pose" , PoseWithCovarianceStamped , self.amclInput)
# below only for debugging build 303, base not moving
@@ -421,3 +421,4 @@
rostest.run(PKG, sys.argv[0], NavStackTest, sys.argv) #, text_mode=True)
+
Modified: pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp 2009-08-26 01:38:16 UTC (rev 22930)
@@ -121,7 +121,7 @@
robot_actions::ActionClient<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door> push_door("push_door");
robot_actions::ActionClient<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty> release_handle("release_handle");
robot_actions::ActionClient<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door> move_base_door("move_base_door");
- robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped> move_base_local("move_base_local");
+ robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped> move_base_local("move_base_local_old");
door_msgs::Door tmp_door;
door_msgs::Door backup_door;
Modified: pkg/trunk/highlevel/doors/doors_core/test/test_planner.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/test/test_planner.cpp 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/highlevel/doors/doors_core/test/test_planner.cpp 2009-08-26 01:38:16 UTC (rev 22930)
@@ -107,7 +107,7 @@
robot_actions::ActionClient<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty> release_handle("release_handle");
robot_actions::ActionClient<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door> move_base_door("move_base_door");
robot_actions::ActionClient<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door> sbpl_door_planner("sbpl_door_planner");
- robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped> move_base_local("move_base_local");
+ robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped> move_base_local("move_base_local_old");
robot_actions::ActionClient<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t> move_arm("move_arm");
door_msgs::Door tmp_door;
Modified: pkg/trunk/highlevel/move_base_stage/move_base/move_base.xml
===================================================================
--- pkg/trunk/highlevel/move_base_stage/move_base/move_base.xml 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/highlevel/move_base_stage/move_base/move_base.xml 2009-08-26 01:38:16 UTC (rev 22930)
@@ -1,5 +1,11 @@
<launch>
- <node pkg="move_base" type="move_base_new" respawn="false" name="move_base" output="screen">
+
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator">
+ <param name="action_name" value="move_base" />
+ </node>
+
+ <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="footprint_padding" value="0.01" />
<param name="controller_frequency" value="10.0" />
Modified: pkg/trunk/highlevel/move_base_stage/move_base_multi_robot.launch
===================================================================
--- pkg/trunk/highlevel/move_base_stage/move_base_multi_robot.launch 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/highlevel/move_base_stage/move_base_multi_robot.launch 2009-08-26 01:38:16 UTC (rev 22930)
@@ -12,6 +12,12 @@
<!-- BEGIN ROBOT 0 -->
<group ns="robot_0">
<param name="~tf_prefix" value="robot_0" />
+
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator">
+ <param name="action_name" value="move_base" />
+ </node>
+
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="controller_frequency" value="10.0" />
<rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
@@ -39,6 +45,12 @@
<!-- BEGIN ROBOT 1 -->
<group ns="robot_1">
<param name="~tf_prefix" value="robot_1" />
+
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator">
+ <param name="action_name" value="move_base" />
+ </node>
+
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="controller_frequency" value="10.0" />
<rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
Modified: pkg/trunk/highlevel/plugs/plugs_core/test/test_approach.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/test/test_approach.cpp 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/highlevel/plugs/plugs_core/test/test_approach.cpp 2009-08-26 01:38:16 UTC (rev 22930)
@@ -84,7 +84,7 @@
robot_actions::ActionClient<pr2_robot_actions::SwitchControllers, pr2_robot_actions::SwitchControllersState, std_msgs::Empty>
switch_controllers("switch_controllers");
robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped>
- move_base_local("move_base_local");
+ move_base_local("move_base_local_old");
Duration(1.0).sleep();
Modified: pkg/trunk/highlevel/plugs/plugs_core/test/test_executive.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/test/test_executive.cpp 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/highlevel/plugs/plugs_core/test/test_executive.cpp 2009-08-26 01:38:16 UTC (rev 22930)
@@ -115,7 +115,7 @@
robot_actions::ActionClient<geometry_msgs::PointStamped, pr2_robot_actions::DetectOutletState, geometry_msgs::PoseStamped>
detect_outlet_coarse("detect_outlet_coarse");
robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped>
- move_base_local("move_base_local");
+ move_base_local("move_base_local_old");
Duration(1.0).sleep();
Modified: pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_outlet_coarse.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_outlet_coarse.cpp 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_outlet_coarse.cpp 2009-08-26 01:38:16 UTC (rev 22930)
@@ -122,7 +122,7 @@
robot_actions::ActionClient<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty> plug_in("plug_in");
robot_actions::ActionClient<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty> unplug("unplug");
robot_actions::ActionClient< plugs_msgs::PlugStow, pr2_robot_actions::StowPlugState, std_msgs::Empty> stow_plug("stow_plug");
- robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped> move_base_local("move_base_local");
+ robot_actions::ActionClient<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped> move_base_local("move_base_local_old");
timeout_medium.sleep();
Modified: pkg/trunk/nav/visual_nav/launch/move_base.launch
===================================================================
--- pkg/trunk/nav/visual_nav/launch/move_base.launch 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/nav/visual_nav/launch/move_base.launch 2009-08-26 01:38:16 UTC (rev 22930)
@@ -1,6 +1,11 @@
<launch>
<master auto="start"/>
<group>
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator">
+ <param name="action_name" value="move_base" />
+ </node>
+
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="footprint_padding" value="0.01" />
Modified: pkg/trunk/nav/visual_nav/launch/move_base_robot.launch
===================================================================
--- pkg/trunk/nav/visual_nav/launch/move_base_robot.launch 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/nav/visual_nav/launch/move_base_robot.launch 2009-08-26 01:38:16 UTC (rev 22930)
@@ -8,6 +8,12 @@
<include file="$(find 2dnav_pr2)/config/ground_plane.xml" />
<node pkg="mux" type="throttle" args="3.0 /move_base/base_local_planner/costmap/voxel_grid /move_base/base_local_planner/costmap/voxel_grid_throttled" />
+
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_translator" machine="four">
+ <param name="action_name" value="move_base" />
+ </node>
+
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" machine="three">
<param name="footprint_padding" value="0.01" />
Modified: pkg/trunk/sandbox/action_translator/include/action_translator/action_translator.h
===================================================================
--- pkg/trunk/sandbox/action_translator/include/action_translator/action_translator.h 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/sandbox/action_translator/include/action_translator/action_translator.h 2009-08-26 01:38:16 UTC (rev 22930)
@@ -102,6 +102,9 @@
// Convert from the old goal type to the new goal type
Goal new_goal = from_old_goal_func_(old_goal);
+ //wait for the action server to start before passing the goal on
+ ac_.waitForActionServerToStart(ros::Duration(5.0));
+
// Send the goal to the action server
ac_.sendGoal(new_goal,
boost::bind(&ActionTranslatorT::handleDone, this, _1, _2),
Modified: pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp
===================================================================
--- pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/sandbox/move_base_client/src/move_base_translator.cpp 2009-08-26 01:38:16 UTC (rev 22930)
@@ -67,9 +67,11 @@
ros::init(argc, argv, "move_base_action_translator");
ros::NodeHandle n;
+ std::string action_name;
+ n.param("~action_name", action_name, std::string("move_base"));
action_translator::ActionTranslator<move_base_msgs::MoveBaseAction, PoseStamped, PoseStamped>
- translator("move_base", &fromOldGoal, NULL, NULL);
+ translator(action_name, &fromOldGoal, NULL, NULL);
robot_actions::ActionRunner runner(10.0);
runner.connect<geometry_msgs::PoseStamped, nav_robot_actions::MoveBaseState, geometry_msgs::PoseStamped>(translator);
Modified: pkg/trunk/sandbox/teleop_anti_collision/launch/run_teleop_anti_collision.xml
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/launch/run_teleop_anti_collision.xml 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/sandbox/teleop_anti_collision/launch/run_teleop_anti_collision.xml 2009-08-26 01:38:16 UTC (rev 22930)
@@ -13,7 +13,13 @@
<!-- The naviagtion stack and associated parameters -->
<!--include file="$(find 2dnav_pr2)/move_base_local/move_base_local.xml" /-->
<node pkg="mux" type="throttle" args="3.0 /move_base_local/local_costmap/voxel_grid /move_base_local/local_costmap/voxel_grid_throttled" />
- <node pkg="move_base" type="move_base_new" respawn="false" name="move_base_local" output="screen" machine="three">
+
+ <!-- Run the move base translator -->
+ <node pkg="move_base_client" type="move_base_translator" name="move_base_local_translator" machine="four">
+ <param name="action_name" value="move_base_local" />
+ </node>
+
+ <node pkg="move_base" type="move_base" respawn="false" name="move_base_local" output="screen" machine="three">
<param name="controller_frequency" value="10.0" />
<param name="base_global_planner" value="CarrotPlanner" />
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server.h 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server.h 2009-08-26 01:38:16 UTC (rev 22930)
@@ -80,12 +80,23 @@
* @param name The name of the action
* @param goal_cb A goal callback to be called when the ActionServer receives a new goal over the wire
* @param cancel_cb A cancel callback to be called when the ActionServer receives a new cancel request over the wire
+ * @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up
*/
ActionServer(ros::NodeHandle n, std::string name,
boost::function<void (GoalHandle)> goal_cb = boost::function<void (GoalHandle)>(),
- boost::function<void (GoalHandle)> cancel_cb = boost::function<void (GoalHandle)>());
+ boost::function<void (GoalHandle)> cancel_cb = boost::function<void (GoalHandle)>(),
+ bool auto_start = true);
/**
+ * @brief Constructor for an ActionServer
+ * @param n A NodeHandle to create a namespace under
+ * @param name The name of the action
+ * @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up
+ */
+ ActionServer(ros::NodeHandle n, std::string name,
+ bool auto_start = true);
+
+ /**
* @brief Register a callback to be invoked when a new goal is received, this will replace any previously registered callback
* @param cb The callback to invoke
*/
@@ -97,8 +108,18 @@
*/
void registerCancelCallback(boost::function<void (GoalHandle)> cb);
+ /**
+ * @brief Explicitly start the action server, used it auto_start is set to false
+ */
+ void start();
+
private:
/**
+ * @brief Initialize all ROS connections and setup timers
+ */
+ void initialize();
+
+ /**
* @brief Publishes a result for a given goal
* @param status The status of the goal with which the result is associated
* @param result The result to publish
@@ -154,6 +175,7 @@
friend class HandleTrackerDeleter<ActionSpec>;
GoalIDGenerator id_generator_;
+ bool started_;
};
};
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server_imp.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server_imp.h 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/action_server_imp.h 2009-08-26 01:38:16 UTC (rev 22930)
@@ -39,9 +39,23 @@
namespace actionlib {
template <class ActionSpec>
ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name,
+ bool auto_start)
+ : ActionServer<ActionSpec>::ActionServer(n, name, boost::function<void (GoalHandle)>(), boost::function<void (GoalHandle)>(), auto_start) {}
+
+ template <class ActionSpec>
+ ActionServer<ActionSpec>::ActionServer(ros::NodeHandle n, std::string name,
boost::function<void (GoalHandle)> goal_cb,
- boost::function<void (GoalHandle)> cancel_cb)
- : node_(n, name), goal_callback_(goal_cb), cancel_callback_(cancel_cb) {
+ boost::function<void (GoalHandle)> cancel_cb,
+ bool auto_start)
+ : node_(n, name), goal_callback_(goal_cb), cancel_callback_(cancel_cb), started_(auto_start) {
+
+ //if we're to autostart... then we'll initialize things
+ if(started_)
+ initialize();
+ }
+
+ template <class ActionSpec>
+ void ActionServer<ActionSpec>::initialize(){
status_pub_ = node_.advertise<actionlib_msgs::GoalStatusArray>("status", 1);
result_pub_ = node_.advertise<ActionResult>("result", 1);
feedback_pub_ = node_.advertise<ActionFeedback>("feedback", 1);
@@ -61,9 +75,8 @@
status_timer_ = node_.createTimer(ros::Duration(1.0 / status_frequency),
boost::bind(&ActionServer::publishStatus, this, _1));
+ }
- }
-
template <class ActionSpec>
void ActionServer<ActionSpec>::registerGoalCallback(boost::function<void (GoalHandle)> cb){
goal_callback_ = cb;
@@ -97,6 +110,11 @@
template <class ActionSpec>
void ActionServer<ActionSpec>::cancelCallback(const boost::shared_ptr<const actionlib_msgs::GoalID>& goal_id){
boost::recursive_mutex::scoped_lock lock(lock_);
+
+ //if we're not started... then we're not actually going to do anything
+ if(!started_)
+ return;
+
//we need to handle a cancel for the user
ROS_DEBUG("The action server has received a new cancel request");
bool goal_id_found = false;
@@ -153,6 +171,10 @@
void ActionServer<ActionSpec>::goalCallback(const boost::shared_ptr<const ActionGoal>& goal){
boost::recursive_mutex::scoped_lock lock(lock_);
+ //if we're not started... then we're not actually going to do anything
+ if(!started_)
+ return;
+
ROS_DEBUG("The action server has received a new goal request");
//we need to check if this goal already lives in the status list
@@ -191,8 +213,19 @@
}
template <class ActionSpec>
+ void ActionServer<ActionSpec>::start(){
+ initialize();
+ started_ = true;
+ }
+
+ template <class ActionSpec>
void ActionServer<ActionSpec>::publishStatus(const ros::TimerEvent& e){
boost::recursive_mutex::scoped_lock lock(lock_);
+
+ //we won't publish status unless we've been started
+ if(!started_)
+ return;
+
publishStatus();
}
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server.h 2009-08-26 01:24:22 UTC (rev 22929)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server.h 2009-08-26 01:38:16 UTC (rev 22930)
@@ -69,8 +69,9 @@
* @param execute_cb Optional callback that gets called in a separate thread whenever
* a new goal is received, allowing users to have blocking callbacks.
* Adding an execute callback also deactivates the goalCallback.
+ * @param auto_start A boolean value that tells the ActionServer wheteher or not to start publishing as soon as it comes up
*/
- SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_cb = NULL);
+ SimpleActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_cb = NULL, bool auto_start = true);
~SimpleActionServer();
@@ -149,6 +150,11 @@
*/
void registerPreemptCallback(boost::function<void ()> cb);
+ /**
+ * @brief Explicitly start the action server, used it auto_start is set to false
+ */
+ void start();
+
private:
/**
* @brief Callback for when the ActionServer receives a new goal and passes it on
Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/server/simple_action_server_imp.h
===================================================================
--- pkg/...
[truncated message content] |
|
From: <ei...@us...> - 2009-08-26 02:05:03
|
Revision: 22933
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22933&view=rev
Author: eitanme
Date: 2009-08-26 02:04:52 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
Fixing the build
Modified Paths:
--------------
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/manifest.xml
pkg/trunk/demos/2dnav_erratic/manifest.xml
pkg/trunk/demos/2dnav_gazebo/manifest.xml
pkg/trunk/demos/erratic_gazebo/manifest.xml
pkg/trunk/demos/milestone2/milestone2_tests/manifest.xml
pkg/trunk/highlevel/move_base_stage/manifest.xml
pkg/trunk/sandbox/object_modeler/manifest.xml
pkg/trunk/stacks/trex/trex_pr2_writing_test/manifest.xml
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/manifest.xml
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/manifest.xml 2009-08-26 01:51:39 UTC (rev 22932)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/manifest.xml 2009-08-26 02:04:52 UTC (rev 22933)
@@ -15,6 +15,7 @@
<depend package="map_server"/>
<depend package="willow_maps"/>
<depend package="move_base"/>
+ <depend package="move_base_client"/>
<depend package="mux"/>
<depend package="backup_safetysound"/>
<depend package="geometry_msgs"/>
Modified: pkg/trunk/demos/2dnav_erratic/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_erratic/manifest.xml 2009-08-26 01:51:39 UTC (rev 22932)
+++ pkg/trunk/demos/2dnav_erratic/manifest.xml 2009-08-26 02:04:52 UTC (rev 22933)
@@ -11,4 +11,5 @@
<depend package="map_server"/>
<depend package="wavefront"/>
<depend package="move_base"/>
+ <depend package="move_base_client"/>
</package>
Modified: pkg/trunk/demos/2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-08-26 01:51:39 UTC (rev 22932)
+++ pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-08-26 02:04:52 UTC (rev 22933)
@@ -7,6 +7,7 @@
<depend package="std_msgs"/>
<depend package="nav_view"/>
<depend package="move_base"/>
+ <depend package="move_base_client"/>
<depend package="gazebo_plugin"/>
<depend package="map_server"/>
<depend package="amcl"/>
Modified: pkg/trunk/demos/erratic_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/erratic_gazebo/manifest.xml 2009-08-26 01:51:39 UTC (rev 22932)
+++ pkg/trunk/demos/erratic_gazebo/manifest.xml 2009-08-26 02:04:52 UTC (rev 22933)
@@ -13,4 +13,5 @@
<depend package="tf"/>
<depend package="rospy"/>
<depend package="std_msgs"/>
+ <depend package="move_base_client"/>
</package>
Modified: pkg/trunk/demos/milestone2/milestone2_tests/manifest.xml
===================================================================
--- pkg/trunk/demos/milestone2/milestone2_tests/manifest.xml 2009-08-26 01:51:39 UTC (rev 22932)
+++ pkg/trunk/demos/milestone2/milestone2_tests/manifest.xml 2009-08-26 02:04:52 UTC (rev 22933)
@@ -10,6 +10,7 @@
<url>http://pr.willowgarage.com/wiki/milestone2_tests</url>
<depend package="topological_map"/>
<depend package="move_base"/>
+ <depend package="move_base_client"/>
<depend package="robot_actions"/>
<depend package="stage"/>
<depend package="roscpp"/>
Modified: pkg/trunk/highlevel/move_base_stage/manifest.xml
===================================================================
--- pkg/trunk/highlevel/move_base_stage/manifest.xml 2009-08-26 01:51:39 UTC (rev 22932)
+++ pkg/trunk/highlevel/move_base_stage/manifest.xml 2009-08-26 02:04:52 UTC (rev 22933)
@@ -13,5 +13,6 @@
<depend package="map_server"/>
<depend package="fake_localization"/>
<depend package="nav_view"/>
+ <depend package="move_base_client"/>
</package>
Modified: pkg/trunk/sandbox/object_modeler/manifest.xml
===================================================================
--- pkg/trunk/sandbox/object_modeler/manifest.xml 2009-08-26 01:51:39 UTC (rev 22932)
+++ pkg/trunk/sandbox/object_modeler/manifest.xml 2009-08-26 02:04:52 UTC (rev 22933)
@@ -11,6 +11,7 @@
<depend package="roscpp"/>
<depend package="std_msgs"/>
<depend package="move_base"/>
+ <depend package="move_base_client"/>
<depend package="nav_robot_actions"/>
<depend package="annotated_map_builder"/>
<depend package="bullet"/>
Modified: pkg/trunk/stacks/trex/trex_pr2_writing_test/manifest.xml
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2_writing_test/manifest.xml 2009-08-26 01:51:39 UTC (rev 22932)
+++ pkg/trunk/stacks/trex/trex_pr2_writing_test/manifest.xml 2009-08-26 02:04:52 UTC (rev 22933)
@@ -14,6 +14,7 @@
<depend package="geometry_msgs"/>
<depend package="roscpp"/>
<depend package="move_base"/>
+ <depend package="move_base_client"/>
<depend package="safety_core"/>
<depend package="nav_robot_actions"/>
<depend package="point_cloud_assembler"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mee...@us...> - 2009-08-26 19:05:53
|
Revision: 23002
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23002&view=rev
Author: meeussen
Date: 2009-08-26 19:05:33 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
make init function mandatory for controllers. remove controllers that do not implement init function from makefile (those controllers haven't worked for more than a month now). open door test passes
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/controller_manifest.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_autotuner.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_constraint_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_pd_controller.h
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_position_smoothing_controller.h
pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/controller_manifest.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_autotuner.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_chain_constraint_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_pd_controller.cpp
pkg/trunk/sandbox/experimental_controllers/src/joint_position_smoothing_controller.cpp
pkg/trunk/stacks/mechanism/controller_interface/include/controller_interface/controller.h
Removed Paths:
-------------
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/dynamic_loader_controller.h
pkg/trunk/sandbox/experimental_controllers/src/dynamic_loader_controller.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/gripper_calibration_controller.h 2009-08-26 19:05:33 UTC (rev 23002)
@@ -34,19 +34,8 @@
#pragma once
-/*
- Example XML:
- <controller type="GripperCalibrationController">
- <calibrate joint="upperarm_roll_right_joint"
- actuator="upperarm_roll_right_act"
- transmission="upperarm_roll_right_trans"
- velocity="0.3" />
- <pid p="15" i="0" d="0" iClamp="0" />
- </controller>
-*/
-
#include "mechanism_model/robot.h"
#include "robot_mechanism_controllers/joint_velocity_controller.h"
#include "realtime_tools/realtime_publisher.h"
@@ -93,42 +82,6 @@
};
-/***************************************************/
-/*! \class controller::GripperCalibrationControllerNode
- \brief Joint Limit Controller ROS Node
-
- This class starts and stops the initialization sequence
-
-*/
-/***************************************************/
-
-class GripperCalibrationControllerNode : public Controller
-{
-public:
- /*!
- * \brief Default Constructor
- *
- */
- GripperCalibrationControllerNode();
-
- /*!
- * \brief Destructor
- */
- ~GripperCalibrationControllerNode();
-
- void update();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
-
-private:
- mechanism::RobotState* robot_;
- GripperCalibrationController c_;
-
- double last_publish_time_;
- realtime_tools::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
-};
-
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h 2009-08-26 19:05:33 UTC (rev 23002)
@@ -98,24 +98,6 @@
};
-class WristCalibrationControllerNode : public Controller
-{
-public:
- WristCalibrationControllerNode();
- ~WristCalibrationControllerNode();
-
- void update();
-
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
-private:
- mechanism::RobotState *robot_;
- WristCalibrationController c_;
-
- double last_publish_time_;
- realtime_tools::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
-};
-
}
#endif
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/controller_manifest.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/controller_manifest.cpp 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/controller_manifest.cpp 2009-08-26 19:05:33 UTC (rev 23002)
@@ -47,7 +47,6 @@
PLUGINLIB_REGISTER_CLASS(CasterCalibrationController, CasterCalibrationController, Controller)
PLUGINLIB_REGISTER_CLASS(CasterController, CasterController, Controller)
PLUGINLIB_REGISTER_CLASS(GripperCalibrationController, GripperCalibrationController, Controller)
-PLUGINLIB_REGISTER_CLASS(GripperCalibrationControllerNode, GripperCalibrationControllerNode, Controller)
PLUGINLIB_REGISTER_CLASS(HeadPositionController, HeadPositionController, Controller)
PLUGINLIB_REGISTER_CLASS(LaserScannerTrajController, LaserScannerTrajController, Controller)
PLUGINLIB_REGISTER_CLASS(LaserScannerTrajControllerNode, LaserScannerTrajControllerNode, Controller)
@@ -55,6 +54,5 @@
//PLUGINLIB_REGISTER_CLASS(Pr2GripperController, Pr2GripperController, Controller)
PLUGINLIB_REGISTER_CLASS(Pr2Odometry, Pr2Odometry, Controller)
PLUGINLIB_REGISTER_CLASS(WristCalibrationController, WristCalibrationController, Controller)
-PLUGINLIB_REGISTER_CLASS(WristCalibrationControllerNode, WristCalibrationControllerNode, Controller)
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp 2009-08-26 19:05:33 UTC (rev 23002)
@@ -41,7 +41,6 @@
namespace controller
{
-ROS_REGISTER_CONTROLLER(GripperCalibrationController)
GripperCalibrationController::GripperCalibrationController()
: state_(INITIALIZED), last_publish_time_(0), joint_(NULL)
@@ -207,52 +206,4 @@
}
-ROS_REGISTER_CONTROLLER(GripperCalibrationControllerNode)
-
-GripperCalibrationControllerNode::GripperCalibrationControllerNode()
-: robot_(NULL), last_publish_time_(0)
-{
}
-
-GripperCalibrationControllerNode::~GripperCalibrationControllerNode()
-{
-}
-
-void GripperCalibrationControllerNode::update()
-{
- c_.update();
-
- if (c_.calibrated())
- {
- if (last_publish_time_ + 0.5 < robot_->getTime())
- {
- assert(pub_calibrated_);
- if (pub_calibrated_->trylock())
- {
- last_publish_time_ = robot_->getTime();
- pub_calibrated_->unlockAndPublish();
- }
- }
- }
-}
-
-bool GripperCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- robot_ = robot;
-
- std::string topic = config->Attribute("name") ? config->Attribute("name") : "";
- if (topic == "")
- {
- fprintf(stderr, "No name given to GripperCalibrationController\n");
- return false;
- }
-
- if (!c_.initXml(robot, config))
- return false;
-
- pub_calibrated_ = new realtime_tools::RealtimePublisher<std_msgs::Empty>(topic + "/calibrated", 1);
-
- return true;
-}
-
-}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp 2009-08-26 19:05:33 UTC (rev 23002)
@@ -35,7 +35,6 @@
namespace controller {
-ROS_REGISTER_CONTROLLER(WristCalibrationController)
WristCalibrationController::WristCalibrationController()
: state_(INITIALIZED), robot_(NULL), last_publish_time_(0)
@@ -384,55 +383,4 @@
}
-ROS_REGISTER_CONTROLLER(WristCalibrationControllerNode)
-
-WristCalibrationControllerNode::WristCalibrationControllerNode()
-: last_publish_time_(0), pub_calibrated_(NULL)
-{
}
-
-WristCalibrationControllerNode::~WristCalibrationControllerNode()
-{
- if (pub_calibrated_)
- delete pub_calibrated_;
-}
-
-void WristCalibrationControllerNode::update()
-{
- c_.update();
-
- if (c_.calibrated())
- {
- if (last_publish_time_ + 0.5 < robot_->getTime())
- {
- assert(pub_calibrated_);
- if (pub_calibrated_->trylock())
- {
- last_publish_time_ = robot_->getTime();
- pub_calibrated_->unlockAndPublish();
- }
- }
- }
-}
-
-bool WristCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- assert(robot);
- robot_ = robot;
-
- std::string name = config->Attribute("name") ? config->Attribute("name") : "";
- if (name == "")
- {
- fprintf(stderr, "No name given to WristCalibrationController\n");
- return false;
- }
-
- if (!c_.initXml(robot, config))
- return false;
-
- pub_calibrated_ = new realtime_tools::RealtimePublisher<std_msgs::Empty>(name + "/calibrated", 1);
-
- return true;
-}
-
-}
Modified: pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/sandbox/experimental_controllers/CMakeLists.txt 2009-08-26 19:05:33 UTC (rev 23002)
@@ -33,16 +33,15 @@
src/cartesian_tff_controller.cpp
src/cartesian_twist_controller_ik.cpp
src/cartesian_hybrid_controller.cpp
- src/dynamic_loader_controller.cpp
- src/joint_position_smoothing_controller.cpp
+# src/joint_position_smoothing_controller.cpp
src/joint_inverse_dynamics_controller.cpp
- src/joint_autotuner.cpp
- src/joint_calibration_controller.cpp
- src/joint_limit_calibration_controller.cpp
- src/joint_blind_calibration_controller.cpp
+# src/joint_autotuner.cpp
+# src/joint_calibration_controller.cpp
+# src/joint_limit_calibration_controller.cpp
+# src/joint_blind_calibration_controller.cpp
src/joint_chain_constraint_controller.cpp
- src/joint_chain_sine_controller.cpp
- src/probe.cpp
+# src/joint_chain_sine_controller.cpp
+# src/probe.cpp
src/head_servoing_controller.cpp
src/arm_trajectory_controller.cpp
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/arm_trajectory_controller.h 2009-08-26 19:05:33 UTC (rev 23002)
@@ -75,7 +75,7 @@
// The maximum number of joints expected in an arm.
static const int MAX_ARM_JOINTS = 7;
- class ArmTrajectoryController : public Controller
+ class ArmTrajectoryController
{
public:
Deleted: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/dynamic_loader_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/dynamic_loader_controller.h 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/dynamic_loader_controller.h 2009-08-26 19:05:33 UTC (rev 23002)
@@ -1,113 +0,0 @@
-/*
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/*
- * Author: Rob Wheeler
- */
-
-#ifndef DYNAMIC_LOADER_CONTROLLER_H
-#define DYNAMIC_LOADER_CONTROLLER_H
-
-#include "controller_interface/controller.h"
-#include <tinyxml/tinyxml.h>
-#include <ltdl.h>
-
-namespace controller {
-
-/***************************************************/
-/*! \class controller::DynamicLoaderController
- \brief Dynamic Loader
-
- This class implements a pseudo-controller that can dynamically
- load a package's shared object and instantiate controllers from
- that shared object.
-
- When the DynamicLoaderController is killed, it shuts down the
- controllers that it started and unloads the shared object.
-
- Example configuration:
- <pre>
- <controller name="dynamic_loader" type="DynamicLoaderController"
- package="my_controllers" lib="libmy_controllers">
-
- <controllers>
-
- <controller type="MyController" name="my_controller1">
- <joint name="joint_to_control" />
- </controller><br>
-
- <controller type="MyController" name="my_controller2">
- <joint name="another_joint_to_control" />
- </controller><br>
-
- </controllers>
-
- </controller>
- </pre>
-
- The above example creates an instance of the DynamicLoaderController
- that loads the shared object libmy_controllers.so from the
- my_controllers package. It then instantiates two controllers,
- my_controller1 and my_controller2, from that shared object.
- When the DynamicLoaderController is killed, it will kill the two
- controllers it started and unload the libmy_controllers.so shared object.
-
-*/
-/***************************************************/
-
-class DynamicLoaderController : public Controller
-{
-public:
- DynamicLoaderController();
- ~DynamicLoaderController();
-
- /*!
- * \brief Specifies the package and shared object to load.
- * \param *robot The robot (not used by this controller).
- * \param *config The XML configuration for this controller
- */
- bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- /*!
- * \brief This function is called in the control loop. For this
- * pseudo-controller, the update function is a noop.
- */
- void update();
-
-private:
- std::vector<std::string> names_;
- lt_dlhandle handle_;
-
- void loadLibrary(std::string& name);
- static void unloadLibrary(std::vector<std::string> names, lt_dlhandle handle);
-};
-
-}
-
-#endif
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_autotuner.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_autotuner.h 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_autotuner.h 2009-08-26 19:05:33 UTC (rev 23002)
@@ -162,19 +162,11 @@
*/
/***************************************************/
-
class JointAutotunerNode : public Controller
{
public:
- /*!
- * \brief Default Constructor
- *
- */
JointAutotunerNode();
- /*!
- * \brief Destructor
- */
~JointAutotunerNode();
void update();
@@ -191,5 +183,6 @@
private:
JointAutotuner *c_;
};
+
}
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_constraint_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_constraint_controller.h 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_chain_constraint_controller.h 2009-08-26 19:05:33 UTC (rev 23002)
@@ -155,6 +155,7 @@
+/*
class JointChainConstraintControllerNode : public Controller
{
public:
@@ -177,7 +178,7 @@
};
-
+*/
} // namespace
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_pd_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_pd_controller.h 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_pd_controller.h 2009-08-26 19:05:33 UTC (rev 23002)
@@ -53,10 +53,10 @@
#include <ros/node.h>
-#include <controller_interface/controller.h>
#include <control_toolbox/pid.h>
#include "misc_utils/advertised_service_guard.h"
#include "misc_utils/subscription_guard.h"
+#include <mechanism_model/robot.h>
// Services
#include <experimental_controllers/SetPDCommand.h>
@@ -66,7 +66,7 @@
namespace controller
{
- class JointPDController : public Controller
+ class JointPDController
{
public:
@@ -138,7 +138,7 @@
*/
/***************************************************/
- class JointPDControllerNode : public Controller
+ class JointPDControllerNode
{
public:
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_position_smoothing_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_position_smoothing_controller.h 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/joint_position_smoothing_controller.h 2009-08-26 19:05:33 UTC (rev 23002)
@@ -122,6 +122,7 @@
*/
/***************************************************/
+
class JointPositionSmoothControllerNode : public Controller
{
public:
@@ -148,5 +149,6 @@
JointPositionSmoothController *c_; /**< The controller. */
};
+
}
Modified: pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/sandbox/experimental_controllers/src/arm_trajectory_controller.cpp 2009-08-26 19:05:33 UTC (rev 23002)
@@ -41,7 +41,6 @@
using namespace controller;
using namespace std;
-ROS_REGISTER_CONTROLLER(ArmTrajectoryController);
static const std::string JointTrajectoryStatusString[7] = {"0 - ACTIVE","1 - DONE","2 - QUEUED","3 - DELETED","4 - FAILED","5 - CANCELED","6 - NUM_STATUS"};
Modified: pkg/trunk/sandbox/experimental_controllers/src/controller_manifest.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/controller_manifest.cpp 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/sandbox/experimental_controllers/src/controller_manifest.cpp 2009-08-26 19:05:33 UTC (rev 23002)
@@ -38,17 +38,16 @@
#include "experimental_controllers/cartesian_hybrid_controller.h"
#include "experimental_controllers/cartesian_twist_controller_ik.h"
-#include "experimental_controllers/dynamic_loader_controller.h"
//#include "experimental_controllers/endeffector_constraint_controller.h"
-#include "experimental_controllers/joint_autotuner.h"
-#include "experimental_controllers/joint_blind_calibration_controller.h"
-#include "experimental_controllers/joint_calibration_controller.h"
+//#include "experimental_controllers/joint_autotuner.h"
+//#include "experimental_controllers/joint_blind_calibration_controller.h"
+//#include "experimental_controllers/joint_calibration_controller.h"
#include "experimental_controllers/joint_chain_constraint_controller.h"
-#include "experimental_controllers/joint_chain_sine_controller.h"
+//#include "experimental_controllers/joint_chain_sine_controller.h"
#include "experimental_controllers/joint_inverse_dynamics_controller.h"
-#include "experimental_controllers/joint_limit_calibration_controller.h"
-#include "experimental_controllers/joint_position_smoothing_controller.h"
-#include "experimental_controllers/probe.h"
+//#include "experimental_controllers/joint_limit_calibration_controller.h"
+//#include "experimental_controllers/joint_position_smoothing_controller.h"
+//#include "experimental_controllers/probe.h"
#include "experimental_controllers/head_servoing_controller.h"
#include "experimental_controllers/joint_trajectory_controller.h"
@@ -64,26 +63,25 @@
//PLUGINLIB_REGISTER_CLASS(PlugControllerNode, PlugControllerNode, Controller)
PLUGINLIB_REGISTER_CLASS(TrajectoryController, TrajectoryController, Controller)
-PLUGINLIB_REGISTER_CLASS(DynamicLoaderController, DynamicLoaderController, Controller)
+//PLUGINLIB_REGISTER_CLASS(DynamicLoaderController, DynamicLoaderController, Controller)
-PLUGINLIB_REGISTER_CLASS(Probe, Probe, Controller)
+//PLUGINLIB_REGISTER_CLASS(Probe, Probe, Controller)
PLUGINLIB_REGISTER_CLASS(CartesianTFFController, CartesianTFFController, Controller)
PLUGINLIB_REGISTER_CLASS(CartesianHybridController, CartesianHybridController, Controller)
PLUGINLIB_REGISTER_CLASS(CartesianHybridControllerNode, CartesianHybridControllerNode, Controller)
//PLUGINLIB_REGISTER_CLASS(EndeffectorConstraintController, EndeffectorConstraintController, Controller)
//PLUGINLIB_REGISTER_CLASS(EndeffectorConstraintControllerNode, EndeffectorConstraintControllerNode, Controller)
-PLUGINLIB_REGISTER_CLASS(JointChainConstraintControllerNode, JointChainConstraintControllerNode, Controller)
+//PLUGINLIB_REGISTER_CLASS(JointChainConstraintControllerNode, JointChainConstraintControllerNode, Controller)
PLUGINLIB_REGISTER_CLASS(JointInverseDynamicsController, JointInverseDynamicsController, Controller);
-PLUGINLIB_REGISTER_CLASS(JointPositionSmoothController, JointPositionSmoothController, Controller)
-PLUGINLIB_REGISTER_CLASS(JointPositionSmoothControllerNode, JointPositionSmoothControllerNode, Controller)
+//PLUGINLIB_REGISTER_CLASS(JointPositionSmoothController, JointPositionSmoothController, Controller)
+//PLUGINLIB_REGISTER_CLASS(JointPositionSmoothControllerNode, JointPositionSmoothControllerNode, Controller)
-PLUGINLIB_REGISTER_CLASS(JointAutotuner, JointAutotuner, Controller)
-PLUGINLIB_REGISTER_CLASS(JointAutotunerNode, JointAutotunerNode, Controller)
-PLUGINLIB_REGISTER_CLASS(JointBlindCalibrationController, JointBlindCalibrationController, Controller)
-PLUGINLIB_REGISTER_CLASS(JointBlindCalibrationControllerNode,JointBlindCalibrationControllerNode, Controller)
-PLUGINLIB_REGISTER_CLASS(JointCalibrationControllerNode,JointCalibrationControllerNode, Controller)
-PLUGINLIB_REGISTER_CLASS(JointChainSineController,JointChainSineController, Controller)
-PLUGINLIB_REGISTER_CLASS(JointLimitCalibrationControllerNode,JointLimitCalibrationControllerNode, Controller)
+//PLUGINLIB_REGISTER_CLASS(JointAutotuner, JointAutotuner, Controller)
+//PLUGINLIB_REGISTER_CLASS(JointBlindCalibrationController, JointBlindCalibrationController, Controller)
+//PLUGINLIB_REGISTER_CLASS(JointBlindCalibrationControllerNode,JointBlindCalibrationControllerNode, Controller)
+//PLUGINLIB_REGISTER_CLASS(JointCalibrationControllerNode,JointCalibrationControllerNode, Controller)
+//PLUGINLIB_REGISTER_CLASS(JointChainSineController,JointChainSineController, Controller)
+//PLUGINLIB_REGISTER_CLASS(JointLimitCalibrationControllerNode,JointLimitCalibrationControllerNode, Controller)
PLUGINLIB_REGISTER_CLASS(CartesianTwistControllerIk,CartesianTwistControllerIk, Controller)
PLUGINLIB_REGISTER_CLASS(HeadServoingController, HeadServoingController, Controller)
Deleted: pkg/trunk/sandbox/experimental_controllers/src/dynamic_loader_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/dynamic_loader_controller.cpp 2009-08-26 19:04:03 UTC (rev 23001)
+++ pkg/trunk/sandbox/experimental_controllers/src/dynamic_loader_controller.cpp 2009-08-26 19:05:33 UTC (rev 23002)
@@ -1,143 +0,0 @@
-/*
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-/*
- * Author: Rob Wheeler
- */
-
-#include "experimental_controllers/dynamic_loader_controller.h"
-#include "mechanism_msgs/SpawnController.h"
-#include "mechanism_msgs/KillController.h"
-#include <iostream>
-
-#include <boost/thread.hpp>
-#include <boost/foreach.hpp>
-#include <boost/algorithm/string/trim.hpp>
-#include <ros/ros.h>
-
-namespace controller {
-
-ROS_REGISTER_CONTROLLER(DynamicLoaderController)
-
-DynamicLoaderController::DynamicLoaderController() : handle_(0)
-{
-}
-
-DynamicLoaderController::~DynamicLoaderController()
-{
- // Shutdown controller cleanly
- boost::thread killThread(boost::bind(DynamicLoaderController::unloadLibrary, names_, handle_));
-}
-
-void DynamicLoaderController::unloadLibrary(std::vector<std::string> names, lt_dlhandle handle)
-{
- mechanism_msgs::KillController::Request req;
- mechanism_msgs::KillController::Response resp;
-
- BOOST_FOREACH(std::string &name, names) {
- req.name = name;
-
- ros::service::call("kill_controller", req, resp);
- }
-
- lt_dlclose(handle);
-}
-
-void DynamicLoaderController::loadLibrary(std::string &name)
-{
- mechanism_msgs::SpawnController::Request req;
- mechanism_msgs::SpawnController::Response resp;
-
- req.name = name;
-
- ros::service::call("spawn_controller", req, resp);
-
- /** \todo Remember to start the controller */
-
- names_.clear();
- if (resp.ok)
- names_.push_back(name);
-}
-
-bool DynamicLoaderController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
- std::string package = config->Attribute("package") ? config->Attribute("package") : "";
- if (package == "") {
- ROS_ERROR("No package given to DynamicLoaderController");
- return false;
- }
-
- std::string lib = config->Attribute("lib") ? config->Attribute("lib") : "";
- if (lib == "") {
- ROS_ERROR("No lib given to DynamicLoaderController");
- return false;
- }
-
- std::string command = "rospack find " + package;
- std::string package_dir;
- char buffer[256];
- FILE *pipe = popen(command.c_str(), "r");
- if (pipe == NULL) {
- ROS_ERROR("Unable to run command: %s", command.c_str());
- return false;
- }
- while (fgets(buffer, sizeof(buffer), pipe) != NULL)
- package_dir.append(buffer);
- pclose(pipe);
-
- int errors = lt_dlinit();
- if (errors) {
- ROS_ERROR("Unable to initialize dynamic loader (%s)", lt_dlerror());
- return false;
- }
-
- std::stringstream path;
- path << boost::algorithm::trim_copy(package_dir) << "/lib/" << lib;
-
- if ((handle_ =...
[truncated message content] |
|
From: <hsu...@us...> - 2009-08-26 19:18:16
|
Revision: 23004
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23004&view=rev
Author: hsujohnhsu
Date: 2009-08-26 19:17:59 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
update plug demo in sim.
Modified Paths:
--------------
pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch
pkg/trunk/demos/plug_in_gazebo/launch/pr2_plug_outlet.launch
pkg/trunk/demos/plug_in_gazebo/plug_defs/plug_defs.xml
pkg/trunk/robot_descriptions/gazebo_worlds/Media/materials/textures/plug_texture.jpg
pkg/trunk/stacks/pr2_common/pr2_defs/defs/base/base_defs.urdf.xacro
pkg/trunk/stacks/pr2_common/pr2_defs/defs/base/base_gazebo.xacro
Modified: pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch
===================================================================
--- pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch 2009-08-26 19:07:07 UTC (rev 23003)
+++ pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch 2009-08-26 19:17:59 UTC (rev 23004)
@@ -3,7 +3,7 @@
<!-- start gazebo -->
<include file="$(find gazebo)/launch/empty_world.launch"/>
- <!-- load pr2 -->
+ <!-- load prf -->
<include file="$(find pr2_defs)/launch/upload_prf.launch" />
<node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" />
Modified: pkg/trunk/demos/plug_in_gazebo/launch/pr2_plug_outlet.launch
===================================================================
--- pkg/trunk/demos/plug_in_gazebo/launch/pr2_plug_outlet.launch 2009-08-26 19:07:07 UTC (rev 23003)
+++ pkg/trunk/demos/plug_in_gazebo/launch/pr2_plug_outlet.launch 2009-08-26 19:17:59 UTC (rev 23004)
@@ -8,7 +8,7 @@
<node pkg="gazebo_tools" type="urdf2factory" args="robot_description_new" respawn="false" output="screen" />
<param name="plug_description" command="$(find xacro)/xacro.py '$(find plug_in_gazebo)/robots/plug_only.xacro.xml'" />
- <node pkg="gazebo_tools" type="urdf2factory" args="plug_description 0 0 0 0 0 0 plug" respawn="false" output="screen" />
+ <node pkg="gazebo_tools" type="urdf2factory" args="plug_description" respawn="false" output="screen" />
<param name="outlet_description" command="$(find xacro)/xacro.py '$(find plug_in_gazebo)/robots/outlet_only.xacro.xml'" />
<node pkg="gazebo_tools" type="urdf2factory" args="outlet_description 0 0 0 0 0 0 outlet" respawn="false" output="screen" />
Modified: pkg/trunk/demos/plug_in_gazebo/plug_defs/plug_defs.xml
===================================================================
--- pkg/trunk/demos/plug_in_gazebo/plug_defs/plug_defs.xml 2009-08-26 19:07:07 UTC (rev 23003)
+++ pkg/trunk/demos/plug_in_gazebo/plug_defs/plug_defs.xml 2009-08-26 19:17:59 UTC (rev 23004)
@@ -15,17 +15,17 @@
<property name="M_PI" value="3.1415926535897931" />
- <property name="plug_x_loc" value="0.25" />
+ <property name="plug_x_loc" value="0.1968" />
<property name="plug_y_loc" value="0.0" />
- <property name="plug_z_loc" value="0.282" />
- <property name="plug_x_size" value="0.07" />
- <property name="plug_y_size" value="0.025" />
+ <property name="plug_z_loc" value="0.341" />
+ <property name="plug_x_size" value="0.025" />
+ <property name="plug_y_size" value="0.06" />
<property name="plug_z_size" value="0.03" />
<!-- joint blocks -->
<joint name="plug_joint" type="planar" >
<parent link="world" />
- <origin xyz="${plug_x_loc} ${plug_y_loc} ${plug_z_loc}" rpy="0 0 ${-M_PI/2}" />
+ <origin xyz="${plug_x_loc} ${plug_y_loc} ${plug_z_loc}" rpy="0 0 0" />
<child link="plug_link" />
</joint>
@@ -33,7 +33,7 @@
<link name="plug_link">
<inertial>
<mass value="0.1" />
- <com xyz="0 0 0.0" />
+ <origin xyz="0 0 0.0" />
<inertia ixx="1.0" ixy="0" ixz="0"
iyy="1.0" iyz="0"
izz="1.0" />
@@ -58,7 +58,7 @@
<mu2>500.0</mu2>
<kp>100000000.0</kp>
<kd>1.0</kd>
- <turnGravityOff>true</turnGravityOff>
+ <turnGravityOff>false</turnGravityOff>
<selfCollide>true</selfCollide>
<dampingFactor>0.5</dampingFactor>
</gazebo>
Modified: pkg/trunk/robot_descriptions/gazebo_worlds/Media/materials/textures/plug_texture.jpg
===================================================================
(Binary files differ)
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/defs/base/base_defs.urdf.xacro
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/defs/base/base_defs.urdf.xacro 2009-08-26 19:07:07 UTC (rev 23003)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/defs/base/base_defs.urdf.xacro 2009-08-26 19:17:59 UTC (rev 23004)
@@ -193,51 +193,126 @@
</link>
+ <pr2_caster suffix="fl" parent="${name}_link">
+ <origin xyz="${caster_offset_x} ${caster_offset_y} ${caster_offset_z}" rpy="0 0 0" />
+ </pr2_caster>
+ <pr2_caster suffix="fr" parent="${name}_link">
+ <origin xyz="${caster_offset_x} ${-caster_offset_y} ${caster_offset_z}" rpy="0 0 0" />
+ </pr2_caster>
+ <pr2_caster suffix="bl" parent="${name}_link">
+ <origin xyz="${-caster_offset_x} ${caster_offset_y} ${caster_offset_z}" rpy="0 0 0" />
+ </pr2_caster>
+ <pr2_caster suffix="br" parent="${name}_link">
+ <origin xyz="${-caster_offset_x} ${-caster_offset_y} ${caster_offset_z}" rpy="0 0 0" />
+ </pr2_caster>
+
+ <!-- gazebo extensions -->
+ <pr2_base_gazebo name="${name}" />
+
<!-- modeling plug on base holder magnet location -->
- <joint name="plug_magnet_joint" type="fixed">
+ <pr2_plug_holder name="${name}"/>
+
+ </macro>
+
+
+ <macro name="pr2_plug_holder" params="name">
+ <joint name="plug_holder_1_joint" type="fixed">
<origin xyz="0.18 0 ${base_height_offset+0.235}" rpy="0 0 0" />
<parent link="${name}_link"/>
- <child link="plug_magnet"/>
+ <child link="plug_holder_1"/>
</joint>
- <link name="plug_magnet" type="laser">
+ <link name="plug_holder_1">
<inertial>
<mass value="0.1" />
<origin xyz="0 0 0.0" />
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1" />
</inertial>
- <visual name="plug_mavisual">
+ <visual>
<origin xyz="0 0 0" rpy="0 0 0" />
- <geometry name="plug_magnet_visual_geom">
+ <geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
- <collision name="plug_macollision">
+ <collision>
<origin xyz="0 0 0.0" rpy="0 0 0" />
- <geometry name="plug_magnet_collision_geom">
+ <geometry>
<box size=".001 .001 .001" />
</geometry>
</collision>
</link>
-
- <pr2_caster suffix="fl" parent="${name}_link">
- <origin xyz="${caster_offset_x} ${caster_offset_y} ${caster_offset_z}" rpy="0 0 0" />
- </pr2_caster>
- <pr2_caster suffix="fr" parent="${name}_link">
- <origin xyz="${caster_offset_x} ${-caster_offset_y} ${caster_offset_z}" rpy="0 0 0" />
- </pr2_caster>
- <pr2_caster suffix="bl" parent="${name}_link">
- <origin xyz="${-caster_offset_x} ${caster_offset_y} ${caster_offset_z}" rpy="0 0 0" />
- </pr2_caster>
- <pr2_caster suffix="br" parent="${name}_link">
- <origin xyz="${-caster_offset_x} ${-caster_offset_y} ${caster_offset_z}" rpy="0 0 0" />
- </pr2_caster>
-
- <!-- gazebo extensions -->
- <pr2_base_gazebo name="${name}" />
-
+ <joint name="plug_holder_2_joint" type="fixed">
+ <origin xyz="0.21 0 ${base_height_offset+0.235}" rpy="0 0 0" />
+ <parent link="${name}_link"/>
+ <child link="plug_holder_2"/>
+ </joint>
+ <link name="plug_holder_2">
+ <inertial>
+ <mass value="0.1" />
+ <origin xyz="0 0 0.0" />
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <box size="0.001 0.001 0.001"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0.0" rpy="0 0 0" />
+ <geometry>
+ <box size=".001 .001 .001" />
+ </geometry>
+ </collision>
+ </link>
+ <joint name="plug_holder_3_joint" type="fixed">
+ <origin xyz="0.20 -.04 ${base_height_offset+0.235}" rpy="0 0 0" />
+ <parent link="${name}_link"/>
+ <child link="plug_holder_3"/>
+ </joint>
+ <link name="plug_holder_3">
+ <inertial>
+ <mass value="0.1" />
+ <origin xyz="0 0 0.0" />
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <box size="0.001 0.001 0.001"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0.0" rpy="0 0 0" />
+ <geometry>
+ <box size=".001 .001 .001" />
+ </geometry>
+ </collision>
+ </link>
+ <joint name="plug_holder_4_joint" type="fixed">
+ <origin xyz="0.20 0.04 ${base_height_offset+0.235}" rpy="0 0 0" />
+ <parent link="${name}_link"/>
+ <child link="plug_holder_4"/>
+ </joint>
+ <link name="plug_holder_4">
+ <inertial>
+ <mass value="0.1" />
+ <origin xyz="0 0 0.0" />
+ <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <geometry>
+ <box size="0.001 0.001 0.001"/>
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0.0" rpy="0 0 0" />
+ <geometry>
+ <box size=".001 .001 .001" />
+ </geometry>
+ </collision>
+ </link>
</macro>
-
-
</robot>
Modified: pkg/trunk/stacks/pr2_common/pr2_defs/defs/base/base_gazebo.xacro
===================================================================
--- pkg/trunk/stacks/pr2_common/pr2_defs/defs/base/base_gazebo.xacro 2009-08-26 19:07:07 UTC (rev 23003)
+++ pkg/trunk/stacks/pr2_common/pr2_defs/defs/base/base_gazebo.xacro 2009-08-26 19:17:59 UTC (rev 23004)
@@ -66,16 +66,16 @@
<rpyOffsets>0 0 0</rpyOffsets>
<interface:position name="p3d_base_position"/>
</controller:ros_p3d>
- <controller:ros_p3d name="p3d_plug_magnet_controller" plugin="libros_p3d.so">
+ <controller:ros_p3d name="p3d_plug_holder_1_controller" plugin="libros_p3d.so">
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
- <bodyName>plug_magnet</bodyName>
- <topicName>plug_magnet_pose_ground_truth</topicName>
+ <bodyName>plug_holder_1</bodyName>
+ <topicName>plug_holder_1_pose_ground_truth</topicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<xyzOffsets>0 0 0</xyzOffsets> <!-- initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
- <interface:position name="p3d_plug_magnet_position"/>
+ <interface:position name="p3d_plug_holder_1_position"/>
</controller:ros_p3d>
</gazebo>
@@ -110,9 +110,18 @@
<!-- modeling plug on base holder magnet location -->
- <gazebo reference="plug_magnet">
+ <gazebo reference="plug_holder_1">
<material value="PR2/Red" />
</gazebo>
+ <gazebo reference="plug_holder_2">
+ <material value="PR2/Red" />
+ </gazebo>
+ <gazebo reference="plug_holder_3">
+ <material value="PR2/Red" />
+ </gazebo>
+ <gazebo reference="plug_holder_4">
+ <material value="PR2/Red" />
+ </gazebo>
</macro>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wa...@us...> - 2009-08-26 20:45:03
|
Revision: 23018
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23018&view=rev
Author: wattsk
Date: 2009-08-26 20:44:56 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
diagnostics 0.1 commit. Removed diagnostic_analyzer/generic_analyzer and integrated into diagnostic_aggregator.
Modified Paths:
--------------
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/manifest.xml
pkg/trunk/stacks/diagnostics/CMakeLists.txt
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/CMakeLists.txt
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/diagnostic_aggregator.h
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/mainpage.dox
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/manifest.xml
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/src/diagnostic_aggregator.cpp
pkg/trunk/stacks/diagnostics/diagnostic_updater/manifest.xml
pkg/trunk/stacks/diagnostics/diagnostics_analysis/manifest.xml
pkg/trunk/stacks/diagnostics/robot_monitor/manifest.xml
pkg/trunk/stacks/diagnostics/runtime_monitor/manifest.xml
pkg/trunk/stacks/diagnostics/self_test/manifest.xml
pkg/trunk/stacks/diagnostics/stack.xml
pkg/trunk/stacks/pr2/pr2_alpha/manifest.xml
Added Paths:
-----------
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/generic_analyzer_plugin.xml
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/diagnostic_analyzer.h
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/diagnostic_item.h
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/generic_analyzer.h
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/src/diagnostic_item.cpp
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/src/generic_analyzer.cpp
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/src/plugin_list.cpp
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/test/
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/test/analyzers.yaml
pkg/trunk/stacks/diagnostics/diagnostic_aggregator/test/cpp_agg.launch
Removed Paths:
-------------
pkg/trunk/stacks/diagnostics/diagnostic_analyzer/
pkg/trunk/stacks/diagnostics/generic_analyzer/
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/manifest.xml 2009-08-26 20:23:30 UTC (rev 23017)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/manifest.xml 2009-08-26 20:44:56 UTC (rev 23018)
@@ -17,7 +17,6 @@
<depend package="pr2_computer_monitor" />
<depend package="pr2_power_board" />
<depend package="mechanism_bringup" />
- <depend package="generic_analyzer" />
<depend package="diagnostic_aggregator" />
<depend package="robot_mechanism_controllers" />
<depend package="pr2_mechanism_controllers" />
Modified: pkg/trunk/stacks/diagnostics/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/diagnostics/CMakeLists.txt 2009-08-26 20:23:30 UTC (rev 23017)
+++ pkg/trunk/stacks/diagnostics/CMakeLists.txt 2009-08-26 20:44:56 UTC (rev 23018)
@@ -16,6 +16,6 @@
# variables.
#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental)
-rosbuild(hardware_test 0.1.0)
+rosbuild(diagnostics 0.1.0)
# After next ROS release, change to new macro
#rosbuild_make_distribution(0.1.0)
Modified: pkg/trunk/stacks/diagnostics/diagnostic_aggregator/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/diagnostics/diagnostic_aggregator/CMakeLists.txt 2009-08-26 20:23:30 UTC (rev 23017)
+++ pkg/trunk/stacks/diagnostics/diagnostic_aggregator/CMakeLists.txt 2009-08-26 20:44:56 UTC (rev 23018)
@@ -22,6 +22,9 @@
#gensrv()
rospack_add_library(diagnostic_aggregator
+ src/plugin_list.cpp
+ src/generic_analyzer.cpp
+ src/diagnostic_item.cpp
src/diagnostic_aggregator.cpp)
Added: pkg/trunk/stacks/diagnostics/diagnostic_aggregator/generic_analyzer_plugin.xml
===================================================================
--- pkg/trunk/stacks/diagnostics/diagnostic_aggregator/generic_analyzer_plugin.xml (rev 0)
+++ pkg/trunk/stacks/diagnostics/diagnostic_aggregator/generic_analyzer_plugin.xml 2009-08-26 20:44:56 UTC (rev 23018)
@@ -0,0 +1,7 @@
+<library path="lib/libdiagnostic_aggregator" >
+ <class name="GenericAnalyzer" type="diagnostic_analyzer::GenericAnalyzer" base_class_type="diagnostic_analyzer::DiagnosticAnalyzer">
+ <description>
+ GenericAnalyzer is default diagnostic analyzer.
+ </description>
+ </class>
+</library>
Modified: pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/diagnostic_aggregator.h
===================================================================
--- pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/diagnostic_aggregator.h 2009-08-26 20:23:30 UTC (rev 23017)
+++ pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/diagnostic_aggregator.h 2009-08-26 20:44:56 UTC (rev 23018)
@@ -47,11 +47,11 @@
#include <diagnostic_msgs/KeyValue.h>
#include "XmlRpcValue.h"
#include "pluginlib/class_loader.h"
-#include "diagnostic_analyzer/diagnostic_analyzer.h"
-#include "diagnostic_analyzer/diagnostic_item.h"
+#include "diagnostic_aggregator/diagnostic_analyzer.h"
+#include "diagnostic_aggregator/diagnostic_item.h"
+#include "diagnostic_aggregator/generic_analyzer.h"
-
namespace diagnostic_aggregator {
/*!
@@ -112,6 +112,9 @@
*/
void init();
+ /*!
+ *\brief Callback for "/diagnostics"
+ */
void diagCallback(const diagnostic_msgs::DiagnosticArray::ConstPtr& diag_msg);
/*!
Added: pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/diagnostic_analyzer.h
===================================================================
--- pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/diagnostic_analyzer.h (rev 0)
+++ pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/diagnostic_analyzer.h 2009-08-26 20:44:56 UTC (rev 23018)
@@ -0,0 +1,116 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, 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: Kevin Watts
+
+#ifndef DIAGNOSTIC_ANALYZER_H
+#define DIAGNOSTIC_ANALYZER_H
+
+#include <map>
+#include <vector>
+#include <ros/ros.h>
+#include <diagnostic_msgs/DiagnosticStatus.h>
+#include <diagnostic_msgs/KeyValue.h>
+#include "diagnostic_aggregator/diagnostic_item.h"
+
+namespace diagnostic_analyzer {
+
+/*!
+ *\brief Base class of all DiagnosticAnalyzers. Loaded by aggregator.
+ *
+ * Base class, loaded by pluginlib. All analyzers must implement these
+ * functions: init, analyze, getPrefix and getName.
+ *
+ * Analyzers must output their data in a tree structure. The tree branches
+ * are marked by '/' in the DiagnosticStatus name of the output.
+ *
+ * Each analyzer should output a "base" DiagnosticStatus, with the name of
+ * "first_prefix/second_prefix" (ex: "/Robot/Motors")
+ *
+ */
+class DiagnosticAnalyzer
+{
+public:
+ /*!
+ *\brief Default constructor, called by pluginlib.
+ */
+ DiagnosticAnalyzer() {}
+
+ virtual ~DiagnosticAnalyzer() {}
+
+ /*!
+ *\brief DiagnosticAnalyzer is initialized with first prefix and namespace.
+ *
+ * The DiagnosticAnalyzer initialized with parameters in its given
+ * namespace. The "first_prefix" is common to all analyzers, and needs to be
+ * prepended to all DiagnosticStatus names.
+ *\param first_prefix : Common to all analyzers, prepended to all processed names. Starts with "/".
+ *\param n : NodeHandle with proper private namespace for analyzer.
+ */
+ virtual bool init(std::string first_prefix, const ros::NodeHandle &n)
+ {
+ ROS_FATAL("DiagnosticAnalyzer did not implement the init function");
+ ROS_BREAK();
+ return false;
+ }
+
+ /*!
+ *\brief Analysis function, output processed data.
+ *
+ *\param msgs : The input map of messages, by status name. StatusPair stores message, count++ if analyzed. Returned array must be deleted by aggregator.
+ */
+ virtual std::vector<diagnostic_msgs::DiagnosticStatus*> analyze(std::map<std::string, diagnostic_item::DiagnosticItem*> msgs)
+ {
+ ROS_FATAL("DiagnosticAnalyzer did not implement the analyze function");
+ ROS_BREAK();
+
+ std::vector<diagnostic_msgs::DiagnosticStatus*> my_vec;
+ return my_vec;
+ }
+
+ /*!
+ *\brief Returns full prefix of analyzer. (ex: '/Robot/Sensors')
+ */
+ virtual std::string getPrefix() { return ""; }
+
+ /*!
+ *\brief Returns nice name for display. (ex: 'Sensors')
+ */
+ virtual std::string getName() { return ""; }
+
+};
+
+}
+
+#endif //DIAGNOSTIC_ANALYZER_H
Added: pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/diagnostic_item.h
===================================================================
--- pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/diagnostic_item.h (rev 0)
+++ pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/diagnostic_item.h 2009-08-26 20:44:56 UTC (rev 23018)
@@ -0,0 +1,120 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, 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: Kevin Watts
+
+#ifndef DIAGNOSTIC_ITEM_H
+#define DIAGNOSTIC_ITEM_H
+
+#include <map>
+#include <string>
+#include <vector>
+#include <ros/ros.h>
+#include <diagnostic_msgs/DiagnosticStatus.h>
+#include <diagnostic_msgs/KeyValue.h>
+
+namespace diagnostic_item {
+
+/*!
+ *\brief Helper class to hold, store DiagnosticStatus messages
+ *
+ * The DiagnosticItem class is used by the DiagnosticAggregator to store
+ * incoming DiagnosticStatus messages. An item stores whether it has been
+ * examined by an analyzer. After it has been converted to a DiagnosticStatus
+ * message with the "toStatusMsg()" functions, it has been "checked". This
+ * is important for processing the "remainder" of DiagnosticStatus messages,
+ * those that haven't been analyzed by other analyzers.
+ */
+class DiagnosticItem
+{
+public:
+ /*!
+ *\brief Constructed from DiagnosticStatus*
+ */
+ DiagnosticItem(const diagnostic_msgs::DiagnosticStatus *status);
+ ~DiagnosticItem();
+
+ /*!
+ *\brief Must have same name as originial status or it won't update.
+ */
+ void update(const diagnostic_msgs::DiagnosticStatus *status);
+
+ /*!
+ *\brief Sets hasChecked() to true
+ */
+ diagnostic_msgs::DiagnosticStatus *toStatusMsg();
+
+ /*!
+ *\brief Sets hasChecked() to true, prepends "prefix/" to name.
+ *
+ *\param prefix : Prepended to name
+ *\param stale : If true, status level is 3
+ */
+ diagnostic_msgs::DiagnosticStatus *toStatusMsg(std::string prefix, bool stale);
+
+ /*
+ *\brief Returns level of DiagnosticStatus message
+ */
+ int8_t getLevel();
+
+ /*!
+ *\brief Message field of DiagnosticStatus
+ */
+ std::string getMessage();
+
+ /*!
+ *\brief Returns name of status
+ */
+ std::string getName();
+
+ /*!
+ *\brief True if item has been converted to DiagnosticStatus
+ */
+ bool hasChecked();
+
+private:
+ bool checked_;
+
+ int8_t level_;
+ std::string output_name_; /**< name_ w/o "/" */
+ std::string name_;
+ std::string message_;
+ std::string hw_id_;
+ std::vector<diagnostic_msgs::KeyValue> values_;
+
+};
+
+}
+
+#endif //DIAGNOSTIC_ITEM_H
Added: pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/generic_analyzer.h
===================================================================
--- pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/generic_analyzer.h (rev 0)
+++ pkg/trunk/stacks/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/generic_analyzer.h 2009-08-26 20:44:56 UTC (rev 23018)
@@ -0,0 +1,160 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, 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: Kevin Watts
+
+#ifndef GENERIC_ANALYZER_H
+#define GENERIC_ANALYZER_H
+
+#include <map>
+#include <ros/ros.h>
+#include <vector>
+#include <string>
+#include "diagnostic_msgs/DiagnosticStatus.h"
+#include "diagnostic_msgs/KeyValue.h"
+#include "diagnostic_aggregator/diagnostic_analyzer.h"
+#include "diagnostic_aggregator/diagnostic_item.h"
+#include "XmlRpcValue.h"
+
+namespace diagnostic_analyzer {
+
+/*!
+ *\brief GenericAnalyzer is most basic DiagnosticAnalyzer
+ *
+ * GenericAnalyzer analyzes diagnostics from list of topics and returns
+ * processed diagnostics data. All analyzed status messages are prepended with
+ * '/FirstPrefix/SecondPrefix', where FirstPrefix is common to all analyzers
+ * (ex: 'PRE') and SecondPrefix is from this analyzer (ex: 'Power System').
+ */
+class GenericAnalyzer : public DiagnosticAnalyzer
+{
+
+public:
+ /*!
+ *\brief Default constructor loaded by pluginlib
+ */
+ GenericAnalyzer();
+
+
+ ~GenericAnalyzer();
+
+ /*!
+ *\brief Initializes GenericAnalyzer from namespace
+ *
+ * NodeHandle is given private namespace to initialize (ex: ~Sensors)
+ * Parameters of NodeHandle must follow this form. See DiagnosticAggregator
+ * for instructions on passing these to the aggregator.
+ *\verbatim
+ * PowerSystem:
+ * type: GenericAnalyzer
+ * prefix: Power System
+ * expected: [
+ * 'IBPS 0',
+ * 'IBPS 1']
+ * startswith: [
+ * 'Smart Battery']
+ * name: [
+ * 'Power Node 1018']
+ * contains: [
+ * 'Battery']
+ *\endverbatim
+ *
+ *\param first_prefix : Prefix for all analyzers (ex: 'Robot')
+ *\param n : NodeHandle in full namespace
+ */
+ bool init(std::string first_prefix, const ros::NodeHandle &n);
+
+ /*!
+ *\brief Initializes analyzer to deal with remaining data
+ *
+ * After all analyzers have been created this analyzer is created to
+ * process all remaining messages. It will prepend "first_prefix/Other"
+ * to all messages that haven't been handled by other analyzers.
+ * The "Other" analyzer is created automatically by the aggregator.
+ */
+ bool initOther(std::string first_prefix);
+
+ /*!
+ *\brief Analyzes DiagnosticStatus messages
+ *
+ */
+ std::vector<diagnostic_msgs::DiagnosticStatus*> analyze(std::map<std::string, diagnostic_item::DiagnosticItem*> msgs);
+
+ /*!
+ *\brief Returns full prefix (ex: "/Robot/Power System")
+ */
+ std::string getPrefix() { return full_prefix_; }
+
+ /*!
+ *\brief Returns nice name (ex: "Power System")
+ */
+ std::string getName() { return nice_name_; }
+
+private:
+ bool other_; /**< True if analyzer is supposed to analyze remaining messages */
+
+ std::string nice_name_;
+ std::string full_prefix_;
+
+ std::vector<std::string> expected_;
+ std::vector<std::string> startswith_;
+ std::vector<std::string> contains_;
+ std::vector<std::string> name_;
+
+ /*!
+ *\brief Stores items by name
+ */
+ std::map<std::string, diagnostic_item::DiagnosticItem*> items_;
+
+ /*!
+ *\brief Updates items_ with messages to analyze. Deletes to_analyze param.
+ */
+ void updateItems(std::vector<diagnostic_msgs::DiagnosticStatus*> to_analyze);
+
+ /*!
+ *\brief Returns items to be analyzed (items that haven't been already)
+ */
+ std::vector<diagnostic_msgs::DiagnosticStatus*> toAnalyzeOther(std::map<std::string, diagnostic_item::DiagnosticItem*> msgs);
+
+ /*!
+ *\brief Returns items that need to be analyzed
+ */
+ std::vector<diagnostic_msgs::DiagnosticStatus*> toAnalyze(std::map<std::string, diagnostic_item::DiagnosticItem*> msgs);
+
+
+};
+
+
+}
+#endif //GENERIC_ANALYZER_H
Modified: pkg/trunk/stacks/diagnostics/diagnostic_aggregator/mainpage.dox
===================================================================
--- pkg/trunk/stacks/diagnostics/diagnostic_aggregator/mainpage.dox 2009-08-26 20:23:30 UTC (rev 23017)
+++ pkg/trunk/stacks/diagnostics/diagnostic_aggregator/mainpage.dox 2009-08-26 20:44:56 UTC (rev 23018)
@@ -4,7 +4,6 @@
\b diagnostic_aggregator aggregates and performs basic analysis on the diagnostics of a robot. This package consists of the base node, or aggregator, and several analyzers to collect and process diagnostics data.
-
\section codeapi Code API
The DiagnosticAggregator class loads "analyzers" which perform analysis on the incoming diagnostics data. Each analyzer is initialized by an XML node by the aggregator.
@@ -18,38 +17,67 @@
Forms a tree with the trunk "Robot" and the child branch "Motors" and the grandchild "EtherCAT Device (head_tilt_motor)". This allows graphical display of diagnostics data.
-The aggregator will publish the DiagnosticArray on the /diagnostics_agg topic at 1 Hz.
+The aggregator will publish the diagnostic_msgs/DiagnosticArray message on the /diagnostics_agg topic at 1 Hz.
\subsection analyzers Analyzers
-The DiagnosticAggregator will create analyzers to store and process the diagnostic data. Each analyzer inherits from the base class diagnostic_analyzer/DiagnosticAnalyzer. Analyzers should be in packages that depend directly on "pluginlib", "diagnostic_aggregator", and "diagnostic_analyzer".
+The DiagnosticAggregator will create analyzers to store and process the diagnostic data. Each analyzer inherits from the base class diagnostic_analyzer/DiagnosticAnalyzer. Analyzers should be in packages that depend directly on "pluginlib" and "diagnostic_aggregator".
-See the DiagnosticAnalyzer base class and the example generic_analyzer/GenericAnalyzer for more information.
+The base analyzer class is the DiagnosticAnalyzer. All derived classes must implement these methods as specified by the Analyzer:
+- \b init
+- \b analyze
+- \b getPrefix
+- \b getName
-\subsubsecion analyzer_behavior Analyzer Behavior
+Analyzers can choose the error state for any DiagnosticStatus message they analyzer. Generally, the "parent" of an analyzer has an error state of the maximum of its children, but it can follow any rule. The header ("/Robot" above) will have the diagnostic level of the greatest of its immediate children, so an error in "Motors" will give an error in "Robot".
-Analyzers can choose the error state for the parents. It should be related to the state of the children, but can follow any rule. The header ("/Robot" above) will have the diagnostic level of the greatest of its immediate children, so an error in "Motors" will give an error in "Robot".
+\subsubsection generic_analyzer Generic Analyzer
+
+\b generic_analyzer holds the GenericAnalyzer class, which is the most basic of the DiagnosticAnalyzer's. It is used by the diagnostic_aggregator/DiagnosticAggregator to store, process and republish diagnostics data. The GenericAnalyzer is loaded by the pluginlib as a DiagnosticAnalyzer plugin. It is the most basic of all DiagnosticAnalyzer's.
+
+The following YAML parameters will create a GenericAnalyzer to motor a PR2 power system.
+\verbatim
+powersystem:
+ type: GenericAnalyzer
+ prefix: Power System
+ expected: [
+ 'IBPS 0',
+ 'IBPS 1']
+ startswith: [
+ 'Smart Battery']
+ name: [
+ 'Power Node 1018']
+ contains: [
+ 'Battery']
+\endverbatim
+That would create an GenericAnalyzer that will process any DiagnosticStatus message that has a name that matches the given criteria ("startswith", "contains", "name", or "expected"). "expected" and "name" status names require an exact match. Any names in the "expected" list will give an error if not observed.
+
\section rosapi ROS API
Nodes:
- \b aggregator_node
-- \b py_agg_node.py (Don't use)
<hr>
\subsection aggregator_node aggregator_node
-aggregator_node subscribes to "/diagnstics" and publishes an aggregated set of data to "/diagnostics_agg"
+aggregator_node subscribes to "/diagnostics" and publishes an aggregated set of data to "/diagnostics_agg". The aggregator will load diagnostic analyzers (like the GenericAnalyzer above) as plugins. The analyzers are specified in the launch file as private parameters.
\subsubsection Usage
+The aggregator is initialized with the prefix as an argument, and YAML to create the analyzers as a private parameter.
\verbatim
-$ aggregator_node Robot
+<node
+ pkg="diagnostic_aggregator" type="aggregator_node"
+ name="diag_agg" args="Robot" >
+ <rosparam command="load"
+ file="$(find diagnostic_aggregator)/test/analyzers.yaml" />
+</node>
\endverbatim
\par Example
-Using the above usage, the diagnostic aggregator will spawn analyzers that group the robot's data by category. The analyzer parameters are as follows:
+Using the above usage, the diagnostic aggregator will spawn analyzers that group the robot's data by category. The analyzer parameters might look like:
\verbatim
sensors:
type: GenericAnalyzer
@@ -60,14 +88,9 @@
type: PR2MotorsDiagnosticAnalyzer
joints:
type: PR2JointsDiagnosticAnalyzer
-other:
- type: GenericAnalyzer
- other: true
\endverbatim
-Each analyzer is started with a namespace, and must have the parameter "type" in that namespace. Analyzers are loaded as plugins with "pluginlib" and inherit from the base class diagnostic_analyzer/DiagnosticAnalyzer.
+Each analyzer is started with a namespace, and must have the parameter "type" in that namespace. Analyzers are loaded as plugins with "pluginlib" and inherit from the base class diagnostic_analyzer/DiagnosticAnalyzer.
-An analyzer with the namespace "other" will be called last so that it can process only data that hasn't been handled by other analyzers.
-
If any analyzer is not properly specified, or returns false on initialization, the aggregator program will exit.
\subsubsection topics ROS topics
Modified: pkg/trunk/stacks/diagnostics/diagnostic_aggregator/manifest.xml
===================================================================
--- pkg/trunk/stacks/diagnostics/diagnostic_aggregator/manifest.xml 2009-08-26 20:23:30 UTC (rev 23017)
+++ pkg/trunk/stacks/diagnostics/diagnostic_aggregator/manifest.xml 2009-08-26 20:44:56 UTC (rev 23018)
@@ -1,16 +1,19 @@
<package>
- <description brief="Aggregates robot diagnostics to allow easy display" >
+ <description brief="Aggregates robot diagnostics and performs online analysis" >
+ <p>diagnostic_aggregator contains the tools to aggregate and analyze robot diagnostics on an active robot. It uses the <b>DiagnosticAggregator</b> class as to aggregate and process data. The aggregator tool creates analyzers according to user specifications, and these analyzers can perform basic tests on diagnostics, such as testing when things are stale, or having known errors. Analyzers are subclasses of <b>DiagnosticAnalyzer</b> and <b>GenericAnalyzer</b> is one of these subclasses and is the most basic analysis tool.</p>
+
+ <p>The DiagnosticAggregator must be run on a robot, and is typically launched in the robot's launch file. Viewing of this aggregated diagnostics is done with the <b>robot_monitor</b> package. </p>
</description>
<author>Kevin Watts</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
- <url>http://pr.willowgarage.com/wiki/diagnostic_aggregator</url>
+ <url>http://www.ros.org/wiki/diagnostic_aggregator</url>
<depend package="diagnostic_msgs" />
<depend package="roscpp" />
<depend package="xmlrpc++" />
<depend package="pluginlib" />
- <depend package="diagnostic_analyzer" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -ldiagnostic_aggregator"/>
+ <diagnostic_aggregator plugin="${prefix}/generic_analyzer_plugin.xml" />
</export>
</package>
Modified: pkg/trunk/stacks/diagnostics/diagnostic_aggregator/src/diagnostic_aggregator.cpp
===================================================================
--- pkg/trunk/stacks/diagnostics/diagnostic_aggregator/src/diagnostic_aggregator.cpp 2009-08-26 20:23:30 UTC (rev 23017)
+++ pkg/trunk/stacks/diagnostics/diagnostic_aggregator/src/diagnostic_aggregator.cpp 2009-08-26 20:44:56 UTC (rev 23018)
@@ -52,6 +52,11 @@
DiagnosticAggregator::~DiagnosticAggregator()
{
clearMessages();
+
+ for (unsigned int i = 0; i < analyzers_.size(); ++i)
+ delete analyzers_[i];
+
+ analyzers_.clear();
}
void DiagnosticAggregator::init()
@@ -67,8 +72,6 @@
XmlRpc::XmlRpcValue::iterator xml_it;
- diagnostic_analyzer::DiagnosticAnalyzer *remainder = NULL;
-
for (xml_it = private_params.begin(); xml_it != private_params.end(); ++xml_it)
{
XmlRpc::XmlRpcValue analyzer_name = xml_it->first;
@@ -84,7 +87,6 @@
}
XmlRpc::XmlRpcValue analyzer_type = analyzer_value["type"];
-
string an_type = analyzer_type;
diagnostic_analyzer::DiagnosticAnalyzer* analyzer = analyzer_loader_.createClassInstance(an_type);
@@ -99,17 +101,15 @@
ROS_FATAL("Unable to initialize analyzer NS: %s, type: %s", ns...
[truncated message content] |
|
From: <is...@us...> - 2009-08-26 21:34:25
|
Revision: 23034
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23034&view=rev
Author: isucan
Date: 2009-08-26 21:34:16 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
fixing some issues caused by the roscpp bug that adds '...' at the end of parameter values
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/collision_map_self_occ.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch
pkg/trunk/util/robot_self_filter/src/self_filter.cpp
Modified: pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp 2009-08-26 21:32:48 UTC (rev 23033)
+++ pkg/trunk/mapping/collision_map/src/collision_map_self_occ.cpp 2009-08-26 21:34:16 UTC (rev 23034)
@@ -244,7 +244,7 @@
updateMap(obstacles);
double sec = (ros::WallTime::now() - tm).toSec();
- ROS_INFO("Updated collision map with %d points at %f Hz", currentMap_.size(), 1.0/sec);
+ ROS_INFO("Updated collision map with %d points at %f Hz", (int)currentMap_.size(), 1.0/sec);
publishCollisionMap(currentMap_, header_, cmapPublisher_);
mapProcessing_.unlock();
@@ -426,7 +426,7 @@
int main (int argc, char** argv)
{
- ros::init(argc, argv, "collision_map_self_occ", ros::init_options::AnonymousName);
+ ros::init(argc, argv, "collision_map_self_occ");
CollisionMapperOcc cm;
cm.run();
Modified: pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp 2009-08-26 21:32:48 UTC (rev 23033)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp 2009-08-26 21:34:16 UTC (rev 23034)
@@ -273,7 +273,7 @@
data_out.channels[j].values.push_back(cloud->channels[j].values[i]);
}
- ROS_DEBUG("Published filtered cloud (%d points out of %d)", data_out.points.size(), cloud->points.size());
+ ROS_DEBUG("Published filtered cloud (%d points out of %d)", (int)data_out.points.size(), (int)cloud->points.size());
cloudPublisher_.publish(data_out);
}
else
@@ -378,7 +378,6 @@
int main(int argc, char **argv)
{
- // Took out anonymous - bmm
ros::init(argc, argv, "clear_known_objects");
ClearKnownObjects cko;
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/collision_map_self_occ.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/collision_map_self_occ.launch 2009-08-26 21:32:48 UTC (rev 23033)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/collision_map_self_occ.launch 2009-08-26 21:34:16 UTC (rev 23034)
@@ -2,7 +2,7 @@
<include file="$(find 3dnav_pr2)/launch/prX.machine" />
- <node machine="four" pkg="collision_map" type="collision_map_self_occ_node" respawn="true" output="screen">
+ <node machine="four" pkg="collision_map" type="collision_map_self_occ_node" name="collision_map_self_occ_node" respawn="true" output="screen">
<remap from="cloud_in" to="full_cloud_filtered" />
<remap from="cloud_incremental_in" to="tilt_scan_cloud_filtered" />
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch 2009-08-26 21:32:48 UTC (rev 23033)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch 2009-08-26 21:34:16 UTC (rev 23034)
@@ -1,5 +1,5 @@
<launch>
- <node pkg="robot_self_filter" type="self_filter" respawn="true" output="screen">
+ <node pkg="robot_self_filter" type="self_filter" respawn="true" name="robot_self_filter" output="screen">
<!-- The topic for the input cloud -->
<remap from="cloud_in" to="tilt_scan_cloud_without_known_objects" />
Modified: pkg/trunk/util/robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/self_filter.cpp 2009-08-26 21:32:48 UTC (rev 23033)
+++ pkg/trunk/util/robot_self_filter/src/self_filter.cpp 2009-08-26 21:34:16 UTC (rev 23034)
@@ -191,7 +191,7 @@
int main(int argc, char **argv)
{
- ros::init(argc, argv, "self_filter", ros::init_options::AnonymousName);
+ ros::init(argc, argv, "self_filter");
SelfFilter s;
ros::spin();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-08-26 21:43:08
|
Revision: 23038
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23038&view=rev
Author: stuglaser
Date: 2009-08-26 21:43:00 +0000 (Wed, 26 Aug 2009)
Log Message:
-----------
Namespacing the controller plugins
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/pr2_mechanism_controllers/controller_plugins.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_effort_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_position_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_ud_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_velocity_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp
pkg/trunk/sandbox/dallas_controller/controller_plugins.xml
pkg/trunk/sandbox/experimental_controllers/controller_plugins.xml
pkg/trunk/sandbox/qualification_controllers/joint_qualification_controllers/controller_plugins.xml
pkg/trunk/sandbox/trajectory_controllers/controller_plugins.xml
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control/mechanism.py
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
Removed Paths:
-------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/controller_manifest.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-08-26 21:43:00 UTC (rev 23038)
@@ -11,17 +11,13 @@
rospack_add_boost_directories()
rospack_add_library( pr2_mechanism_controllers
- src/controller_manifest.cpp
src/laser_scanner_traj_controller.cpp
src/head_position_controller.cpp
src/caster_controller.cpp
-#src/caster_controller_effort.cpp
src/caster_calibration_controller.cpp
-#src/caster_calibration_controller_effort.cpp
src/wrist_calibration_controller.cpp
src/gripper_calibration_controller.cpp
src/base_kinematics.cpp
- #src/pr2_gripper_controller.cpp
src/pr2_odometry.cpp
src/pr2_base_controller.cpp
)
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/controller_plugins.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/controller_plugins.xml 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/controller_plugins.xml 2009-08-26 21:43:00 UTC (rev 23038)
@@ -1,13 +1,13 @@
<library path="lib/libpr2_mechanism_controllers">
- <class name="CasterCalibrationController" type="CasterCalibrationController" base_class_type="Controller" />
- <class name="CasterController" type="CasterController" base_class_type="Controller" />
- <class name="CasterControllerNode" type="CasterControllerNode" base_class_type="Controller" />
- <class name="GripperCalibrationController" type="GripperCalibrationController" base_class_type="Controller" />
- <class name="GripperCalibrationControllerNode" type="GripperCalibrationControllerNode" base_class_type="Controller" />
- <class name="HeadPositionController" type="HeadPositionController" base_class_type="Controller" />
- <class name="LaserScannerTrajController" type="LaserScannerTrajController" base_class_type="Controller" />
- <class name="LaserScannerTrajControllerNode" type="LaserScannerTrajControllerNode" base_class_type="Controller" />
- <class name="Pr2BaseController" type="Pr2BaseController" base_class_type="Controller" />
- <class name="Pr2Odometry" type="Pr2Odometry" base_class_type="Controller" />
- <class name="WristCalibrationController" type="WristCalibrationController" base_class_type="Controller" />
+ <class name="CasterCalibrationController" type="controller::CasterCalibrationController" base_class_type="controller::Controller" />
+ <class name="CasterController" type="controller::CasterController" base_class_type="controller::Controller" />
+ <class name="CasterControllerNode" type="controller::CasterControllerNode" base_class_type="controller::Controller" />
+ <class name="GripperCalibrationController" type="controller::GripperCalibrationController" base_class_type="controller::Controller" />
+ <class name="GripperCalibrationControllerNode" type="controller::GripperCalibrationControllerNode" base_class_type="controller::Controller" />
+ <class name="HeadPositionController" type="controller::HeadPositionController" base_class_type="controller::Controller" />
+ <class name="LaserScannerTrajController" type="controller::LaserScannerTrajController" base_class_type="controller::Controller" />
+ <class name="LaserScannerTrajControllerNode" type="controller::LaserScannerTrajControllerNode" base_class_type="controller::Controller" />
+ <class name="Pr2BaseController" type="controller::Pr2BaseController" base_class_type="controller::Controller" />
+ <class name="Pr2Odometry" type="controller::Pr2Odometry" base_class_type="controller::Controller" />
+ <class name="WristCalibrationController" type="controller::WristCalibrationController" base_class_type="controller::Controller" />
</library>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2009-08-26 21:43:00 UTC (rev 23038)
@@ -32,11 +32,12 @@
*/
#include "pr2_mechanism_controllers/caster_calibration_controller.h"
+#include "pluginlib/class_list_macros.h"
+PLUGINLIB_REGISTER_CLASS(CasterCalibrationController, controller::CasterCalibrationController, controller::Controller)
+
namespace controller {
-ROS_REGISTER_CONTROLLER(CasterCalibrationController)
-
CasterCalibrationController::CasterCalibrationController()
: robot_(NULL), state_(INITIALIZED),
joint_(NULL), wheel_l_joint_(NULL), wheel_r_joint_(NULL), last_publish_time_(0)
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller.cpp 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller.cpp 2009-08-26 21:43:00 UTC (rev 23038)
@@ -32,14 +32,15 @@
*/
#include "pr2_mechanism_controllers/caster_controller.h"
+#include "pluginlib/class_list_macros.h"
+PLUGINLIB_REGISTER_CLASS(CasterController, controller::CasterController, controller::Controller)
+
namespace controller {
const double CasterController::WHEEL_RADIUS = 0.079;
const double CasterController::WHEEL_OFFSET = 0.049;
-ROS_REGISTER_CONTROLLER(CasterController);
-
CasterController::CasterController()
: steer_velocity_(0), drive_velocity_(0)
{
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/src/controller_manifest.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/controller_manifest.cpp 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/controller_manifest.cpp 2009-08-26 21:43:00 UTC (rev 23038)
@@ -1,58 +0,0 @@
-/*
- * Copyright (c) 2009, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include "pluginlib/class_list_macros.h"
-#include "controller_interface/controller.h"
-
-#include "pr2_mechanism_controllers/caster_calibration_controller.h"
-#include "pr2_mechanism_controllers/caster_controller.h"
-#include "pr2_mechanism_controllers/gripper_calibration_controller.h"
-#include "pr2_mechanism_controllers/head_position_controller.h"
-#include "pr2_mechanism_controllers/laser_scanner_traj_controller.h"
-#include "pr2_mechanism_controllers/pr2_base_controller.h"
-//#include "pr2_mechanism_controllers/pr2_gripper_controller.h"
-#include "pr2_mechanism_controllers/pr2_odometry.h"
-#include "pr2_mechanism_controllers/wrist_calibration_controller.h"
-
-
-using namespace controller;
-
-
-PLUGINLIB_REGISTER_CLASS(CasterCalibrationController, CasterCalibrationController, Controller)
-PLUGINLIB_REGISTER_CLASS(CasterController, CasterController, Controller)
-PLUGINLIB_REGISTER_CLASS(GripperCalibrationController, GripperCalibrationController, Controller)
-PLUGINLIB_REGISTER_CLASS(HeadPositionController, HeadPositionController, Controller)
-PLUGINLIB_REGISTER_CLASS(LaserScannerTrajController, LaserScannerTrajController, Controller)
-PLUGINLIB_REGISTER_CLASS(LaserScannerTrajControllerNode, LaserScannerTrajControllerNode, Controller)
-PLUGINLIB_REGISTER_CLASS(Pr2BaseController, Pr2BaseController, Controller)
-//PLUGINLIB_REGISTER_CLASS(Pr2GripperController, Pr2GripperController, Controller)
-PLUGINLIB_REGISTER_CLASS(Pr2Odometry, Pr2Odometry, Controller)
-PLUGINLIB_REGISTER_CLASS(WristCalibrationController, WristCalibrationController, Controller)
-
-
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp 2009-08-26 21:43:00 UTC (rev 23038)
@@ -33,15 +33,17 @@
*********************************************************************/
#include "pr2_mechanism_controllers/gripper_calibration_controller.h"
-#include <ros/time.h>
+#include "ros/time.h"
+#include "pluginlib/class_list_macros.h"
using namespace std;
using namespace controller;
+PLUGINLIB_REGISTER_CLASS(GripperCalibrationController, controller::GripperCalibrationController, controller::Controller)
+
namespace controller
{
-
GripperCalibrationController::GripperCalibrationController()
: state_(INITIALIZED), last_publish_time_(0), joint_(NULL)
{
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_position_controller.cpp 2009-08-26 21:43:00 UTC (rev 23038)
@@ -33,18 +33,18 @@
*********************************************************************/
-#include <pr2_mechanism_controllers/head_position_controller.h>
-#include <mechanism_control/mechanism_control.h>
-#include<cmath>
+#include "pr2_mechanism_controllers/head_position_controller.h"
+#include <cmath>
+#include "mechanism_control/mechanism_control.h"
+#include "pluginlib/class_list_macros.h"
using namespace tf;
using namespace std;
+PLUGINLIB_REGISTER_CLASS(HeadPositionController, controller::HeadPositionController, controller::Controller)
+
namespace controller {
-ROS_REGISTER_CONTROLLER(HeadPositionController)
-
-
HeadPositionController::HeadPositionController()
: robot_state_(NULL),
point_head_notifier_(NULL),
@@ -60,7 +60,7 @@
bool HeadPositionController::init(mechanism::RobotState *robot_state, const ros::NodeHandle &n)
{
node_ = n;
-
+
// get name link names from the param server
if (!node_.getParam("pan_link_name", pan_link_name_)){
ROS_ERROR("HeadPositionController: No pan link name found on parameter server (namespace: %s)",
@@ -72,7 +72,7 @@
node_.getNamespace().c_str());
return false;
}
-
+
// test if we got robot pointer
assert(robot_state);
robot_state_ = robot_state;
@@ -117,18 +117,18 @@
void HeadPositionController::command(const mechanism_msgs::JointStatesConstPtr& command_msg)
{
- assert(command_msg->joints.size() == 2);
+ assert(command_msg->joints.size() == 2);
if(command_msg->joints[0].name == head_pan_controller_.joint_state_->joint_->name_)
{
pan_out_ = command_msg->joints[0].position;
tilt_out_ = command_msg->joints[1].position;
}
- else
+ else
{
pan_out_ = command_msg->joints[1].position;
tilt_out_ = command_msg->joints[0].position;
}
-
+
}
void HeadPositionController::pointHead(const tf::MessageNotifier<geometry_msgs::PointStamped>::MessagePtr& point_msg)
@@ -141,7 +141,7 @@
// convert to reference frame of pan link parent
tf_.transformPoint(pan_parent, pan_point, pan_point);
- //calculate the pan angle
+ //calculate the pan angle
pan_out_ = atan2(pan_point.y(), pan_point.x());
Stamped<Point> tilt_point;
@@ -150,8 +150,8 @@
tf_.transformPoint(pan_link_name_, tilt_point, tilt_point);
//calculate the tilt angle
tilt_out_ = atan2(-tilt_point.z(), sqrt(tilt_point.x()*tilt_point.x() + tilt_point.y()*tilt_point.y()));
-
+
}
void HeadPositionController::pointFrameOnHead(const tf::MessageNotifier<geometry_msgs::PointStamped>::MessagePtr& point_msg)
@@ -182,7 +182,7 @@
Stamped<Point> tilt_point;
pointStampedMsgToTF(*point_msg, tilt_point);
tf_.transformPoint(pan_link_name_, tilt_point, tilt_point);
-
+
radius = pow(tilt_point.x(),2)+pow(tilt_point.y(),2);
x_intercept = sqrt(fabs(radius-pow(frame.getOrigin().z(),2)));
delta_theta = atan2(-tilt_point.z(), sqrt(tilt_point.x()*tilt_point.x() + tilt_point.y()*tilt_point.y()))-atan2(frame.getOrigin().z(),x_intercept);
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2009-08-26 21:43:00 UTC (rev 23038)
@@ -31,18 +31,20 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
+
+#include "pr2_mechanism_controllers/laser_scanner_traj_controller.h"
#include <algorithm>
-#include <pr2_mechanism_controllers/laser_scanner_traj_controller.h>
-#include <angles/angles.h>
+#include <cmath>
+#include "angles/angles.h"
+#include "pluginlib/class_list_macros.h"
-#include <math.h>
+PLUGINLIB_REGISTER_CLASS(LaserScannerTrajController, controller::LaserScannerTrajController, controller::Controller)
+PLUGINLIB_REGISTER_CLASS(LaserScannerTrajControllerNode, controller::LaserScannerTrajControllerNode, controller::Controller)
using namespace std ;
using namespace controller ;
using namespace filters ;
-ROS_REGISTER_CONTROLLER(LaserScannerTrajController)
-
LaserScannerTrajController::LaserScannerTrajController() : traj_(1)
{
tracking_offset_ = 0 ;
@@ -239,14 +241,14 @@
joint_state_->joint_->joint_limit_min_,
joint_state_->joint_->joint_limit_max_,
error) ;
-
+
double dt = time - last_time_ ;
double d_error = (error-last_error_)/dt ;
std::vector<double> vin;
vin.push_back(d_error);
std::vector<double> vout = vin;
-
+
d_error_filter.update(vin, vout) ;
double filtered_d_error = vout[0];
@@ -450,7 +452,6 @@
}*/
-ROS_REGISTER_CONTROLLER(LaserScannerTrajControllerNode)
LaserScannerTrajControllerNode::LaserScannerTrajControllerNode(): node_(ros::Node::instance()), c_()
{
prev_profile_segment_ = -1 ;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-08-26 21:43:00 UTC (rev 23038)
@@ -35,11 +35,13 @@
* Author: Sachin Chitta and Matthew Piccoli
*/
-#include <pr2_mechanism_controllers/pr2_base_controller.h>
+#include "pr2_mechanism_controllers/pr2_base_controller.h"
+#include "pluginlib/class_list_macros.h"
-using namespace controller;
-ROS_REGISTER_CONTROLLER(Pr2BaseController)
+PLUGINLIB_REGISTER_CLASS(Pr2BaseController, controller::Pr2BaseController, controller::Controller)
+namespace controller {
+
Pr2BaseController::Pr2BaseController()
{
//init variables
@@ -135,7 +137,7 @@
control_toolbox::Pid p_i_d;
state_publisher_->msg_.joint_name[j + base_kin_.num_casters_] = base_kin_.wheel_[j].joint_name_;
if(!p_i_d.init(ros::NodeHandle(node_,base_kin_.wheel_[j].joint_name_)))
- {
+ {
ROS_ERROR("Could not initialize pid for %s",base_kin_.wheel_[j].joint_name_.c_str());
return false;
}
@@ -483,3 +485,4 @@
this->setCommand(base_vel_msg_);
pthread_mutex_unlock(&pr2_base_controller_lock_);
}
+} // namespace
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp 2009-08-26 21:43:00 UTC (rev 23038)
@@ -35,11 +35,13 @@
* Author: Sachin Chitta and Matthew Piccoli
*/
-#include <pr2_mechanism_controllers/pr2_odometry.h>
+#include "pr2_mechanism_controllers/pr2_odometry.h"
+#include "pluginlib/class_list_macros.h"
-using namespace controller;
-ROS_REGISTER_CONTROLLER(Pr2Odometry)
+PLUGINLIB_REGISTER_CLASS(Pr2Odometry, controller::Pr2Odometry, controller::Controller)
+namespace controller {
+
Pr2Odometry::Pr2Odometry()
{
}
@@ -88,7 +90,7 @@
fit_residual_ = Eigen::MatrixXf::Zero(16, 1);
odometry_residual_ = Eigen::MatrixXf::Zero(16, 1);
-
+
weight_matrix_ = Eigen::MatrixXf::Identity(16, 16);
odometry_publisher_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(node_,odom_frame_, 1));
@@ -175,11 +177,11 @@
double odom_multiplier;
if (residual < 0.05)
{
- odom_multiplier = ((residual-0.00001)*(0.99999/0.04999))+0.00001;
+ odom_multiplier = ((residual-0.00001)*(0.99999/0.04999))+0.00001;
}
else
{
- odom_multiplier = ((residual-0.05)*(19.0/0.15))+1.0;
+ odom_multiplier = ((residual-0.05)*(19.0/0.15))+1.0;
}
odom_multiplier = fmax(0.00001, fmin(100.0, odom_multiplier));
odom_multiplier *= 2.0;
@@ -291,7 +293,6 @@
}
return fit_soln_;
}
-;
Eigen::MatrixXf Pr2Odometry::findWeightMatrix(Eigen::MatrixXf residual, std::string weight_type)
{
@@ -337,7 +338,6 @@
}
return w_fit;
}
-;
void Pr2Odometry::publish()
{
@@ -352,7 +352,7 @@
if(transform_publisher_->trylock())
{
-
+
double x(0.), y(0.0), yaw(0.0), vx(0.0), vy(0.0), vyaw(0.0);
this->getOdometry(x, y, yaw, vx, vy, vyaw);
@@ -387,7 +387,8 @@
transform_publisher_->unlockAndPublish();
}
-
+
last_publish_time_ = current_time_;
-};
+}
+} // namespace
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp 2009-08-26 21:43:00 UTC (rev 23038)
@@ -32,7 +32,10 @@
*/
#include "pr2_mechanism_controllers/wrist_calibration_controller.h"
+#include "pluginlib/class_list_macros.h"
+PLUGINLIB_REGISTER_CLASS(WristCalibrationController, controller::WristCalibrationController, controller::Controller)
+
namespace controller {
Modified: pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/robot_mechanism_controllers/CMakeLists.txt 2009-08-26 21:43:00 UTC (rev 23038)
@@ -6,7 +6,6 @@
genmsg()
gensrv()
set(_srcs
- src/controller_manifest.cpp
src/cartesian_pose_controller.cpp
src/cartesian_twist_controller.cpp
src/cartesian_wrench_controller.cpp
Modified: pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml 2009-08-26 21:43:00 UTC (rev 23038)
@@ -1,16 +1,14 @@
<library path="lib/librobot_mechanism_controllers">
- <class name="JointEffortController" type="JointEffortController" base_class_type="Controller" />
+ <class name="JointEffortController" type="controller::JointEffortController" base_class_type="controller::Controller" />
+ <class name="JointVelocityController" type="controller::JointVelocityController" base_class_type="controller::Controller" />
+ <class name="JointPositionController" type="controller::JointPositionController" base_class_type="controller::Controller" />
- <class name="JointVelocityController" type="JointVelocityController" base_class_type="Controller" />
- <class name="JointPositionController" type="JointPositionController" base_class_type="Controller" />
+ <class name="CartesianWrenchController" type="controller::CartesianWrenchController" base_class_type="controller::Controller" />
+ <class name="CartesianTwistController" type="controller::CartesianTwistController" base_class_type="controller::Controller" />
+ <class name="CartesianPoseController" type="controller::CartesianPoseController" base_class_type="controller::Controller" />
- <class name="CartesianWrenchController" type="CartesianWrenchController" base_class_type="Controller" />
- <class name="CartesianTwistController" type="CartesianTwistController" base_class_type="Controller" />
- <class name="CartesianPoseController" type="CartesianPoseController" base_class_type="Controller" />
+ <class name="JointUDCalibrationController" type="controller::JointUDCalibrationController" base_class_type="controller::Controller" />
- <class name="JointUDCalibrationController" type="JointUDCalibrationController" base_class_type="Controller" />
-
- <class name="TriggerController" type="TriggerController" base_class_type="Controller" />
- <class name="TriggerControllerNode" type="TriggerControllerNode" base_class_type="Controller" />
-
+ <class name="TriggerController" type="controller::TriggerController" base_class_type="controller::Controller" />
+ <class name="TriggerControllerNode" type="controller::TriggerControllerNode" base_class_type="controller::Controller" />
</library>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-08-26 21:43:00 UTC (rev 23038)
@@ -33,21 +33,21 @@
+#include "robot_mechanism_controllers/cartesian_pose_controller.h"
#include <algorithm>
-#include <mechanism_control/mechanism_control.h>
#include "kdl/chainfksolverpos_recursive.hpp"
-#include "robot_mechanism_controllers/cartesian_pose_controller.h"
+#include "mechanism_control/mechanism_control.h"
+#include "pluginlib/class_list_macros.h"
using namespace KDL;
using namespace tf;
using namespace std;
+PLUGINLIB_REGISTER_CLASS(CartesianPoseController, controller::CartesianPoseController, controller::Controller)
namespace controller {
-ROS_REGISTER_CONTROLLER(CartesianPoseController)
-
CartesianPoseController::CartesianPoseController()
: robot_state_(NULL),
jnt_to_pose_solver_(NULL),
@@ -108,7 +108,7 @@
}
// subscribe to pose commands
- command_notifier_.reset(new MessageNotifier<geometry_msgs::PoseStamped>(tf_,
+ command_notifier_.reset(new MessageNotifier<geometry_msgs::PoseStamped>(tf_,
boost::bind(&CartesianPoseController::command, this, _1),
node_.getNamespace() + "/command", root_name_, 10));
// realtime publisher for control state
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp 2009-08-26 21:43:00 UTC (rev 23038)
@@ -31,19 +31,19 @@
* Author: Wim Meeussen
*/
+#include "robot_mechanism_controllers/cartesian_twist_controller.h"
#include <algorithm>
-#include <mechanism_control/mechanism_control.h>
+#include "mechanism_control/mechanism_control.h"
#include "kdl/chainfksolvervel_recursive.hpp"
-#include "robot_mechanism_controllers/cartesian_twist_controller.h"
+#include "pluginlib/class_list_macros.h"
using namespace KDL;
+PLUGINLIB_REGISTER_CLASS(CartesianTwistController, controller::CartesianTwistController, controller::Controller)
namespace controller {
-ROS_REGISTER_CONTROLLER(CartesianTwistController)
-
CartesianTwistController::CartesianTwistController()
: robot_state_(NULL),
jnt_to_twist_solver_(NULL)
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp 2009-08-26 21:43:00 UTC (rev 23038)
@@ -31,17 +31,17 @@
* Author: Wim Meeussen
*/
+#include "robot_mechanism_controllers/cartesian_wrench_controller.h"
#include <algorithm>
-#include <robot_mechanism_controllers/cartesian_wrench_controller.h>
+#include "pluginlib/class_list_macros.h"
using namespace KDL;
+PLUGINLIB_REGISTER_CLASS(CartesianWrenchController, controller::CartesianWrenchController, controller::Controller)
+
namespace controller {
-
-ROS_REGISTER_CONTROLLER(CartesianWrenchController)
-
CartesianWrenchController::CartesianWrenchController()
: robot_state_(NULL),
jnt_to_jac_solver_(NULL)
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp 2009-08-26 21:41:07 UTC (rev 23037)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/controller_manifest.cpp 2009-08-26 21:43:00 UTC (rev 23038)
@@ -1,56 +0,0 @@
-/*
- * Copyright (c) 2009, 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 co...
[truncated message content] |
|
From: <bla...@us...> - 2009-08-27 01:33:34
|
Revision: 23086
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23086&view=rev
Author: blaisegassend
Date: 2009-08-27 01:33:28 +0000 (Thu, 27 Aug 2009)
Log Message:
-----------
Conflated TriggerController and TriggerController Node in response to #2602
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/trigger_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp
pkg/trunk/drivers/cam/camera_trigger_test/four_cams.launch
pkg/trunk/drivers/cam/camera_trigger_test/test_controllers.xml
pkg/trunk/pr2/qualification/tests/forearm_cam_test/ethercat.launch
Removed Paths:
-------------
pkg/trunk/pr2/qualification/tests/forearm_cam_test/trigger_controller.xml
Modified: pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml 2009-08-27 01:32:43 UTC (rev 23085)
+++ pkg/trunk/controllers/robot_mechanism_controllers/controller_plugins.xml 2009-08-27 01:33:28 UTC (rev 23086)
@@ -10,5 +10,4 @@
<class name="JointUDCalibrationController" type="controller::JointUDCalibrationController" base_class_type="controller::Controller" />
<class name="TriggerController" type="controller::TriggerController" base_class_type="controller::Controller" />
- <class name="TriggerControllerNode" type="controller::TriggerControllerNode" base_class_type="controller::Controller" />
</library>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/trigger_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/trigger_controller.h 2009-08-27 01:32:43 UTC (rev 23085)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/trigger_controller.h 2009-08-27 01:33:28 UTC (rev 23086)
@@ -220,41 +220,24 @@
private:
double getTick();
+
+ bool setWaveformSrv(trigger_configuration &req,
+ robot_mechanism_controllers::SetWaveform::Response &resp);
mechanism::RobotState * robot_;
ActuatorCommand *actuator_command_;
double prev_tick_;
+ ros::ServiceServer set_waveform_handle_;
+ ros::NodeHandle node_handle_;
+
// Configuration of controller.
trigger_configuration config_;
std::string actuator_name_;
};
-/** @class TriggerControllerNode
-* @brief Provides a thin wrapper for ROS communicaition with the trigger controller
-*/
-class TriggerControllerNode : public Controller
-{
-public:
- TriggerControllerNode();
-
- virtual ~TriggerControllerNode();
-
- void update();
-
- bool init(mechanism::RobotState *robot, const ros::NodeHandle &n);
-
-private:
- bool setWaveformSrv(trigger_configuration &req,
- robot_mechanism_controllers::SetWaveform::Response &resp);
-
- ros::ServiceServer set_waveform_handle_;
- ros::NodeHandle node_handle_;
- TriggerController * c_;
};
-};
-
#endif
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp 2009-08-27 01:32:43 UTC (rev 23085)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp 2009-08-27 01:33:28 UTC (rev 23086)
@@ -62,7 +62,6 @@
#include "pluginlib/class_list_macros.h"
PLUGINLIB_REGISTER_CLASS(TriggerController, controller::TriggerController, controller::Controller)
-PLUGINLIB_REGISTER_CLASS(TriggerControllerNode, controller::TriggerControllerNode, controller::Controller)
using std::string;
using namespace controller;
@@ -114,6 +113,12 @@
bool TriggerController::init(mechanism::RobotState *robot, const ros::NodeHandle& n)
{
+ node_handle_ = n;
+
+ ROS_DEBUG("LOADING TRIGGER CONTROLLER NODE");
+ //string prefix = config->Attribute("name");
+ //ROS_DEBUG_STREAM("the prefix is "<<prefix);
+
assert(robot);
robot_=robot;
@@ -154,6 +159,8 @@
prev_tick_ = getTick();
+ set_waveform_handle_ = node_handle_.advertiseService("set_waveform", &TriggerController::setWaveformSrv, this);
+
ROS_DEBUG("TriggerController::init completed successfully"
" rr=%f ph=%f al=%i r=%i p=%i dc=%f.",
config_.rep_rate, config_.phase, config_.active_low, config_.running, config_.pulsed, config_.duty_cycle);
@@ -161,67 +168,23 @@
return true;
}
-//------------------- NODE -------------
-
-ROS_REGISTER_CONTROLLER(TriggerControllerNode);
-
-TriggerControllerNode::TriggerControllerNode()
-{
- ROS_DEBUG("creating controller node...");
- c_ = new TriggerController();
- ROS_DEBUG("done");
-}
-
-TriggerControllerNode::~TriggerControllerNode()
-{
- delete c_;
-}
-
-void TriggerControllerNode::update()
-{
- c_->update();
-}
-
-bool TriggerControllerNode::setWaveformSrv(
+bool TriggerController::setWaveformSrv(
trigger_configuration &req,
robot_mechanism_controllers::SetWaveform::Response &resp)
{
// FIXME This should be safe despite the asynchronous barrier. Should I
// be doing anything special to ensure that things get written in order?
- c_->config_.running = false; // Turn off pulsing before we start.
- c_->config_.rep_rate = req.rep_rate;
- c_->config_.phase = req.phase;
- c_->config_.duty_cycle = req.duty_cycle;
- c_->config_.active_low = !!req.active_low;
- c_->config_.pulsed = !!req.pulsed;
- c_->config_.running = !!req.running;
+ config_.running = false; // Turn off pulsing before we start.
+ config_.rep_rate = req.rep_rate;
+ config_.phase = req.phase;
+ config_.duty_cycle = req.duty_cycle;
+ config_.active_low = !!req.active_low;
+ config_.pulsed = !!req.pulsed;
+ config_.running = !!req.running;
ROS_DEBUG("TriggerController::setWaveformSrv completed successfully"
- " rr=%f ph=%f al=%i r=%i p=%i dc=%f.", c_->config_.rep_rate, c_->config_.phase,
- c_->config_.active_low, c_->config_.running, c_->config_.pulsed, c_->config_.duty_cycle);
+ " rr=%f ph=%f al=%i r=%i p=%i dc=%f.", config_.rep_rate, config_.phase,
+ config_.active_low, config_.running, config_.pulsed, config_.duty_cycle);
return true;
}
-
-bool TriggerControllerNode::init(mechanism::RobotState *robot, const ros::NodeHandle& n)
-{
- node_handle_ = n;
- //Init the model.
-
- ROS_DEBUG("LOADING TRIGGER CONTROLLER NODE");
- //string prefix = config->Attribute("name");
- //ROS_DEBUG_STREAM("the prefix is "<<prefix);
-
- // Parses subcontroller configuration
- if(c_->init(robot, node_handle_))
- {
- set_waveform_handle_ = node_handle_.advertiseService("set_waveform", &TriggerControllerNode::setWaveformSrv, this);
-
- // Default parameters
-
- ROS_DEBUG("DONE LOADING TRIGGER CONTROLLER NODE");
- return true;
- }
- ROS_DEBUG("ERROR LOADING TRIGGER CONTROLLER NODE");
- return false;
-}
Modified: pkg/trunk/drivers/cam/camera_trigger_test/four_cams.launch
===================================================================
--- pkg/trunk/drivers/cam/camera_trigger_test/four_cams.launch 2009-08-27 01:32:43 UTC (rev 23085)
+++ pkg/trunk/drivers/cam/camera_trigger_test/four_cams.launch 2009-08-27 01:33:28 UTC (rev 23086)
@@ -79,11 +79,11 @@
<!-- Spwns the controllers -->
<node name="spawner" pkg="mechanism_control" type="spawner.py" output="screen" args="trig_controller led_controller" />
<group ns="trig_controller">
- <param name="type" type="string" value="TriggerControllerNode" />
+ <param name="type" type="string" value="TriggerController" />
<param name="actuator" type="string" value="trig_controller" />
</group>
<group ns="led_controller">
- <param name="type" type="string" value="TriggerControllerNode" />
+ <param name="type" type="string" value="TriggerController" />
<param name="actuator" type="string" value="led_controller" />
</group>
Modified: pkg/trunk/drivers/cam/camera_trigger_test/test_controllers.xml
===================================================================
--- pkg/trunk/drivers/cam/camera_trigger_test/test_controllers.xml 2009-08-27 01:32:43 UTC (rev 23085)
+++ pkg/trunk/drivers/cam/camera_trigger_test/test_controllers.xml 2009-08-27 01:33:28 UTC (rev 23086)
@@ -1,9 +1,9 @@
<controllers>
-<controller name="cam_controller" type="TriggerControllerNode">
+<controller name="cam_controller" type="TriggerController">
<actuator name="fl_caster_l_wheel_motor" />
<waveform rep_rate="24" active_low="0" phase="0" duty_cycle=".5" running="1" pulsed="1" />
</controller>
-<controller name="led_controller" type="TriggerControllerNode">
+<controller name="led_controller" type="TriggerController">
<actuator name="bl_caster_l_wheel_motor" />
<waveform rep_rate="10" active_low="1" phase="0" duty_cycle=".5" running="0" pulsed="1" />
</controller>
Modified: pkg/trunk/pr2/qualification/tests/forearm_cam_test/ethercat.launch
===================================================================
--- pkg/trunk/pr2/qualification/tests/forearm_cam_test/ethercat.launch 2009-08-27 01:32:43 UTC (rev 23085)
+++ pkg/trunk/pr2/qualification/tests/forearm_cam_test/ethercat.launch 2009-08-27 01:33:28 UTC (rev 23086)
@@ -4,11 +4,11 @@
<!-- Spwns the controllers -->
<node pkg="mechanism_control" type="spawner.py" output="screen" args="trig_controller led_controller" />
<group ns="trig_controller">
- <param name="type" type="string" value="TriggerControllerNode" />
+ <param name="type" type="string" value="TriggerController" />
<param name="actuator" type="string" value="forearm_cam_qual_trig" />
</group>
<group ns="led_controller">
- <param name="type" type="string" value="TriggerControllerNode" />
+ <param name="type" type="string" value="TriggerController" />
<param name="actuator" type="string" value="forearm_cam_qual_led" />
</group>
Deleted: pkg/trunk/pr2/qualification/tests/forearm_cam_test/trigger_controller.xml
===================================================================
--- pkg/trunk/pr2/qualification/tests/forearm_cam_test/trigger_controller.xml 2009-08-27 01:32:43 UTC (rev 23085)
+++ pkg/trunk/pr2/qualification/tests/forearm_cam_test/trigger_controller.xml 2009-08-27 01:33:28 UTC (rev 23086)
@@ -1,10 +0,0 @@
-<controllers>
-<controller name="trig_controller" type="TriggerControllerNode">
- <actuator name="forearm_cam_qual_trig" />
- <waveform rep_rate="1" active_low="0" phase="0" duty_cycle=".5" running="0" pulsed="1" />
-</controller>
-<controller name="led_controller" type="TriggerControllerNode">
- <actuator name="forearm_cam_qual_led" />
- <waveform rep_rate="1" active_low="0" phase="0" duty_cycle=".5" running="0" pulsed="1" />
-</controller>
-</controllers>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wa...@us...> - 2009-08-27 01:34:42
|
Revision: 23088
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23088&view=rev
Author: wattsk
Date: 2009-08-27 01:34:33 +0000 (Thu, 27 Aug 2009)
Log Message:
-----------
Renamed wg_hardware_roslaunch to roslaunch_caller
Modified Paths:
--------------
pkg/trunk/pr2/life_test/bin/gui
pkg/trunk/pr2/life_test/manifest.xml
pkg/trunk/pr2/life_test/src/life_test/life_test.py
pkg/trunk/pr2/life_test/src/life_test/ui.py
pkg/trunk/pr2/qualification/manifest.xml
pkg/trunk/pr2/qualification/src/qualification/component_qual.py
pkg/trunk/pr2/qualification/src/qualification/pr2_qual_panel.py
pkg/trunk/pr2/qualification/src/qualification/qual_frame.py
pkg/trunk/pr2/qualification/tests/simple_example/test_node.py
pkg/trunk/pr2/roslaunch_caller/manifest.xml
pkg/trunk/sandbox/pr2_startup/manifest.xml
pkg/trunk/sandbox/pr2_startup/src/pr2_startup/ui.py
pkg/trunk/sandbox/webteleop/manifest.xml
pkg/trunk/sandbox/webteleop/src/server.py
Added Paths:
-----------
pkg/trunk/pr2/roslaunch_caller/
Removed Paths:
-------------
pkg/trunk/pr2/roslaunch_caller/src/wg_hardware_roslaunch/
pkg/trunk/pr2/wg_hardware_roslaunch/
Modified: pkg/trunk/pr2/life_test/bin/gui
===================================================================
--- pkg/trunk/pr2/life_test/bin/gui 2009-08-27 01:34:31 UTC (rev 23087)
+++ pkg/trunk/pr2/life_test/bin/gui 2009-08-27 01:34:33 UTC (rev 23088)
@@ -45,5 +45,5 @@
app.MainLoop()
except Exception, e:
print "Caught exception in TestManagerApp Main Loop"
- print e
- e
+ import traceback
+ trackback.print_exc()
Modified: pkg/trunk/pr2/life_test/manifest.xml
===================================================================
--- pkg/trunk/pr2/life_test/manifest.xml 2009-08-27 01:34:31 UTC (rev 23087)
+++ pkg/trunk/pr2/life_test/manifest.xml 2009-08-27 01:34:33 UTC (rev 23088)
@@ -31,7 +31,7 @@
<depend package="runtime_monitor" />
<depend package="invent_client" />
<depend package="diagnostic_msgs" />
- <depend package="wg_hardware_roslaunch" />
+ <depend package="roslaunch_caller" />
<depend package="mechanism_msgs" />
<depend package="geometry_msgs" />
</package>
Modified: pkg/trunk/pr2/life_test/src/life_test/life_test.py
===================================================================
--- pkg/trunk/pr2/life_test/src/life_test/life_test.py 2009-08-27 01:34:31 UTC (rev 23087)
+++ pkg/trunk/pr2/life_test/src/life_test/life_test.py 2009-08-27 01:34:33 UTC (rev 23088)
@@ -50,7 +50,6 @@
import rospy
from std_srvs.srv import *
-#from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue, DiagnosticString
# Stuff from life_test package
@@ -67,7 +66,8 @@
from email.mime.base import MIMEBase
from email import Encoders
-import wg_hardware_roslaunch.roslaunch_caller as roslaunch_caller
+
+from roslaunch_caller import roslaunch_caller
class TestMonitorPanel(wx.Panel):
Modified: pkg/trunk/pr2/life_test/src/life_test/ui.py
===================================================================
--- pkg/trunk/pr2/life_test/src/life_test/ui.py 2009-08-27 01:34:31 UTC (rev 23087)
+++ pkg/trunk/pr2/life_test/src/life_test/ui.py 2009-08-27 01:34:33 UTC (rev 23088)
@@ -65,7 +65,7 @@
from life_test import *
from test_param import *
-import wg_hardware_roslaunch.roslaunch_caller as roslaunch_caller
+from roslaunch_caller import roslaunch_caller
class TestManagerFrame(wx.Frame):
def __init__(self, parent):
Modified: pkg/trunk/pr2/qualification/manifest.xml
===================================================================
--- pkg/trunk/pr2/qualification/manifest.xml 2009-08-27 01:34:31 UTC (rev 23087)
+++ pkg/trunk/pr2/qualification/manifest.xml 2009-08-27 01:34:33 UTC (rev 23088)
@@ -19,7 +19,7 @@
<depend package="pr2_alpha" />
<depend package="pr2_head_cart" />
<depend package="invent_client" />
- <depend package="wg_hardware_roslaunch" />
+ <depend package="roslaunch_caller" />
<depend package="forearm_cam" />
<depend package="opencv_latest" />
<depend package="rxtools" />
Modified: pkg/trunk/pr2/qualification/src/qualification/component_qual.py
===================================================================
--- pkg/trunk/pr2/qualification/src/qualification/component_qual.py 2009-08-27 01:34:31 UTC (rev 23087)
+++ pkg/trunk/pr2/qualification/src/qualification/component_qual.py 2009-08-27 01:34:33 UTC (rev 23088)
@@ -61,10 +61,9 @@
import runtime_monitor
from runtime_monitor.monitor_panel import MonitorPanel
-import wg_hardware_roslaunch.roslaunch_caller as roslaunch_caller
+from roslaunch_caller import roslaunch_caller
TESTS_DIR = os.path.join(roslib.packages.get_pkg_dir('qualification'), 'tests')
-
CONFIG_DIR = os.path.join(roslib.packages.get_pkg_dir('qualification'), 'config')
Modified: pkg/trunk/pr2/qualification/src/qualification/pr2_qual_panel.py
===================================================================
--- pkg/trunk/pr2/qualification/src/qualification/pr2_qual_panel.py 2009-08-27 01:34:31 UTC (rev 23087)
+++ pkg/trunk/pr2/qualification/src/qualification/pr2_qual_panel.py 2009-08-27 01:34:33 UTC (rev 23088)
@@ -53,6 +53,8 @@
from qualification.test import *
from qualification.qual_frame import *
+from roslaunch_caller import roslaunch_caller
+
import traceback
ONBOARD_DIR = os.path.join(roslib.packages.get_pkg_dir('qualification'), 'onboard')
Modified: pkg/trunk/pr2/qualification/src/qualification/qual_frame.py
===================================================================
--- pkg/trunk/pr2/qualification/src/qualification/qual_frame.py 2009-08-27 01:34:31 UTC (rev 23087)
+++ pkg/trunk/pr2/qualification/src/qualification/qual_frame.py 2009-08-27 01:34:33 UTC (rev 23088)
@@ -32,7 +32,7 @@
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
-### Author: Kevin Watt
+### Author: Kevin Watts
import roslib
import roslib.packages
@@ -65,19 +65,21 @@
import runtime_monitor
from runtime_monitor.monitor_panel import MonitorPanel
-import wg_hardware_roslaunch.roslaunch_caller as roslaunch_caller
+from roslaunch_caller import roslaunch_caller
import rxtools
import rxtools.cppwidgets
-## Passed to qualification manager
+##\brief Passed to qualification manager
+##
+## Holds data about item to be qualified. Subtests can hold more data
class QualTestObject():
def __init__(self, name, serial):
self.name = name
self.serial = serial
-## Loads instructions for operator. Displays instructions in HTML window.
+##\brief Loads instructions for operator. Displays instructions in HTML window.
class InstructionsPanel(wx.Panel):
def __init__(self, parent, resource, qualification_frame, file):
wx.Panel.__init__(self, parent)
@@ -108,7 +110,9 @@
def on_cancel(self, event):
# We reset here and don't cancel, because nothing is running
self._manager.reset()
-
+
+##\brief Displays waiting page, subtest results
+##
## Plots panel displays a waiting page while waiting for subtests to
## complete, displays subtest results, displays runtime monitor
class PlotsPanel(wx.Panel):
@@ -140,15 +144,15 @@
# Make runtime monitor panel, console panel
self._notebook = xrc.XRCCTRL(self._panel, 'results_notebook')
- self.create_monitor()
- self.create_rxconsole()
+ self._create_monitor()
+ self._create_rxconsole()
- def create_monitor(self):
+ def _create_monitor(self):
self._monitor_panel = MonitorPanel(self._notebook)
self._monitor_panel.SetSize(wx.Size(400, 500))
self._notebook.AddPage(self._monitor_panel, "Runtime Monitor")
- def create_rxconsole(self):
+ def _create_rxconsole(self):
rxtools.cppwidgets.initRoscpp("qualification_console", False)
self._rxconsole_panel = rxtools.cppwidgets.RosoutPanel(self._notebook)
@@ -192,8 +196,10 @@
def on_cancel(self, event):
self._manager.cancel()
-## Displays results of qualification tests. Results come in as a TestResult
-## class, which makes an HTML page
+##\brief Displays results of entire qualification tests
+##
+## Displays results of qualification tests. Results come in as a QualTestResult
+## class, which makes an HTML page. Users must submit to inventory system.
class ResultsPanel(wx.Panel):
def __init__(self, parent, resource, qualification_frame):
wx.Panel.__init__(self, parent)
@@ -226,11 +232,11 @@
def on_submit(self, event):
self._manager.submit_results(self._notesbox.GetValue(), self._dir_picker.GetPath())
-## Prestartup or shutdown scripts, returns timeout msg
+##\brief Prestartup or shutdown scripts, returns timeout msg
def script_timeout(timeout):
return ScriptDoneRequest(1, "Automatic timeout. Timeout: %s" % timeout)
-## Subtest timeout, returns timeout message
+##\brief Subtest timeout, returns timeout message
def subtest_timeout(timeout):
msg = 'Automated timeout. Timeout: %s' % timeout
@@ -242,13 +248,15 @@
return result
+##\brief Makes waiting page for subtests
def make_html_waiting_page(subtest_name, index, len_subtests):
html = '<html>\n<H2 align=center>Waiting for Subtest to Complete</H2>\n'
html += '<H3 align=center>Test Name: %s</H3>\n' % subtest_name
html += '<H4 align=center>Test %d of %d</H4>\n</html>' % (index + 1, len_subtests)
return html
-## Main frame of qualification
+##\brief Main frame of qualification
+##
## Loads tests, launches them and records results
class QualificationFrame(wx.Frame):
def __init__(self, parent):
@@ -732,8 +740,6 @@
self.log('Launches stopped.')
-
-
## Launches shutdown script
def test_finished(self):
if (self._current_test is not None and self._current_test.getShutdownScript() != None):
Modified: pkg/trunk/pr2/qualification/tests/simple_example/test_node.py
===================================================================
--- pkg/trunk/pr2/qualification/tests/simple_example/test_node.py 2009-08-27 01:34:31 UTC (rev 23087)
+++ pkg/trunk/pr2/qualification/tests/simple_example/test_node.py 2009-08-27 01:34:33 UTC (rev 23088)
@@ -37,7 +37,7 @@
import rospy
from std_srvs.srv import *
-from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, DiagnosticString
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
from time import sleep
@@ -59,8 +59,7 @@
stat.name = 'Test Node'
stat.message = 'OK'
stat.hardware_id = 'HW ID'
- stat.strings = [ DiagnosticString(label='Node Status', value='OK') ]
- stat.values = []
+ stat.values = [ KeyValue('Node Status', 'OK') ]
msg.status.append(stat)
while not rospy.is_shutdown():
Property changes on: pkg/trunk/pr2/roslaunch_caller
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/pr2/wg_hardware_roslaunch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Modified: pkg/trunk/pr2/roslaunch_caller/manifest.xml
===================================================================
--- pkg/trunk/pr2/wg_hardware_roslaunch/manifest.xml 2009-08-26 21:15:54 UTC (rev 23029)
+++ pkg/trunk/pr2/roslaunch_caller/manifest.xml 2009-08-27 01:34:33 UTC (rev 23088)
@@ -1,10 +1,10 @@
<package>
- <description brief='Puts roslaunch in a library for hardware test calling'>
+ <description brief='Puts roslaunch in a library for API use'>
</description>
- <author>Kevin Watts (wa...@wi...)</author>
+ <author>Kevin Watts (wa...@wi...), Ken Conley (kw...@wi...)</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="rospy" />
<depend package="roslaunch" />
- <url></url>
+ <url>http://www.ros.org/wiki/roslaunch_caller</url>
</package>
Modified: pkg/trunk/sandbox/pr2_startup/manifest.xml
===================================================================
--- pkg/trunk/sandbox/pr2_startup/manifest.xml 2009-08-27 01:34:31 UTC (rev 23087)
+++ pkg/trunk/sandbox/pr2_startup/manifest.xml 2009-08-27 01:34:33 UTC (rev 23088)
@@ -8,7 +8,7 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/pr2_startup</url>
- <depend package="wg_hardware_roslaunch" />
+ <depend package="roslaunch_caller" />
<depend package="rospy" />
<depend package="std_msgs" />
<depend package="robot_mechanism_controllers" />
Modified: pkg/trunk/sandbox/pr2_startup/src/pr2_startup/ui.py
===================================================================
--- pkg/trunk/sandbox/pr2_startup/src/pr2_startup/ui.py 2009-08-27 01:34:31 UTC (rev 23087)
+++ pkg/trunk/sandbox/pr2_startup/src/pr2_startup/ui.py 2009-08-27 01:34:33 UTC (rev 23088)
@@ -50,9 +50,8 @@
import time
from wx import xrc
+from roslaunch_caller import roslaunch_caller
-import wg_hardware_roslaunch.roslaunch_caller as roslaunch_caller
-
class PR2StartUpAppFrame(wx.Frame):
def __init__(self, parent):
wx.Frame.__init__(self, parent, wx.ID_ANY, "PR2StartUp")
Modified: pkg/trunk/sandbox/webteleop/manifest.xml
===================================================================
--- pkg/trunk/sandbox/webteleop/manifest.xml 2009-08-27 01:34:31 UTC (rev 23087)
+++ pkg/trunk/sandbox/webteleop/manifest.xml 2009-08-27 01:34:33 UTC (rev 23088)
@@ -14,7 +14,7 @@
<depend package="bullet_python"/>
<depend package="bullet"/>
<depend package="tf"/>
- <depend package="wg_hardware_roslaunch"/>
+ <depend package="roslaunch_caller" />
<depend package="pr2_default_controllers" />
<depend package="joy" />
Modified: pkg/trunk/sandbox/webteleop/src/server.py
===================================================================
--- pkg/trunk/sandbox/webteleop/src/server.py 2009-08-27 01:34:31 UTC (rev 23087)
+++ pkg/trunk/sandbox/webteleop/src/server.py 2009-08-27 01:34:33 UTC (rev 23088)
@@ -26,7 +26,7 @@
import roslaunch
import roslaunch.pmon
-import wg_hardware_roslaunch.roslaunch_caller as roslaunch_caller
+from roslaunch_caller import roslaunch_caller
from nav_msgs import *
from nav_msgs.msg import *
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-08-27 17:21:32
|
Revision: 23152
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23152&view=rev
Author: vijaypradeep
Date: 2009-08-27 17:21:19 +0000 (Thu, 27 Aug 2009)
Log Message:
-----------
Moving camera_offsetter out of experimental
Added Paths:
-----------
pkg/trunk/stacks/calibration/camera_offsetter/
pkg/trunk/stacks/calibration/camera_offsetter/CMakeLists.txt
pkg/trunk/stacks/calibration/camera_offsetter/Makefile
pkg/trunk/stacks/calibration/camera_offsetter/config/
pkg/trunk/stacks/calibration/camera_offsetter/config/hcb.narrow_stereo_to_laser.offset
pkg/trunk/stacks/calibration/camera_offsetter/config/prg.narrow_stereo.offset
pkg/trunk/stacks/calibration/camera_offsetter/include/
pkg/trunk/stacks/calibration/camera_offsetter/include/camera_offsetter/
pkg/trunk/stacks/calibration/camera_offsetter/include/camera_offsetter/generic_offsetter.h
pkg/trunk/stacks/calibration/camera_offsetter/launch/
pkg/trunk/stacks/calibration/camera_offsetter/launch/narrow_stereo_offsetter.launch
pkg/trunk/stacks/calibration/camera_offsetter/launch/narrow_stereo_offsetter_to_laser.launch
pkg/trunk/stacks/calibration/camera_offsetter/manifest.xml
pkg/trunk/stacks/calibration/camera_offsetter/src/
pkg/trunk/stacks/calibration/camera_offsetter/src/keyboard_twist_generator.cpp
pkg/trunk/stacks/calibration/camera_offsetter/src/mono_offsetter.cpp
pkg/trunk/stacks/calibration/camera_offsetter/src/stereo_offsetter.cpp
pkg/trunk/stacks/calibration/camera_offsetter/test/
pkg/trunk/stacks/calibration/camera_offsetter/test/test_stereo_offsetter.launch
Removed Paths:
-------------
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/CMakeLists.txt
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/Makefile
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/hcb.narrow_stereo_to_laser.offset
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/prg.narrow_stereo.offset
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/include/camera_offsetter/generic_offsetter.h
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter.launch
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter_to_laser.launch
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/manifest.xml
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/keyboard_twist_generator.cpp
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/mono_offsetter.cpp
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/stereo_offsetter.cpp
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/test/test_stereo_offsetter.launch
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/CMakeLists.txt 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/CMakeLists.txt 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,32 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Set the build type. Options are:
-# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
-# Debug : w/ debug symbols, w/o optimization
-# Release : w/o debug symbols, w/ optimization
-# RelWithDebInfo : w/ debug symbols, w/ optimization
-# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rospack(camera_offsetter)
-
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-#uncomment if you have defined messages
-#genmsg()
-#uncomment if you have defined services
-#gensrv()
-
-#common commands for building c++ executables and libraries
-#rospack_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rospack_add_boost_directories()
-#rospack_link_boost(${PROJECT_NAME} thread)
-rospack_add_executable(mono_offsetter src/mono_offsetter.cpp)
-rospack_add_executable(stereo_offsetter src/stereo_offsetter.cpp)
-rospack_add_executable(keyboard_twist_generator src/keyboard_twist_generator.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/Makefile
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/Makefile 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/Makefile 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/hcb.narrow_stereo_to_laser.offset
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/hcb.narrow_stereo_to_laser.offset 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/hcb.narrow_stereo_to_laser.offset 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,2 +0,0 @@
-0.024 0 0.244
-4.56455e-05 -0.00872199 0.00523311 0.999948
\ No newline at end of file
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/prg.narrow_stereo.offset
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/prg.narrow_stereo.offset 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/prg.narrow_stereo.offset 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,2 +0,0 @@
--0.000235494 -0.000141299 0.0269986
-0.00261665 -0.00523329 9.12921e-06 0.999983
\ No newline at end of file
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/include/camera_offsetter/generic_offsetter.h
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/include/camera_offsetter/generic_offsetter.h 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/include/camera_offsetter/generic_offsetter.h 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,183 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-//! \author Vijay Pradeep
-
-#ifndef CAMERA_OFFSETTER_GENERIC_OFFSETTER_H_
-#define CAMERA_OFFSETTER_GENERIC_OFFSETTER_H_
-
-#include <ros/ros.h>
-
-#include <tf/transform_broadcaster.h>
-#include <tf/transform_datatypes.h>
-
-#include <geometry_msgs/Pose.h>
-#include <geometry_msgs/Twist.h>
-
-#include <boost/thread.hpp>
-#include <boost/thread/mutex.hpp>
-
-#include <fstream>
-
-namespace camera_offsetter
-{
-
-class GenericOffsetter
-{
-public:
- GenericOffsetter() : virtual_offset_( btQuaternion(0,0,0,1))
- {
- pose_sub_ = nh_.subscribe("~virtual_pose", 1, &GenericOffsetter::poseCb, this);
- twist_sub_ = nh_.subscribe("~virtual_twist", 1, &GenericOffsetter::twistCb, this);
-
- nh_.param("~position_scaling", position_scaling_, 1.0);
- nh_.param("~angular_scaling", angular_scaling_, 1.0);
-
- nh_.param("~config_file", config_file_, std::string("N/A"));
-
- nh_.param("~frame_suffix", frame_suffix_, std::string("_offset"));
-
- readConfig();
- }
-
- void readConfig()
- {
- if(config_file_==std::string("N/A"))
- {
- ROS_WARN("No config file is set");
- return;
- }
-
- geometry_msgs::PosePtr p(new geometry_msgs::Pose());
- std::fstream f (config_file_.c_str(), std::fstream::in);
- if(!f.is_open())
- {
- ROS_WARN("Couldn't open config file");
- return;
- }
- f >> p->position.x;
- f >> p->position.y;
- f >> p->position.z;
- f >> p->orientation.x;
- f >> p->orientation.y;
- f >> p->orientation.z;
- f >> p->orientation.w;
- ROS_INFO_STREAM(p->orientation.x);
- ROS_INFO_STREAM(p);
- poseCb(p);
- }
- void saveConfig()
- {
- if(config_file_==std::string("N/A"))
- {
- ROS_WARN("No config file is set");
- return;
- }
- std::fstream f (config_file_.c_str(), std::fstream::out);
- f << virtual_offset_.getOrigin().x() << " ";
- f << virtual_offset_.getOrigin().y() << " ";
- f << virtual_offset_.getOrigin().z() << std::endl;
- f << virtual_offset_.getRotation().x()<< " ";
- f << virtual_offset_.getRotation().y()<< " ";
- f << virtual_offset_.getRotation().z()<< " ";
- f << virtual_offset_.getRotation().w();
- }
-
- void poseCb(const geometry_msgs::PoseConstPtr& msg)
- {
- boost::mutex::scoped_lock lock(offset_mutex_);
- tf::poseMsgToTF(*msg, virtual_offset_);
- }
-
- void twistCb(const geometry_msgs::TwistConstPtr& msg)
- {
- boost::mutex::scoped_lock lock(offset_mutex_);
-
- btVector3 rot_vector(msg->angular.x, msg->angular.y, msg->angular.z);
- double angle = rot_vector.length();
-
- btQuaternion rot;
- if (angle < 1e-6)
- {
- rot = btQuaternion(0,0,0,1);
- }
- else
- rot = btQuaternion( rot_vector, angle);
-
- btVector3 trans( msg->linear.x, msg->linear.y, msg->linear.z);
-
- btTransform incrementalT(rot, trans);
-
- virtual_offset_ = virtual_offset_ * incrementalT;
-
- saveConfig();
- }
-
- void publishTransform(const ros::Time& time, const std::string frame_id, const std::string parent_id)
- {
- boost::mutex::scoped_lock lock(offset_mutex_);
- printf("Sending Transform: Trans:(%.3f, %.3f, %.3f) Q:(%.3f, %.3f , %.3f, %.3f)\n",
- virtual_offset_.getOrigin().x(),
- virtual_offset_.getOrigin().y(),
- virtual_offset_.getOrigin().z(),
- virtual_offset_.getRotation().x(),
- virtual_offset_.getRotation().y(),
- virtual_offset_.getRotation().z(),
- virtual_offset_.getRotation().w());
- tf_.sendTransform(virtual_offset_, time, frame_id, parent_id);
- }
-protected:
- std::string frame_suffix_;
-
-private:
- ros::NodeHandle nh_;
-
- ros::Subscriber pose_sub_;
- ros::Subscriber twist_sub_;
-
- tf::TransformBroadcaster tf_;
-
- double position_scaling_;
- double angular_scaling_;
-
- std::string config_file_;
-
- boost::mutex offset_mutex_;
- btTransform virtual_offset_;
-} ;
-
-
-}
-
-#endif
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter.launch
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter.launch 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter.launch 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,14 +0,0 @@
-<launch>
-
- <!-- node type="spacenav_node" pkg="spacenav_node" name="spacenav_node" / -->
- <node type="stereo_offsetter" pkg="camera_offsetter" name="narrow_stereo_offsetter" output="screen">
- <remap from="~virtual_twist" to="keyboard_twist" />
- <param name="cam_name" type="string" value="/narrow_stereo" />
- <param name="position_scaling" type="double" value=".0001" />
- <param name="angular_scaling" type="double" value="1.0" />
- <param name="cam_name" type="string" value="/narrow_stereo" />
- <param name="config_file" type="string" value="$(find camera_offsetter)/config/$(env ROBOT).narrow_stereo.offset" />
-
- </node>
-
-</launch>
\ No newline at end of file
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter_to_laser.launch
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter_to_laser.launch 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter_to_laser.launch 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,16 +0,0 @@
-
-<launch>
-
- <!-- node type="spacenav_node" pkg="spacenav_node" name="spacenav_node" / -->
- <node type="stereo_offsetter" pkg="camera_offsetter" name="narrow_stereo_offsetter" output="screen">
- <remap from="~virtual_twist" to="keyboard_twist" />
- <param name="cam_name" type="string" value="/narrow_stereo" />
- <param name="position_scaling" type="double" value=".0001" />
- <param name="angular_scaling" type="double" value="1.0" />
- <param name="cam_name" type="string" value="/narrow_stereo" />
- <param name="config_file" type="string" value="$(find camera_offsetter)/config/$(env ROBOT).narrow_stereo_to_laser.offset" />
- <param name="frame_suffix" type="string" value="_offset_laser" />
-
- </node>
-
-</launch>
\ No newline at end of file
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/manifest.xml
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/manifest.xml 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/manifest.xml 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,22 +0,0 @@
-<package>
- <description brief="Hand tweak camera extrinsics">
- Provides a tool to make small changes to the location of a monocular camera or stereocamera attached to the robot
- </description>
- <author>Vijay Pradeep, Alex Sorokin</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://pr.willowgarage.com/wiki/camera_offsetter</url>
-
- <!-- ROS -->
- <depend package="roscpp" />
-
- <!-- geometry -->
- <depend package="tf" />
-
- <!-- common_msgs -->
- <depend package="geometry_msgs" />
- <depend package="sensor_msgs" />
-
- <!-- stereo -->
- <depend package="stereo_msgs" />
-</package>
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/keyboard_twist_generator.cpp
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/keyboard_twist_generator.cpp 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/keyboard_twist_generator.cpp 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,131 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-//! \author Vijay Pradeep
-
-#include <ros/ros.h>
-#include <geometry_msgs/Twist.h>
-
-#include <unistd.h>
-#include <termios.h>
-
-int main(int argc, char** argv)
-{
- // Setup terminal settings for getchar
- const int fd = fileno(stdin);
- termios prev_flags ;
- tcgetattr(fd, &prev_flags) ;
- termios flags ;
- tcgetattr(fd,&flags);
- flags.c_lflag &= ~ICANON; // set raw (unset canonical modes)
- flags.c_cc[VMIN] = 0; // i.e. min 1 char for blocking, 0 chars for non-blocking
- flags.c_cc[VTIME] = 0; // block if waiting for char
- tcsetattr(fd,TCSANOW,&flags);
-
- ros::init(argc, argv, "keyboard_twist_generator");
-
- ros::NodeHandle nh;
-
- ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("keyboard_twist", 10);
-
- static const double trans = .001;
- static const double rot = .1 * 3.14 / 180.0;
-
-
- enum ShiftMode { ROTATION=0, TRANSLATION=1 };
-
- ShiftMode cur_shift_mode = TRANSLATION;
-
- while (nh.ok())
- {
- char c = getchar();
-
- geometry_msgs::Twist twist;
-
- geometry_msgs::Vector3* vec = NULL;
- double shift_val;
- switch (cur_shift_mode)
- {
- case ROTATION:
- vec = &twist.angular;
- shift_val = rot;
- break;
- case TRANSLATION:
- vec = &twist.linear;
- shift_val = trans;
- break;
- default:
- shift_val = 0.0;
- ROS_FATAL("unknown shift type [%u]", cur_shift_mode);
- break;
- }
-
- bool should_publish = false;
-
- switch (c)
- {
- case 'x': vec->x = -shift_val; should_publish = true; break;
- case 'X': vec->x = shift_val; should_publish = true; break;
- case 'y': vec->y = -shift_val; should_publish = true; break;
- case 'Y': vec->y = shift_val; should_publish = true; break;
- case 'z': vec->z = -shift_val; should_publish = true; break;
- case 'Z': vec->z = shift_val; should_publish = true; break;
- case 't':
- case 'T':
- ROS_INFO("Switching to translation mode");
- cur_shift_mode = TRANSLATION;
- break;
- case 'r':
- case 'R':
- ROS_INFO("Switching to rotation mode");
- cur_shift_mode = ROTATION;
- break;
- default:
- usleep(100);
- break;
- }
-
- if (should_publish)
- {
- ROS_INFO("Publishing Twist: Translation(%.3f, %.3f, %.3f) Rotation(%.3f, %.3f, %.3f)",
- twist.linear.x, twist.linear.y, twist.linear.z,
- twist.angular.x, twist.angular.y, twist.angular.z);
- pub.publish(twist);
- }
- }
-
- tcsetattr(fd,TCSANOW, &prev_flags) ; // Undo any terminal changes that we made
-
- return 0;
-}
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/mono_offsetter.cpp
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/mono_offsetter.cpp 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/mono_offsetter.cpp 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,98 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-//! \author Vijay Pradeep
-
-#include "camera_offsetter/generic_offsetter.h"
-#include "sensor_msgs/Image.h"
-#include "sensor_msgs/CameraInfo.h"
-
-namespace camera_offsetter
-{
-
-class MonoOffsetter : public GenericOffsetter
-{
-public:
- MonoOffsetter()
- {
- if (!nh_.getParam("~cam_name", cam_name_))
- ROS_FATAL("Couldn't find param [~cam_name]");
-
- image_sub_ = nh_.subscribe(cam_name_ + "/image_rect", 1, &MonoOffsetter::imageCb, this);
- info_sub_ = nh_.subscribe(cam_name_ + "/cam_info", 1, &MonoOffsetter::infoCb, this);
-
- image_pub_ = nh_.advertise<sensor_msgs::Image>(cam_name_ + "_offset/image_rect", 1);
- info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(cam_name_ + "_offset/cam_info", 1);
- }
-
- void imageCb(const sensor_msgs::ImageConstPtr& msg)
- {
- sensor_msgs::Image next_image = *msg;
- next_image.header.frame_id = msg->header.frame_id + frame_suffix_;
- image_pub_.publish(next_image);
-
- publishTransform(msg->header.stamp, next_image.header.frame_id, msg->header.frame_id);
- }
-
- void infoCb(const sensor_msgs::CameraInfoConstPtr& msg)
- {
- sensor_msgs::CameraInfo next_info = *msg;
- next_info.header.frame_id = msg->header.frame_id + frame_suffix_;
- info_pub_.publish(next_info);
- }
-
-private:
- ros::NodeHandle nh_;
- ros::Subscriber image_sub_;
- ros::Subscriber info_sub_;
-
- ros::Publisher image_pub_;
- ros::Publisher info_pub_;
-
-
- std::string cam_name_;
-} ;
-
-}
-
-using namespace camera_offsetter;
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "mono_offsetter");
-
- MonoOffsetter offsetter;
-
- ros::spin();
-}
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/stereo_offsetter.cpp
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/stereo_offsetter.cpp 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/stereo_offsetter.cpp 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,84 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-//! \author Vijay Pradeep
-
-#include "camera_offsetter/generic_offsetter.h"
-#include "stereo_msgs/RawStereo.h"
-
-namespace camera_offsetter
-{
-
-class StereoOffsetter : public GenericOffsetter
-{
-public:
- StereoOffsetter()
- {
- if (!nh_.getParam("~cam_name", cam_name_))
- ROS_FATAL("Couldn't find param [~cam_name]");
-
- raw_stereo_sub_ = nh_.subscribe(cam_name_ + "/raw_stereo", 1, &StereoOffsetter::rawStereoCb, this);
-
- raw_stereo_pub_ = nh_.advertise<stereo_msgs::RawStereo>(cam_name_ + "_offset/raw_stereo", 1);
- }
-
- void rawStereoCb(const stereo_msgs::RawStereoConstPtr& msg)
- {
- stereo_msgs::RawStereo next_raw_stereo = *msg;
- next_raw_stereo.header.frame_id = msg->header.frame_id + frame_suffix_;
- raw_stereo_pub_.publish(next_raw_stereo);
-
- publishTransform(msg->header.stamp, next_raw_stereo.header.frame_id, msg->header.frame_id);
- }
-
-private:
- ros::NodeHandle nh_;
- ros::Subscriber raw_stereo_sub_;
- ros::Publisher raw_stereo_pub_;
-
- std::string cam_name_;
-} ;
-
-}
-
-using namespace camera_offsetter;
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "stereo_offsetter");
-
- StereoOffsetter offsetter;
-
- ros::spin();
-}
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/test/test_stereo_offsetter.launch
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/test/test_stereo_offsetter.launch 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/test/test_stereo_offsetter.launch 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,11 +0,0 @@
-<launch>
-
- <!-- node type="spacenav_node" pkg="spacenav_node" name="spacenav_node" / -->
- <node type="stereo_offsetter" pkg="camera_offsetter" name="narrow_stereo_offsetter" output="screen">
- <remap from="~virtual_twist" to="keyboard_twist" />
- <param name="cam_name" type="string" value="/narrow_stereo" />
- <param name="position_scaling" type="double" value=".0001" />
- <param name="angular_scaling" type="double" value="1.0" />
- </node>
-
-</launch>
\ No newline at end of file
Copied: pkg/trunk/stacks/calibration/camera_offsetter/CMakeLists.txt (from rev 23150, pkg/trunk/calibration_experimental/sandbox/camera_offsetter/CMakeLists.txt)
===================================================================
--- pkg/trunk/stacks/calibration/camera_offsetter/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/calibration/camera_offsetter/CMakeLists.txt 2009-08-27 17:21:19 UTC (rev 23152)
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rospack(camera_offsetter)
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default pat...
[truncated message content] |
|
From: <ei...@us...> - 2009-08-27 21:43:47
|
Revision: 23180
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23180&view=rev
Author: eitanme
Date: 2009-08-27 21:43:37 +0000 (Thu, 27 Aug 2009)
Log Message:
-----------
Adding the robot self filter to the navigation stack. Also, updating launch files on pre so that the robot actually works. Hopefully, this will save other people a lot of pain.
Modified Paths:
--------------
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/lasers_and_filters.xml
pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/manifest.xml
pkg/trunk/demos/2dnav_gazebo/config/lasers_and_filters.launch
pkg/trunk/demos/2dnav_gazebo/manifest.xml
pkg/trunk/demos/milestone2/milestone2_actions/config/perception.xml
pkg/trunk/demos/milestone2/milestone2_actions/manifest.xml
pkg/trunk/sandbox/3dnav_pr2/launch/pr2_planning_environment.launch
pkg/trunk/stacks/pr2/pr2_alpha/pre.launch
pkg/trunk/stacks/pr2/pr2_alpha/pre.machine
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/lasers_and_filters.xml
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/lasers_and_filters.xml 2009-08-27 21:41:34 UTC (rev 23179)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/config/lasers_and_filters.xml 2009-08-27 21:43:37 UTC (rev 23180)
@@ -1,4 +1,7 @@
<launch>
+
+<include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
+
<node pkg="mux" type="throttle" args="3.0 base_scan base_scan_throttled" />
<node pkg="mux" type="throttle" args="3.0 tilt_scan tilt_scan_throttled" />
<node pkg="mux" type="throttle" args="3.0 tilt_scan_filtered tilt_scan_filtered_throttled" />
@@ -12,16 +15,124 @@
<rosparam command="load" ns="scan_filters" file="$(find laser_scan)/default_scan_shadows.yaml" />
<rosparam command="load" ns="cloud_filters" file="$(find laser_scan)/point_cloud_footprint.filters.yaml" />
<param name="scan_topic" value="tilt_scan" />
- <param name="cloud_topic" value="tilt_scan_filtered" />
+ <param name="cloud_topic" value="tilt_scan_shadow_filtered" />
<param name="high_fidelity" value="true" />
</node>
+<!-- Filter for tilt laser scans that hit the body of the robot -->
+<node pkg="robot_self_filter" type="self_filter" name="tilt_laser_self_filter" respawn="true" output="screen">
+
+ <!-- The topic for the input cloud -->
+ <remap from="cloud_in" to="tilt_scan_shadow_filtered" />
+
+ <!-- The topic for the output cloud -->
+ <remap from="cloud_out" to="tilt_scan_filtered" />
+
+ <!-- The frame of the sensor used to obtain the data to be
+ filtered; This parameter is optional. If it is not specified,
+ shadow points will be considered outside -->
+ <param name="sensor_frame" type="string" value="laser_tilt_link" />
+
+ <!-- Minimum distance to sensor (for point not to be considered inside) -->
+ <param name="min_sensor_dist" type="double" value="0.01" />
+
+ <!-- The padding to be added for the body parts the robot can see -->
+ <param name="self_see_padd" type="double" value="0.05" />
+
+ <!-- The scaling to be added for the body parts the robot can see -->
+ <param name="self_see_scale" type="double" value="1.0" />
+
+ <!-- The names of the links the sensor can see -->
+ <param name="self_see_links" type="string" value="l_upper_arm_link
+ l_upper_arm_roll_link
+ l_elbow_flex_link
+ l_forearm_link
+ l_forearm_roll_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ l_gripper_l_finger_link
+ l_gripper_l_finger_tip_link
+ l_gripper_r_finger_link
+ l_gripper_r_finger_tip_link
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ r_upper_arm_link
+ r_upper_arm_roll_link
+ r_elbow_flex_link
+ r_forearm_link
+ r_forearm_roll_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ r_gripper_l_finger_link
+ r_gripper_l_finger_tip_link
+ r_gripper_r_finger_link
+ r_gripper_r_finger_tip_link
+ r_shoulder_pan_link
+ r_shoulder_lift_link
+ base_laser
+ base_link" />
+
+</node>
+
<!-- Filter for base laser shadowing/veiling -->
<node pkg="laser_scan" type="scan_to_cloud" respawn="true" machine="three" name="base_shadow_filter" >
<rosparam command="load" ns="scan_filters" file="$(find laser_scan)/default_scan_shadows.yaml" />
<rosparam command="load" ns="cloud_filters" file="$(find laser_scan)/point_cloud_footprint.filters.yaml" />
<param name="scan_topic" value="base_scan" />
- <param name="cloud_topic" value="base_scan_marking" />
+ <param name="cloud_topic" value="base_scan_shadow_filtered" />
</node>
+<!-- Filter for base laser scans that hit the body of the robot -->
+<node pkg="robot_self_filter" type="self_filter" name="base_laser_self_filter" respawn="true" output="screen">
+
+ <!-- The topic for the input cloud -->
+ <remap from="cloud_in" to="base_scan_shadow_filtered" />
+
+ <!-- The topic for the output cloud -->
+ <remap from="cloud_out" to="base_scan_marking" />
+
+ <!-- The frame of the sensor used to obtain the data to be
+ filtered; This parameter is optional. If it is not specified,
+ shadow points will be considered outside -->
+ <param name="sensor_frame" type="string" value="base_laser" />
+
+ <!-- Minimum distance to sensor (for point not to be considered inside) -->
+ <param name="min_sensor_dist" type="double" value="0.01" />
+
+ <!-- The padding to be added for the body parts the robot can see -->
+ <param name="self_see_padd" type="double" value="0.01" />
+
+ <!-- The scaling to be added for the body parts the robot can see -->
+ <param name="self_see_scale" type="double" value="1.0" />
+
+ <!-- The names of the links the sensor can see -->
+ <param name="self_see_links" type="string" value="l_upper_arm_link
+ l_upper_arm_roll_link
+ l_elbow_flex_link
+ l_forearm_link
+ l_forearm_roll_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ l_gripper_l_finger_link
+ l_gripper_l_finger_tip_link
+ l_gripper_r_finger_link
+ l_gripper_r_finger_tip_link
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ r_upper_arm_link
+ r_upper_arm_roll_link
+ r_elbow_flex_link
+ r_forearm_link
+ r_forearm_roll_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ r_gripper_l_finger_link
+ r_gripper_l_finger_tip_link
+ r_gripper_r_finger_link
+ r_gripper_r_finger_tip_link
+ r_shoulder_pan_link
+ r_shoulder_lift_link" />
+
+</node>
+
</launch>
Modified: pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/manifest.xml
===================================================================
--- pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/manifest.xml 2009-08-27 21:41:34 UTC (rev 23179)
+++ pkg/trunk/applications/2dnav_pr2_app/2dnav_pr2/manifest.xml 2009-08-27 21:43:37 UTC (rev 23180)
@@ -19,6 +19,8 @@
<depend package="mux"/>
<depend package="backup_safetysound"/>
<depend package="geometry_msgs"/>
+ <depend package="robot_self_filter"/>
+ <depend package="3dnav_pr2"/>
<!-- packages used in the test scripts -->
<depend package="robot_actions"/>
Modified: pkg/trunk/demos/2dnav_gazebo/config/lasers_and_filters.launch
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/config/lasers_and_filters.launch 2009-08-27 21:41:34 UTC (rev 23179)
+++ pkg/trunk/demos/2dnav_gazebo/config/lasers_and_filters.launch 2009-08-27 21:43:37 UTC (rev 23180)
@@ -1,4 +1,7 @@
<launch>
+
+<include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
+
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_experimental_controllers)/laser_tilt/laser_tilt_controller.xml" />
<node pkg="pr2_mechanism_controllers" type="send_periodic_cmd.py" args="laser_tilt_controller linear 2 .65 .25" />
@@ -35,25 +38,128 @@
</node>
<!-- Filter for tilt laser shadowing/veiling -->
-<!-- subscribes tilt_scan and publishes tilt_scan_filtered -->
-<node pkg="laser_scan" type="scan_shadows_filter_node" respawn="true" machine="three" name="tilt_shadow_filter">
+<node pkg="laser_scan" type="scan_to_cloud" respawn="true" machine="three" name="tilt_shadow_filter">
+ <rosparam command="load" ns="scan_filters" file="$(find laser_scan)/default_scan_shadows.yaml" />
+ <rosparam command="load" ns="cloud_filters" file="$(find laser_scan)/point_cloud_footprint.filters.yaml" />
<param name="scan_topic" value="tilt_scan" />
- <param name="cloud_topic" value="tilt_scan_filtered" />
+ <param name="cloud_topic" value="tilt_scan_shadow_filtered" />
+ <param name="high_fidelity" value="true" />
</node>
+<!-- Filter for tilt laser scans that hit the body of the robot -->
+<node pkg="robot_self_filter" type="self_filter" name="tilt_laser_self_filter" respawn="true" output="screen">
+
+ <!-- The topic for the input cloud -->
+ <remap from="cloud_in" to="tilt_scan_shadow_filtered" />
+
+ <!-- The topic for the output cloud -->
+ <remap from="cloud_out" to="tilt_scan_filtered" />
+
+ <!-- The frame of the sensor used to obtain the data to be
+ filtered; This parameter is optional. If it is not specified,
+ shadow points will be considered outside -->
+ <param name="sensor_frame" type="string" value="laser_tilt_link" />
+
+ <!-- Minimum distance to sensor (for point not to be considered inside) -->
+ <param name="min_sensor_dist" type="double" value="0.01" />
+
+ <!-- The padding to be added for the body parts the robot can see -->
+ <param name="self_see_padd" type="double" value="0.01" />
+
+ <!-- The scaling to be added for the body parts the robot can see -->
+ <param name="self_see_scale" type="double" value="1.0" />
+
+ <!-- The names of the links the sensor can see -->
+ <param name="self_see_links" type="string" value="l_upper_arm_link
+ l_upper_arm_roll_link
+ l_elbow_flex_link
+ l_forearm_link
+ l_forearm_roll_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ l_gripper_l_finger_link
+ l_gripper_l_finger_tip_link
+ l_gripper_r_finger_link
+ l_gripper_r_finger_tip_link
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ r_upper_arm_link
+ r_upper_arm_roll_link
+ r_elbow_flex_link
+ r_forearm_link
+ r_forearm_roll_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ r_gripper_l_finger_link
+ r_gripper_l_finger_tip_link
+ r_gripper_r_finger_link
+ r_gripper_r_finger_tip_link
+ r_shoulder_pan_link
+ r_shoulder_lift_link
+ base_laser
+ base_link" />
+
+</node>
+
<!-- Filter for base laser shadowing/veiling -->
-<node pkg="laser_scan" type="scan_shadows_filter_node" respawn="true" machine="three" name="base_shadow_filter_non_preserve" >
+<node pkg="laser_scan" type="scan_to_cloud" respawn="true" machine="three" name="base_shadow_filter" >
+ <rosparam command="load" ns="scan_filters" file="$(find laser_scan)/default_scan_shadows.yaml" />
+ <rosparam command="load" ns="cloud_filters" file="$(find laser_scan)/point_cloud_footprint.filters.yaml" />
<param name="scan_topic" value="base_scan" />
- <param name="cloud_topic" value="base_scan_marking" />
- <param name="laser_max_range" value="30.0" />
+ <param name="cloud_topic" value="base_scan_shadow_filtered" />
</node>
-<!-- for clearing we want preservative -->
-<node pkg="laser_scan" type="scan_shadows_filter_node" respawn="true" machine="three" name="base_shadow_filter_preserve" >
- <param name="scan_topic" value="base_scan" />
- <param name="cloud_topic" value="base_scan_clearing" />
- <param name="preservative" value="true" />
- <param name="laser_max_range" value="30.0" />
+<!-- Filter for base laser scans that hit the body of the robot -->
+<node pkg="robot_self_filter" type="self_filter" name="base_laser_self_filter" respawn="true" output="screen">
+
+ <!-- The topic for the input cloud -->
+ <remap from="cloud_in" to="base_scan_shadow_filtered" />
+
+ <!-- The topic for the output cloud -->
+ <remap from="cloud_out" to="base_scan_marking" />
+
+ <!-- The frame of the sensor used to obtain the data to be
+ filtered; This parameter is optional. If it is not specified,
+ shadow points will be considered outside -->
+ <param name="sensor_frame" type="string" value="base_laser" />
+
+ <!-- Minimum distance to sensor (for point not to be considered inside) -->
+ <param name="min_sensor_dist" type="double" value="0.01" />
+
+ <!-- The padding to be added for the body parts the robot can see -->
+ <param name="self_see_padd" type="double" value="0.01" />
+
+ <!-- The scaling to be added for the body parts the robot can see -->
+ <param name="self_see_scale" type="double" value="1.0" />
+
+ <!-- The names of the links the sensor can see -->
+ <param name="self_see_links" type="string" value="l_upper_arm_link
+ l_upper_arm_roll_link
+ l_elbow_flex_link
+ l_forearm_link
+ l_forearm_roll_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ l_gripper_l_finger_link
+ l_gripper_l_finger_tip_link
+ l_gripper_r_finger_link
+ l_gripper_r_finger_tip_link
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ r_upper_arm_link
+ r_upper_arm_roll_link
+ r_elbow_flex_link
+ r_forearm_link
+ r_forearm_roll_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ r_gripper_l_finger_link
+ r_gripper_l_finger_tip_link
+ r_gripper_r_finger_link
+ r_gripper_r_finger_tip_link
+ r_shoulder_pan_link
+ r_shoulder_lift_link" />
+
</node>
Modified: pkg/trunk/demos/2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-08-27 21:41:34 UTC (rev 23179)
+++ pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-08-27 21:43:37 UTC (rev 23180)
@@ -20,6 +20,8 @@
<depend package="point_cloud_assembler"/>
<depend package="semantic_point_annotator"/>
<depend package="or_robot_self_filter"/>
+ <depend package="robot_self_filter"/>
+ <depend package="3dnav_pr2"/>
<depend package="rviz"/>
<!-- Adding experimental controllers as a dependency b/c of tuckarm move -->
Modified: pkg/trunk/demos/milestone2/milestone2_actions/config/perception.xml
===================================================================
--- pkg/trunk/demos/milestone2/milestone2_actions/config/perception.xml 2009-08-27 21:41:34 UTC (rev 23179)
+++ pkg/trunk/demos/milestone2/milestone2_actions/config/perception.xml 2009-08-27 21:43:37 UTC (rev 23180)
@@ -1,4 +1,7 @@
<launch>
+
+ <include file="$(find 3dnav_pr2)/launch/pr2_planning_environment.launch" />
+
<include file="$(find pr2_alpha)/$(env ROBOT).machine" />
<node pkg="mux" type="throttle" args="3.0 base_scan base_scan_throttled" />
@@ -26,18 +29,126 @@
<rosparam command="load" ns="scan_filters" file="$(find laser_scan)/default_scan_shadows.yaml" />
<rosparam command="load" ns="cloud_filters" file="$(find laser_scan)/point_cloud_footprint.filters.yaml" />
<param name="scan_topic" value="tilt_scan" />
- <param name="cloud_topic" value="tilt_scan_filtered" />
+ <param name="cloud_topic" value="tilt_scan_shadow_filtered" />
<param name="high_fidelity" value="true" />
</node>
+ <!-- Filter for tilt laser scans that hit the body of the robot -->
+ <node pkg="robot_self_filter" type="self_filter" name="tilt_laser_self_filter" respawn="true" output="screen">
+
+ <!-- The topic for the input cloud -->
+ <remap from="cloud_in" to="tilt_scan_shadow_filtered" />
+
+ <!-- The topic for the output cloud -->
+ <remap from="cloud_out" to="tilt_scan_filtered" />
+
+ <!-- The frame of the sensor used to obtain the data to be
+ filtered; This parameter is optional. If it is not specified,
+ shadow points will be considered outside -->
+ <param name="sensor_frame" type="string" value="laser_tilt_link" />
+
+ <!-- Minimum distance to sensor (for point not to be considered inside) -->
+ <param name="min_sensor_dist" type="double" value="0.01" />
+
+ <!-- The padding to be added for the body parts the robot can see -->
+ <param name="self_see_padd" type="double" value="0.01" />
+
+ <!-- The scaling to be added for the body parts the robot can see -->
+ <param name="self_see_scale" type="double" value="1.0" />
+
+ <!-- The names of the links the sensor can see -->
+ <param name="self_see_links" type="string" value="l_upper_arm_link
+ l_upper_arm_roll_link
+ l_elbow_flex_link
+ l_forearm_link
+ l_forearm_roll_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ l_gripper_l_finger_link
+ l_gripper_l_finger_tip_link
+ l_gripper_r_finger_link
+ l_gripper_r_finger_tip_link
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ r_upper_arm_link
+ r_upper_arm_roll_link
+ r_elbow_flex_link
+ r_forearm_link
+ r_forearm_roll_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ r_gripper_l_finger_link
+ r_gripper_l_finger_tip_link
+ r_gripper_r_finger_link
+ r_gripper_r_finger_tip_link
+ r_shoulder_pan_link
+ r_shoulder_lift_link
+ base_laser
+ base_link" />
+
+ </node>
+
<!-- Filter for base laser shadowing/veiling -->
<node pkg="laser_scan" type="scan_to_cloud" respawn="true" machine="three" name="base_shadow_filter" >
<rosparam command="load" ns="scan_filters" file="$(find laser_scan)/default_scan_shadows.yaml" />
<rosparam command="load" ns="cloud_filters" file="$(find laser_scan)/point_cloud_footprint.filters.yaml" />
<param name="scan_topic" value="base_scan" />
- <param name="cloud_topic" value="base_scan_marking" />
+ <param name="cloud_topic" value="base_scan_shadow_filtered" />
</node>
+ <!-- Filter for base laser scans that hit the body of the robot -->
+ <node pkg="robot_self_filter" type="self_filter" name="base_laser_self_filter" respawn="true" output="screen">
+
+ <!-- The topic for the input cloud -->
+ <remap from="cloud_in" to="base_scan_shadow_filtered" />
+
+ <!-- The topic for the output cloud -->
+ <remap from="cloud_out" to="base_scan_marking" />
+
+ <!-- The frame of the sensor used to obtain the data to be
+ filtered; This parameter is optional. If it is not specified,
+ shadow points will be considered outside -->
+ <param name="sensor_frame" type="string" value="base_laser" />
+
+ <!-- Minimum distance to sensor (for point not to be considered inside) -->
+ <param name="min_sensor_dist" type="double" value="0.01" />
+
+ <!-- The padding to be added for the body parts the robot can see -->
+ <param name="self_see_padd" type="double" value="0.01" />
+
+ <!-- The scaling to be added for the body parts the robot can see -->
+ <param name="self_see_scale" type="double" value="1.0" />
+
+ <!-- The names of the links the sensor can see -->
+ <param name="self_see_links" type="string" value="l_upper_arm_link
+ l_upper_arm_roll_link
+ l_elbow_flex_link
+ l_forearm_link
+ l_forearm_roll_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ l_gripper_l_finger_link
+ l_gripper_l_finger_tip_link
+ l_gripper_r_finger_link
+ l_gripper_r_finger_tip_link
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ r_upper_arm_link
+ r_upper_arm_roll_link
+ r_elbow_flex_link
+ r_forearm_link
+ r_forearm_roll_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ r_gripper_l_finger_link
+ r_gripper_l_finger_tip_link
+ r_gripper_r_finger_link
+ r_gripper_r_finger_tip_link
+ r_shoulder_pan_link
+ r_shoulder_lift_link" />
+
+ </node>
+
<!-- Laser scan assembler for tilt laser -->
<node pkg="point_cloud_assembler" type="laser_scan_assembler_srv" name="laser_scan_assembler" respawn="true">
<remap from="scan_in" to="tilt_scan"/>
Modified: pkg/trunk/demos/milestone2/milestone2_actions/manifest.xml
===================================================================
--- pkg/trunk/demos/milestone2/milestone2_actions/manifest.xml 2009-08-27 21:41:34 UTC (rev 23179)
+++ pkg/trunk/demos/milestone2/milestone2_actions/manifest.xml 2009-08-27 21:43:37 UTC (rev 23180)
@@ -16,4 +16,6 @@
<depend package="pr2_experimental_controllers"/>
<depend package="switch_controller_translator"/>
<depend package="mux"/>
+ <depend package="robot_self_filter"/>
+ <depend package="3dnav_pr2"/>
</package>
Modified: pkg/trunk/sandbox/3dnav_pr2/launch/pr2_planning_environment.launch
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/launch/pr2_planning_environment.launch 2009-08-27 21:41:34 UTC (rev 23179)
+++ pkg/trunk/sandbox/3dnav_pr2/launch/pr2_planning_environment.launch 2009-08-27 21:43:37 UTC (rev 23180)
@@ -1,7 +1,7 @@
<launch>
<!-- send parameters for collision checking for PR2; this includes parameters for the self filter -->
- <rosparam command="load" ns="robot_description_collision" file="$(find 3dnav_pr2)/params/collision/collision_checks_right_arm.yaml" />
+ <rosparam command="load" ns="robot_description_collision" file="$(find 3dnav_pr2)/params/collision/collision_checks_both_arms.yaml" />
<!-- send parameters needed for motion planning -->
<rosparam command="load" ns="robot_description_planning" file="$(find 3dnav_pr2)/params/planning/planning.yaml" />
Modified: pkg/trunk/stacks/pr2/pr2_alpha/pre.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/pre.launch 2009-08-27 21:41:34 UTC (rev 23179)
+++ pkg/trunk/stacks/pr2/pr2_alpha/pre.launch 2009-08-27 21:43:37 UTC (rev 23180)
@@ -14,7 +14,7 @@
<!-- Power Board Control Node -->
<param name="power_board_serial" value="1021"/>
- <node machine="two" pkg="pr2_power_board" type="power_node" respawn="true"/>
+ <node machine="two" pkg="pr2_power_board" type="power_node" name="power_node" respawn="true"/>
<!-- Robot state publisher -->
<node machine="two" pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
@@ -23,7 +23,7 @@
</node>
<!-- Battery Monitor -->
- <node machine="two" pkg="ocean_battery_driver" type="ocean_server" respawn="true">
+ <node machine="two" pkg="ocean_battery_driver" name="ocean_server" type="ocean_server" respawn="true">
<param name="number_of_ports" type="int" value="4" />
<param name="port0" type="string" value="/dev/ttyUSB0" />
<param name="port1" type="string" value="/dev/ttyUSB1" />
@@ -32,7 +32,7 @@
<param name="debug_level" type="int" value="0" />
</node>
- <node pkg="power_monitor" type="power_monitor" respawn="true"/>
+ <node pkg="power_monitor" type="power_monitor" name="power_monitor" respawn="true"/>
<!-- Base Laser -->
@@ -60,7 +60,7 @@
<param name="imu/frameid" type="string" value="imu" />
<param name="imu/autocalibrate" type="bool" value="true" />
<param name="imu/angular_velocity_stdev" type="double" value="0.00017" />
- <node machine="realtime" pkg="imu_node" type="imu_node" output="screen"/>
+ <node machine="realtime" pkg="imu_node" type="imu_node" name="imu_node" output="screen"/>
<!-- Forearm Cameras -->
<node machine="two" name="forearm_cam_r" pkg="forearm_cam" type="forearm_node" respawn="false" output="screen">
@@ -93,26 +93,26 @@
<!-- Sound
- <node pkg="sound_play" type="soundplay_node.py" machine="three" />
+ <node pkg="sound_play" type="soundplay_node.py" name="soundplay_node" machine="three" />
-->
<!-- Runtime Diagnostics Logging -->
- <node pkg="rosrecord" type="rosrecord" args="-f /hwlog/pre_runtime_automatic /diagnostics" />
+ <node pkg="rosrecord" type="rosrecord" name="rosrecord_diagnostics" args="-f /hwlog/pre_runtime_automatic /diagnostics" />
<!-- NTP monitoring script Warns to console if sync error -->
- <node pkg="pr2_computer_monitor" type="ntp_monitor.py" args="pre2" machine="realtime"/>
- <node pkg="pr2_computer_monitor" type="ntp_monitor.py" args="fw1 7000" machine="two"/>
+ <node pkg="pr2_computer_monitor" name="realtime_ntp_monitor" type="ntp_monitor.py" args="pre2" machine="realtime"/>
+ <node pkg="pr2_computer_monitor" name="two_ntp_monitor" type="ntp_monitor.py" args="fw1 7000" machine="two"/>
<!-- Disk usage monitoring script Warns to console if disk full -->
- <node pkg="pr2_computer_monitor" type="hd_monitor.py" args="$(optenv HOME /home)" machine="realtime"/>
- <node pkg="pr2_computer_monitor" type="hd_monitor.py" args="$(optenv HOME /home)" machine="two"/>
+ <node pkg="pr2_computer_monitor" name="realtime_hd_monitor" type="hd_monitor.py" args="$(optenv HOME /home)" machine="realtime"/>
+ <node pkg="pr2_computer_monitor" name="two_hd_monitor" type="hd_monitor.py" args="$(optenv HOME /home)" machine="two"/>
<!-- Monitor CPU temp, usage -->
- <node pkg="pr2_computer_monitor" type="cpu_monitor.py" machine="realtime" />
- <node pkg="pr2_computer_monitor" type="cpu_monitor.py" machine="two" />
+ <node pkg="pr2_computer_monitor" name="realtime_cpu_monitor" type="cpu_monitor.py" machine="realtime" />
+ <node pkg="pr2_computer_monitor" name="two_cpu_monitor" type="cpu_monitor.py" machine="two" />
<!-- Joint states diagnostics logging -->
- <node pkg="mechanism_control" type="joints_to_diagnostics.py" machine="realtime" />
+ <node pkg="mechanism_control" name="joint_state_logging" type="joints_to_diagnostics.py" machine="realtime" />
<!-- Diagnostics aggregation -->
<node pkg="diagnostic_aggregator" type="aggregator_node"
Modified: pkg/trunk/stacks/pr2/pr2_alpha/pre.machine
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/pre.machine 2009-08-27 21:41:34 UTC (rev 23179)
+++ pkg/trunk/stacks/pr2/pr2_alpha/pre.machine 2009-08-27 21:43:37 UTC (rev 23180)
@@ -5,6 +5,8 @@
<machine name="realtime" address="pre1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="true"/>
<machine name="two" address="pre2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
+ <machine name="three" address="pre1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
+ <machine name="four" address="pre2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
<machine name="stereo" address="pre2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X