Menu
▾
▴
personalrobots-commit
|
From: <stu...@us...> - 2009-04-23 20:09:34
|
Revision: 14346
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14346&view=rev
Author: stuglaser
Date: 2009-04-23 20:09:28 +0000 (Thu, 23 Apr 2009)
Log Message:
-----------
Broke dependency of robot_mechanism_controllers on robot_kinematics
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_tff_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller_ik.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_chain_constraint_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_twist_controller_ik.h 2009-04-23 20:09:28 UTC (rev 14346)
@@ -35,8 +35,11 @@
#define CARTESIAN_TWIST_CONTROLLER_H
#include <vector>
+#include <boost/scoped_ptr.hpp>
#include <kdl/chain.hpp>
#include <kdl/frames.hpp>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainiksolvervel_pinv.hpp>
#include <ros/node.h>
#include <robot_msgs/Twist.h>
#include <mechanism_model/controller.h>
@@ -44,7 +47,6 @@
#include <robot_mechanism_controllers/cartesian_wrench_controller.h>
#include <robot_mechanism_controllers/joint_velocity_controller.h>
#include <control_toolbox/pid.h>
-#include <boost/scoped_ptr.hpp>
namespace controller {
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h 2009-04-23 20:09:28 UTC (rev 14346)
@@ -67,7 +67,7 @@
KDL::Wrench wrench_desi_;
private:
- bool publishDiagnostics(int level, const string& message);
+ bool publishDiagnostics(int level, const std::string& message);
ros::Node* node_;
std::string controller_name_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-04-23 20:09:28 UTC (rev 14346)
@@ -39,6 +39,7 @@
#include "mechanism_model/chain.h"
#include "kdl/chain.hpp"
#include "kdl/frames.hpp"
+#include "kdl/chainfksolver.hpp"
#include "ros/node.h"
#include "robot_msgs/Wrench.h"
#include "misc_utils/subscription_guard.h"
@@ -48,8 +49,6 @@
#include "joy/Joy.h"
#include "Eigen/LU"
#include "Eigen/Core"
-#include "robot_kinematics/robot_kinematics.h"
-
#include <robot_msgs/VisualizationMarker.h>
@@ -87,10 +86,10 @@
Eigen::MatrixXf joint_constraint_force_;
Eigen::MatrixXf joint_constraint_jac_;
Eigen::MatrixXf joint_constraint_null_space_;
-
+
Eigen::MatrixXf task_jac_;
Eigen::MatrixXf identity_;
- Eigen::MatrixXf identity_joint_;
+ Eigen::MatrixXf identity_joint_;
Eigen::MatrixXf constraint_null_space_;
Eigen::MatrixXf constraint_torq_;
Eigen::MatrixXf joint_constraint_torq_;
@@ -107,8 +106,8 @@
double f_x_max;
double f_r_max;
double f_pose_max;
- double f_limit_max;
-
+ double f_limit_max;
+
double desired_roll_;
double desired_pitch_;
double desired_yaw_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_chain_constraint_controller.h 2009-04-23 20:09:28 UTC (rev 14346)
@@ -36,7 +36,7 @@
/*! \class controller::JointChainConstraintController
\brief Adds joint position constraints to any connected chain of joints.
- Wrenches are in the cartesian frame in the root frame on the tip frame.
+ Wrenches are in the cartesian frame in the root frame on the tip frame.
Example config:<br>
@@ -51,6 +51,9 @@
#define JOINT_CHAIN_CONSTRAINT_CONTROLLER_H
#include <vector>
+#include <cstdio>
+#include <cstring>
+#include <string>
#include <kdl/chain.hpp>
#include <kdl/frames.hpp>
#include <kdl/chainjnttojacsolver.hpp>
@@ -63,15 +66,11 @@
#include "mechanism_model/chain.h"
#include "tf/transform_datatypes.h"
-#include <stdio.h>
-#include <string.h>
#include "misc_utils/advertised_service_guard.h"
-
#include "Eigen/Geometry"
#include "Eigen/LU"
#include "Eigen/Core"
-#include "robot_kinematics/robot_kinematics.h"
namespace controller {
@@ -88,7 +87,7 @@
int joint_chain_index_;
double joint_error_;
bool remove_;
-
+
control_toolbox::Pid pid_;
};
@@ -97,7 +96,7 @@
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-
+
JointChainConstraintController();
~JointChainConstraintController();
@@ -107,15 +106,15 @@
* \param tip_name The tip of the chain.
* \param *robot The robot.
*/
- bool init(mechanism::RobotState *robot, const std::string& root_name,
+ bool init(mechanism::RobotState *robot, const std::string& root_name,
const std::string& tip_name, const std::string& controller_name);
void update();
-
+
// input of the controller
KDL::Wrench wrench_desired_;
-
+
private:
void computeConstraintTorques();
void computeConstraintJacobian();
@@ -136,22 +135,22 @@
KDL::ChainJntToJacSolver *jnt_to_jac_solver_;
KDL::JntArray jnt_pos_, jnt_eff_;
KDL::Jacobian chain_kdl_jacobian_;
-
+
Eigen::MatrixXf identity_;
Eigen::MatrixXf chain_eigen_jacobian_;
Eigen::MatrixXf task_torque_;
-
+
// joint constraint matrices and vectors
Eigen::MatrixXf joint_constraint_torque_;
Eigen::MatrixXf joint_constraint_jacobian_;
Eigen::MatrixXf joint_constraint_null_space_;
-
+
double last_time_;
-
+
bool initialized_;
AdvertisedServiceGuard change_constraints_guard_;
-
+
};
@@ -176,7 +175,7 @@
robot_msgs::Wrench wrench_msg_;
-
+
};
} // namespace
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-04-23 20:09:28 UTC (rev 14346)
@@ -39,26 +39,24 @@
#include "mechanism_model/chain.h"
#include "kdl/chain.hpp"
#include "kdl/frames.hpp"
+#include "kdl/chainfksolver.hpp"
#include "ros/node.h"
#include "robot_msgs/Wrench.h"
#include "robot_msgs/PoseStamped.h"
#include "robot_msgs/Transform.h"
#include "robot_mechanism_controllers/PlugInternalState.h"
#include "robot_srvs/SetPoseStamped.h"
-#include <control_toolbox/pid.h>
+#include "control_toolbox/pid.h"
#include "misc_utils/subscription_guard.h"
#include "mechanism_model/controller.h"
#include "tf/transform_datatypes.h"
#include "tf/transform_listener.h"
#include "misc_utils/advertised_service_guard.h"
+#include "realtime_tools/realtime_publisher.h"
-#include <realtime_tools/realtime_publisher.h>
-
-
#include "Eigen/Geometry"
#include "Eigen/LU"
#include "Eigen/Core"
-#include "robot_kinematics/robot_kinematics.h"
#include <robot_msgs/VisualizationMarker.h>
@@ -80,7 +78,7 @@
void setToolOffset(const tf::Transform &);
std::string root_name_;
-
+
// input of the controller
KDL::Wrench wrench_desi_;
Eigen::Matrix<float,6,1> task_wrench_;
@@ -133,10 +131,10 @@
double f_limit_max;
double last_time_;
bool initialized_;
-
-
-
-
+
+
+
+
control_toolbox::Pid roll_pid_; /**< Internal PID controller. */
control_toolbox::Pid pitch_pid_; /**< Internal PID controller. */
control_toolbox::Pid yaw_pid_; /**< Internal PID controller. */
Modified: pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-04-23 20:09:28 UTC (rev 14346)
@@ -22,7 +22,6 @@
<depend package="joy" />
<depend package="eigen" />
<depend package="filters" />
- <depend package="robot_kinematics" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -34,8 +34,8 @@
#include <algorithm>
-#include <robot_kinematics/robot_kinematics.h>
#include <mechanism_control/mechanism_control.h>
+#include "kdl/chainfksolverpos_recursive.hpp"
#include "robot_mechanism_controllers/cartesian_pose_controller.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_tff_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_tff_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_tff_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -31,10 +31,10 @@
* Author: Wim Meeussen
*/
+#include "robot_mechanism_controllers/cartesian_tff_controller.h"
#include <algorithm>
-#include <robot_kinematics/robot_kinematics.h>
#include <mechanism_control/mechanism_control.h>
-#include "robot_mechanism_controllers/cartesian_tff_controller.h"
+#include <kdl/chainfksolvervel_recursive.hpp>
using namespace KDL;
@@ -131,7 +131,7 @@
ROS_ERROR("CartesianTFFController: could not get instance to mechanism control");
return false;
}
- string output;
+ std::string output;
if (!node_->getParam(controller_name_+"/output", output)){
ROS_ERROR("CartesianTFFController: No ouptut name found on parameter server");
return false;
@@ -171,7 +171,7 @@
}
// set initial position, twist
- FrameVel frame_twist;
+ FrameVel frame_twist;
chain_.getVelocities(robot_state_->joint_states_, jnt_posvel_);
jnt_to_twist_solver_->JntToCart(jnt_posvel_, frame_twist);
pose_meas_old_ = frame_twist.value();
@@ -193,7 +193,7 @@
chain_.getVelocities(robot_state_->joint_states_, jnt_posvel_);
// get cartesian twist and pose
- FrameVel frame_twist;
+ FrameVel frame_twist;
jnt_to_twist_solver_->JntToCart(jnt_posvel_, frame_twist);
pose_meas_ = frame_twist.value();
twist_meas_ = pose_meas_.M.Inverse() * (frame_twist.deriv());
@@ -212,7 +212,7 @@
else if (mode_[i] == tff_msg_.POSITION)
wrench_desi_[i] = twist_to_wrench_[i] * (pos_pid_controller_[i].updatePid(position_[i] - value_[i], dt));
}
-
+
// send wrench to wrench controller
wrench_controller_->wrench_desi_ = (pose_meas_.M * wrench_desi_);
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -34,8 +34,8 @@
#include <algorithm>
-#include <robot_kinematics/robot_kinematics.h>
#include <mechanism_control/mechanism_control.h>
+#include "kdl/chainfksolverpos_recursive.hpp"
#include "robot_mechanism_controllers/cartesian_trajectory_controller.h"
@@ -116,7 +116,7 @@
ROS_ERROR("CartesianTrajectoryController: could not get instance to mechanism control");
return false;
}
- string output;
+ std::string output;
if (!node_->getParam(controller_name_+"/output", output)){
ROS_ERROR("CartesianTrajectoryController: No ouptut name found on parameter server");
return false;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -32,8 +32,8 @@
*/
#include <algorithm>
-#include <robot_kinematics/robot_kinematics.h>
#include <mechanism_control/mechanism_control.h>
+#include "kdl/chainfksolvervel_recursive.hpp"
#include "robot_mechanism_controllers/cartesian_twist_controller.h"
using namespace KDL;
@@ -89,7 +89,7 @@
chain_.toKDL(kdl_chain_);
// create solver
- jnt_to_twist_solver_.reset(new ChainFkSolverVel_recursive(kdl_chain_));
+ jnt_to_twist_solver_.reset(new KDL::ChainFkSolverVel_recursive(kdl_chain_));
jnt_posvel_.resize(kdl_chain_.getNrOfJoints());
// constructs 3 identical pid controllers: for the x,y and z translations
@@ -113,7 +113,7 @@
ROS_ERROR("CartesianTwistController: could not get instance to mechanism control");
return false;
}
- string output;
+ std::string output;
if (!node_->getParam(controller_name_+"/output", output)){
ROS_ERROR("CartesianTwistController: No ouptut name found on parameter server");
return false;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller_ik.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller_ik.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_twist_controller_ik.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -31,9 +31,9 @@
* Author: Wim Meeussen
*/
+#include "robot_mechanism_controllers/cartesian_twist_controller_ik.h"
#include <algorithm>
-#include <robot_kinematics/robot_kinematics.h>
-#include "robot_mechanism_controllers/cartesian_twist_controller_ik.h"
+#include <kdl/chainfksolvervel_recursive.hpp>
using namespace KDL;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_wrench_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -32,7 +32,6 @@
*/
#include <algorithm>
-#include <robot_kinematics/robot_kinematics.h>
#include <robot_mechanism_controllers/cartesian_wrench_controller.h>
@@ -156,7 +155,7 @@
-bool CartesianWrenchController::publishDiagnostics(int level, const string& message)
+bool CartesianWrenchController::publishDiagnostics(int level, const std::string& message)
{
if (diagnostics_time_ + diagnostics_interval_ < ros::Time::now())
{
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -30,10 +30,13 @@
/*
* Author: Melonee Wise
*/
+
+#include "robot_mechanism_controllers/endeffector_constraint_controller.h"
+#include <algorithm>
#include "angles/angles.h"
#include "urdf/parser.h"
-#include <algorithm>
-#include "robot_mechanism_controllers/endeffector_constraint_controller.h"
+#include "kdl/chainfksolverpos_recursive.hpp"
+#include "kdl/chainjnttojacsolver.hpp"
#define DEBUG 0 // easy debugging
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_chain_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_chain_constraint_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_chain_constraint_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -30,11 +30,11 @@
/*
* Author: Melonee Wise
*/
-
+
+#include "robot_mechanism_controllers/joint_chain_constraint_controller.h"
+#include <algorithm>
#include "angles/angles.h"
-#include <algorithm>
-#include "robot_kinematics/robot_kinematics.h"
-#include "robot_mechanism_controllers/joint_chain_constraint_controller.h"
+#include <kdl/chainfksolvervel_recursive.hpp>
using namespace KDL;
@@ -58,10 +58,10 @@
}
-bool JointChainConstraintController::init(mechanism::RobotState *robot_state,
- const string& root_name,
- const string& tip_name,
- const string& controller_name)
+bool JointChainConstraintController::init(mechanism::RobotState *robot_state,
+ const std::string& root_name,
+ const std::string& tip_name,
+ const std::string& controller_name)
{
ROS_INFO("Intializing JointChainConstraintController,\"%s\", between \"%s\" and \"%s\".", controller_name.c_str(), root_name.c_str(), tip_name.c_str());
@@ -84,9 +84,9 @@
jnt_pos_.resize(kdl_chain_.getNrOfJoints());
jnt_eff_.resize(kdl_chain_.getNrOfJoints());
chain_kdl_jacobian_.resize(kdl_chain_.getNrOfJoints());
-
+
wrench_desired_ = Wrench::Zero();
-
+
// create the required matrices
joint_constraint_torque_ = Eigen::MatrixXf::Zero(kdl_chain_.getNrOfJoints(), 1);
// TODO: make sure to change this in the future to have mixed constraint conditions on joints
@@ -99,7 +99,7 @@
// advertise service
node_->advertiseService(controller_name_ + "/add_constraints", &JointChainConstraintController::addConstraint, this);
- change_constraints_guard_.set(controller_name_ + "/add_constraints");
+ change_constraints_guard_.set(controller_name_ + "/add_constraints");
return true;
}
@@ -111,13 +111,13 @@
// check if joints are calibrated
if (!mechanism_chain_.allCalibrated(robot_state_->joint_states_))
return;
-
+
// get joint positions
mechanism_chain_.getPositions(robot_state_->joint_states_, jnt_pos_);
-
+
// get the chain jacobian
jnt_to_jac_solver_->JntToJac(jnt_pos_, chain_kdl_jacobian_);
-
+
// TODO: Write a function for doing this conversion
//convert to eigen for easier math
for (unsigned int i = 0; i < 6; i++)
@@ -127,21 +127,21 @@
chain_eigen_jacobian_(i,j) = chain_kdl_jacobian_(i,j);
}
}
-
+
// compute constraint torques
computeConstraintTorques();
-
+
// compute constraint torques
computeConstraintJacobian();
-
+
// compute constraint torques
computeConstraintNullSpace();
-
- // compute the task torque on the joints
+
+ // compute the task torque on the joints
for (unsigned int i=0; i<6; i++)
task_wrench_(i) = wrench_desired_(i);
task_torque_ = joint_constraint_null_space_ * chain_eigen_jacobian_.transpose() * task_wrench_;
-
+
for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); ++i)
{
jnt_eff_(i) = joint_constraint_torque_(i) + task_torque_(i);
@@ -158,23 +158,23 @@
joint_constraint_torque_.setZero();
// check there are constraints to enforce
if(list_size_ == 0)
- {
+ {
return;
}
-
+
double time = robot_state_->hw_->current_time_;
double error(0);
std::list<ConstraintState>::iterator it = constraint_list_.begin();
for(int i = 0 ; i < list_size_; ++i)
{
-
+
int joint_type = (*it).joint_->type_;
-
+
if(joint_type == mechanism::JOINT_ROTARY)
{
double limit_min = (*it).joint_->joint_limit_min_;
double limit_max = (*it).joint_->joint_limit_max_;
-
+
angles::shortest_angular_distance_with_limits((*it).threshold_start_, jnt_pos_((*it).joint_chain_index_), limit_min, limit_max, error);
}
@@ -189,9 +189,9 @@
// check the sign of the constraint
double sign = -((*it).threshold_start_ - (*it).nullspace_start_)/fabs((*it).threshold_start_ - (*it).nullspace_start_);
-
+
(*it).joint_error_ = error;
-
+
if(sign*error > 0)
{
double temp_constraint_torque = (*it).pid_.updatePid(error, time - last_time_);
@@ -201,7 +201,7 @@
// sum the torques
joint_constraint_torque_((*it).joint_chain_index_) = joint_constraint_torque_((*it).joint_chain_index_)+ temp_constraint_torque;
}
- else
+ else
{
// make error 0 for better accounting
(*it).joint_error_ = 0;
@@ -209,7 +209,7 @@
it++;
}
last_time_ = time;
-
+
}
@@ -219,11 +219,11 @@
joint_constraint_jacobian_.setZero();
// check there are constraints to enforce
if(list_size_ == 0)
- {
+ {
return;
}
std::list<ConstraintState>::iterator it = constraint_list_.begin();
-
+
for(int i = 0 ; i < list_size_; ++i)
{
// check to turn off control authority
@@ -233,7 +233,7 @@
}
it++;
}
-
+
}
@@ -247,12 +247,12 @@
}
-bool JointChainConstraintController::addConstraint(robot_mechanism_controllers::ChangeConstraints::Request &req,
+bool JointChainConstraintController::addConstraint(robot_mechanism_controllers::ChangeConstraints::Request &req,
robot_mechanism_controllers::ChangeConstraints::Response &resp)
{
ConstraintState temp;
bool found = false;
-
+
for(unsigned int i =0; i < kdl_chain_.getNrOfJoints(); i++)
{
std::string name = mechanism_chain_.getJoint(i)->name_;
@@ -265,7 +265,7 @@
break;
}
}
-
+
if (!found) {
ROS_ERROR("Unable to find joint with ID: \"%s\"", req.constraint.joint_name.c_str());
resp.add_ok = 0;
@@ -273,7 +273,7 @@
}
temp.pid_.initPid(req.constraint.p,req.constraint.i,req.constraint.d,req.constraint.i_clamp, -req.constraint.i_clamp);
-
+
temp.id_ = req.constraint.id;
temp.joint_name_ = req.constraint.joint_name;
temp.threshold_start_ = req.constraint.threshold_start;
@@ -281,11 +281,11 @@
temp.max_constraint_torque_ = req.constraint.max_constraint_torque;
temp.remove_ = false;
temp.joint_error_ = 0;
-
+
constraint_list_.push_back(temp);
list_size_++;
ROS_INFO("Added constraint on \"%s\" with ID: \"%s\".", req.constraint.joint_name.c_str(),req.constraint.id.c_str());
-
+
resp.add_ok = 1;
return true;
@@ -316,10 +316,10 @@
controller_name_ = config->Attribute("name");
// get name of root and tip
- string root_name, tip_name;
- node_->param(controller_name_+"/root_name", root_name, string("no_name_given"));
- node_->param(controller_name_+"/tip_name", tip_name, string("no_name_given"));
-
+ std::string root_name, tip_name;
+ node_->param(controller_name_+"/root_name", root_name, std::string("no_name_given"));
+ node_->param(controller_name_+"/tip_name", tip_name, std::string("no_name_given"));
+
// initialize wrench controller
if (!controller_.init(robot, root_name, tip_name, controller_name_))
{
@@ -330,8 +330,8 @@
// subscribe to wrench commands
node_->subscribe(controller_name_ + "/command", wrench_msg_,
&JointChainConstraintControllerNode::command, this, 1);
-
+
return true;
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-04-23 20:09:28 UTC (rev 14346)
@@ -30,11 +30,13 @@
/*
* Author: Melonee Wise
*/
+#include "robot_mechanism_controllers/plug_controller.h"
+#include <algorithm>
#include "angles/angles.h"
#include "urdf/parser.h"
-#include <algorithm>
-#include "robot_mechanism_controllers/plug_controller.h"
#include "tf_conversions/tf_kdl.h"
+#include "kdl/chainfksolverpos_recursive.hpp"
+#include "kdl/chainjnttojacsolver.hpp"
#define DEBUG 0 // easy debugging
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2009-04-23 20:08:45 UTC (rev 14345)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2009-04-23 20:09:28 UTC (rev 14346)
@@ -91,11 +91,11 @@
// get controller
controller::Controller* controller = getControllerByName(name);
if (controller == NULL) return false;
-
+
// cast controller to ControllerType
ControllerType* controller_type = dynamic_cast< ControllerType* >(controller);
if (controller_type == NULL) return false;
-
+
// copy result
c = controller_type;
return true;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-24 00:49:45
|
Revision: 14380
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14380&view=rev
Author: tfoote
Date: 2009-04-24 00:48:32 +0000 (Fri, 24 Apr 2009)
Log Message:
-----------
robot_msgs/Polyline2D -> robot_msgs/Polyline with underlying type deprecated_msgs/Point2DFloat32 -> robot_msgs/Point
Modified Paths:
--------------
pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp
pkg/trunk/deprecated/highlevel_controllers/src/move_base_door.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp
pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/governor_node.h
pkg/trunk/deprecated/trajectory_rollout/src/governor_node.cpp
pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/angles.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/areas.h
pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/areas.cpp
pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/nav/amcl/src/amcl_node.cpp
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/nav/people_aware_nav/src/is_person_on_path.cpp
pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
pkg/trunk/prcore/bullet/swig/pybtTransform.h
pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp
Removed Paths:
-------------
pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -64,11 +64,11 @@
* - @b "odom"/deprecated_msgs::RobotBase2DOdom : for tracking position
* Publishes to (name / type):
- * - @b "raw_obstacles"robot_msgs::Polyline2D : contains all workspace obstacles in a window around the robot
- * - @b "inflated_obstacles"/robot_msgs::Polyline2D : contains c-space expansion, up to the inscribed radius of the robot
- * - @b "gui_path"/robot_msgs::Polyline2D : the global path currently being followed
- * - @b "local_path"/robot_msgs::Polyline2D : the local trajectory currently being followed
- * - @b "robot_footprint"/robot_msgs::Polyline2D : the footprint of the robot based on current position and orientation
+ * - @b "raw_obstacles"robot_msgs::Polyline : contains all workspace obstacles in a window around the robot
+ * - @b "inflated_obstacles"/robot_msgs::Polyline : contains c-space expansion, up to the inscribed radius of the robot
+ * - @b "gui_path"/robot_msgs::Polyline : the global path currently being followed
+ * - @b "local_path"/robot_msgs::Polyline : the local trajectory currently being followed
+ * - @b "robot_footprint"/robot_msgs::Polyline : the footprint of the robot based on current position and orientation
* - @b "cmd_vel"/robot_msgs::PoseDot : for dispatching velocity commands to the base controller
* - @b "head_controller/head_track_point"/robot_msgs::PointStamped : publishes a local goal to the head controller
* <hr>
@@ -113,7 +113,7 @@
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/PointCloud.h>
#include <deprecated_msgs/Pose2DFloat32.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <robot_srvs/StaticMap.h>
#include <robot_msgs/PointStamped.h>
#include <algorithm>
@@ -336,17 +336,17 @@
footprint_, inscribedRadius, circumscribedRadius);
// Advertize messages to publish cost map updates
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("raw_obstacles", 1);
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("inflated_obstacles", 1);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("raw_obstacles", 1);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("inflated_obstacles", 1);
// Advertize message to publish the global plan
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("gui_path", 1);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("gui_path", 1);
// Advertize message to publish local plan
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("local_path", 1);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("local_path", 1);
// Advertize message to publish robot footprint
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("robot_footprint", 1);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("robot_footprint", 1);
// Advertize message to publish velocity cmds
ros::Node::instance()->advertise<robot_msgs::PoseDot>("cmd_vel", 1);
@@ -646,7 +646,7 @@
void MoveBase::publishFootprint(double x, double y, double th){
std::vector<deprecated_msgs::Point2DFloat32> footprint = controller_->drawFootprint(x, y, th);
- robot_msgs::Polyline2D footprint_msg;
+ robot_msgs::Polyline footprint_msg;
footprint_msg.header.frame_id = global_frame_;
footprint_msg.set_points_size(footprint.size());
footprint_msg.color.r = 1.0;
@@ -656,12 +656,13 @@
for(unsigned int i = 0; i < footprint.size(); ++i){
footprint_msg.points[i].x = footprint[i].x;
footprint_msg.points[i].y = footprint[i].y;
+ footprint_msg.points[i].z = 0;
}
ros::Node::instance()->publish("robot_footprint", footprint_msg);
}
void MoveBase::publishPath(bool isGlobal, const std::list<deprecated_msgs::Pose2DFloat32>& path) {
- robot_msgs::Polyline2D guiPathMsg;
+ robot_msgs::Polyline guiPathMsg;
guiPathMsg.header.frame_id = global_frame_;
guiPathMsg.set_points_size(path.size());
@@ -670,6 +671,7 @@
const deprecated_msgs::Pose2DFloat32& w = *it;
guiPathMsg.points[i].x = w.x;
guiPathMsg.points[i].y = w.y;
+ guiPathMsg.points[i].z = 0;
i++;
}
@@ -904,7 +906,7 @@
}
// First publish raw obstacles in red
- robot_msgs::Polyline2D pointCloudMsg;
+ robot_msgs::Polyline pointCloudMsg;
pointCloudMsg.header.frame_id = global_frame_;
unsigned int pointCount = rawObstacles.size();
pointCloudMsg.set_points_size(pointCount);
@@ -916,6 +918,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = rawObstacles[i].first;
pointCloudMsg.points[i].y = rawObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;
}
if (!ros::Node::instance()->ok()) {
@@ -934,6 +937,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = inflatedObstacles[i].first;
pointCloudMsg.points[i].y = inflatedObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;
}
if (!ros::Node::instance()->ok()) {
@@ -970,7 +974,7 @@
}
// First publish raw obstacles in red
- robot_msgs::Polyline2D pointCloudMsg;
+ robot_msgs::Polyline pointCloudMsg;
pointCloudMsg.header.frame_id = global_frame_;
unsigned int pointCount = rawObstacles.size();
pointCloudMsg.set_points_size(pointCount);
@@ -982,6 +986,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = rawObstacles[i].first;
pointCloudMsg.points[i].y = rawObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;
}
ros::Node::instance()->publish("raw_obstacles", pointCloudMsg);
@@ -997,6 +1002,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = inflatedObstacles[i].first;
pointCloudMsg.points[i].y = inflatedObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;;
}
ros::Node::instance()->publish("inflated_obstacles", pointCloudMsg);
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base_door.cpp
===================================================================
--- pkg/trunk/deprecated/highlevel_controllers/src/move_base_door.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base_door.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -69,7 +69,7 @@
#include <robot_msgs/JointTraj.h>
#include <angles/angles.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
using namespace old_costmap_2d;
using namespace deprecated_msgs;
@@ -297,13 +297,14 @@
void MoveBaseDoor::publishTraj(robot_msgs::JointTraj &traj, std::string path_type, std::string publish_frame_id)
{
- robot_msgs::Polyline2D gui_path_msg;
+ robot_msgs::Polyline gui_path_msg;
gui_path_msg.header.frame_id = publish_frame_id;
gui_path_msg.set_points_size(traj.points.size());
for(int i=0; i< (int) traj.points.size(); i++)
{
gui_path_msg.points[i].x = traj.points[i].positions[0];
gui_path_msg.points[i].y = traj.points[i].positions[1];
+ gui_path_msg.points[i].z = 0;
}
if(path_type == std::string("full path"))
Modified: pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -53,7 +53,7 @@
#include <old_costmap_2d/costmap_2d.h>
// For GUI debug
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
//window length for remembering laser data (seconds)
static const double WINDOW_LENGTH = 1.0;
@@ -90,7 +90,7 @@
//laser scan message
laser_scan::LaserScan laser_msg_;
- robot_msgs::Polyline2D pointcloud_msg_;
+ robot_msgs::Polyline pointcloud_msg_;
//projector for the laser
laser_scan::LaserProjection projector_;
Modified: pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -63,8 +63,8 @@
* - @b "obstacle_cloud"/robot_msgs::PointCloud : low obstacles near the ground
* Publishes to (name / type):
- * - @b "raw_obstacles"robot_msgs::Polyline2D : contains all workspace obstacles in a window around the robot
- * - @b "inflated_obstacles"/robot_msgs::Polyline2D : contains c-space expansion, up to the inscribed radius of the robot
+ * - @b "raw_obstacles"robot_msgs::Polyline : contains all workspace obstacles in a window around the robot
+ * - @b "inflated_obstacles"/robot_msgs::Polyline : contains c-space expansion, up to the inscribed radius of the robot
* <hr>
*
* @section parameters ROS parameters (in addition to base class parameters):
@@ -104,7 +104,7 @@
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/PointCloud.h>
#include <deprecated_msgs/Pose2DFloat32.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <robot_srvs/StaticMap.h>
#include <robot_msgs/PointStamped.h>
#include <algorithm>
@@ -302,8 +302,8 @@
local_map_accessor_ = new CostMapAccessor(*costMap_, local_access_mapsize_, 0.0, 0.0);
// Advertize messages to publish cost map updates
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("raw_obstacles", 1);
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("inflated_obstacles", 1);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("raw_obstacles", 1);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("inflated_obstacles", 1);
// Advertise costmap service
// Might be worth eventually having a dedicated node provide this service and all
@@ -517,7 +517,7 @@
}
// First publish raw obstacles in red
- robot_msgs::Polyline2D pointCloudMsg;
+ robot_msgs::Polyline pointCloudMsg;
pointCloudMsg.header.frame_id = global_frame_;
unsigned int pointCount = rawObstacles.size();
pointCloudMsg.set_points_size(pointCount);
@@ -529,6 +529,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = rawObstacles[i].first;
pointCloudMsg.points[i].y = rawObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;
}
if (!ros::Node::instance()->ok()) {
@@ -547,6 +548,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = inflatedObstacles[i].first;
pointCloudMsg.points[i].y = inflatedObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;
}
if (!ros::Node::instance()->ok()) {
@@ -583,7 +585,7 @@
}
// First publish raw obstacles in red
- robot_msgs::Polyline2D pointCloudMsg;
+ robot_msgs::Polyline pointCloudMsg;
pointCloudMsg.header.frame_id = global_frame_;
unsigned int pointCount = rawObstacles.size();
pointCloudMsg.set_points_size(pointCount);
@@ -595,6 +597,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = rawObstacles[i].first;
pointCloudMsg.points[i].y = rawObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;
}
ros::Node::instance()->publish("raw_obstacles", pointCloudMsg);
@@ -610,6 +613,7 @@
for(unsigned int i=0;i<pointCount;i++){
pointCloudMsg.points[i].x = inflatedObstacles[i].first;
pointCloudMsg.points[i].y = inflatedObstacles[i].second;
+ pointCloudMsg.points[i].z = 0;
}
ros::Node::instance()->publish("inflated_obstacles", pointCloudMsg);
Modified: pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/governor_node.h
===================================================================
--- pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/governor_node.h 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/deprecated/trajectory_rollout/include/trajectory_rollout/governor_node.h 2009-04-24 00:48:32 UTC (rev 14380)
@@ -46,7 +46,7 @@
#include <deprecated_msgs/RobotBase2DOdom.h>
//for GUI debugging
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <deprecated_msgs/Point2DFloat32.h>
@@ -161,7 +161,7 @@
deprecated_msgs::RobotBase2DOdom odom_msg_;
//outgoing messages
- robot_msgs::Polyline2D poly_line_msg_;
+ robot_msgs::Polyline poly_line_msg_;
robot_msgs::PoseDot cmd_vel_msg_;
//since both odomReceived and processPlan access robot_vel we need to lock
@@ -177,8 +177,8 @@
double cycle_time_;
//for debugging output
- robot_msgs::Polyline2D path_msg;
- robot_msgs::Polyline2D footprint_msg;
+ robot_msgs::Polyline path_msg;
+ robot_msgs::Polyline footprint_msg;
};
Modified: pkg/trunk/deprecated/trajectory_rollout/src/governor_node.cpp
===================================================================
--- pkg/trunk/deprecated/trajectory_rollout/src/governor_node.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/deprecated/trajectory_rollout/src/governor_node.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -52,10 +52,10 @@
robot_vel_.stamp_ = ros::Time();
//so we can draw the local path
- advertise<robot_msgs::Polyline2D>("local_path", 10);
+ advertise<robot_msgs::Polyline>("local_path", 10);
//so we can draw the robot footprint to help with debugging
- advertise<robot_msgs::Polyline2D>("robot_footprint", 10);
+ advertise<robot_msgs::Polyline>("robot_footprint", 10);
advertise<robot_msgs::PoseDot>("cmd_vel", 1);
subscribe("wavefront_plan", plan_msg_, &GovernorNode::planReceived, 1);
@@ -166,6 +166,7 @@
path.getPoint(i, pt_x, pt_y, pt_th);
path_msg.points[i].x = pt_x;
path_msg.points[i].y = pt_y;
+ path_msg.points[i].z = 0;
//so we can draw the footprint on the map
if(i == 0){
@@ -188,6 +189,7 @@
for(unsigned int i = 0; i < footprint.size(); ++i){
footprint_msg.points[i].x = footprint[i].x;
footprint_msg.points[i].y = footprint[i].y;
+ footprint_msg.points[i].z = 0;
//printf("(%.2f, %.2f)\n", footprint_msg.points[i].x, footprint_msg.points[i].y);
}
publish("robot_footprint", footprint_msg);
Modified: pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -59,9 +59,9 @@
ros_node_.param("~control_topic_name", control_topic_name_, std::string("/base/trajectory_controller/trajectory_command"));
//for display purposes
- ros_node_.advertise<robot_msgs::Polyline2D>("gui_path", 1);
- ros_node_.advertise<robot_msgs::Polyline2D>("local_path", 1);
- ros_node_.advertise<robot_msgs::Polyline2D>("robot_footprint", 1);
+ ros_node_.advertise<robot_msgs::Polyline>("gui_path", 1);
+ ros_node_.advertise<robot_msgs::Polyline>("local_path", 1);
+ ros_node_.advertise<robot_msgs::Polyline>("robot_footprint", 1);
//pass on some parameters to the components of the move base node if they are not explicitly overridden
//(perhaps the controller and the planner could operate in different frames)
@@ -357,7 +357,7 @@
std::vector<robot_msgs::Point> footprint;
planner_->computeOrientedFootprint(getPose2D(global_pose_), planner_->footprint_, footprint);
- robot_msgs::Polyline2D footprint_msg;
+ robot_msgs::Polyline footprint_msg;
footprint_msg.header.frame_id = global_frame_;
footprint_msg.set_points_size(footprint.size());
footprint_msg.color.r = 1.0;
@@ -367,6 +367,7 @@
for(unsigned int i = 0; i < footprint.size(); ++i){
footprint_msg.points[i].x = footprint[i].x;
footprint_msg.points[i].y = footprint[i].y;
+ footprint_msg.points[i].z = footprint[i].z;
ROS_DEBUG("Footprint:%d:: %f, %f\n",i,footprint[i].x,footprint[i].y);
}
ros_node_.publish("robot_footprint", footprint_msg);
@@ -374,12 +375,13 @@
void MoveBaseDoorAction::publishPath(const std::vector<robot_actions::Pose2D>& path, std::string topic, double r, double g, double b, double a){
// Extract the plan in world co-ordinates
- robot_msgs::Polyline2D gui_path_msg;
+ robot_msgs::Polyline gui_path_msg;
gui_path_msg.header.frame_id = global_frame_;
gui_path_msg.set_points_size(path.size());
for(unsigned int i=0; i < path.size(); i++){
gui_path_msg.points[i].x = path[i].x;
gui_path_msg.points[i].y = path[i].y;
+ gui_path_msg.points[i].z = 0;
}
gui_path_msg.color.r = r;
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/angles.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/angles.h 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/angles.h 2009-04-24 00:48:32 UTC (rev 14380)
@@ -39,7 +39,7 @@
#include <robot_msgs/PointStamped.h>
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/Polygon3D.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <point_cloud_mapping/geometry/point.h>
#include <point_cloud_mapping/geometry/nearest.h>
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/areas.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/areas.h 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/areas.h 2009-04-24 00:48:32 UTC (rev 14380)
@@ -37,7 +37,7 @@
#include <robot_msgs/Point32.h>
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/Polygon3D.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <point_cloud_mapping/geometry/nearest.h>
@@ -80,7 +80,7 @@
double compute2DPolygonalArea (const robot_msgs::PointCloud &points, const std::vector<double> &normal);
double compute2DPolygonalArea (const robot_msgs::Polygon3D &polygon, const std::vector<double> &normal);
void convexHull2D (const robot_msgs::PointCloud &points, const std::vector<int> &indices, const std::vector<double> &coeff, robot_msgs::Polygon3D &hull);
- void convexHull2D (const std::vector<robot_msgs::Point32> &points, robot_msgs::Polyline2D &hull);
+ void convexHull2D (const std::vector<robot_msgs::Point32> &points, robot_msgs::Polyline &hull);
bool isPointIn2DPolygon (const robot_msgs::Point32 &point, const robot_msgs::Polygon3D &polygon);
}
Modified: pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/areas.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/areas.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/areas.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -165,7 +165,7 @@
std::sort (epoints_demean.begin (), epoints_demean.end (), comparePoint2D);
- robot_msgs::Polyline2D hull_2d;
+ robot_msgs::Polyline hull_2d;
convexHull2D (epoints_demean, hull_2d);
int nr_points_hull = hull_2d.points.size ();
@@ -211,10 +211,10 @@
* \note (code snippet inspired from http://www.softsurfer.com/Archive/algorithm_0109/algorithm_0109.htm)
* Copyright 2001, softSurfer (www.softsurfer.com)
* \param points the 2D projected point cloud representing a planar model
- * \param hull the resultant 2D convex hull model as a \a Polyline2D
+ * \param hull the resultant 2D convex hull model as a \a Polyline
*/
void
- convexHull2D (const std::vector<robot_msgs::Point32> &points, robot_msgs::Polyline2D &hull)
+ convexHull2D (const std::vector<robot_msgs::Point32> &points, robot_msgs::Polyline &hull)
{
int nr_points = points.size ();
hull.points.resize (nr_points + 1);
@@ -237,17 +237,20 @@
++top;
hull.points[top].x = points[0].x;
hull.points[top].y = points[0].y;
+ hull.points[top].z = points[0].z;
// A nontrivial segment
if (points[minmax].y != points[0].y)
{
++top;
hull.points[top].x = points[minmax].x;
hull.points[top].y = points[minmax].y;
+ hull.points[top].z = points[minmax].z;
}
++top;
// Add the polygon's endpoint
hull.points[top].x = points[0].x;
hull.points[top].y = points[0].y;
+ hull.points[top].z = points[0].z;
hull.points.resize (top + 1);
return;
}
@@ -263,6 +266,7 @@
// Add the polygon's endpoint
hull.points[top].x = points[0].x;
hull.points[top].y = points[0].y;
+ hull.points[top].z = points[0].z;
i = minmax;
while (++i <= maxmin)
@@ -286,6 +290,7 @@
++top;
hull.points[top].x = points[i].x;
hull.points[top].y = points[i].y;
+ hull.points[top].z = points[i].z;
}
// Next, compute the upper hull above the bottom hull
@@ -295,6 +300,7 @@
// Add the point with max X and max Y coordinates to the hull
hull.points[top].x = points[nr_points - 1].x;
hull.points[top].y = points[nr_points - 1].y;
+ hull.points[top].z = points[nr_points - 1].z;
}
// The bottom point of the upper hull stack
bot = top;
@@ -322,6 +328,7 @@
hull.points[top].x = points[i].x;
hull.points[top].y = points[i].y;
+ hull.points[top].z = points[i].z;
}
if (minmax != 0)
@@ -330,6 +337,7 @@
// Add the polygon's endpoint
hull.points[top].x = points[0].x;
hull.points[top].y = points[0].y;
+ hull.points[top].z = points[0].z;
}
hull.points.resize (top + 1);
return;
Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-24 00:48:32 UTC (rev 14380)
@@ -42,7 +42,7 @@
#include <costmap_2d/costmap_2d.h>
#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/Point.h>
-#include <robot_msgs/Polyline2D.h>
+#include <robot_msgs/Polyline.h>
#include <tf/transform_listener.h>
#include <vector>
#include <robot_msgs/Point.h>
Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -40,7 +40,7 @@
NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& cost_map) : ros_node_(ros_node), tf_(tf),
cost_map_(cost_map), planner_(cost_map.cellSizeX(), cost_map.cellSizeY()) {
//advertise our plan visualization
- ros_node_.advertise<robot_msgs::Polyline2D>("~navfn/plan", 1);
+ ros_node_.advertise<robot_msgs::Polyline>("~navfn/plan", 1);
//read parameters for the planner
ros_node_.param("~/navfn/costmap/global_frame", global_frame_, std::string("map"));
@@ -197,13 +197,14 @@
return;
// Extract the plan in world co-ordinates, we assume the path is all in the same frame
- robot_msgs::Polyline2D gui_path_msg;
+ robot_msgs::Polyline gui_path_msg;
gui_path_msg.header.frame_id = path[0].header.frame_id;
gui_path_msg.header.stamp = path[0].header.stamp;
gui_path_msg.set_points_size(path.size());
for(unsigned int i=0; i < path.size(); i++){
gui_path_msg.points[i].x = path[i].pose.position.x;
gui_path_msg.points[i].y = path[i].pose.position.y;
+ gui_path_msg.points[i].z = path[i].pose.position.z;
}
gui_path_msg.color.r = r;
Modified: pkg/trunk/nav/amcl/src/amcl_node.cpp
===================================================================
--- pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-04-24 00:48:32 UTC (rev 14380)
@@ -68,7 +68,7 @@
- @b "amcl_pose" robot_msgs/PoseWithCovariance : robot's estimated pose in the map, with covariance
- @b "particlecloud" robot_msgs/ParticleCloud : the set of pose estimates being maintained by the filter.
- @b "tf_message" tf/tfMessage : publishes the transform from "odom" (which can be remapped via the ~odom_frame_id parameter) to "map"
-- @b "gui_laser" robot_msgs/Polyline2D : re-projected laser scans (for visualization)
+- @b "gui_laser" robot_msgs/Polyline : re-projected laser scans (for visualization)
Offers services (name type):
- @b "global_localization" std_srvs/Empty : Initiate global localization, wherein all particles are dispersed randomly through the free space in the map.
@@ -144,7 +144,7 @@
#include "robot_msgs/Pose.h"
#include "robot_srvs/StaticMap.h"
#include "std_srvs/Empty.h"
-#include "robot_msgs/Polyline2D.h"
+#include "robot_msgs/Polyline.h"
// For transform support
#include "tf/transform_broadcaster.h"
@@ -369,7 +369,7 @@
ros::Node::instance()->advertise<robot_msgs::PoseWithCovariance>("amcl_pose",2);
ros::Node::instance()->advertise<robot_msgs::ParticleCloud>("particlecloud",2);
- ros::Node::instance()->advertise<robot_msgs::Polyline2D>("gui_laser",2);
+ ros::Node::instance()->advertise<robot_msgs::Polyline>("gui_laser",2);
ros::Node::instance()->advertiseService("global_localization",
&AmclNode::globalLocalizationCallback,
this);
@@ -741,7 +741,7 @@
if((gui_publish_rate.toSec() > 0.0) &&
(now - gui_laser_last_publish_time) >= gui_publish_rate)
{
- robot_msgs::Polyline2D point_cloud;
+ robot_msgs::Polyline point_cloud;
point_cloud.header = laser_scan->header;
point_cloud.header.frame_id = "map";
point_cloud.set_points_size(laser_scan->ranges.size());
@@ -760,6 +760,7 @@
lp.setY(laser_scan->ranges[i] *
sin(laser_scan->angle_min +
i * laser_scan->angle_increment));
+ lp.setZ(0); ///\todo Brian please verify --Tully
try
{
@@ -773,6 +774,7 @@
point_cloud.points[i].x = gp.x();
point_cloud.points[i].y = gp.y();
+ point_cloud.points[i].z = gp.z();
}
ros::Node::instance()->publish("gui_laser", point_cloud);
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-24 00:47:16 UTC (rev 14379)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-24 00:48:32 UTC (rev 14380)
@@ -53,7 +53,6 @@
#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/Point.h>
-#include <robot_msgs/Polyline2D.h>
#include <laser_scan/LaserScan.h>
#include <tf/message_notifier.h>
Modified: pkg/trunk/nav/bas...
[truncated message content] |
|
From: <ei...@us...> - 2009-04-24 01:54:03
|
Revision: 14385
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14385&view=rev
Author: eitanme
Date: 2009-04-24 01:46:02 +0000 (Fri, 24 Apr 2009)
Log Message:
-----------
Make sure transforms are available before starting any navstack component. Also, use sim time in simulator launch files
Modified Paths:
--------------
pkg/trunk/highlevel/test_nav/move_base/move_base.launch
pkg/trunk/highlevel/test_nav/move_base_local/move_base_local.launch
pkg/trunk/highlevel/test_nav/move_base_maze/move_base_maze.launch
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
Modified: pkg/trunk/highlevel/test_nav/move_base/move_base.launch
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base/move_base.launch 2009-04-24 01:33:28 UTC (rev 14384)
+++ pkg/trunk/highlevel/test_nav/move_base/move_base.launch 2009-04-24 01:46:02 UTC (rev 14385)
@@ -1,6 +1,7 @@
<launch>
<master auto="start"/>
<group name="wg">
+ <param name="/use_sim_time" value="true"/>
<node pkg="nav" type="move_base" respawn="false" name="move_base_node" output="screen">
<remap from="/move_base_node/activate" to="/goal" />
<remap from="~base_local_planner/global_plan" to="/gui_path" />
Modified: pkg/trunk/highlevel/test_nav/move_base_local/move_base_local.launch
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base_local/move_base_local.launch 2009-04-24 01:33:28 UTC (rev 14384)
+++ pkg/trunk/highlevel/test_nav/move_base_local/move_base_local.launch 2009-04-24 01:46:02 UTC (rev 14385)
@@ -1,6 +1,7 @@
<launch>
<master auto="start"/>
<group name="wg">
+ <param name="/use_sim_time" value="true"/>
<node pkg="nav" type="move_base_local" respawn="false" name="local_controller_node" output="screen">
<remap from="/local_controller_node/activate" to="/goal" />
<remap from="~base_local_planner/global_plan" to="/gui_path" />
Modified: pkg/trunk/highlevel/test_nav/move_base_maze/move_base_maze.launch
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base_maze/move_base_maze.launch 2009-04-24 01:33:28 UTC (rev 14384)
+++ pkg/trunk/highlevel/test_nav/move_base_maze/move_base_maze.launch 2009-04-24 01:46:02 UTC (rev 14385)
@@ -1,6 +1,7 @@
<launch>
<master auto="start"/>
<group name="wg">
+ <param name="/use_sim_time" value="true"/>
<node pkg="nav" type="move_base" respawn="false" name="move_base_node" output="screen">
<remap from="/move_base_node/activate" to="/goal" />
<remap from="~base_local_planner/global_plan" to="/gui_path" />
Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-24 01:33:28 UTC (rev 14384)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-24 01:46:02 UTC (rev 14385)
@@ -46,6 +46,11 @@
ros_node_.param("~/navfn/costmap/global_frame", global_frame_, std::string("map"));
ros_node_.param("~/navfn/costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
ros_node_.param("~/navfn/transform_tolerance", transform_tolerance_, 0.2);
+
+ //we need to make sure that the transform between the robot base frame and the global frame is available
+ while(!tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(5.0))){
+ ROS_WARN("Waiting on transform from %s to %s to become available before running the planner", robot_base_frame_.c_str(), global_frame_.c_str());
+ }
//we'll get the parameters for the robot radius from the costmap we're associated with
ros_node_.param("~navfn/costmap/inscribed_radius", inscribed_radius_, 0.325);
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-24 01:33:28 UTC (rev 14384)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-24 01:46:02 UTC (rev 14385)
@@ -67,6 +67,11 @@
ros_node.param("~base_local_planner/costmap/global_frame", global_frame_, string("map"));
ros_node.param("~base_local_planner/costmap/robot_base_frame", robot_base_frame_, string("base_link"));
+ //we need to make sure that the transform between the robot base frame and the global frame is available
+ while(!tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(5.0))){
+ ROS_WARN("Waiting on transform from %s to %s to become available before running the controller", robot_base_frame_.c_str(), global_frame_.c_str());
+ }
+
ros_node.param("~base_local_planner/yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
ros_node.param("~base_local_planner/xy_goal_tolerance", xy_goal_tolerance_, 0.10);
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-24 01:33:28 UTC (rev 14384)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-24 01:46:02 UTC (rev 14385)
@@ -56,6 +56,11 @@
ros_node_.param("~" + prefix + "/costmap/global_frame", global_frame_, string("map"));
ros_node_.param("~" + prefix + "/costmap/robot_base_frame", robot_base_frame_, string("base_link"));
+ //we need to make sure that the transform between the robot base frame and the global frame is available
+ while(!tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(5.0))){
+ ROS_ERROR("Waiting on transform from %s to %s to become available before running costmap", robot_base_frame_.c_str(), global_frame_.c_str());
+ }
+
ros_node_.param("~" + prefix + "/costmap/transform_tolerance", transform_tolerance_, 0.2);
//now we need to split the topics based on whitespace which we can use a stringstream for
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-24 20:19:33
|
Revision: 14414
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14414&view=rev
Author: tfoote
Date: 2009-04-24 20:19:23 +0000 (Fri, 24 Apr 2009)
Log Message:
-----------
deprecating robot_kinematics
Modified Paths:
--------------
pkg/trunk/deprecated/robot_kinematics/manifest.xml
Added Paths:
-----------
pkg/trunk/deprecated/robot_kinematics/
Removed Paths:
-------------
pkg/trunk/util/kinematics/robot_kinematics/
Property changes on: pkg/trunk/deprecated/robot_kinematics
___________________________________________________________________
Added: svn:mergeinfo
+
Modified: pkg/trunk/deprecated/robot_kinematics/manifest.xml
===================================================================
--- pkg/trunk/util/kinematics/robot_kinematics/manifest.xml 2009-04-24 20:01:54 UTC (rev 14413)
+++ pkg/trunk/deprecated/robot_kinematics/manifest.xml 2009-04-24 20:19:23 UTC (rev 14414)
@@ -3,7 +3,7 @@
</description>
<author>Sachin Chitta sa...@wi..., Advait Jain aj...@wi...</author>
<license>BSD</license>
-<review status="unreviewed" notes=""/>
+<review status="deprecated" notes=""/>
<url>http://pr.willowgarage.com</url>
<depend package="wg_robot_description_parser"/>
<depend package="libTF"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-24 20:25:50
|
Revision: 14415
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14415&view=rev
Author: tfoote
Date: 2009-04-24 20:25:48 +0000 (Fri, 24 Apr 2009)
Log Message:
-----------
moving newmat into deprecated for that's where it should be
Added Paths:
-----------
pkg/trunk/deprecated/newmat10/
Removed Paths:
-------------
pkg/trunk/3rdparty/newmat10/
Property changes on: pkg/trunk/deprecated/newmat10
___________________________________________________________________
Added: svn:ignore
+ newmat
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-24 20:27:42
|
Revision: 14416
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14416&view=rev
Author: tfoote
Date: 2009-04-24 20:27:41 +0000 (Fri, 24 Apr 2009)
Log Message:
-----------
tf_conversions should not be in prcore for it has heavy weight external dependencies like kdl and eigen
Added Paths:
-----------
pkg/trunk/util/tf_conversions/
Removed Paths:
-------------
pkg/trunk/prcore/tf_conversions/
Property changes on: pkg/trunk/util/tf_conversions
___________________________________________________________________
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tpr...@us...> - 2009-04-24 23:04:49
|
Revision: 14422
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14422&view=rev
Author: tpratkanis
Date: 2009-04-24 23:04:40 +0000 (Fri, 24 Apr 2009)
Log Message:
-----------
Huge change to robot_actions. See ros users.
Modified Paths:
--------------
pkg/trunk/deprecated/highlevel_controllers/include/highlevel_controllers/move_base.hh
pkg/trunk/deprecated/highlevel_controllers/manifest.xml
pkg/trunk/deprecated/highlevel_controllers/src/action_container.cpp
pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp
pkg/trunk/deprecated/highlevel_controllers/src/recharge_controller.cpp
pkg/trunk/highlevel/doors/doors_core/include/doors_core/door_reactive_planner.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h
pkg/trunk/highlevel/doors/doors_core/manifest.xml
pkg/trunk/highlevel/doors/doors_core/src/action_runner.cpp
pkg/trunk/highlevel/doors/doors_core/src/door_reactive_planner.cpp
pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
pkg/trunk/highlevel/doors/doors_core/test/trigger_doors_detector.cpp
pkg/trunk/highlevel/doors/doors_core/test/trigger_doors_opener.cpp
pkg/trunk/highlevel/doors/doors_core/test/trigger_handle_release.cpp
pkg/trunk/highlevel/executive_python/manifest.xml
pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
pkg/trunk/highlevel/executive_python/src/recharge_adapter.py
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/executive_trex_pr2/adapter_utilities.h
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/executive_trex_pr2/adapters.h
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/adapters.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc
pkg/trunk/highlevel/nav/include/nav/move_base.h
pkg/trunk/highlevel/nav/include/nav/move_base_local.h
pkg/trunk/highlevel/nav/manifest.xml
pkg/trunk/highlevel/nav/src/move_base.cpp
pkg/trunk/highlevel/nav/src/move_base_local.cpp
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_detect_outlet_fine.h
pkg/trunk/highlevel/plugs/plugs_core/manifest.xml
pkg/trunk/highlevel/plugs/plugs_core/src/action_runner.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_executive.cpp
pkg/trunk/highlevel/robot_actions/CMakeLists.txt
pkg/trunk/highlevel/robot_actions/manifest.xml
pkg/trunk/highlevel/safety/safety_core/manifest.xml
pkg/trunk/highlevel/safety/safety_core/src/action_runner.cpp
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/action_mechanism_control.h
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/mechanism/mechanism_control/src/action_mechanism_control.cpp
pkg/trunk/mechanism/mechanism_control/src/action_runner.cpp
pkg/trunk/mechanism/mechanism_control/test/action_runner_test.cpp
pkg/trunk/nav/people_aware_nav/src/lanes.lisp
pkg/trunk/nav/visual_nav/manifest.xml
pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp
pkg/trunk/nav/wavefront/manifest.xml
pkg/trunk/nav/wavefront/src/cli.cpp
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
pkg/trunk/vision/outlet_detection/manifest.xml
pkg/trunk/vision/outlet_detection/src/outlet_spotting.cpp
pkg/trunk/vision/people/manifest.xml
pkg/trunk/vision/people/src/people_follower.cpp
pkg/trunk/vision/people/src/people_follower.h
Added Paths:
-----------
pkg/trunk/highlevel/pr2_robot_actions/
pkg/trunk/highlevel/pr2_robot_actions/CMakeLists.txt
pkg/trunk/highlevel/pr2_robot_actions/Makefile
pkg/trunk/highlevel/pr2_robot_actions/manifest.xml
pkg/trunk/highlevel/pr2_robot_actions/msg/
pkg/trunk/highlevel/pr2_robot_actions/msg/CheckPathState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/DetectOutletState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/DetectPlugOnBaseState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/DoorActionState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/LocalizePlugInGripperState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/MoveAndGraspPlugState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseDoorState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseStateNew.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/NoArgumentsActionState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/NotifyDoorBlockedState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/Pose2D.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/RechargeState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/ShellCommandState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/StowPlugState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/SwitchControllers.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/SwitchControllersState.msg
pkg/trunk/highlevel/robot_actions/msg/
pkg/trunk/highlevel/test_robot_actions/
pkg/trunk/highlevel/test_robot_actions/CMakeLists.txt
pkg/trunk/highlevel/test_robot_actions/Makefile
pkg/trunk/highlevel/test_robot_actions/manifest.xml
pkg/trunk/highlevel/test_robot_actions/msg/
pkg/trunk/highlevel/test_robot_actions/msg/TestState.msg
pkg/trunk/highlevel/test_robot_actions/test/
pkg/trunk/highlevel/test_robot_actions/test/test.xml
pkg/trunk/highlevel/test_robot_actions/test/utest.cc
Removed Paths:
-------------
pkg/trunk/highlevel/pr2_robot_actions/msg/ActionStatus.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/CheckPathState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/DetectOutletState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/DetectPlugOnBaseState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/DoorActionState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/LocalizePlugInGripperState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/MoveAndGraspPlugState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseDoorState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseStateNew.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/NoArgumentsActionState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/NotifyDoorBlockedState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/Pose2D.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/RechargeState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/ShellCommandState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/StowPlugState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/SwitchControllers.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/SwitchControllersState.msg
pkg/trunk/highlevel/robot_actions/msg/
pkg/trunk/highlevel/robot_actions/test/
pkg/trunk/highlevel/test_robot_actions/test/test.xml
pkg/trunk/highlevel/test_robot_actions/test/utest.cc
Modified: pkg/trunk/deprecated/highlevel_controllers/include/highlevel_controllers/move_base.hh
===================================================================
--- pkg/trunk/deprecated/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-04-24 22:37:34 UTC (rev 14421)
+++ pkg/trunk/deprecated/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-04-24 23:04:40 UTC (rev 14422)
@@ -51,8 +51,8 @@
#include <mpglue/plan.h>
// Message structures used
-#include <robot_actions/MoveBaseState.h>
-#include <robot_actions/Pose2D.h>
+#include <pr2_robot_actions/MoveBaseState.h>
+#include <pr2_robot_actions/Pose2D.h>
#include <robot_msgs/PoseDot.h>
#include <laser_scan/LaserScan.h>
#include <deprecated_msgs/RobotBase2DOdom.h>
@@ -82,7 +82,7 @@
namespace ros {
namespace highlevel_controllers {
- class MoveBase : public HighlevelController<robot_actions::MoveBaseState, robot_actions::Pose2D> {
+ class MoveBase : public HighlevelController<pr2_robot_actions::MoveBaseState, pr2_robot_actions::Pose2D> {
public:
Modified: pkg/trunk/deprecated/highlevel_controllers/manifest.xml
===================================================================
--- pkg/trunk/deprecated/highlevel_controllers/manifest.xml 2009-04-24 22:37:34 UTC (rev 14421)
+++ pkg/trunk/deprecated/highlevel_controllers/manifest.xml 2009-04-24 23:04:40 UTC (rev 14422)
@@ -19,6 +19,7 @@
<depend package="robot_msgs"/>
<depend package="robot_srvs"/>
<depend package="robot_actions"/>
+ <depend package="pr2_robot_actions"/>
<depend package="pr2_mechanism_controllers"/>
<depend package="pr2_msgs"/>
<depend package="trajectory_rollout"/>
Modified: pkg/trunk/deprecated/highlevel_controllers/src/action_container.cpp
===================================================================
--- pkg/trunk/deprecated/highlevel_controllers/src/action_container.cpp 2009-04-24 22:37:34 UTC (rev 14421)
+++ pkg/trunk/deprecated/highlevel_controllers/src/action_container.cpp 2009-04-24 23:04:40 UTC (rev 14422)
@@ -36,7 +36,7 @@
#include <robot_actions/action_runner.h>
#include <robot_actions/action.h>
-#include <robot_actions/ShellCommandState.h>
+#include <pr2_robot_actions/ShellCommandState.h>
#include <std_msgs/String.h>
#include <cstdlib>
@@ -83,7 +83,7 @@
robot_actions::ActionRunner runner(10.0);
highlevel_controllers::ShellCommand shell_command;
- runner.connect<std_msgs::String, robot_actions::ShellCommandState, std_msgs::String>(shell_command);
+ runner.connect<std_msgs::String, pr2_robot_actions::ShellCommandState, std_msgs::String>(shell_command);
runner.run();
Modified: pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp 2009-04-24 22:37:34 UTC (rev 14421)
+++ pkg/trunk/deprecated/highlevel_controllers/src/move_base.cpp 2009-04-24 23:04:40 UTC (rev 14422)
@@ -128,7 +128,7 @@
namespace highlevel_controllers {
MoveBase::MoveBase()
- : HighlevelController<robot_actions::MoveBaseState, robot_actions::Pose2D>("move_base", "feedback", "activate"),
+ : HighlevelController<pr2_robot_actions::MoveBaseState, pr2_robot_actions::Pose2D>("move_base", "feedback", "activate"),
tf_(*ros::Node::instance(), true, ros::Duration(10)), // cache for 10 sec, no extrapolation
controller_(NULL),
costMap_(NULL),
Modified: pkg/trunk/deprecated/highlevel_controllers/src/recharge_controller.cpp
===================================================================
--- pkg/trunk/deprecated/highlevel_controllers/src/recharge_controller.cpp 2009-04-24 22:37:34 UTC (rev 14421)
+++ pkg/trunk/deprecated/highlevel_controllers/src/recharge_controller.cpp 2009-04-24 23:04:40 UTC (rev 14422)
@@ -66,7 +66,7 @@
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
-#include <robot_actions/RechargeState.h>
+#include <pr2_robot_actions/RechargeState.h>
#include <robot_msgs/BatteryState.h>
#include <cstdlib>
@@ -268,7 +268,7 @@
highlevel_controllers::RechargeController recharge_controller("rechargeController");
- runner.connect<std_msgs::Float32, robot_actions::RechargeState, std_msgs::Float32>(recharge_controller);
+ runner.connect<std_msgs::Float32, pr2_robot_actions::RechargeState, std_msgs::Float32>(recharge_controller);
runner.run();
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/door_reactive_planner.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/door_reactive_planner.h 2009-04-24 22:37:34 UTC (rev 14421)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/door_reactive_planner.h 2009-04-24 23:04:40 UTC (rev 14422)
@@ -37,7 +37,7 @@
#include <ros/node.h>
//messages
-#include <robot_actions/Pose2D.h>
+#include <pr2_robot_actions/Pose2D.h>
#include <robot_msgs/Vector3.h>
#include <robot_msgs/Point.h>
@@ -82,7 +82,7 @@
* @param start The position from which the robot is starting
* @param best_path The best path returned by the planner (note this could be a zero length path if no plan is found
*/
- bool makePlan(const robot_actions::Pose2D &start, std::vector<robot_actions::Pose2D> &best_path);
+ bool makePlan(const pr2_robot_actions::Pose2D &start, std::vector<pr2_robot_actions::Pose2D> &best_path);
/**
* @brief Set door information for the planner
@@ -96,7 +96,7 @@
* @param footprint_spec The footprint of the robot
* @param oriented_footprint The oriented footprint of the robot
*/
- bool computeOrientedFootprint(const robot_actions::Pose2D &position, const std::vector<robot_msgs::Point>& footprint_spec, std::vector<robot_msgs::Point>& oriented_footprint);
+ bool computeOrientedFootprint(const pr2_robot_actions::Pose2D &position, const std::vector<robot_msgs::Point>& footprint_spec, std::vector<robot_msgs::Point>& oriented_footprint);
std::vector<robot_msgs::Point> footprint_; /**< The footprint of the robot */
@@ -104,7 +104,7 @@
* @brief Get the goal position from the planner
* @return The goal position for the planner
*/
- bool getGoal(robot_actions::Pose2D &goal);
+ bool getGoal(pr2_robot_actions::Pose2D &goal);
private:
@@ -152,7 +152,7 @@
double centerline_angle_; /**< Angle that the normal to the door makes in the path_frame when the door is closed */
- robot_actions::Pose2D goal_; /**< Goal position on the other side of the doorway */
+ pr2_robot_actions::Pose2D goal_; /**< Goal position on the other side of the doorway */
bool door_information_set_ ; /**< Has door information been set before invoking the planner */
@@ -163,7 +163,7 @@
* @param p Position of the robot
* @param q Position of the robot
*/
- double distance(const robot_actions::Pose2D &p, const robot_actions::Pose2D &q);
+ double distance(const pr2_robot_actions::Pose2D &p, const pr2_robot_actions::Pose2D &q);
/**
* @brief Translational projected distance between two positions
@@ -171,7 +171,7 @@
* @param q Position of the robot
* @param angle angle of the vector to project onto of the robot
*/
- double projectedDistance(const robot_actions::Pose2D &p, const robot_actions::Pose2D &q, const double &angle);
+ double projectedDistance(const pr2_robot_actions::Pose2D &p, const pr2_robot_actions::Pose2D &q, const double &angle);
/**
* @brief Create a linear path between two positions
@@ -179,7 +179,7 @@
* @param fp Final position
* @param return_path The returned path
*/
- bool createLinearPath(const robot_actions::Pose2D &cp,const robot_actions::Pose2D &fp, std::vector<robot_actions::Pose2D> &return_path);
+ bool createLinearPath(const pr2_robot_actions::Pose2D &cp,const pr2_robot_actions::Pose2D &fp, std::vector<pr2_robot_actions::Pose2D> &return_path);
/**
* @brief Get the final position corresponding to motion along a straight line at angle (delta_angle) from the centerline_angle. The final position
@@ -189,7 +189,7 @@
* @param distance_to_centerline shortest distance from the current position to the centerline of the doorway
* @param end_position final position returned by this function
*/
- void getFinalPosition(const robot_actions::Pose2D ¤t_position, const double &delta_angle, const double &distance_to_centerline, robot_actions::Pose2D &end_position);
+ void getFinalPosition(const pr2_robot_actions::Pose2D ¤t_position, const double &delta_angle, const double &distance_to_centerline, pr2_robot_actions::Pose2D &end_position);
/**
* @brief The cost of a point in the cost map
@@ -206,7 +206,7 @@
* @param return_path The checked path to be returned
* @param costmap_frame_id The frame in which the map is specified
*/
- void checkPath(const std::vector<robot_actions::Pose2D> &path, const std::string &path_frame_id, std::vector<robot_actions::Pose2D> &return_path, std::string &costmap_frame_id);
+ void checkPath(const std::vector<pr2_robot_actions::Pose2D> &path, const std::string &path_frame_id, std::vector<pr2_robot_actions::Pose2D> &return_path, std::string &costmap_frame_id);
/**
* @brief Transform a path from one frame to another frame
@@ -215,7 +215,7 @@
* @param path_out The transformed path
* @param frame_in The frame in which the output path is specified
*/
- void transformPath(const std::vector<robot_actions::Pose2D> &path_in, const std::string &frame_in, std::vector<robot_actions::Pose2D> &path_out, const std::string &frame_out);
+ void transformPath(const std::vector<pr2_robot_actions::Pose2D> &path_in, const std::string &frame_in, std::vector<pr2_robot_actions::Pose2D> &path_out, const std::string &frame_out);
/**
* @brief Transform a 2D point from one frame to another
@@ -224,7 +224,7 @@
* @param path_out The transformed point
* @param frame_in The frame in which the output point is specified
*/
- void transform2DPose(const robot_actions::Pose2D &point_in, const std::string original_frame_id, robot_actions::Pose2D &point_out, const std::string &transform_frame_id);
+ void transform2DPose(const pr2_robot_actions::Pose2D &point_in, const std::string original_frame_id, pr2_robot_actions::Pose2D &point_out, const std::string &transform_frame_id);
};
}
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h 2009-04-24 22:37:34 UTC (rev 14421)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h 2009-04-24 23:04:40 UTC (rev 14422)
@@ -41,8 +41,8 @@
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
-#include <robot_actions/MoveBaseState.h>
-#include <robot_actions/Pose2D.h>
+#include <pr2_robot_actions/MoveBaseState.h>
+#include <pr2_robot_actions/Pose2D.h>
#include <robot_msgs/JointTraj.h>
#include <ros/node.h>
@@ -62,7 +62,7 @@
* @class MoveBaseDoorAction
* @brief A class adhering to the robot_actions::Action interface that moves the robot base to a goal location.
*/
- class MoveBaseDoorAction : public robot_actions::Action<robot_msgs::Door, robot_actions::Pose2D> {
+ class MoveBaseDoorAction : public robot_actions::Action<robot_msgs::Door, pr2_robot_actions::Pose2D> {
public:
/**
* @brief Constructor for the actions
@@ -83,7 +83,7 @@
* @param feedback Feedback that the action gives to a higher-level monitor, in this case, the position of the robot
* @return The result of the execution, ie: Success, Preempted, Aborted, etc.
*/
- virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_actions::Pose2D& feedback);
+ virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, pr2_robot_actions::Pose2D& feedback);
private:
/**
@@ -102,7 +102,7 @@
/**
* @brief Publish a path for visualization purposes
*/
- void publishPath(const std::vector<robot_actions::Pose2D>& path, std::string topic, double r, double g, double b, double a);
+ void publishPath(const std::vector<pr2_robot_actions::Pose2D>& path, std::string topic, double r, double g, double b, double a);
/**
* @brief Make a new global plan (the goal is specified using the setDoor() function while start is specified using the
@@ -165,35 +165,35 @@
costmap_2d::Costmap2D planner_cost_map_;
door_reactive_planner::DoorReactivePlanner* planner_;
- std::vector<robot_actions::Pose2D> global_plan_;
+ std::vector<pr2_robot_actions::Pose2D> global_plan_;
std::vector<robot_msgs::Point> footprint_;
std::string global_frame_, control_frame_, robot_base_frame_;
bool valid_plan_;
boost::recursive_mutex lock_;
robot_msgs::Door door_;
- robot_actions::Pose2D goal_;
+ pr2_robot_actions::Pose2D goal_;
tf::Stamped<tf::Pose> global_pose_;
double xy_goal_tolerance_, yaw_goal_tolerance_, min_abs_theta_vel_;
double inscribed_radius_, circumscribed_radius_, inflation_radius_;
double controller_frequency_;
- std::vector<robot_actions::Pose2D> empty_plan_;
+ std::vector<pr2_robot_actions::Pose2D> empty_plan_;
std::string control_topic_name_;
/**
- * @brief Transform a tf::Stamped<tf::Pose> to robot_actions::Pose2D
+ * @brief Transform a tf::Stamped<tf::Pose> to pr2_robot_actions::Pose2D
* @param pose The pose object to be transformed
- * @return Transformed object of type robot_actions::Pose2D
+ * @return Transformed object of type pr2_robot_actions::Pose2D
*/
- robot_actions::Pose2D getPose2D(const tf::Stamped<tf::Pose> &pose);
+ pr2_robot_actions::Pose2D getPose2D(const tf::Stamped<tf::Pose> &pose);
/**
* @brief dispatch control commands
* @param plan_in The plan to be dispatched
*/
- void dispatchControl(const std::vector<robot_actions::Pose2D> &plan_in);
+ void dispatchControl(const std::vector<pr2_robot_actions::Pose2D> &plan_in);
};
};
#endif
Modified: pkg/trunk/highlevel/doors/doors_core/manifest.xml
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/manifest.xml 2009-04-24 22:37:34 UTC (rev 14421)
+++ pkg/trunk/highlevel/doors/doors_core/manifest.xml 2009-04-24 23:04:40 UTC (rev 14422)
@@ -13,6 +13,7 @@
<depend package="robot_msgs" />
<depend package="robot_srvs" />
<depend package="robot_actions" />
+ <depend package="pr2_robot_actions" />
<depend package="kdl" />
<depend package="angles" />
<depend package="door_handle_detector" />
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_runner.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_runner.cpp 2009-04-24 22:37:34 UTC (rev 14421)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_runner.cpp 2009-04-24 23:04:40 UTC (rev 14422)
@@ -42,7 +42,7 @@
#include "doors_core/action_release_handle.h"
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
-#include <robot_actions/DoorActionState.h>
+#include <pr2_robot_actions/DoorActionState.h>
using namespace door_handle_detector;
@@ -66,12 +66,12 @@
ReleaseHandleAction release(node);
robot_actions::ActionRunner runner(10.0);
- runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(detect_door);
- runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(detect_handle);
- runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(grasp);
- runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(unlatch);
- runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(open);
- runner.connect<robot_msgs::Door, robot_actions::DoorActionState, robot_msgs::Door>(release);
+ runner.connect<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door>(detect_door);
+ runner.connect<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door>(detect_handle);
+ runner.connect<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door>(grasp);
+ runner.connect<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door>(unlatch);
+ runner.connect<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door>(open);
+ runner.connect<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door>(release);
runner.run();
node.spin();
Modified: pkg/trunk/highlevel/doors/doors_core/src/door_reactive_planner.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/door_reactive_planner.cpp 2009-04-24 22:37:34 UTC (rev 14421)
+++ pkg/trunk/highlevel/doors/doors_core/src/door_reactive_planner.cpp 2009-04-24 23:04:40 UTC (rev 14422)
@@ -114,7 +114,7 @@
door_information_set_ = true;
}
-bool DoorReactivePlanner::getGoal(robot_actions::Pose2D &goal)
+bool DoorReactivePlanner::getGoal(pr2_robot_actions::Pose2D &goal)
{
if(!door_information_set_)
return false;
@@ -122,7 +122,7 @@
return true;
}
-bool DoorReactivePlanner::computeOrientedFootprint(const robot_actions::Pose2D &position, const std::vector<robot_msgs::Point>& footprint_spec, std::vector<robot_msgs::Point>& oriented_footprint)
+bool DoorReactivePlanner::computeOrientedFootprint(const pr2_robot_actions::Pose2D &position, const std::vector<robot_msgs::Point>& footprint_spec, std::vector<robot_msgs::Point>& oriented_footprint)
{
if(footprint_spec.size() < 3)//if we have no footprint... do nothing
{
@@ -141,20 +141,20 @@
return true;
}
-double DoorReactivePlanner::distance(const robot_actions::Pose2D &p, const robot_actions::Pose2D &q)
+double DoorReactivePlanner::distance(const pr2_robot_actions::Pose2D &p, const pr2_robot_actions::Pose2D &q)
{
return sqrt(pow(p.x-q.x,2)+pow(p.y-q.y,2));
}
-double DoorReactivePlanner::projectedDistance(const robot_actions::Pose2D &p, const robot_actions::Pose2D &q, const double &angle)
+double DoorReactivePlanner::projectedDistance(const pr2_robot_actions::Pose2D &p, const pr2_robot_actions::Pose2D &q, const double &angle)
{
return ((q.x-p.x)*cos(angle)+(q.y-p.y)*sin(angle));
}
-bool DoorReactivePlanner::createLinearPath(const robot_actions::Pose2D &cp,const robot_actions::Pose2D &fp, std::vector<robot_actions::Pose2D> &return_path)
+bool DoorReactivePlanner::createLinearPath(const pr2_robot_actions::Pose2D &cp,const pr2_robot_actions::Pose2D &fp, std::vector<pr2_robot_actions::Pose2D> &return_path)
{
ROS_DEBUG("Creating trajectory from: (%f,%f) to (%f,%f)",cp.x,cp.y,fp.x,fp.y);
- robot_actions::Pose2D temp;
+ pr2_robot_actions::Pose2D temp;
double dist_trans = distance(cp,fp);
double dist_rot = fabs(angles::normalize_angle(cp.th-fp.th));
@@ -182,7 +182,7 @@
return true;
}
-void DoorReactivePlanner::getFinalPosition(const robot_actions::Pose2D ¤t_position, const double &delta_angle, const double &distance_to_centerline, robot_actions::Pose2D &end_position)
+void DoorReactivePlanner::getFinalPosition(const pr2_robot_actions::Pose2D ¤t_position, const double &delta_angle, const double &distance_to_centerline, pr2_robot_actions::Pose2D &end_position)
{
double new_explore_distance;
double global_explore_angle;
@@ -215,17 +215,17 @@
}
-bool DoorReactivePlanner::makePlan(const robot_actions::Pose2D &start, std::vector<robot_actions::Pose2D> &best_path_control_frame)
+bool DoorReactivePlanner::makePlan(const pr2_robot_actions::Pose2D &start, std::vector<pr2_robot_actions::Pose2D> &best_path_control_frame)
{
if(!door_information_set_)
{
ROS_ERROR("Door information not set");
return false;
}
- std::vector<robot_actions::Pose2D> best_path_costmap_frame;
- robot_actions::Pose2D end_position;
- std::vector<robot_actions::Pose2D> checked_path;
- std::vector<robot_actions::Pose2D> linear_path;
+ std::vector<pr2_robot_actions::Pose2D> best_path_costmap_frame;
+ pr2_robot_actions::Pose2D end_position;
+ std::vector<pr2_robot_actions::Pose2D> checked_path;
+ std::vector<pr2_robot_actions::Pose2D> linear_path;
double min_distance_to_goal(FLT_MAX);
@@ -291,7 +291,7 @@
return true;
}
-void DoorReactivePlanner::checkPath(const std::vector<robot_actions::Pose2D> &path, const std::string &control_frame_id, std::vector<robot_actions::Pose2D> &return_path, std::string &costmap_frame_id)
+void DoorReactivePlanner::checkPath(const std::vector<pr2_robot_actions::Pose2D> &path, const std::string &control_frame_id, std::vector<pr2_robot_actions::Pose2D> &return_path, std::string &costmap_frame_id)
{
int index(0);
double theta;
@@ -305,7 +305,7 @@
{
index = i;
std::vector<robot_msgs::Point> oriented_footprint;
- robot_actions::Pose2D out_pose;
+ pr2_robot_actions::Pose2D out_pose;
transform2DPose(path[i],control_frame_id, out_pose, costmap_frame_id);
computeOrientedFootprint(out_pose, footprint_, oriented_footprint);
@@ -355,7 +355,7 @@
return true;
}
-void DoorReactivePlanner::transformPath(const std::vector<robot_actions::Pose2D> &path_in, const std::string &frame_in, std::vector<robot_actions::Pose2D> &path_out, const std::string &frame_out)
+void DoorReactivePlanner::transformPath(const std::vector<pr2_robot_actions::Pose2D> &path_in, const std::string &frame_in, std::vector<pr2_robot_actions::Pose2D> &path_out, const std::string &frame_out)
{
path_out.resize((int) path_in.size());
for(int i=0; i < (int) path_out.size(); i++)
@@ -364,7 +364,7 @@
}
}
-void DoorReactivePlanner::transform2DPose(const robot_actions::Pose2D &point_in, const std::string original_frame_id, robot_actions::Pose2D &point_out, const std::string &transform_frame_id)
+void DoorReactivePlanner::transform2DPose(const pr2_robot_actions::Pose2D &point_in, const std::string original_frame_id, pr2_robot_actions::Pose2D &point_out, const std::string &transform_frame_id)
{
btQuaternion qt;
tf::Stamped<tf::Pose> pose;
Modified: pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-04-24 22:37:34 UTC (rev 14421)
+++ pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-04-24 23:04:40 UTC (rev 14422)
@@ -36,7 +36,7 @@
*********************************************************************/
#include <doors_core/move_base_door_action.h>
#include <doors_core/door_reactive_planner.h>
-#include <robot_actions/MoveBaseDoorState.h>
+#include <pr2_robot_actions/MoveBaseDoorState.h>
using namespace base_local_planner;
@@ -46,7 +46,7 @@
namespace nav
{
MoveBaseDoorAction::MoveBaseDoorAction(ros::Node& ros_node,...
[truncated message content] |
|
From: <tf...@us...> - 2009-04-27 20:25:00
|
Revision: 14476
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14476&view=rev
Author: tfoote
Date: 2009-04-27 20:24:56 +0000 (Mon, 27 Apr 2009)
Log Message:
-----------
preemptive blacklisting before blowing away highlevel_controllers
Added Paths:
-----------
pkg/trunk/demos/tabletop_manipulation/ROS_BUILD_BLACKLIST
pkg/trunk/deprecated/old_2dnav_pr2/ROS_BUILD_BLACKLIST
pkg/trunk/highlevel/test_executive_trex_pr2/ROS_BUILD_BLACKLIST
Added: pkg/trunk/demos/tabletop_manipulation/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/ROS_BUILD_BLACKLIST 2009-04-27 20:24:56 UTC (rev 14476)
@@ -0,0 +1 @@
+dependency highlevel controllers moved to graveyard
\ No newline at end of file
Added: pkg/trunk/deprecated/old_2dnav_pr2/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/deprecated/old_2dnav_pr2/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/deprecated/old_2dnav_pr2/ROS_BUILD_BLACKLIST 2009-04-27 20:24:56 UTC (rev 14476)
@@ -0,0 +1 @@
+dependency highlevel_controllers moved to graveyard
\ No newline at end of file
Added: pkg/trunk/highlevel/test_executive_trex_pr2/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/highlevel/test_executive_trex_pr2/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/highlevel/test_executive_trex_pr2/ROS_BUILD_BLACKLIST 2009-04-27 20:24:56 UTC (rev 14476)
@@ -0,0 +1 @@
+dependency highlevel_controllers moved to graveyard
\ No newline at end of file
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-27 20:43:22
|
Revision: 14479
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14479&view=rev
Author: tfoote
Date: 2009-04-27 20:43:13 +0000 (Mon, 27 Apr 2009)
Log Message:
-----------
supporessing error messages
Modified Paths:
--------------
pkg/trunk/deprecated/old_2dnav_pr2/manifest.xml
pkg/trunk/highlevel/test_executive_trex_pr2/manifest.xml
Modified: pkg/trunk/deprecated/old_2dnav_pr2/manifest.xml
===================================================================
--- pkg/trunk/deprecated/old_2dnav_pr2/manifest.xml 2009-04-27 20:32:37 UTC (rev 14478)
+++ pkg/trunk/deprecated/old_2dnav_pr2/manifest.xml 2009-04-27 20:43:13 UTC (rev 14479)
@@ -10,8 +10,8 @@
<depend package="robot_pose_ekf"/>
<depend package="map_server"/>
<depend package="executive_trex_pr2"/>
- <depend package="trajectory_rollout"/>
- <depend package="highlevel_controllers"/>
+ <!--depend package="trajectory_rollout"/>
+ <depend package="highlevel_controllers"/ THese need to be replaced commented to suppress warnings. See ticket:1234-->
<depend package="pr2_alpha"/>
<depend package="pr2_experimental_controllers"/>
<depend package="semantic_point_annotator"/>
Modified: pkg/trunk/highlevel/test_executive_trex_pr2/manifest.xml
===================================================================
--- pkg/trunk/highlevel/test_executive_trex_pr2/manifest.xml 2009-04-27 20:32:37 UTC (rev 14478)
+++ pkg/trunk/highlevel/test_executive_trex_pr2/manifest.xml 2009-04-27 20:43:13 UTC (rev 14479)
@@ -9,7 +9,7 @@
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com</url>
- <depend package="highlevel_controllers"/>
+ <!--depend package="highlevel_controllers"/ Removed see ticket:1235-->
<depend package="executive_trex_pr2"/>
<depend package="fake_localization"/>
<depend package="map_server"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-27 21:34:46
|
Revision: 14484
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14484&view=rev
Author: tfoote
Date: 2009-04-27 21:34:34 +0000 (Mon, 27 Apr 2009)
Log Message:
-----------
resolving ticket:937
Modified Paths:
--------------
pkg/trunk/deprecated/amcl_player/amcl_player.cc
pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp
pkg/trunk/nav/amcl/src/amcl_node.cpp
pkg/trunk/nav/map_server/manifest.xml
pkg/trunk/nav/map_server/src/image_loader.cpp
pkg/trunk/nav/map_server/src/main.cpp
pkg/trunk/nav/map_server/src/map_saver.cpp
pkg/trunk/nav/map_server/test/rtest.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
pkg/trunk/prcore/robot_msgs/msg/MapMetaData.msg
pkg/trunk/prcore/robot_srvs/srv/StaticMap.srv
pkg/trunk/visualization/rviz/src/rviz/displays/map_display.cpp
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp
pkg/trunk/world_models/topological_map/include/topological_map/ros_topological_map.h
Added Paths:
-----------
pkg/trunk/prcore/robot_msgs/msg/OccGrid.msg
Removed Paths:
-------------
pkg/trunk/prcore/robot_msgs/msg/OccMap2D.msg
Modified: pkg/trunk/deprecated/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/deprecated/amcl_player/amcl_player.cc 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/deprecated/amcl_player/amcl_player.cc 2009-04-27 21:34:34 UTC (rev 14484)
@@ -250,13 +250,13 @@
usleep(1000000);
}
printf("Received a %d X %d map @ %.3f m/pix\n",
- resp.map.width,
- resp.map.height,
- resp.map.resolution);
+ resp.map.info.width,
+ resp.map.info.height,
+ resp.map.info.resolution);
- this->sx = resp.map.width;
- this->sy = resp.map.height;
- this->resolution = resp.map.resolution;
+ this->sx = resp.map.info.width;
+ this->sy = resp.map.info.height;
+ this->resolution = resp.map.info.resolution;
// Convert to player format
this->mapdata = new char[this->sx*this->sy];
for(int i=0;i<this->sx*this->sy;i++)
Modified: pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -114,12 +114,12 @@
usleep(1000000);
}
printf("Received a %d X %d map @ %.3f m/pix\n",
- resp.map.width,
- resp.map.height,
- resp.map.resolution);
+ resp.map.info.width,
+ resp.map.info.height,
+ resp.map.info.resolution);
int sx, sy;
- sx = resp.map.width;
- sy = resp.map.height;
+ sx = resp.map.info.width;
+ sy = resp.map.info.height;
// Convert to player format
unsigned char* mapdata = new unsigned char[sx*sy];
for(int i=0;i<sx*sy;i++)
@@ -131,7 +131,7 @@
else
mapdata[i] = 0;
}
- costmap_.setStaticMap(sx, sy, resp.map.resolution, mapdata);
+ costmap_.setStaticMap(sx, sy, resp.map.info.resolution, mapdata);
delete[] mapdata;
tf_.setWithEulers("FRAMEID_LASER",
Modified: pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -264,12 +264,12 @@
}
ROS_DEBUG("Received a %d X %d map at %f m/pix\n",
- resp.map.width, resp.map.height, resp.map.resolution);
+ resp.map.info.width, resp.map.info.height, resp.map.info.resolution);
// We are treating cells with no information as lethal obstacles based on the input data. This is not ideal but
// our planner and controller do not reason about the no obstacle case
std::vector<unsigned char> inputData;
- unsigned int numCells = resp.map.width * resp.map.height;
+ unsigned int numCells = resp.map.info.width * resp.map.info.height;
for(unsigned int i = 0; i < numCells; i++){
inputData.push_back((unsigned char) resp.map.data[i]);
}
@@ -282,21 +282,26 @@
ros::Node::instance()->param("~costmap_2d/raytrace_range", rayTraceRange, 10.0);
ros::Node::instance()->param("~costmap_2d/obstacle_range", obstacleRange, 10.0);
- costMap_ = new CostMap2D((unsigned int)resp.map.width, (unsigned int)resp.map.height,
- inputData , resp.map.resolution,
+ costMap_ = new CostMap2D((unsigned int)resp.map.info.width, (unsigned int)resp.map.info.height,
+ inputData , resp.map.info.resolution,
lethalObstacleThreshold, maxZ_, zLB, zUB,
inflationRadius, circumscribedRadius, inscribedRadius, weight,
obstacleRange, rayTraceRange, raytraceWindow);
// set up costmap service response
- costmap_response_.map.width=resp.map.width;
- costmap_response_.map.height=resp.map.height;
- costmap_response_.map.resolution=resp.map.resolution;
- costmap_response_.map.origin.x = 0.0;
- costmap_response_.map.origin.y = 0.0;
- costmap_response_.map.origin.th = 0.0;
- costmap_response_.map.set_data_size(resp.map.width*resp.map.height);
+ costmap_response_.map.info.width=resp.map.info.width;
+ costmap_response_.map.info.height=resp.map.info.height;
+ costmap_response_.map.info.resolution=resp.map.info.resolution;
+ costmap_response_.map.info.origin.position.x = 0.0;
+ costmap_response_.map.info.origin.position.y = 0.0;
+ costmap_response_.map.info.origin.position.z = 0.0;
+ costmap_response_.map.info.origin.orientation.x = 0.0;
+ costmap_response_.map.info.origin.orientation.y = 0.0;
+ costmap_response_.map.info.origin.orientation.z = 0.0;
+ costmap_response_.map.info.origin.orientation.w = 1.0;
+ costmap_response_.map.set_data_size(resp.map.info.width*resp.map.info.height);
+
ros::Node::instance()->param("~costmap_2d/local_access_mapsize", local_access_mapsize_, 4.0);
global_map_accessor_ = new CostMapAccessor(*costMap_);
local_map_accessor_ = new CostMapAccessor(*costMap_, local_access_mapsize_, 0.0, 0.0);
@@ -699,7 +704,7 @@
{
const unsigned char* costmap = getCostMap().getMap();
res = costmap_response_;
- copy(costmap, costmap+res.map.width*res.map.height, res.map.data.begin());
+ copy(costmap, costmap+res.map.info.width*res.map.info.height, res.map.data.begin());
return true;
}
Modified: pkg/trunk/nav/amcl/src/amcl_node.cpp
===================================================================
--- pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -400,15 +400,15 @@
d.sleep();
}
ROS_INFO("Received a %d X %d map @ %.3f m/pix\n",
- resp.map.width,
- resp.map.height,
- resp.map.resolution);
+ resp.map.info.width,
+ resp.map.info.height,
+ resp.map.info.resolution);
- map->size_x = resp.map.width;
- map->size_y = resp.map.height;
- map->scale = resp.map.resolution;
- map->origin_x = resp.map.origin.x + (map->size_x / 2) * map->scale;
- map->origin_y = resp.map.origin.y + (map->size_y / 2) * map->scale;
+ map->size_x = resp.map.info.width;
+ map->size_y = resp.map.info.height;
+ map->scale = resp.map.info.resolution;
+ map->origin_x = resp.map.info.origin.position.x + (map->size_x / 2) * map->scale;
+ map->origin_y = resp.map.info.origin.position.y + (map->size_y / 2) * map->scale;
// Convert to player format
map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
ROS_ASSERT(map->cells);
Modified: pkg/trunk/nav/map_server/manifest.xml
===================================================================
--- pkg/trunk/nav/map_server/manifest.xml 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/map_server/manifest.xml 2009-04-27 21:34:34 UTC (rev 14484)
@@ -12,6 +12,7 @@
<depend package="roscpp"/>
<depend package="rospy" />
<depend package="robot_srvs"/>
+ <depend package="bullet"/>
<export>
<cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread`"/>
</export>
Modified: pkg/trunk/nav/map_server/src/image_loader.cpp
===================================================================
--- pkg/trunk/nav/map_server/src/image_loader.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/map_server/src/image_loader.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -74,17 +74,21 @@
}
// Copy the image data into the map structure
- resp->map.width = img->w;
- resp->map.height = img->h;
- resp->map.resolution = res;
+ resp->map.info.width = img->w;
+ resp->map.info.height = img->h;
+ resp->map.info.resolution = res;
/// @todo Make the map's origin configurable, probably from within the
/// comment section of the image file.
- resp->map.origin.x = 0.0;
- resp->map.origin.y = 0.0;
- resp->map.origin.th = 0.0;
+ resp->map.info.origin.position.x = 0.0;
+ resp->map.info.origin.position.y = 0.0;
+ resp->map.info.origin.position.z = 0.0;
+ resp->map.info.origin.orientation.x = 0.0;
+ resp->map.info.origin.orientation.y = 0.0;
+ resp->map.info.origin.orientation.z = 0.0;
+ resp->map.info.origin.orientation.w = 1.0;
// Allocate space to hold the data
- resp->map.set_data_size(resp->map.width * resp->map.height);
+ resp->map.set_data_size(resp->map.info.width * resp->map.info.height);
// Get values that we'll need to iterate through the pixels
rowstride = img->pitch;
@@ -92,9 +96,9 @@
// Copy pixel data into the map structure
pixels = (unsigned char*)(img->pixels);
- for(j = 0; j < resp->map.height; j++)
+ for(j = 0; j < resp->map.info.height; j++)
{
- for (i = 0; i < resp->map.width; i++)
+ for (i = 0; i < resp->map.info.width; i++)
{
// Compute mean of RGB for this pixel
p = pixels + j*rowstride + i*n_channels;
@@ -117,11 +121,11 @@
/// @todo Make the color thresholds configurable, probably from
/// within the comments section of the image file.
if(occ > 0.65)
- resp->map.data[MAP_IDX(resp->map.width,i,resp->map.height - j - 1)] = +100;
+ resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = +100;
else if(occ < 0.1)
- resp->map.data[MAP_IDX(resp->map.width,i,resp->map.height - j - 1)] = 0;
+ resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = 0;
else
- resp->map.data[MAP_IDX(resp->map.width,i,resp->map.height - j - 1)] = -1;
+ resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = -1;
}
}
Modified: pkg/trunk/nav/map_server/src/main.cpp
===================================================================
--- pkg/trunk/nav/map_server/src/main.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/map_server/src/main.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -146,15 +146,15 @@
printf("[map_server] loading map from image \"%s\"\n", fname);
map_server::loadMapFromFile(&ms.map_resp_,fname,res,negate);
printf("[map_server] read a %d X %d map @ %.3lf m/cell\n",
- ms.map_resp_.map.width,
- ms.map_resp_.map.height,
- ms.map_resp_.map.resolution);
-
+ ms.map_resp_.map.info.width,
+ ms.map_resp_.map.info.height,
+ ms.map_resp_.map.info.resolution);
+ ///\todo This could be optimzed regarding ticket:937
ms.meta_data_message_.map_load_time = ros::Time::now();
- ms.meta_data_message_.resolution = ms.map_resp_.map.resolution;
- ms.meta_data_message_.width = ms.map_resp_.map.width;
- ms.meta_data_message_.height = ms.map_resp_.map.height;
- ms.meta_data_message_.origin = ms.map_resp_.map.origin;
+ ms.meta_data_message_.resolution = ms.map_resp_.map.info.resolution;
+ ms.meta_data_message_.width = ms.map_resp_.map.info.width;
+ ms.meta_data_message_.height = ms.map_resp_.map.info.height;
+ ms.meta_data_message_.origin = ms.map_resp_.map.info.origin;
ms.advertiseService("static_map", &MapServer::mapCallback);
ms.advertise("map_metadata", ms.meta_data_message_, &MapServer::metadataSubscriptionCallback, 1);
Modified: pkg/trunk/nav/map_server/src/map_saver.cpp
===================================================================
--- pkg/trunk/nav/map_server/src/map_saver.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/map_server/src/map_saver.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -57,7 +57,7 @@
#include "ros/node.h"
#include "robot_srvs/StaticMap.h"
-
+#include "LinearMath/btMatrix3x3.h"
using namespace std;
/**
@@ -82,18 +82,18 @@
usleep(1000000);
}
printf("Received a %d X %d map @ %.3f m/pix\n",
- resp.map.width,
- resp.map.height,
- resp.map.resolution);
+ resp.map.info.width,
+ resp.map.info.height,
+ resp.map.info.resolution);
FILE* out = fopen("map.pgm", "w");
fprintf(out, "P5\n# CREATOR: Map_generator.cpp %.3f m/pix\n%d %d\n255\n",
- resp.map.resolution, resp.map.width, resp.map.height);
- for(unsigned int y = 0; y < resp.map.height; y++) {
- for(unsigned int x = 0; x < resp.map.width; x++) {
- unsigned int i = x + (resp.map.height - y - 1) * resp.map.width;
+ resp.map.info.resolution, resp.map.info.width, resp.map.info.height);
+ for(unsigned int y = 0; y < resp.map.info.height; y++) {
+ for(unsigned int x = 0; x < resp.map.info.width; x++) {
+ unsigned int i = x + (resp.map.info.height - y - 1) * resp.map.info.width;
if (resp.map.data[i] == 0) { //occ [0,0.1)
fputc(254, out);
} else if (resp.map.data[i] == +100) { //occ (0.65,1]
@@ -120,9 +120,13 @@
unknown: [129]
*/
+ robot_msgs::Quaternion & orientation = resp.map.info.origin.orientation;
+ btMatrix3x3 mat(btQuaternion(orientation.x, orientation.y, orientation.z, orientation.w));
+ double yaw, pitch, roll;
+ mat.getEulerZYX(yaw, pitch, roll);
fprintf(yaml, "resolution: %f\norigin: [%f, %f, %f]\n#\nvacant: [254]\noccupied: [0]\ninterpolate: 0\nunknown: [206]\n",
- resp.map.resolution, resp.map.origin.x, resp.map.origin.y, resp.map.origin.th);
+ resp.map.info.resolution, resp.map.info.origin.position.x, resp.map.info.origin.position.y, yaw);
fclose(yaml);
Modified: pkg/trunk/nav/map_server/test/rtest.cpp
===================================================================
--- pkg/trunk/nav/map_server/test/rtest.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/map_server/test/rtest.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -94,10 +94,10 @@
}
}
ASSERT_TRUE(call_result);
- ASSERT_FLOAT_EQ(resp.map.resolution, g_valid_image_res);
- ASSERT_EQ(resp.map.width, g_valid_image_width);
- ASSERT_EQ(resp.map.height, g_valid_image_height);
- for(unsigned int i=0; i < resp.map.width * resp.map.height; i++)
+ ASSERT_FLOAT_EQ(resp.map.info.resolution, g_valid_image_res);
+ ASSERT_EQ(resp.map.info.width, g_valid_image_width);
+ ASSERT_EQ(resp.map.info.height, g_valid_image_height);
+ for(unsigned int i=0; i < resp.map.info.width * resp.map.info.height; i++)
ASSERT_EQ(g_valid_image_content[i], resp.map.data[i]);
}
catch(...)
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -31,6 +31,8 @@
#include "ogre_tools/wx_ogre_render_window.h"
+#include "robot_srvs/StaticMap.h"
+
#include "ros/common.h"
#include "ros/node.h"
#include <tf/transform_listener.h>
@@ -207,15 +209,15 @@
return;
}
printf("Received a %d X %d map @ %.3f m/pix\n",
- resp.map.width,
- resp.map.height,
- resp.map.resolution);
+ resp.map.info.width,
+ resp.map.info.height,
+ resp.map.info.resolution);
- map_resolution_ = resp.map.resolution;
+ map_resolution_ = resp.map.info.resolution;
// Pad dimensions to power of 2
- map_width_ = resp.map.width;//(int)pow(2,ceil(log2(resp.map.width)));
- map_height_ = resp.map.height;//(int)pow(2,ceil(log2(resp.map.height)));
+ map_width_ = resp.map.info.width;//(int)pow(2,ceil(log2(resp.map.info.width)));
+ map_height_ = resp.map.info.height;//(int)pow(2,ceil(log2(resp.map.info.height)));
//printf("Padded dimensions to %d X %d\n", map_width_, map_height_);
@@ -224,14 +226,14 @@
unsigned char* pixels = new unsigned char[pixels_size];
memset(pixels, 255, pixels_size);
- for(unsigned int j=0;j<resp.map.height;j++)
+ for(unsigned int j=0;j<resp.map.info.height;j++)
{
- for(unsigned int i=0;i<resp.map.width;i++)
+ for(unsigned int i=0;i<resp.map.info.width;i++)
{
unsigned char val;
- if(resp.map.data[j*resp.map.width+i] == 100)
+ if(resp.map.data[j*resp.map.info.width+i] == 100)
val = 0;
- else if(resp.map.data[j*resp.map.width+i] == 0)
+ else if(resp.map.data[j*resp.map.info.width+i] == 0)
val = 255;
else
val = 127;
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-04-27 21:34:34 UTC (rev 14484)
@@ -36,7 +36,6 @@
#include "robot_msgs/PoseStamped.h"
#include "robot_msgs/Polyline.h"
#include "robot_msgs/PoseWithCovariance.h"
-#include "robot_srvs/StaticMap.h"
#include <OGRE/OgreTexture.h>
#include <OGRE/OgreMaterial.h>
Modified: pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
===================================================================
--- pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -314,23 +314,27 @@
/// @todo Dynamically determine bounding box for map
GMapping::Point wmin(-100.0, -100.0);
GMapping::Point wmax(100.0, 100.0);
- map_.map.resolution = delta_;
- map_.map.origin.x = wmin.x;
- map_.map.origin.y = wmin.y;
- map_.map.origin.th = 0.0;
+ map_.map.info.resolution = delta_;
+ map_.map.info.origin.position.x = wmin.x;
+ map_.map.info.origin.position.y = wmin.y;
+ map_.map.info.origin.position.z = 0.0;
+ map_.map.info.origin.orientation.x = 0.0;
+ map_.map.info.origin.orientation.y = 0.0;
+ map_.map.info.origin.orientation.z = 0.0;
+ map_.map.info.origin.orientation.w = 1.0;
GMapping::Point center;
center.x=(wmin.x + wmax.x) / 2.0;
center.y=(wmin.y + wmax.y) / 2.0;
GMapping::ScanMatcherMap smap(center, wmin.x, wmin.y, wmax.x, wmax.y,
- map_.map.resolution);
+ map_.map.info.resolution);
GMapping::IntPoint imin = smap.world2map(wmin);
GMapping::IntPoint imax = smap.world2map(wmax);
- map_.map.width = imax.x - imin.x;
- map_.map.height = imax.y - imin.y;
- map_.map.data.resize(map_.map.width * map_.map.height);
+ map_.map.info.width = imax.x - imin.x;
+ map_.map.info.height = imax.y - imin.y;
+ map_.map.data.resize(map_.map.info.width * map_.map.info.height);
ROS_DEBUG("Trajectory tree:");
for(GMapping::GridSlamProcessor::TNode* n = best.node;
@@ -360,11 +364,11 @@
double occ=smap.cell(p);
assert(occ <= 1.0);
if(occ < 0)
- map_.map.data[MAP_IDX(map_.map.width, x, y)] = -1;
+ map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
else if(occ > 0.1)
- map_.map.data[MAP_IDX(map_.map.width, x, y)] = (int)round(occ*100.0);
+ map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
else
- map_.map.data[MAP_IDX(map_.map.width, x, y)] = 0;
+ map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
}
}
got_map_ = true;
Modified: pkg/trunk/nav/wavefront/src/wavefront_node.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -304,13 +304,13 @@
d.sleep();
}
printf("Received a %d X %d map @ %.3f m/pix\n",
- resp.map.width,
- resp.map.height,
- resp.map.resolution);
+ resp.map.info.width,
+ resp.map.info.height,
+ resp.map.info.resolution);
char* mapdata;
int sx, sy;
- sx = resp.map.width;
- sy = resp.map.height;
+ sx = resp.map.info.width;
+ sy = resp.map.info.height;
// Convert to player format
mapdata = new char[sx*sy];
for(int i=0;i<sx*sy;i++)
@@ -342,7 +342,7 @@
}
delete[] mapdata;
- this->plan->scale = resp.map.resolution;
+ this->plan->scale = resp.map.info.resolution;
this->plan->size_x = sx;
this->plan->size_y = sy;
this->plan->origin_x = 0.0;
Modified: pkg/trunk/prcore/robot_msgs/msg/MapMetaData.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/MapMetaData.msg 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/prcore/robot_msgs/msg/MapMetaData.msg 2009-04-27 21:34:34 UTC (rev 14484)
@@ -8,4 +8,4 @@
uint32 height
# The origin of the map [m, m, rad]. This is the real-world pose of the
# cell (0,0) in the map.
-deprecated_msgs/Pose2DFloat32 origin
\ No newline at end of file
+robot_msgs/Pose origin
\ No newline at end of file
Added: pkg/trunk/prcore/robot_msgs/msg/OccGrid.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/OccGrid.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/OccGrid.msg 2009-04-27 21:34:34 UTC (rev 14484)
@@ -0,0 +1,10 @@
+# A 2-D grid map, in which each cell represents the probability of
+# occupancy. Occupancy values are integers in the range [0,100], or -1
+# for unknown.
+
+#MetaData for the map
+MapMetaData info
+
+# The map data, in row-major order, starting with (0,0). Occupancy
+# probabilities are in the range [0,100]. Unknown is -1.
+byte[] data
Deleted: pkg/trunk/prcore/robot_msgs/msg/OccMap2D.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/OccMap2D.msg 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/prcore/robot_msgs/msg/OccMap2D.msg 2009-04-27 21:34:34 UTC (rev 14484)
@@ -1,16 +0,0 @@
-# A 2-D grid map, in which each cell represents the probability of
-# occupancy. Occupancy values are integers in the range [0,100], or -1
-# for unknown.
-
-# The map resolution [m/cell]
-float32 resolution
-# Map width [cells]
-uint32 width
-# Map height [cells]
-uint32 height
-# The origin of the map [m, m, rad]. This is the real-world pose of the
-# cell (0,0) in the map.
-deprecated_msgs/Pose2DFloat32 origin
-# The map data, in row-major order, starting with (0,0). Occupancy
-# probabilities are in the range [0,100]. Unknown is -1.
-byte[] data
Modified: pkg/trunk/prcore/robot_srvs/srv/StaticMap.srv
===================================================================
--- pkg/trunk/prcore/robot_srvs/srv/StaticMap.srv 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/prcore/robot_srvs/srv/StaticMap.srv 2009-04-27 21:34:34 UTC (rev 14484)
@@ -1,2 +1,2 @@
---
-robot_msgs/OccMap2D map
+robot_msgs/OccGrid map
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/map_display.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/map_display.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/map_display.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -194,15 +194,15 @@
clear();
ROS_DEBUG("Received a %d X %d map @ %.3f m/pix\n",
- resp.map.width,
- resp.map.height,
- resp.map.resolution);
+ resp.map.info.width,
+ resp.map.info.height,
+ resp.map.info.resolution);
- resolution_ = resp.map.resolution;
+ resolution_ = resp.map.info.resolution;
// Pad dimensions to power of 2
- width_ = resp.map.width;//(int)pow(2,ceil(log2(resp.map.width)));
- height_ = resp.map.height;//(int)pow(2,ceil(log2(resp.map.height)));
+ width_ = resp.map.info.width;//(int)pow(2,ceil(log2(resp.map.info.width)));
+ height_ = resp.map.info.height;//(int)pow(2,ceil(log2(resp.map.info.height)));
//printf("Padded dimensions to %d X %d\n", width_, height_);
@@ -211,14 +211,14 @@
unsigned char* pixels = new unsigned char[pixels_size];
memset(pixels, 255, pixels_size);
- for(unsigned int j=0;j<resp.map.height;j++)
+ for(unsigned int j=0;j<resp.map.info.height;j++)
{
- for(unsigned int i=0;i<resp.map.width;i++)
+ for(unsigned int i=0;i<resp.map.info.width;i++)
{
unsigned char val;
- if(resp.map.data[j*resp.map.width+i] == 100)
+ if(resp.map.data[j*resp.map.info.width+i] == 100)
val = 0;
- else if(resp.map.data[j*resp.map.width+i] == 0)
+ else if(resp.map.data[j*resp.map.info.width+i] == 0)
val = 255;
else
val = 127;
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h 2009-04-27 21:34:34 UTC (rev 14484)
@@ -41,7 +41,6 @@
#include <ros/console.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/observation_buffer.h>
-#include <robot_srvs/StaticMap.h>
#include <robot_msgs/Polyline.h>
#include <map>
#include <vector>
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -36,6 +36,9 @@
*********************************************************************/
#include <costmap_2d/costmap_2d_ros.h>
+#include <robot_srvs/StaticMap.h>
+
+
using namespace std;
using namespace tf;
using namespace robot_msgs;
@@ -141,7 +144,7 @@
usleep(1000000);
}
ROS_INFO("Received a %d X %d map at %f m/pix\n",
- map_resp.map.width, map_resp.map.height, map_resp.map.resolution);
+ map_resp.map.info.width, map_resp.map.info.height, map_resp.map.info.resolution);
//check if the user has set any parameters that will be overwritten
bool user_map_params = false;
@@ -156,16 +159,16 @@
// We are treating cells with no information as lethal obstacles based on the input data. This is not ideal but
// our planner and controller do not reason about the no obstacle case
- unsigned int numCells = map_resp.map.width * map_resp.map.height;
+ unsigned int numCells = map_resp.map.info.width * map_resp.map.info.height;
for(unsigned int i = 0; i < numCells; i++){
input_data.push_back((unsigned char) map_resp.map.data[i]);
}
- map_width = (unsigned int)map_resp.map.width;
- map_height = (unsigned int)map_resp.map.height;
- map_resolution = map_resp.map.resolution;
- map_origin_x = map_resp.map.origin.x;
- map_origin_y = map_resp.map.origin.y;
+ map_width = (unsigned int)map_resp.map.info.width;
+ map_height = (unsigned int)map_resp.map.info.height;
+ map_resolution = map_resp.map.info.resolution;
+ map_origin_x = map_resp.map.info.origin.position.x;
+ map_origin_y = map_resp.map.info.origin.position.y;
}
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp 2009-04-27 21:28:13 UTC (rev 14483)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp 2009-04-27 21:34:34 UTC (rev 14484)
@@ -80,12 +80,12 @@
usleep(1000000);
}
ROS_INFO("Received a %d X %d map at %f m/pix\n",
- map_resp.map.width, map_resp.map.height, map_resp.map.resolution);
+ map_resp.map.info.width, map_resp.map.info.height, map_resp.map.info.resolution);
// We are treating cells with no information as lethal obstacles based on the input data. This is not ideal but
// our planner and controller do not reason about the no obstacle case
std::vector<unsigned char> input_data;
- unsigned int numCells = map_resp.map.width * map_resp.map.height;
+ unsigned int numCells = map_resp.map.info.width * map_resp.map.info.height;
for(unsigned int i = 0; i < numCells; i++){
input_data.push_back((unsigned char) map_resp.map.data[i]);
}
@@ -93,8 +93,8 @@
struct timeval start, end;
double start_t, end_t, t_diff;
gettimeofday(&start, NULL);
- new_costmap_ = new Costmap2D((unsigned int)map_resp.map.width, (unsigned int)map_resp.map.height,
- map_resp.map.resolution, 0.0, 0.0, 0.325, 0.46, 0.55, 2.5, 2.0, 3.0, 1.0, input_data, 100);
+ new_costmap_ = new Costmap2D((unsigned int)map_resp.map.info.width, (unsigned int)map_resp.map.info.height,
+ map_resp.map.info.resolution, 0.0, 0.0, 0.325, 0.46, 0.55, 2.5, 2.0, 3.0, 1.0, input_data, 100);
get...
[truncated message content] |
|
From: <tf...@us...> - 2009-04-28 00:22:43
|
Revision: 14520
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14520&view=rev
Author: tfoote
Date: 2009-04-28 00:22:41 +0000 (Tue, 28 Apr 2009)
Log Message:
-----------
moving robot_actions into prcore
Added Paths:
-----------
pkg/trunk/prcore/robot_actions/
Removed Paths:
-------------
pkg/trunk/highlevel/robot_actions/
Property changes on: pkg/trunk/prcore/robot_actions
___________________________________________________________________
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-28 04:23:03
|
Revision: 14542
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14542&view=rev
Author: tfoote
Date: 2009-04-28 04:23:02 +0000 (Tue, 28 Apr 2009)
Log Message:
-----------
renaming ROS_REGISTER_FILTER to FILTERS_REGISTER_FILTER as per API review ticket:1149
Modified Paths:
--------------
pkg/trunk/prcore/filters/include/filters/filter_base.h
pkg/trunk/prcore/filters/include/filters/mean.h
pkg/trunk/prcore/filters/include/filters/median.h
pkg/trunk/prcore/filters/include/filters/transfer_function.h
pkg/trunk/prcore/filters/mainpage.dox
pkg/trunk/prcore/filters/test/test_chain.cpp
pkg/trunk/prcore/filters/test/test_factory.cpp
pkg/trunk/prcore/laser_scan/include/laser_scan/intensity_filter.h
pkg/trunk/prcore/laser_scan/include/laser_scan/median_filter.h
pkg/trunk/prcore/laser_scan/include/laser_scan/scan_shadows_filter.h
pkg/trunk/util/iir_filters/include/iir_filters/iir.h
Modified: pkg/trunk/prcore/filters/include/filters/filter_base.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/filter_base.h 2009-04-28 03:45:38 UTC (rev 14541)
+++ pkg/trunk/prcore/filters/include/filters/filter_base.h 2009-04-28 04:23:02 UTC (rev 14542)
@@ -336,7 +336,7 @@
/** The Macro which makes registering a Filter with it's type easy */
-#define ROS_REGISTER_FILTER(c,t) \
+#define FILTERS_REGISTER_FILTER(c,t) \
filters::FilterBase<t> * Filters_New_##c##__##t() {return new c< t >;}; \
bool ROS_FILTER_## c ## _ ## t = \
filters::FilterFactory<t>::Instance().Register(filters::getFilterID<t>(std::string(#c)), Filters_New_##c##__##t);
Modified: pkg/trunk/prcore/filters/include/filters/mean.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/mean.h 2009-04-28 03:45:38 UTC (rev 14541)
+++ pkg/trunk/prcore/filters/include/filters/mean.h 2009-04-28 04:23:02 UTC (rev 14542)
@@ -177,8 +177,8 @@
return true;
};
-ROS_REGISTER_FILTER(MeanFilter, double)
-ROS_REGISTER_FILTER(MeanFilter, float)
+FILTERS_REGISTER_FILTER(MeanFilter, double)
+FILTERS_REGISTER_FILTER(MeanFilter, float)
}
Modified: pkg/trunk/prcore/filters/include/filters/median.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/median.h 2009-04-28 03:45:38 UTC (rev 14541)
+++ pkg/trunk/prcore/filters/include/filters/median.h 2009-04-28 04:23:02 UTC (rev 14542)
@@ -205,8 +205,8 @@
return true;
};
-ROS_REGISTER_FILTER(MedianFilter, double)
-ROS_REGISTER_FILTER(MedianFilter, float)
+FILTERS_REGISTER_FILTER(MedianFilter, double)
+FILTERS_REGISTER_FILTER(MedianFilter, float)
}
Modified: pkg/trunk/prcore/filters/include/filters/transfer_function.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/transfer_function.h 2009-04-28 03:45:38 UTC (rev 14541)
+++ pkg/trunk/prcore/filters/include/filters/transfer_function.h 2009-04-28 04:23:02 UTC (rev 14542)
@@ -110,8 +110,8 @@
};
-ROS_REGISTER_FILTER(TransferFunctionFilter, double)
-ROS_REGISTER_FILTER(TransferFunctionFilter, float)
+FILTERS_REGISTER_FILTER(TransferFunctionFilter, double)
+FILTERS_REGISTER_FILTER(TransferFunctionFilter, float)
template <typename T>
TransferFunctionFilter<T>::TransferFunctionFilter()
Modified: pkg/trunk/prcore/filters/mainpage.dox
===================================================================
--- pkg/trunk/prcore/filters/mainpage.dox 2009-04-28 03:45:38 UTC (rev 14541)
+++ pkg/trunk/prcore/filters/mainpage.dox 2009-04-28 04:23:02 UTC (rev 14542)
@@ -39,7 +39,7 @@
The requirements for making a filter:
- A filter must be a templated class (MyFilterType<T>) derived from filters::FilterBase<T>
- The filter must implement the methods configure() and update(const std::vector<T>& data_in, std::vector<T>& data_out) and have a default constructor and destructor.
- - For every valid template value you must call ROS_REGISTER_FILTER(MyFilterType, TemplateType)
+ - For every valid template value you must call FILTERS_REGISTER_FILTER(MyFilterType, TemplateType)
Here are the filters generic filters which are currently implemented and realtime safe:
Modified: pkg/trunk/prcore/filters/test/test_chain.cpp
===================================================================
--- pkg/trunk/prcore/filters/test/test_chain.cpp 2009-04-28 03:45:38 UTC (rev 14541)
+++ pkg/trunk/prcore/filters/test/test_chain.cpp 2009-04-28 04:23:02 UTC (rev 14542)
@@ -63,8 +63,8 @@
};
-ROS_REGISTER_FILTER(TestFilter, double)
-ROS_REGISTER_FILTER(TestFilter, float)
+FILTERS_REGISTER_FILTER(TestFilter, double)
+FILTERS_REGISTER_FILTER(TestFilter, float)
static std::string mean_filter_5 = "<filter type=\"MeanFilter\" name=\"mean_test_5\"> <params number_of_observations=\"5\"/></filter>";
Modified: pkg/trunk/prcore/filters/test/test_factory.cpp
===================================================================
--- pkg/trunk/prcore/filters/test/test_factory.cpp 2009-04-28 03:45:38 UTC (rev 14541)
+++ pkg/trunk/prcore/filters/test/test_factory.cpp 2009-04-28 04:23:02 UTC (rev 14542)
@@ -59,9 +59,9 @@
};
-ROS_REGISTER_FILTER(TestFilter, double)
-ROS_REGISTER_FILTER(TestFilter, float)
-ROS_REGISTER_FILTER(TestFilter, int)
+FILTERS_REGISTER_FILTER(TestFilter, double)
+FILTERS_REGISTER_FILTER(TestFilter, float)
+FILTERS_REGISTER_FILTER(TestFilter, int)
Modified: pkg/trunk/prcore/laser_scan/include/laser_scan/intensity_filter.h
===================================================================
--- pkg/trunk/prcore/laser_scan/include/laser_scan/intensity_filter.h 2009-04-28 03:45:38 UTC (rev 14541)
+++ pkg/trunk/prcore/laser_scan/include/laser_scan/intensity_filter.h 2009-04-28 04:23:02 UTC (rev 14542)
@@ -117,7 +117,7 @@
}
} ;
-ROS_REGISTER_FILTER(LaserScanIntensityFilter, LaserScan);
+FILTERS_REGISTER_FILTER(LaserScanIntensityFilter, LaserScan);
}
#endif // LASER_SCAN_INTENSITY_FILTER_H
Modified: pkg/trunk/prcore/laser_scan/include/laser_scan/median_filter.h
===================================================================
--- pkg/trunk/prcore/laser_scan/include/laser_scan/median_filter.h 2009-04-28 03:45:38 UTC (rev 14541)
+++ pkg/trunk/prcore/laser_scan/include/laser_scan/median_filter.h 2009-04-28 04:23:02 UTC (rev 14542)
@@ -80,7 +80,7 @@
typedef laser_scan::LaserScan laser_scan_laser_scan;
-ROS_REGISTER_FILTER(LaserMedianFilter, laser_scan_laser_scan);
+FILTERS_REGISTER_FILTER(LaserMedianFilter, laser_scan_laser_scan);
template <typename T>
LaserMedianFilter<T>::LaserMedianFilter():
Modified: pkg/trunk/prcore/laser_scan/include/laser_scan/scan_shadows_filter.h
===================================================================
--- pkg/trunk/prcore/laser_scan/include/laser_scan/scan_shadows_filter.h 2009-04-28 03:45:38 UTC (rev 14541)
+++ pkg/trunk/prcore/laser_scan/include/laser_scan/scan_shadows_filter.h 2009-04-28 04:23:02 UTC (rev 14542)
@@ -148,7 +148,7 @@
} ;
typedef laser_scan::LaserScan laser_scan_laser_scan;
-ROS_REGISTER_FILTER(ScanShadowsFilter, laser_scan_laser_scan);
+FILTERS_REGISTER_FILTER(ScanShadowsFilter, laser_scan_laser_scan);
}
#endif //LASER_SCAN_SHADOWS_FILTER_H
Modified: pkg/trunk/util/iir_filters/include/iir_filters/iir.h
===================================================================
--- pkg/trunk/util/iir_filters/include/iir_filters/iir.h 2009-04-28 03:45:38 UTC (rev 14541)
+++ pkg/trunk/util/iir_filters/include/iir_filters/iir.h 2009-04-28 04:23:02 UTC (rev 14542)
@@ -109,8 +109,8 @@
filters::FilterBase<T > * tf_filter_;
};
-ROS_REGISTER_FILTER(IIRFilter, double)
-ROS_REGISTER_FILTER(IIRFilter, float)
+FILTERS_REGISTER_FILTER(IIRFilter, double)
+FILTERS_REGISTER_FILTER(IIRFilter, float)
template <typename T>
IIRFilter<T>::IIRFilter()
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <bla...@us...> - 2009-04-28 21:58:11
|
Revision: 14585
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14585&view=rev
Author: blaisegassend
Date: 2009-04-28 21:58:00 +0000 (Tue, 28 Apr 2009)
Log Message:
-----------
Put the trigger_configuration typedef into the controller namespace.
Modified Paths:
--------------
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/forearm_cam/src/nodes/forearm_node.cpp
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-04-28 21:40:08 UTC (rev 14584)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/trigger_controller.h 2009-04-28 21:58:00 UTC (rev 14585)
@@ -58,10 +58,9 @@
*
*/
-typedef robot_mechanism_controllers::SetWaveform::Request trigger_configuration;
-
namespace controller
{
+typedef robot_mechanism_controllers::SetWaveform::Request trigger_configuration;
class TriggerControllerNode;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp 2009-04-28 21:40:08 UTC (rev 14584)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/trigger_controller.cpp 2009-04-28 21:58:00 UTC (rev 14585)
@@ -99,6 +99,9 @@
// ROS_INFO("Changed to: %i (%s)", active, actuator_name_.c_str()); // KILLME
}
}
+
+ //if (actuator_command_->digital_out_ && !(active ^ config_.active_low))
+ // ROS_DEBUG("digital out falling at time %f", robot_->hw_->current_time_);
actuator_command_->digital_out_ = active ^ config_.active_low;
Modified: pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp 2009-04-28 21:40:08 UTC (rev 14584)
+++ pkg/trunk/drivers/cam/forearm_cam/src/nodes/forearm_node.cpp 2009-04-28 21:58:00 UTC (rev 14585)
@@ -190,7 +190,7 @@
std::string serial_number_;
std::string hwinfo_;
std::string mode_name_;
- trigger_configuration trig_req_;
+ controller::trigger_configuration trig_req_;
robot_mechanism_controllers::SetWaveform::Response trig_rsp_;
DiagnosticUpdater<ForearmNode> diagnostic_;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-04-28 22:52:34
|
Revision: 14588
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14588&view=rev
Author: eitanme
Date: 2009-04-28 22:52:15 +0000 (Tue, 28 Apr 2009)
Log Message:
-----------
There was an inconsistency with naming in the costmap_2d package. Costmap was used in some places... and CostMap in others. To make this worse... member variables also were either costmap_ or cost_map_. To agree with the package name... a costmap is a Costmap is a costmap_.
Modified Paths:
--------------
pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h
pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
pkg/trunk/highlevel/nav/include/nav/move_base.h
pkg/trunk/highlevel/nav/include/nav/move_base_local.h
pkg/trunk/highlevel/nav/src/move_base.cpp
pkg/trunk/highlevel/nav/src/move_base_local.cpp
pkg/trunk/motion_planning/navfn/include/navfn/navfn.h
pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
pkg/trunk/motion_planning/navfn/src/navfn.cpp
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/motion_planning/navfn/src/navtest.cpp
pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/src/costmap_model.cpp
pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/nav/base_local_planner/test/utest.cpp
pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h
pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_publisher.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_2d_publisher.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -157,7 +157,7 @@
/**
* @brief Resets the costmaps to the static map outside a given window
*/
- void resetCostMaps();
+ void resetCostmaps();
ros::Node& ros_node_;
tf::TransformListener& tf_;
Modified: pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -79,7 +79,7 @@
//create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
planner_cost_map_ros_ = new Costmap2DROS(ros_node_, tf_, "");
- planner_cost_map_ros_->getCostMapCopy(planner_cost_map_);
+ planner_cost_map_ros_->getCostmapCopy(planner_cost_map_);
//initialize the door opening planner
planner_ = new DoorReactivePlanner(ros_node_, tf_,&planner_cost_map_,control_frame_,global_frame_);
@@ -144,7 +144,7 @@
return;
//make a plan for controller
- planner_cost_map_ros_->getCostMapCopy(planner_cost_map_);
+ planner_cost_map_ros_->getCostmapCopy(planner_cost_map_);
//make sure we clear the robot's footprint from the cost map
clearRobotFootprint(planner_cost_map_);
@@ -248,7 +248,7 @@
updateGlobalPose();
//make sure to update the cost_map we'll use for this cycle
- //controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
+ //controller_cost_map_ros_->getCostmapCopy(controller_cost_map_);
//make sure that we clear the robot footprint in the cost map
//clearRobotFootprint(controller_cost_map_);
@@ -349,7 +349,7 @@
return true;
}
- void MoveBaseDoorAction::resetCostMaps(){
+ void MoveBaseDoorAction::resetCostmaps(){
planner_cost_map_ros_->resetMapOutsideWindow(5.0, 5.0);
}
Modified: pkg/trunk/highlevel/nav/include/nav/move_base.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/highlevel/nav/include/nav/move_base.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -94,14 +94,14 @@
/**
* @brief Resets the costmaps to the static map outside a given window
*/
- void resetCostMaps();
+ void resetCostmaps();
ros::Node& ros_node_;
tf::TransformListener& tf_;
bool run_planner_;
base_local_planner::TrajectoryPlannerROS* tc_;
- costmap_2d::Costmap2DROS* planner_cost_map_ros_, *controller_cost_map_ros_;
- costmap_2d::Costmap2D planner_cost_map_, controller_cost_map_;
+ costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
+ costmap_2d::Costmap2D planner_costmap_, controller_costmap_;
navfn::NavfnROS* planner_;
std::vector<robot_msgs::PoseStamped> global_plan_;
Modified: pkg/trunk/highlevel/nav/include/nav/move_base_local.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base_local.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/highlevel/nav/include/nav/move_base_local.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -95,13 +95,13 @@
/**
* @brief Resets the costmaps to the static map outside a given window
*/
- void resetCostMaps();
+ void resetCostmaps();
ros::Node& ros_node_;
tf::TransformListener& tf_;
base_local_planner::TrajectoryPlannerROS* tc_;
- costmap_2d::Costmap2DROS* controller_cost_map_ros_;
- costmap_2d::Costmap2D controller_cost_map_;
+ costmap_2d::Costmap2DROS* controller_costmap_ros_;
+ costmap_2d::Costmap2D controller_costmap_;
std::vector<robot_msgs::Point> footprint_;
std::string robot_base_frame_;
Modified: pkg/trunk/highlevel/nav/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/highlevel/nav/src/move_base.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -44,7 +44,7 @@
namespace nav {
MoveBase::MoveBase(ros::Node& ros_node, tf::TransformListener& tf) :
Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped>(ros_node.getName()), ros_node_(ros_node), tf_(tf),
- run_planner_(true), tc_(NULL), planner_cost_map_ros_(NULL), controller_cost_map_ros_(NULL),
+ run_planner_(true), tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
planner_(NULL), valid_plan_(false), new_plan_(false) {
//get some parameters that will be global to the move base node
@@ -60,16 +60,16 @@
ros_node_.param("~navfn/costmap/circumscribed_radius", circumscribed_radius, 0.46);
//create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
- planner_cost_map_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("navfn"));
- planner_cost_map_ros_->getCostMapCopy(planner_cost_map_);
+ planner_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("navfn"));
+ planner_costmap_ros_->getCostmapCopy(planner_costmap_);
//initialize the NavFn planner
- planner_ = new NavfnROS(ros_node_, tf_, planner_cost_map_);
- ROS_INFO("MAP SIZE: %d, %d", planner_cost_map_.cellSizeX(), planner_cost_map_.cellSizeY());
+ planner_ = new NavfnROS(ros_node_, tf_, planner_costmap_);
+ ROS_INFO("MAP SIZE: %d, %d", planner_costmap_.cellSizeX(), planner_costmap_.cellSizeY());
- //create the ros wrapper for the controller's cost_map... and initializer a pointer we'll use with the underlying map
- controller_cost_map_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"));
- controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
+ //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
+ controller_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"));
+ controller_costmap_ros_->getCostmapCopy(controller_costmap_);
robot_msgs::Point pt;
//create a square footprint
@@ -92,7 +92,7 @@
footprint_.push_back(pt);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_cost_map_, footprint_, &planner_cost_map_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_, &planner_costmap_);
//TODO:spawn planning thread here?
}
@@ -104,24 +104,24 @@
if(tc_ != NULL)
delete tc_;
- if(planner_cost_map_ros_ != NULL)
- delete planner_cost_map_ros_;
+ if(planner_costmap_ros_ != NULL)
+ delete planner_costmap_ros_;
- if(controller_cost_map_ros_ != NULL)
- delete controller_cost_map_ros_;
+ if(controller_costmap_ros_ != NULL)
+ delete controller_costmap_ros_;
}
void MoveBase::makePlan(const robot_msgs::PoseStamped& goal){
//since this gets called on handle activate
- if(planner_cost_map_ros_ == NULL)
+ if(planner_costmap_ros_ == NULL)
return;
//update the copy of the costmap the planner uses
- planner_cost_map_ros_->getCostMapCopy(planner_cost_map_);
+ planner_costmap_ros_->getCostmapCopy(planner_costmap_);
//since we have a controller that knows the full footprint of the robot... we may as well clear it
- tc_->clearRobotFootprint(planner_cost_map_);
+ tc_->clearRobotFootprint(planner_costmap_);
std::vector<robot_msgs::PoseStamped> global_plan;
bool valid_plan = planner_->makePlan(goal, global_plan);
@@ -181,11 +181,11 @@
//push the feedback out
update(feedback);
- //make sure to update the cost_map we'll use for this cycle
- controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
+ //make sure to update the costmap we'll use for this cycle
+ controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//check that the observation buffers for the costmap are current
- if(!controller_cost_map_ros_->isCurrent()){
+ if(!controller_costmap_ros_->isCurrent()){
ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
continue;
}
@@ -203,7 +203,7 @@
}
//get observations for the non-costmap controllers
std::vector<Observation> observations;
- controller_cost_map_ros_->getMarkingObservations(observations);
+ controller_costmap_ros_->getMarkingObservations(observations);
valid_control = tc_->computeVelocityCommands(cmd_vel, observations);
}
else{
@@ -229,7 +229,7 @@
//if planning fails here... try to revert to the static map outside a given area
if(!valid_plan_){
- resetCostMaps();
+ resetCostmaps();
}
}
@@ -247,9 +247,9 @@
return robot_actions::PREEMPTED;
}
- void MoveBase::resetCostMaps(){
- planner_cost_map_ros_->resetMapOutsideWindow(5.0, 5.0);
- controller_cost_map_ros_->resetMapOutsideWindow(5.0, 5.0);
+ void MoveBase::resetCostmaps(){
+ planner_costmap_ros_->resetMapOutsideWindow(5.0, 5.0);
+ controller_costmap_ros_->resetMapOutsideWindow(5.0, 5.0);
}
};
Modified: pkg/trunk/highlevel/nav/src/move_base_local.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_local.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/highlevel/nav/src/move_base_local.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -44,7 +44,7 @@
namespace nav {
MoveBaseLocal::MoveBaseLocal(ros::Node& ros_node, tf::TransformListener& tf) :
Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped>(ros_node.getName()), ros_node_(ros_node), tf_(tf),
- tc_(NULL), controller_cost_map_ros_(NULL) {
+ tc_(NULL), controller_costmap_ros_(NULL) {
//get some parameters that will be global to the move base node
ros_node_.param("~base_local_planner/robot_base_frame", robot_base_frame_, std::string("base_link"));
@@ -58,9 +58,9 @@
ros_node_.param("~base_local_planner/costmap/inscribed_radius", inscribed_radius, 0.325);
ros_node_.param("~base_local_planner/costmap/circumscribed_radius", circumscribed_radius, 0.46);
- //create the ros wrapper for the controller's cost_map... and initializer a pointer we'll use with the underlying map
- controller_cost_map_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"));
- controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
+ //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
+ controller_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"));
+ controller_costmap_ros_->getCostmapCopy(controller_costmap_);
robot_msgs::Point pt;
//create a square footprint
@@ -83,15 +83,15 @@
footprint_.push_back(pt);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_cost_map_, footprint_, &controller_cost_map_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_, &controller_costmap_);
}
MoveBaseLocal::~MoveBaseLocal(){
if(tc_ != NULL)
delete tc_;
- if(controller_cost_map_ros_ != NULL)
- delete controller_cost_map_ros_;
+ if(controller_costmap_ros_ != NULL)
+ delete controller_costmap_ros_;
}
void MoveBaseLocal::getRobotPose(std::string frame, tf::Stamped<tf::Pose>& pose){
@@ -134,11 +134,11 @@
//push the feedback out
update(feedback);
- //make sure to update the cost_map we'll use for this cycle
- controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
+ //make sure to update the costmap we'll use for this cycle
+ controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//check that the observation buffers for the costmap are current
- if(!controller_cost_map_ros_->isCurrent()){
+ if(!controller_costmap_ros_->isCurrent()){
ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
continue;
}
@@ -155,7 +155,7 @@
tc_->updatePlan(global_plan);
//get observations for the non-costmap controllers
std::vector<Observation> observations;
- controller_cost_map_ros_->getMarkingObservations(observations);
+ controller_costmap_ros_->getMarkingObservations(observations);
valid_control = tc_->computeVelocityCommands(cmd_vel, observations);
//give the base the velocity command
@@ -202,8 +202,8 @@
return true;
}
- void MoveBaseLocal::resetCostMaps(){
- controller_cost_map_ros_->resetMapOutsideWindow(5.0, 5.0);
+ void MoveBaseLocal::resetCostmaps(){
+ controller_costmap_ros_->resetMapOutsideWindow(5.0, 5.0);
}
};
Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -50,7 +50,7 @@
#define COST_OBS 254 // 255 and 254 for forbidden regions
#define COST_OBS_ROS 253 // ROS values of 253 are obstacles
#define COST_NEUTRAL 50 // Set this to "open space" value
-#define COST_FACTOR 3 // Used for translating costs in NavFn::setCostMap()
+#define COST_FACTOR 3 // Used for translating costs in NavFn::setCostmap()
// Define the cost type in the case that it is not set. However, this allows
// clients to modify it without changing the file. Arguably, it is better to require it to
@@ -104,7 +104,7 @@
void setNavArr(int nx, int ny); /**< sets or resets the size of the map */
int nx, ny, ns; /**< size of grid, in pixels */
- void setCostMap(const COSTTYPE *cmap, bool isROS=true); /**< sets up the cost map */
+ void setCostmap(const COSTTYPE *cmap, bool isROS=true); /**< sets up the cost map */
bool calcNavFnAstar(); /**< calculates a plan, returns true if found */
bool calcNavFnDijkstra(); /**< calculates the full navigation function */
float *getPathX(); /**< x-coordinates of path */
Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -56,7 +56,7 @@
* @param tf A reference to a TransformListener
* @param cos_map A reference to the costmap to use
*/
- NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& cost_map);
+ NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& costmap);
/**
* @brief Given a goal pose in the world, compute a plan
@@ -91,7 +91,7 @@
void clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my);
ros::Node& ros_node_;
tf::TransformListener& tf_;
- costmap_2d::Costmap2D& cost_map_;
+ costmap_2d::Costmap2D& costmap_;
NavFn planner_;
std::string global_frame_, robot_base_frame_;
double transform_tolerance_; // timeout before transform errors
Modified: pkg/trunk/motion_planning/navfn/src/navfn.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/motion_planning/navfn/src/navfn.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -223,7 +223,7 @@
//
void
-NavFn::setCostMap(const COSTTYPE *cmap, bool isROS)
+NavFn::setCostmap(const COSTTYPE *cmap, bool isROS)
{
COSTTYPE *cm = costarr;
if (isROS) // ROS-type cost array
Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -37,8 +37,8 @@
#include <navfn/navfn_ros.h>
namespace navfn {
- NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& cost_map) : ros_node_(ros_node), tf_(tf),
- cost_map_(cost_map), planner_(cost_map.cellSizeX(), cost_map.cellSizeY()) {
+ NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& costmap) : ros_node_(ros_node), tf_(tf),
+ costmap_(costmap), planner_(costmap.cellSizeX(), costmap.cellSizeY()) {
//advertise our plan visualization
ros_node_.advertise<robot_msgs::Polyline>("~navfn/plan", 1);
@@ -60,7 +60,7 @@
double NavfnROS::getPointPotential(const robot_msgs::Point& world_point){
unsigned int mx, my;
- if(!cost_map_.worldToMap(world_point.x, world_point.y, mx, my))
+ if(!costmap_.worldToMap(world_point.x, world_point.y, mx, my))
return DBL_MAX;
unsigned int index = my * planner_.nx + mx;
@@ -68,10 +68,10 @@
}
bool NavfnROS::computePotential(const robot_msgs::Point& world_point){
- planner_.setCostMap(cost_map_.getCharMap());
+ planner_.setCostmap(costmap_.getCharMap());
unsigned int mx, my;
- if(!cost_map_.worldToMap(world_point.x, world_point.y, mx, my))
+ if(!costmap_.worldToMap(world_point.x, world_point.y, mx, my))
return false;
int map_start[2];
@@ -93,12 +93,12 @@
global_pose.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
//set the associated costs in the cost map to be free
- cost_map_.setCost(mx, my, costmap_2d::FREE_SPACE);
+ costmap_.setCost(mx, my, costmap_2d::FREE_SPACE);
double max_inflation_dist = inflation_radius_ + inscribed_radius_;
//make sure to re-inflate obstacles in the affected region
- cost_map_.reinflateWindow(global_pose.getOrigin().x(), global_pose.getOrigin().y(), max_inflation_dist, max_inflation_dist);
+ costmap_.reinflateWindow(global_pose.getOrigin().x(), global_pose.getOrigin().y(), max_inflation_dist, max_inflation_dist);
}
@@ -143,13 +143,13 @@
double wy = global_pose.getOrigin().y();
unsigned int mx, my;
- if(!cost_map_.worldToMap(wx, wy, mx, my))
+ if(!costmap_.worldToMap(wx, wy, mx, my))
return false;
//clear the starting cell within the costmap because we know it can't be an obstacle
clearRobotCell(global_pose, mx, my);
- planner_.setCostMap(cost_map_.getCharMap());
+ planner_.setCostmap(costmap_.getCharMap());
int map_start[2];
map_start[0] = mx;
@@ -158,7 +158,7 @@
wx = goal_pose.getOrigin().x();
wy = goal_pose.getOrigin().y();
- if(!cost_map_.worldToMap(wx, wy, mx, my))
+ if(!costmap_.worldToMap(wx, wy, mx, my))
return false;
int map_goal[2];
@@ -182,7 +182,7 @@
//convert the plan to world coordinates
double world_x, world_y;
- cost_map_.mapToWorld(cell_x, cell_y, world_x, world_y);
+ costmap_.mapToWorld(cell_x, cell_y, world_x, world_y);
robot_msgs::PoseStamped pose;
pose.header.stamp = ros::Time::now();
Modified: pkg/trunk/motion_planning/navfn/src/navtest.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navtest.cpp 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/motion_planning/navfn/src/navtest.cpp 2009-04-28 22:52:15 UTC (rev 14588)
@@ -71,8 +71,8 @@
COSTTYPE *cmap = NULL;
// cmap = readPGM("maps/willow-full-0.05.pgm",&sx,&sy);
cmap = readPGM("maps/navfn_troubles.pgm",&sx,&sy,true);
- // cmap = readPGM("initial_cost_map_1165_945.pgm",&sx,&sy,true);
- // cmap = readPGM("initial_cost_map_2332_1825.pgm",&sx,&sy,true);
+ // cmap = readPGM("initial_costmap_1165_945.pgm",&sx,&sy,true);
+ // cmap = readPGM("initial_costmap_2332_1825.pgm",&sx,&sy,true);
if (cmap)
{
nav = new NavFn(sx,sy);
@@ -149,7 +149,7 @@
// set up cost map from file, if it exists
if (cmap)
{
- nav->setCostMap(cmap);
+ nav->setCostmap(cmap);
nav->setupNavFn(true);
}
else
@@ -171,7 +171,7 @@
// set up cost map from file, if it exists
if (cmap)
{
- nav->setCostMap(cmap);
+ nav->setCostmap(cmap);
nav->setupNavFn(true);
}
else
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -51,10 +51,10 @@
public:
/**
* @brief Constructor for the CostmapModel
- * @param cost_map The costmap that should be used
+ * @param costmap The costmap that should be used
* @return
*/
- CostmapModel(const costmap_2d::Costmap2D& cost_map);
+ CostmapModel(const costmap_2d::Costmap2D& costmap);
/**
* @brief Destructor for the world model
@@ -100,7 +100,7 @@
*/
double pointCost(int x, int y);
- const costmap_2d::Costmap2D& cost_map_; ///< @brief Allows access of costmap obstacle information
+ const costmap_2d::Costmap2D& costmap_; ///< @brief Allows access of costmap obstacle information
};
};
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -73,7 +73,7 @@
/**
* @brief Constructs a trajectory controller
* @param world_model The WorldModel the trajectory controller uses to check for collisions
- * @param cost_map A reference to the CostMap the controller should use
+ * @param costmap A reference to the Costmap the controller should use
* @param footprint_spec A polygon representing the footprint of the robot. (Must be convex)
* @param inscribed_radius The radius of the inscribed circle of the robot
* @param circumscribed_radius The radius of the circumscribed circle of the robot
@@ -102,7 +102,7 @@
* @param y_vels A vector of the y velocities the controller will explore
*/
TrajectoryPlanner(WorldModel& world_model,
- const costmap_2d::Costmap2D& cost_map,
+ const costmap_2d::Costmap2D& costmap,
std::vector<robot_msgs::Point> footprint_spec,
double inscribed_radius, double circumscribed_radius,
double acc_lim_x = 1.0, double acc_lim_y = 1.0, double acc_lim_theta = 1.0,
@@ -249,7 +249,7 @@
void setPathCells();
MapGrid map_; ///< @brief The local map grid where we propagate goal and path distance
- const costmap_2d::Costmap2D& cost_map_; ///< @brief Provides access to cost map information
+ const costmap_2d::Costmap2D& costmap_; ///< @brief Provides access to cost map information
WorldModel& world_model_; ///< @brief The world model that the controller uses for collision detection
std::vector<robot_msgs::Point> footprint_spec_; ///< @brief The footprint specification of the robot
@@ -304,7 +304,7 @@
check_cell->path_mark = true;
//if the cell is an obstacle set the max path distance
- unsigned char cost = cost_map_.getCost(check_cell->cx, check_cell->cy);
+ unsigned char cost = costmap_.getCost(check_cell->cx, check_cell->cy);
if(!map_(check_cell->cx, check_cell->cy).within_robot && (cost == costmap_2d::LETHAL_OBSTACLE || cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)){
check_cell->path_dist = map_.map_.size();
return;
@@ -328,7 +328,7 @@
check_cell->goal_mark = true;
//if the cell is an obstacle set the max goal distance
- unsigned char cost = cost_map_.getCost(check_cell->cx, check_cell->cy);
+ unsigned char cost = costmap_.getCost(check_cell->cx, check_cell->cy);
if(!map_(check_cell->cx, check_cell->cy).within_robot && (cost == costmap_2d::LETHAL_OBSTACLE || cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)){
check_cell->goal_dist = map_.map_.size();
return;
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-28 22:52:10 UTC (rev 14587)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-28 22:52:15 UTC (rev 14588)
@@ -77,12 +77,12 @@
* @brief Constructs the ros wrapper
* @param ros_node The node that is running the controller, used to get parameters from the parameter server
* @param tf A reference to a transform listener
- * @param cost_map The cost map to use for assigning costs to trajectories
+ * @param costmap The cost map to use for assigning costs to trajectories
* @param footprint_spec A polygon representing the footprint of the robot. (Must be convex)
* @param planner_map Used to size the map for the freespace controller... this will go away once a rolling window version of the point grid is in place
*/
TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
- costmap_2d::Costmap2D& cost_map, std::vector<robot_msgs::Point> footprint_spec,
+ costmap_2d::Costmap2D& costmap, std::vector<robot_msgs::Point> footprint_spec,
const costmap_2d::Costmap2D* planner_map = NULL);
/**
@@ -130,9 +130,9 @@
/**
* @brief Clear the footprint of the robot in a given cost map
- * @param cost_map The costmap to apply the clearing opertaion on
+ * @param costmap The costmap to apply the clearing opertaion on
*/
- void clearRobotFootprint(costmap_2d::Costmap2D& cost_map);
+ void clearRobotFootprint(costmap_2d::Costmap2D& costmap);
private:
/**
@@ -191,9 +191,9 @@
/**
* @brief Clear the footprint of the robot in a given cost map
* @param global_pose The pose of the robot in the global frame
- * @param cost_map The costmap to apply the clearing opertaion on
+ * @param costmap The costmap to apply the clearing opertaion on
*/
- void clearRobotFootprint(const tf::Stamped<tf::Pose>& global_pose, costmap_2d::Costmap2D& cost_map);
+ void clearRobotFootprint(const tf::Stamped<tf::Pose>& global_pose, costmap_2d::Costmap2D& costmap);
/**
* @brief Publish a plan for visualization purposes
@@ -206,7 +206,7 @@
WorldModel* world_model_; ///< @brief The world model that the controller will use
TrajectoryPlanner* tc_; ///< @brief The trajectory controller
- costmap_2d::Costmap2D& cost_map_; ///< @brief The costmap the controller will use
+ costmap_2d::Costmap2D& costmap_; ///< @brief The costmap the controller will use
tf::MessageNotifier<laser_sc...
[truncated message content] |
|
From: <tf...@us...> - 2009-04-29 08:00:30
|
Revision: 14612
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14612&view=rev
Author: tfoote
Date: 2009-04-29 08:00:27 +0000 (Wed, 29 Apr 2009)
Log Message:
-----------
moving pr2_robot_actions/MoveBaseStateNew to nav_robot_actions/MoveBaseState and deleting the deprecated version. Porting all usages to the new version. And moving all usages of the new one to the new location.
Modified Paths:
--------------
pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
pkg/trunk/highlevel/doors/doors_core/manifest.xml
pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/executive_trex_pr2/adapters.h
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/adapters.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc
pkg/trunk/highlevel/nav/include/nav/move_base.h
pkg/trunk/highlevel/nav/include/nav/move_base_local.h
pkg/trunk/highlevel/nav/manifest.xml
pkg/trunk/highlevel/nav/src/move_base.cpp
pkg/trunk/highlevel/nav/src/move_base_local.cpp
pkg/trunk/nav/wavefront/manifest.xml
pkg/trunk/nav/wavefront/src/cli.cpp
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
Added Paths:
-----------
pkg/trunk/nav/nav_robot_actions/CMakeLists.txt
pkg/trunk/nav/nav_robot_actions/Makefile
pkg/trunk/nav/nav_robot_actions/msg/
pkg/trunk/nav/nav_robot_actions/msg/MoveBaseState.msg
Removed Paths:
-------------
pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseStateNew.msg
Modified: pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-04-29 08:00:27 UTC (rev 14612)
@@ -48,7 +48,7 @@
import rospy, rostest
from std_msgs.msg import *
from robot_actions.msg import *
-from pr2_robot_actions.msg import *
+from nav_robot_actions.msg import *
from robot_msgs.msg import *
from deprecated_msgs.msg import *
from tf.transformations import *
Modified: pkg/trunk/highlevel/doors/doors_core/manifest.xml
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/manifest.xml 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/doors/doors_core/manifest.xml 2009-04-29 08:00:27 UTC (rev 14612)
@@ -13,6 +13,7 @@
<depend package="robot_msgs" />
<depend package="robot_srvs" />
<depend package="robot_actions" />
+ <depend package="nav_robot_actions" />
<depend package="pr2_robot_actions" />
<depend package="kdl" />
<depend package="angles" />
Modified: pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp 2009-04-29 08:00:27 UTC (rev 14612)
@@ -46,7 +46,7 @@
#include <pr2_robot_actions/DoorActionState.h>
#include <robot_actions/NoArgumentsActionState.h>
#include <pr2_robot_actions/SwitchControllersState.h>
-#include <pr2_robot_actions/MoveBaseStateNew.h>
+#include <nav_robot_actions/MoveBaseState.h>
#include "doors_core/executive_functions.h"
@@ -92,7 +92,7 @@
robot_actions::ActionClient<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door> open_door("open_door");
robot_actions::ActionClient<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door> release_handle("release_handle");
robot_actions::ActionClient<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door> move_base_door("move_base_door");
- robot_actions::ActionClient<robot_msgs::PoseStamped, pr2_robot_actions::MoveBaseStateNew, robot_msgs::PoseStamped> move_base_local("move_base_local");
+ robot_actions::ActionClient<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped> move_base_local("move_base_local");
timeout_medium.sleep();
robot_msgs::Door tmp_door;
Modified: pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/navigation_adapter.py 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/executive_python/src/navigation_adapter.py 2009-04-29 08:00:27 UTC (rev 14612)
@@ -38,14 +38,14 @@
roslib.load_manifest('executive_python')
import rospy
import random
-from pr2_robot_actions.msg import MoveBaseStateNew
+from nav_robot_actions.msg import MoveBaseState
from robot_msgs.msg import PoseStamped
class NavigationAdapter:
def __init__(self, no_plan_limit, time_limit, state_topic, goal_topic):
self.no_plan_limit = no_plan_limit
self.time_limit = time_limit
- rospy.Subscriber(state_topic, MoveBaseStateNew, self.update)
+ rospy.Subscriber(state_topic, MoveBaseState, self.update)
self.pub = rospy.Publisher(goal_topic, PoseStamped)
self.state = None
self.start_time = rospy.get_time()
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/executive_trex_pr2/adapters.h
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/executive_trex_pr2/adapters.h 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/executive_trex_pr2/adapters.h 2009-04-29 08:00:27 UTC (rev 14612)
@@ -53,7 +53,7 @@
#include <pr2_robot_actions/CheckPathState.h>
#include <pr2_robot_actions/NotifyDoorBlockedState.h>
#include <pr2_robot_actions/ShellCommandState.h>
-#include <pr2_robot_actions/MoveBaseStateNew.h>
+#include <nav_robot_actions/MoveBaseState.h>
#include <pr2_robot_actions/RechargeState.h>
#include <pr2_robot_actions/DetectPlugOnBaseState.h>
#include <pr2_robot_actions/MoveAndGraspPlugState.h>
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml 2009-04-29 08:00:27 UTC (rev 14612)
@@ -10,6 +10,7 @@
<depend package="doors_core" />
<depend package="outlet_detection" />
<depend package="robot_actions" />
+ <depend package="nav_robot_actions" />
<depend package="pr2_robot_actions" />
<depend package="pr2_msgs" />
<depend package="std_msgs" />
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/adapters.cc
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/adapters.cc 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/adapters.cc 2009-04-29 08:00:27 UTC (rev 14612)
@@ -74,11 +74,11 @@
/***********************************************************************
* @brief MoveBase actions with a pose message for goal and feedback
**********************************************************************/
- class MoveBaseAdapter: public ROSActionAdapter<robot_msgs::PoseStamped, pr2_robot_actions::MoveBaseStateNew, robot_msgs::PoseStamped> {
+ class MoveBaseAdapter: public ROSActionAdapter<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped> {
public:
MoveBaseAdapter(const LabelStr& agentName, const TiXmlElement& configData)
- : ROSActionAdapter<robot_msgs::PoseStamped, pr2_robot_actions::MoveBaseStateNew, robot_msgs::PoseStamped>(agentName, configData){
+ : ROSActionAdapter<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped>(agentName, configData){
}
virtual void fillDispatchParameters(robot_msgs::PoseStamped& msg, const TokenId& goalToken){
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc 2009-04-29 08:00:27 UTC (rev 14612)
@@ -247,11 +247,11 @@
// Navigation actions
executive_trex_pr2::SimpleStubAction<robot_msgs::PoseStamped> move_base("move_base");
if (getComponentParam("/trex/enable_move_base"))
- runner.connect<robot_msgs::PoseStamped, pr2_robot_actions::MoveBaseStateNew, robot_msgs::PoseStamped>(move_base);
+ runner.connect<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped>(move_base);
executive_trex_pr2::SimpleStubAction<robot_msgs::PoseStamped> move_base_local("move_base_local");
if (getComponentParam("/trex/enable_move_base_local"))
- runner.connect<robot_msgs::PoseStamped, pr2_robot_actions::MoveBaseStateNew, robot_msgs::PoseStamped>(move_base_local);
+ runner.connect<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped>(move_base_local);
// Misc.
executive_trex_pr2::SimpleStubAction<std_msgs::Float32> recharge("recharge_controller");
Modified: pkg/trunk/highlevel/nav/include/nav/move_base.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base.h 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/nav/include/nav/move_base.h 2009-04-29 08:00:27 UTC (rev 14612)
@@ -38,7 +38,7 @@
#define NAV_MOVE_BASE_ACTION_H_
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
-#include <pr2_robot_actions/MoveBaseStateNew.h>
+#include <nav_robot_actions/MoveBaseState.h>
#include <robot_msgs/PoseStamped.h>
#include <ros/node.h>
#include <costmap_2d/costmap_2d_ros.h>
Modified: pkg/trunk/highlevel/nav/include/nav/move_base_local.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base_local.h 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/nav/include/nav/move_base_local.h 2009-04-29 08:00:27 UTC (rev 14612)
@@ -38,7 +38,7 @@
#define NAV_MOVE_BASE_ACTION_H_
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
-#include <pr2_robot_actions/MoveBaseStateNew.h>
+#include <nav_robot_actions/MoveBaseState.h>
#include <robot_msgs/PoseStamped.h>
#include <ros/node.h>
#include <costmap_2d/costmap_2d_ros.h>
Modified: pkg/trunk/highlevel/nav/manifest.xml
===================================================================
--- pkg/trunk/highlevel/nav/manifest.xml 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/nav/manifest.xml 2009-04-29 08:00:27 UTC (rev 14612)
@@ -14,6 +14,7 @@
<depend package="deprecated_msgs"/>
<depend package="robot_actions"/>
<depend package="pr2_robot_actions"/>
+ <depend package="nav_robot_actions"/>
<depend package="base_local_planner"/>
<depend package="costmap_2d"/>
<depend package="tf"/>
Modified: pkg/trunk/highlevel/nav/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base.cpp 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/nav/src/move_base.cpp 2009-04-29 08:00:27 UTC (rev 14612)
@@ -261,7 +261,7 @@
nav::MoveBase move_base(ros_node, tf);
robot_actions::ActionRunner runner(20.0);
- runner.connect<robot_msgs::PoseStamped, pr2_robot_actions::MoveBaseStateNew, robot_msgs::PoseStamped>(move_base);
+ runner.connect<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped>(move_base);
runner.run();
ros_node.spin();
Modified: pkg/trunk/highlevel/nav/src/move_base_local.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_local.cpp 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/nav/src/move_base_local.cpp 2009-04-29 08:00:27 UTC (rev 14612)
@@ -39,7 +39,7 @@
using namespace base_local_planner;
using namespace costmap_2d;
using namespace robot_actions;
-using namespace pr2_robot_actions;
+using namespace nav_robot_actions;
namespace nav {
MoveBaseLocal::MoveBaseLocal(ros::Node& ros_node, tf::TransformListener& tf) :
@@ -215,7 +215,7 @@
nav::MoveBaseLocal move_base(ros_node, tf);
robot_actions::ActionRunner runner(20.0);
- runner.connect<robot_msgs::PoseStamped, pr2_robot_actions::MoveBaseStateNew, robot_msgs::PoseStamped>(move_base);
+ runner.connect<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped>(move_base);
runner.run();
ros_node.spin();
Deleted: pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseState.msg
===================================================================
--- pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseState.msg 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseState.msg 2009-04-29 08:00:27 UTC (rev 14612)
@@ -1,11 +0,0 @@
-# This will be deprecated
-Header header
-
-# Status
-robot_actions/ActionStatus status
-
-# Target position and orientation
-Pose2D goal
-
-# Actual position and orientation
-Pose2D feedback
\ No newline at end of file
Deleted: pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseStateNew.msg
===================================================================
--- pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseStateNew.msg 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseStateNew.msg 2009-04-29 08:00:27 UTC (rev 14612)
@@ -1,10 +0,0 @@
-Header header
-
-# Status
-robot_actions/ActionStatus status
-
-# Target position and orientation
-robot_msgs/PoseStamped goal
-
-# Actual position and orientation
-robot_msgs/PoseStamped feedback
Copied: pkg/trunk/nav/nav_robot_actions/CMakeLists.txt (from rev 14422, pkg/trunk/highlevel/pr2_robot_actions/CMakeLists.txt)
===================================================================
--- pkg/trunk/nav/nav_robot_actions/CMakeLists.txt (rev 0)
+++ pkg/trunk/nav/nav_robot_actions/CMakeLists.txt 2009-04-29 08:00:27 UTC (rev 14612)
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(robot_actions)
+genmsg()
Copied: pkg/trunk/nav/nav_robot_actions/Makefile (from rev 14422, pkg/trunk/highlevel/pr2_robot_actions/Makefile)
===================================================================
--- pkg/trunk/nav/nav_robot_actions/Makefile (rev 0)
+++ pkg/trunk/nav/nav_robot_actions/Makefile 2009-04-29 08:00:27 UTC (rev 14612)
@@ -0,0 +1,2 @@
+include $(shell rospack find mk)/cmake.mk
+
Added: pkg/trunk/nav/nav_robot_actions/msg/MoveBaseState.msg
===================================================================
--- pkg/trunk/nav/nav_robot_actions/msg/MoveBaseState.msg (rev 0)
+++ pkg/trunk/nav/nav_robot_actions/msg/MoveBaseState.msg 2009-04-29 08:00:27 UTC (rev 14612)
@@ -0,0 +1,10 @@
+Header header
+
+# Status
+robot_actions/ActionStatus status
+
+# Target position and orientation
+robot_msgs/PoseStamped goal
+
+# Actual position and orientation
+robot_msgs/PoseStamped feedback
Modified: pkg/trunk/nav/wavefront/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront/manifest.xml 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/nav/wavefront/manifest.xml 2009-04-29 08:00:27 UTC (rev 14612)
@@ -8,6 +8,6 @@
<depend package="robot_srvs" />
<depend package="robot_msgs" />
<depend package="robot_actions" />
- <depend package="pr2_robot_actions" />
+ <depend package="nav_robot_actions" />
<depend package="tf"/>
</package>
Modified: pkg/trunk/nav/wavefront/src/cli.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/cli.cpp 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/nav/wavefront/src/cli.cpp 2009-04-29 08:00:27 UTC (rev 14612)
@@ -2,14 +2,14 @@
#include <stdlib.h>
#include "ros/node.h"
#include "ros/publisher.h"
-#include "pr2_robot_actions/MoveBaseState.h"
+#include "nav_robot_actions/MoveBaseState.h"
#include "robot_msgs/PoseStamped.h"
#include "tf/tf.h"
class WavefrontCLI : public ros::Node
{
public:
- pr2_robot_actions::MoveBaseState wf_state;
+ nav_robot_actions::MoveBaseState wf_state;
robot_msgs::PoseStamped wf_goal;
enum { WF_IDLE, WF_SEEKING_GOAL, WF_DONE } state;
Modified: pkg/trunk/nav/wavefront/src/wavefront_node.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-04-29 08:00:27 UTC (rev 14612)
@@ -67,7 +67,7 @@
Publishes to (name / type):
- @b "cmd_vel" robot_msgs/PoseDot : velocity commands to robot
-- @b "state" pr2_robot_actions/MoveBaseState : current planner state (e.g., goal reached, no path)
+- @b "state" nav_robot_actions/MoveBaseState : current planner state (e.g., goal reached, no path)
- @b "gui_path" robot_msgs/Polyline : current global path (for visualization)
- @b "gui_laser" robot_msgs/Polyline : re-projected laser scans (for visualization)
@@ -101,7 +101,7 @@
#include "boost/thread/mutex.hpp"
// The messages that we'll use
-#include <pr2_robot_actions/MoveBaseState.h>
+#include <nav_robot_actions/MoveBaseState.h>
#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/PointCloud.h>
@@ -149,7 +149,7 @@
REACHED_GOAL
} planner_state;
- tf::Stamped<btTransform> global_pose;
+ tf::Stamped<btTransform> global_pose_;
// If we can't reach the goal, note when it happened and keep trying a bit
//ros::Time stuck_time;
@@ -157,6 +157,7 @@
bool enable;
// Current goal
double goal[3];
+ tf::Stamped<tf::Pose> goalPose_;
// Direction that we're rotating in order to assume the goal
// orientation
int rotate_dir;
@@ -197,7 +198,7 @@
//MsgRobotBase2DOdom odomMsg;
robot_msgs::Polyline polylineMsg;
robot_msgs::Polyline pointcloudMsg;
- pr2_robot_actions::MoveBaseState pstate;
+ nav_robot_actions::MoveBaseState pstate;
//MsgRobotBase2DOdom prevOdom;
bool firstodom;
@@ -283,7 +284,7 @@
tf(*this, true, ros::Duration(10)) // cache for 10 sec, no extrapolation
{
// Initialize global pose. Will be set in control loop based on actual data.
- ///\todo does this need to be initialized? global_pose.setIdentity();
+ ///\todo does this need to be initialized? global_pose_.setIdentity();
// set a few parameters. leave defaults just as in the ctor initializer list
param("~dist_eps", dist_eps, 1.0);
@@ -360,7 +361,7 @@
this->firstodom = true;
- advertise<pr2_robot_actions::MoveBaseState>("state",1);
+ advertise<nav_robot_actions::MoveBaseState>("state",1);
advertise<robot_msgs::Polyline>("gui_path",1);
advertise<robot_msgs::Polyline>("gui_laser",1);
advertise<robot_msgs::PoseDot>("cmd_vel",1);
@@ -384,15 +385,14 @@
this->enable = 1;
if(this->enable){
- tf::Stamped<tf::Pose> goalPose;
- tf::PoseStampedMsgToTF(goalMsg, goalPose);
+ tf::PoseStampedMsgToTF(goalMsg, this->goalPose_);
// Populate goal data
- this->goal[0] = goalPose.getOrigin().getX();
- this->goal[1] = goalPose.getOrigin().getY();
+ this->goal[0] = this->goalPose_.getOrigin().getX();
+ this->goal[1] = this->goalPose_.getOrigin().getY();
double yaw, pitch, roll;
- goalPose.getBasis().getEulerZYX(yaw, pitch, roll);
+ this->goalPose_.getBasis().getEulerZYX(yaw, pitch, roll);
this->goal[2] = yaw;
this->planner_state = NEW_GOAL;
@@ -412,17 +412,12 @@
this->pstate.status.value = this->pstate.status.SUCCESS;
}
- double yaw,pitch,roll;
- btMatrix3x3 mat = global_pose.getBasis();
- mat.getEulerZYX(yaw, pitch, roll);
+ robot_msgs::PoseStamped pose_msg;
+ tf::PoseStampedTFToMsg(global_pose_, pose_msg);
// Fill out and publish response
- this->pstate.feedback.x = global_pose.getOrigin().x();
- this->pstate.feedback.y = global_pose.getOrigin().y();
- this->pstate.feedback.th = yaw;
- this->pstate.goal.x = this->goal[0];
- this->pstate.goal.y = this->goal[1];
- this->pstate.goal.th = this->goal[2];
+ this->pstate.feedback = pose_msg;
+ this->pstate.goal = goalMsg;
publish("state",this->pstate);
this->lock.unlock();
}
@@ -579,7 +574,7 @@
// laserMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else
try
{
- this->tf.transformPose("/map", robotPose, global_pose);
+ this->tf.transformPose("/map", robotPose, global_pose_);
}
catch(tf::LookupException& ex)
{
@@ -626,12 +621,12 @@
{
double yaw,pitch,roll; //fixme make cleaner namespace
- btMatrix3x3 mat = global_pose.getBasis();
+ btMatrix3x3 mat = global_pose_.getBasis();
mat.getEulerZYX(yaw, pitch, roll);
// Are we done?
if(plan_check_done(this->plan,
- global_pose.getOrigin().x(), global_pose.getOrigin().y(), yaw,
+ global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), yaw,
this->goal[0], this->goal[1], this->goal[2],
this->dist_eps, this->ang_eps))
{
@@ -651,11 +646,11 @@
// Try a local plan
if((this->planner_state == NEW_GOAL) ||
- (plan_do_local(this->plan, global_pose.getOrigin().x(), global_pose.getOrigin().y(),
+ (plan_do_local(this->plan, global_pose_.getOrigin().x(), global_pose_.getOrigin().y(),
this->plan_halfwidth) < 0))
{
// Fallback on global plan
- if(plan_do_global(this->plan, global_pose.getOrigin().x(), global_pose.getOrigin().y(),
+ if(plan_do_global(this->plan, global_pose_.getOrigin().x(), global_pose_.getOrigin().y(),
this->goal[0], this->goal[1]) < 0)
{
// no global plan
@@ -685,7 +680,7 @@
{
// global plan succeeded; now try the local plan again
this->printed_warning = false;
- if(plan_do_local(this->plan, global_pose.getOrigin().x(), global_pose.getOrigin().y(),
+ if(plan_do_local(this->plan, global_pose_.getOrigin().x(), global_pose_.getOrigin().y(),
this->plan_halfwidth) < 0)
{
// no local plan; better luck next time through
@@ -706,12 +701,12 @@
// double yaw,pitch,roll; //used temporarily earlier fixme make cleaner
//btMatrix3x3
- mat = global_pose.getBasis();
+ mat = global_pose_.getBasis();
mat.getEulerZYX(yaw, pitch, roll);
if(plan_compute_diffdrive_cmds(this->plan, &vx, &va,
&this->rotate_dir,
- global_pose.getOrigin().x(), global_pose.getOrigin().y(),
+ global_pose_.getOrigin().x(), global_pose_.getOrigin().y(),
yaw,
this->goal[0], this->goal[1],
this->goal[2],
@@ -759,21 +754,16 @@
default:
assert(0);
}
- double yaw,pitch,roll;
- btMatrix3x3 mat = global_pose.getBasis();
- mat.getEulerZYX(yaw, pitch, roll);
-
if(this->enable && (this->planner_state == PURSUING_GOAL))
this->pstate.status.value = this->pstate.status.ACTIVE;
else
this->pstate.status.value = this->pstate.status.SUCCESS;
- this->pstate.feedback.x = global_pose.getOrigin().x();
- this->pstate.feedback.y = global_pose.getOrigin().y();
- this->pstate.feedback.th = yaw;
- this->pstate.goal.x = this->goal[0];
- this->pstate.goal.y = this->goal[1];
- this->pstate.goal.th = this->goal[2];
+ robot_msgs::PoseStamped pose_out;
+ tf::PoseStampedTFToMsg(global_pose_, pose_out);
+ this->pstate.feedback = pose_out;
+ tf::PoseStampedTFToMsg(this->goalPose_, pose_out);
+ this->pstate.goal = pose_out;
publish("state",this->pstate);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-29 08:01:08
|
Revision: 14613
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14613&view=rev
Author: tfoote
Date: 2009-04-29 08:01:07 +0000 (Wed, 29 Apr 2009)
Log Message:
-----------
renaming visualization_tools to visualization_core as per ros meeting decision
Added Paths:
-----------
pkg/trunk/visualization_core/
Removed Paths:
-------------
pkg/trunk/visualization_tools/
Property changes on: pkg/trunk/visualization_core
___________________________________________________________________
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-29 08:13:57
|
Revision: 14615
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14615&view=rev
Author: tfoote
Date: 2009-04-29 08:13:55 +0000 (Wed, 29 Apr 2009)
Log Message:
-----------
Renaming prcore to common following ros meeting decision.
Added Paths:
-----------
pkg/trunk/common/
Removed Paths:
-------------
pkg/trunk/prcore/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-04-29 20:15:51
|
Revision: 14633
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14633&view=rev
Author: hsujohnhsu
Date: 2009-04-29 20:15:41 +0000 (Wed, 29 Apr 2009)
Log Message:
-----------
name change gripper.xacro.xml -> l_gripper.xacro.xml and r_gripper.xacro.xml
update gripper examples in arm_gazebo.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/l_gripper.launch
pkg/trunk/robot_descriptions/pr2/pr2_defs/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/demos/arm_gazebo/l_gripper_controller.xml
pkg/trunk/demos/arm_gazebo/l_gripper_effort_controller.xml
pkg/trunk/demos/arm_gazebo/r_gripper.launch
pkg/trunk/demos/arm_gazebo/r_gripper_controller.xml
pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/l_gripper.xacro.xml
pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/r_gripper.xacro.xml
Removed Paths:
-------------
pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/gripper.xacro.xml
Modified: pkg/trunk/demos/arm_gazebo/l_gripper.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_gripper.launch 2009-04-29 20:13:59 UTC (rev 14632)
+++ pkg/trunk/demos/arm_gazebo/l_gripper.launch 2009-04-29 20:15:41 UTC (rev 14633)
@@ -4,7 +4,7 @@
-->
<group name="wg">
<!-- send pr2_l_arm.xml to param server -->
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/gripper.xacro.xml'" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/l_gripper.xacro.xml'" />
<!-- -g flag runs gazebo in gui-less mode -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
Added: pkg/trunk/demos/arm_gazebo/l_gripper_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_gripper_controller.xml (rev 0)
+++ pkg/trunk/demos/arm_gazebo/l_gripper_controller.xml 2009-04-29 20:15:41 UTC (rev 14633)
@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<controllers>
+ <controller name="l_gripper_controller" topic="l_gripper_controller" type="JointPositionControllerNode">
+ <joint name="l_gripper_joint">
+ <pid p="0.1" d="0.0" i="0.0" iClamp="0" />
+ </joint>
+ </controller>
+</controllers>
Added: pkg/trunk/demos/arm_gazebo/l_gripper_effort_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_gripper_effort_controller.xml (rev 0)
+++ pkg/trunk/demos/arm_gazebo/l_gripper_effort_controller.xml 2009-04-29 20:15:41 UTC (rev 14633)
@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<controllers>
+ <controller name="l_gripper_controller" topic="l_gripper_controller" type="JointEffortControllerNode">
+ <joint name="l_gripper_joint">
+ <pid p="1.0" d="0.0" i="0.0" iClamp="0" />
+ </joint>
+ </controller>
+</controllers>
Added: pkg/trunk/demos/arm_gazebo/r_gripper.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_gripper.launch (rev 0)
+++ pkg/trunk/demos/arm_gazebo/r_gripper.launch 2009-04-29 20:15:41 UTC (rev 14633)
@@ -0,0 +1,38 @@
+<launch>
+ <!--
+ Run a PR2 left arm in simulation
+ -->
+ <group name="wg">
+ <!-- send pr2_r_arm.xml to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_gripper.xacro.xml'" />
+
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen"/>
+
+ <!-- start arm controller -->
+ <!--
+ <node pkg="mechanism_control" type="spawner.py" args="$(find arm_gazebo)/r_arm_default_controller.xml" respawn="false" />
+ -->
+
+ <!-- send arm a command -->
+ <!--
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set r_gripper_controller 0.5" respawn="false" output="screen" />
+ -->
+
+ <!-- for visualization -->
+ <!--
+ <node pkg="rviz" type="rviz" respawn="false" output="screen" />
+ <include file="$(find arm_gazebo)/debug_plot.launch"/>
+ -->
+
+ </group>
+</launch>
+
Added: pkg/trunk/demos/arm_gazebo/r_gripper_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_gripper_controller.xml (rev 0)
+++ pkg/trunk/demos/arm_gazebo/r_gripper_controller.xml 2009-04-29 20:15:41 UTC (rev 14633)
@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<controllers>
+ <controller name="r_gripper_controller" topic="r_gripper_controller" type="JointPositionControllerNode">
+ <joint name="r_gripper_joint">
+ <pid p="1.0" d="0.0" i="0.0" iClamp="0" />
+ </joint>
+ </controller>
+</controllers>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/CMakeLists.txt 2009-04-29 20:13:59 UTC (rev 14632)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/CMakeLists.txt 2009-04-29 20:15:41 UTC (rev 14633)
@@ -54,16 +54,24 @@
ARGS ${pr2rarm} > ${pr2rarm_expanded}
DEPENDS ${pr2rarm} ${def_files})
-set(pr2gripper "${CMAKE_CURRENT_SOURCE_DIR}/robots/gripper.xacro.xml")
-set(pr2gripper_expanded "${CMAKE_CURRENT_SOURCE_DIR}/robots/gripper.expanded.xml")
+set(pr2lgripper "${CMAKE_CURRENT_SOURCE_DIR}/robots/l_gripper.xacro.xml")
+set(pr2lgripper_expanded "${CMAKE_CURRENT_SOURCE_DIR}/robots/l_gripper.expanded.xml")
add_custom_command(
- OUTPUT ${pr2gripper_expanded}
+ OUTPUT ${pr2lgripper_expanded}
COMMAND ${xacro_PACKAGE_PATH}/xacro.py
- ARGS ${pr2gripper} > ${pr2gripper_expanded}
- DEPENDS ${pr2gripper} ${def_files})
+ ARGS ${pr2lgripper} > ${pr2lgripper_expanded}
+ DEPENDS ${pr2lgripper} ${def_files})
+set(pr2rgripper "${CMAKE_CURRENT_SOURCE_DIR}/robots/r_gripper.xacro.xml")
+set(pr2rgripper_expanded "${CMAKE_CURRENT_SOURCE_DIR}/robots/r_gripper.expanded.xml")
+add_custom_command(
+ OUTPUT ${pr2rgripper_expanded}
+ COMMAND ${xacro_PACKAGE_PATH}/xacro.py
+ ARGS ${pr2rgripper} > ${pr2rgripper_expanded}
+ DEPENDS ${pr2rgripper} ${def_files})
+
#iv files
# iterate through all the stl files and generate *.mesh and *.iv files
file(GLOB pr2_stl_files ${CMAKE_CURRENT_SOURCE_DIR}/meshes/*.stl)
@@ -132,6 +140,6 @@
endforeach(it)
-add_custom_target(media_files ALL DEPENDS ${pr2robot_expanded} ${pr2prototype1_expanded} ${pr2larm_expanded} ${pr2rarm_expanded} ${pr2gripper_expanded} ${pr2_gen_files})
+add_custom_target(media_files ALL DEPENDS ${pr2robot_expanded} ${pr2prototype1_expanded} ${pr2larm_expanded} ${pr2rarm_expanded} ${pr2lgripper_expanded} ${pr2rgripper_expanded} ${pr2_gen_files})
#TODO; stop bin and lib from being generated
Deleted: pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/gripper.xacro.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/gripper.xacro.xml 2009-04-29 20:13:59 UTC (rev 14632)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/gripper.xacro.xml 2009-04-29 20:15:41 UTC (rev 14633)
@@ -1,72 +0,0 @@
-<?xml version="1.0"?>
-<robot name="pr2"
- xmlns:xi="http://www.w3.org/2001/XInclude"
- xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
- xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
- xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
- xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
- xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
- xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
- xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
- xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
- xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
- xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
- xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
-
-
- <!-- declare the path where all models/textures/materials are stored -->
- <resource location="ros-pkg://pr2_defs/meshes"/>
-
- <include filename="$(find pr2_defs)/defs/gripper_defs.xml" />
- <include filename="$(find pr2_defs)/defs/gazebo_defs.xml" />
-
- <pr2_gripper side="l" parent="base_link" />
-
- <!-- Solid Base -->
- <joint name="base_joint" type="planar">
- </joint>
- <link name="base_link"><!-- link specifying the base of the robot -->
- <parent name=" world" />
- <!-- rotation of a local coordinate frame attached to the link with respect to a global coordinate frame -->
- <origin xyz=" 0 0 1.002 " rpy=" 0 0 0" /> <!-- position of a local coordinate frame attached to the link with respect to the parent link's coordinate frame -->
- <joint name="base_joint" />
- <inertial>
- <mass value="1000" />
- <com xyz=" 0 0 0 " /> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
- <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000" />
- </inertial>
- <visual>
- <origin xyz="0 0 -1" rpy="0 0 0 " /> <!-- location defined with respect to the link origin in a local coordinate frame -->
- <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
- <map name="gazebo_material" flag="gazebo">
- <elem key="material">Gazebo/White</elem>
- </map>
- <geometry name="pr2_base_mesh_file">
- <mesh scale="20 20 0.01" />
- </geometry>
- </visual>
- <collision>
- <origin xyz="0 0 -1" rpy="0.0 0.0 0.0 " /> <!-- default box is centered at the origin -->
- <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
- <geometry name="base_collision_geom"> <!-- think about putting mesh here as well -->
- <box size="20 20 0.01" />
- </geometry>
- </collision>
- </link>
-
- <map name="gazebo_material" flag="gazebo">
- <verbatim>
- <!-- ros_p3d for position groundtruth -->
- <controller:ros_p3d name="p3d_l_gripper_palm_controller" plugin="libros_p3d.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
- <bodyName>l_gripper_palm_link</bodyName>
- <topicName>l_gripper_palm_pose_ground_truth</topicName>
- <frameName>map</frameName>
- <interface:position name="p3d_l_gripper_palm_position"/>
- </controller:ros_p3d>
- </verbatim>
- </map>
-
-
-</robot>
Copied: pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/l_gripper.xacro.xml (from rev 14585, pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/gripper.xacro.xml)
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/l_gripper.xacro.xml (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/l_gripper.xacro.xml 2009-04-29 20:15:41 UTC (rev 14633)
@@ -0,0 +1,72 @@
+<?xml version="1.0"?>
+<robot name="pr2"
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
+
+
+ <!-- declare the path where all models/textures/materials are stored -->
+ <resource location="ros-pkg://pr2_defs/meshes"/>
+
+ <include filename="$(find pr2_defs)/defs/gripper_defs.xml" />
+ <include filename="$(find pr2_defs)/defs/gazebo_defs.xml" />
+
+ <pr2_gripper side="l" parent="base_link" />
+
+ <!-- Solid Base -->
+ <joint name="base_joint" type="planar">
+ </joint>
+ <link name="base_link"><!-- link specifying the base of the robot -->
+ <parent name=" world" />
+ <!-- rotation of a local coordinate frame attached to the link with respect to a global coordinate frame -->
+ <origin xyz=" 0 0 1.002 " rpy=" 0 0 0" /> <!-- position of a local coordinate frame attached to the link with respect to the parent link's coordinate frame -->
+ <joint name="base_joint" />
+ <inertial>
+ <mass value="1000" />
+ <com xyz=" 0 0 0 " /> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 -1" rpy="0 0 0 " /> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/White</elem>
+ </map>
+ <geometry name="pr2_base_mesh_file">
+ <mesh scale="20 20 0.01" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 -1" rpy="0.0 0.0 0.0 " /> <!-- default box is centered at the origin -->
+ <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <geometry name="base_collision_geom"> <!-- think about putting mesh here as well -->
+ <box size="20 20 0.01" />
+ </geometry>
+ </collision>
+ </link>
+
+ <map name="gazebo_material" flag="gazebo">
+ <verbatim>
+ <!-- ros_p3d for position groundtruth -->
+ <controller:ros_p3d name="p3d_l_gripper_palm_controller" plugin="libros_p3d.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>l_gripper_palm_link</bodyName>
+ <topicName>l_gripper_palm_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="p3d_l_gripper_palm_position"/>
+ </controller:ros_p3d>
+ </verbatim>
+ </map>
+
+
+</robot>
Property changes on: pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/l_gripper.xacro.xml
___________________________________________________________________
Added: svn:mergeinfo
+
Copied: pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/r_gripper.xacro.xml (from rev 14585, pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/gripper.xacro.xml)
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/r_gripper.xacro.xml (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/r_gripper.xacro.xml 2009-04-29 20:15:41 UTC (rev 14633)
@@ -0,0 +1,72 @@
+<?xml version="1.0"?>
+<robot name="pr2"
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
+
+
+ <!-- declare the path where all models/textures/materials are stored -->
+ <resource location="ros-pkg://pr2_defs/meshes"/>
+
+ <include filename="$(find pr2_defs)/defs/gripper_defs.xml" />
+ <include filename="$(find pr2_defs)/defs/gazebo_defs.xml" />
+
+ <pr2_gripper side="r" parent="base_link" />
+
+ <!-- Solid Base -->
+ <joint name="base_joint" type="planar">
+ </joint>
+ <link name="base_link"><!-- link specifying the base of the robot -->
+ <parent name=" world" />
+ <!-- rotation of a local coordinate frame attached to the link with respect to a global coordinate frame -->
+ <origin xyz=" 0 0 1.002 " rpy=" 0 0 0" /> <!-- position of a local coordinate frame attached to the link with respect to the parent link's coordinate frame -->
+ <joint name="base_joint" />
+ <inertial>
+ <mass value="1000" />
+ <com xyz=" 0 0 0 " /> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 -1" rpy="0 0 0 " /> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/White</elem>
+ </map>
+ <geometry name="pr2_base_mesh_file">
+ <mesh scale="20 20 0.01" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 -1" rpy="0.0 0.0 0.0 " /> <!-- default box is centered at the origin -->
+ <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <geometry name="base_collision_geom"> <!-- think about putting mesh here as well -->
+ <box size="20 20 0.01" />
+ </geometry>
+ </collision>
+ </link>
+
+ <map name="gazebo_material" flag="gazebo">
+ <verbatim>
+ <!-- ros_p3d for position groundtruth -->
+ <controller:ros_p3d name="p3d_r_gripper_palm_controller" plugin="libros_p3d.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>r_gripper_palm_link</bodyName>
+ <topicName>r_gripper_palm_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="p3d_r_gripper_palm_position"/>
+ </controller:ros_p3d>
+ </verbatim>
+ </map>
+
+
+</robot>
Property changes on: pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/r_gripper.xacro.xml
___________________________________________________________________
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-04-29 20:49:56
|
Revision: 14634
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14634&view=rev
Author: hsujohnhsu
Date: 2009-04-29 20:49:45 +0000 (Wed, 29 Apr 2009)
Log Message:
-----------
update erratic robot with better masses.
use a world with higher accuracy.
release caster steering link.
Modified Paths:
--------------
pkg/trunk/demos/erratic_gazebo/erratic.launch
pkg/trunk/robot_descriptions/erratic/erratic_defs/robots/erratic.xml
Added Paths:
-----------
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/simple_erratic.world
Modified: pkg/trunk/demos/erratic_gazebo/erratic.launch
===================================================================
--- pkg/trunk/demos/erratic_gazebo/erratic.launch 2009-04-29 20:15:41 UTC (rev 14633)
+++ pkg/trunk/demos/erratic_gazebo/erratic.launch 2009-04-29 20:49:45 UTC (rev 14634)
@@ -5,7 +5,7 @@
<param name="robotdesc/erratic" command="$(find xacro)/xacro.py '$(find erratic_defs)/robots/erratic.xml'" />
<!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/simple.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/simple_erratic.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
Modified: pkg/trunk/robot_descriptions/erratic/erratic_defs/robots/erratic.xml
===================================================================
--- pkg/trunk/robot_descriptions/erratic/erratic_defs/robots/erratic.xml 2009-04-29 20:15:41 UTC (rev 14633)
+++ pkg/trunk/robot_descriptions/erratic/erratic_defs/robots/erratic.xml 2009-04-29 20:49:45 UTC (rev 14634)
@@ -20,10 +20,11 @@
<joint name="base_joint" />
<inertial>
- <mass value="20" />
+ <mass value="10" />
<com xyz="-0.1 0 ${base_size_z/2}" />
- <inertia ixx="5.652232699207" ixy="-0.009719934438" ixz="1.293988226423"
- iyy="5.669473158652" iyz="-0.007379583694" izz="3.683196351726" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0"
+ iyy="1.0" iyz="0.0"
+ izz="1.0" />
</inertial>
<visual>
@@ -53,8 +54,9 @@
<inertial>
<mass value="0.01"/>
<com xyz="0 0 ${base_size_z/2}" />
- <inertia ixx="5.652232699207" ixy="-0.009719934438" ixz="1.293988226423"
- iyy="5.669473158652" iyz="-0.007379583694" izz="3.683196351726" />
+ <inertia ixx="0.0652232699207" ixy="0.0" ixz="0.0"
+ iyy="0.0669473158652" iyz="0.0"
+ izz="0.0683196351726" />
</inertial>
<visual>
@@ -93,8 +95,9 @@
<inertial>
<mass value="0.01" /> <!-- check jmh 20081205 -->
<com xyz=" 0 0 0 " />
- <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983"
- iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943" />
+ <inertia ixx="0.012411765597" ixy="0.0" ixz="0.0"
+ iyy="0.015218160428" iyz="0.0"
+ izz="0.011763977943" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
@@ -134,8 +137,9 @@
<inertial>
<mass value="0.1"/>
<com xyz="0 0 ${base_size_z/2}" />
- <inertia ixx="5.652232699207" ixy="-0.009719934438" ixz="1.293988226423"
- iyy="5.669473158652" iyz="-0.007379583694" izz="3.683196351726" />
+ <inertia ixx="0.01" ixy="0.0" ixz="0.0"
+ iyy="0.01" iyz="0.0"
+ izz="0.01" />
</inertial>
<visual>
@@ -160,20 +164,22 @@
<parent name="base_caster_box_link" />
<origin xyz="-0.265 0 0.0" rpy="0 0 0" />
- <!--<joint name="base_caster_support_joint" type="revolute">
+ <joint name="base_caster_support_joint" type="revolute">
<axis xyz="0 0 1" />
<anchor xyz="0.01 0 0" />
<limit effort="100" velocity="100" k_velocity="0" />
<calibration values="1.5 -1" />
<joint_properties damping="0.0" friction="0.0" />
- </joint>-->
+ </joint>
+ <!--
<joint name="base_caster_support_joint" type="fixed"/>
-
+ -->
<inertial>
<mass value="0.01"/>
<com xyz="0 0 0" />
- <inertia ixx="5.652232699207" ixy="-0.009719934438" ixz="1.293988226423"
- iyy="5.669473158652" iyz="-0.007379583694" izz="3.683196351726" />
+ <inertia ixx="0.01" ixy="0.0" ixz="0.0"
+ iyy="0.01" iyz="0.0"
+ izz="0.01" />
</inertial>
<visual>
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/simple_erratic.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/simple_erratic.world (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/simple_erratic.world 2009-04-29 20:49:45 UTC (rev 14634)
@@ -0,0 +1,263 @@
+<?xml version="1.0"?>
+
+<gazebo:world
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
+ xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geo="http://willowgarage.com/xmlschema/#geo"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >
+
+ <verbosity>5</verbosity>
+
+<!-- cfm is 1e-5 for single precision -->
+<!-- erp is typically .1-.8 -->
+<!-- here's the global contact cfm/erp -->
+ <physics:ode>
+ <stepTime>0.001</stepTime>
+ <gravity>0 0 -9.8</gravity>
+ <cfm>0.000000000001</cfm>
+ <erp>0.2</erp>
+ <quickStep>true</quickStep>
+ <quickStepIters>300</quickStepIters>
+ <quickStepW>1.3</quickStepW>
+ </physics:ode>
+
+ <geo:origin>
+ <lat>37.4270909558</lat><lon>-122.077919338</lon>
+ </geo:origin>
+
+ <rendering:gui>
+ <type>fltk</type>
+ <imageSize>1024 800</imageSize>
+ <pos>0 0</pos>
+ <frames>
+ <row height="100%">
+ <camera width="100%">
+ <xyz>0 0 20</xyz>
+ <rpy>0 90 90</rpy>
+ </camera>
+ </row>
+ </frames>
+ </rendering:gui>
+
+
+ <rendering:ogre>
+ <ambient>1.0 1.0 1.0 1.0</ambient>
+ <sky>
+ <material>Gazebo/CloudySky</material>
+ </sky>
+ <gazeboPath>media</gazeboPath>
+ <grid>false</grid>
+ <maxUpdateRate>10</maxUpdateRate>
+ </rendering:ogre>
+
+
+ <model:physical name="gplane">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <static>true</static>
+
+ <body:plane name="plane">
+ <geom:plane name="plane">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
+ <normal>0 0 1</normal>
+ <size>51.3 51.3</size>
+ <!-- map3.png -->
+ <material>PR2/floor_texture</material>
+ </geom:plane>
+ </body:plane>
+ </model:physical>
+
+ <!-- The "desk"-->
+ <!-- small desks -->
+ <model:physical name="../gazebo_objects/desk1_model">
+ <xyz>2.28 -0.21 0</xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <include embedded="true">
+ <!--xi:include href="../gazebo_objects/desk6.model" /--> <!-- block legs -->
+ <xi:include href="../gazebo_objects/desk4.model" /> <!-- skinny pole legs -->
+ </include>
+ </model:physical>
+
+ <!-- The second "desk"-->
+ <model:physical name="../gazebo_objects/desk2_model">
+ <xyz>2.25 -3.0 0</xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <include embedded="true">
+ <xi:include href="../gazebo_objects/desk5.model" />
+ </include>
+ </model:physical>
+
+ <!-- The small cylinder "cup" -->
+ <model:physical name="cylinder1_model">
+ <xyz> 2.5 0.0 1.5</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <body:cylinder name="cylinder1_body">
+ <geom:cylinder name="cylinder1_geom">
+ <kp>100000000.0</kp>
+ <kd>0.1</kd>
+ <mesh>default</mesh>
+ <size>0.025 0.075</size>
+ <mass> 0.05</mass>
+ <visual>
+ <size> 0.05 0.05 0.075</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_cylinder</mesh>
+ </visual>
+ </geom:cylinder>
+ <geom:box name="cylinder1_base_geom">
+ <kp>100000000.0</kp>
+ <kd>0.1</kd>
+ <mesh>default</mesh>
+ <xyz>0.0 0.0 -0.033</xyz>
+ <size>0.05 0.05 0.01</size>
+ <mass> 0.01</mass>
+ <visual>
+ <size> 0.05 0.05 0.01</size>
+ <material>Gazebo/Fish</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:cylinder>
+ </model:physical>
+
+ <!-- The small box "cup" -->
+ <model:physical name="../gazebo_objects/object1">
+ <xyz>2.28 -0.21 0</xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <include embedded="true">
+ <xi:include href="../gazebo_objects/object1.model" />
+ </include>
+ </model:physical>
+
+
+
+ <!-- The small ball -->
+ <model:physical name="sphere1_model">
+ <xyz> 2.5 -2.8 1.5</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <body:sphere name="sphere1_body">
+ <geom:sphere name="sphere1_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
+ <mesh>default</mesh>
+ <size> 0.15</size>
+ <mass> 1.0</mass>
+ <visual>
+ <size> 0.3 0.3 0.3</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_sphere</mesh>
+ </visual>
+ </geom:sphere>
+ </body:sphere>
+ </model:physical>
+
+ <!-- The large ball map3.png -->
+ <model:physical name="sphere2_model">
+ <xyz> 5.85 4.35 1.55</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <body:sphere name="sphere2_body">
+ <geom:sphere name="sphere2_geom">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
+ <mesh>default</mesh>
+ <size> 1.0</size>
+ <mass> 1.0</mass>
+ <visual>
+ <size> 2.0 2.0 2.0</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_sphere</mesh>
+ </visual>
+ </geom:sphere>
+ </body:sphere>
+ </model:physical>
+
+ <!-- The wall in front map3.png -->
+ <model:physical name="wall_2_model">
+ <xyz> 11.6 -1.55 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_2_body">
+ <geom:box name="wall_2_geom">
+ <mesh>default</mesh>
+ <size>2.1 32.8 2.0</size>
+ <visual>
+ <size>2.1 32.8 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- The wall behind -->
+ <model:physical name="wall_1_model">
+ <xyz> -11.3 -1.45 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_1_body">
+ <geom:box name="wall_1_geom">
+ <mesh>default</mesh>
+ <size>0.4 24.0 2.0</size>
+ <visual>
+ <size>0.4 24.0 2.0</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- The wall 3 -->
+ <model:physical name="wall_3_model">
+ <xyz> 6.7 8.05 1.0</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_3_body">
+ <geom:box name="wall_3_geom">
+ <mesh>default</mesh>
+ <size>7.5 1.2 2.0</size>
+ <visual>
+ <size>7.5 1.2 2.0</size>
+ <material>Gazebo/Chrome</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:empty name="factory_model">
+ <controller:factory name="factory_controller">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <interface:factory name="factory_iface"/>
+ </controller:factory>
+ </model:empty>
+
+ <!-- White Directional light -->
+ <!--
+ <model:renderable name="directional_white">
+ <light>
+ <type>directional</type>
+ <direction>0 -0.5 -0.5</direction>
+ <diffuseColor>0.4 0.4 0.4</diffuseColor>
+ <specularColor>0.0 0.0 0.0</specularColor>
+ <attenuation>1 0.0 1.0 0.4</attenuation>
+ </light>
+ </model:renderable>
+ -->
+
+
+</gazebo:world>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2009-04-30 20:46:19
|
Revision: 14682
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14682&view=rev
Author: jfaustwg
Date: 2009-04-30 20:46:11 +0000 (Thu, 30 Apr 2009)
Log Message:
-----------
Move VisualizationMarker and Polyline to visualization_msgs
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/demos/plug_in/manifest.xml
pkg/trunk/demos/tabletop_manipulation/manifest.xml
pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py
pkg/trunk/demos/tabletop_manipulation/scripts/executive.py
pkg/trunk/deprecated/old_costmap_2d/manifest.xml
pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp
pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
pkg/trunk/drivers/motor/ethercat_hardware/src/pressure_node.py
pkg/trunk/drivers/robot/pr2/fingertip_pressure/manifest.xml
pkg/trunk/drivers/robot/pr2/fingertip_pressure/scripts/pressure_visualization_marker.py
pkg/trunk/highlevel/doors/doors_core/manifest.xml
pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
pkg/trunk/mapping/collision_map/manifest.xml
pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h
pkg/trunk/mapping/door_handle_detector/manifest.xml
pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp
pkg/trunk/mapping/door_handle_detector/src/handle_detector.cpp
pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/mapping/door_stereo_detector/manifest.xml
pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
pkg/trunk/mapping/door_tracker/manifest.xml
pkg/trunk/mapping/door_tracker/src/door_database.cpp
pkg/trunk/mapping/door_tracker/src/door_tracker.cpp
pkg/trunk/mapping/hallway_tracker/manifest.xml
pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/angles.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/areas.h
pkg/trunk/mapping/point_cloud_mapping/manifest.xml
pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/areas.cpp
pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
pkg/trunk/motion_planning/navfn/manifest.xml
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/motion_planning/ompl_planning/manifest.xml
pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
pkg/trunk/nav/amcl/manifest.xml
pkg/trunk/nav/amcl/src/amcl_node.cpp
pkg/trunk/nav/base_local_planner/manifest.xml
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/nav/nav_view/manifest.xml
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/nav/people_aware_nav/manifest.xml
pkg/trunk/nav/people_aware_nav/src/is_person_on_path.cpp
pkg/trunk/nav/visual_nav/manifest.xml
pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp
pkg/trunk/nav/wavefront/manifest.xml
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
pkg/trunk/vision/outlet_detection/manifest.xml
pkg/trunk/vision/outlet_detection/src/outlet_spotting.cpp
pkg/trunk/vision/recognition_lambertian/manifest.xml
pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
pkg/trunk/vision/visual_odometry/manifest.xml
pkg/trunk/vision/visual_odometry/nodes/camview_py.py
pkg/trunk/vision/visual_odometry/nodes/corrector.py
pkg/trunk/vision/visual_odometry/nodes/lineperf.py
pkg/trunk/vision/visual_odometry/src/marker.py
pkg/trunk/visualization/rviz/manifest.xml
pkg/trunk/visualization/rviz/src/rviz/displays/marker_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/marker_display.h
pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.h
pkg/trunk/visualization/rviz/src/test/marker_test.cpp
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_publisher.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
pkg/trunk/world_models/costmap_2d/manifest.xml
pkg/trunk/world_models/costmap_2d/src/costmap_2d_publisher.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_test.cpp
pkg/trunk/world_models/test_collision_space/manifest.xml
pkg/trunk/world_models/test_collision_space/src/test_cs_point_inclusion.cpp
Added Paths:
-----------
pkg/trunk/visualization_core/visualization_msgs/
pkg/trunk/visualization_core/visualization_msgs/CMakeLists.txt
pkg/trunk/visualization_core/visualization_msgs/Makefile
pkg/trunk/visualization_core/visualization_msgs/mainpage.dox
pkg/trunk/visualization_core/visualization_msgs/manifest.xml
pkg/trunk/visualization_core/visualization_msgs/msg/
pkg/trunk/visualization_core/visualization_msgs/msg/Polyline.msg
pkg/trunk/visualization_core/visualization_msgs/msg/VisualizationMarker.msg
Removed Paths:
-------------
pkg/trunk/common/robot_msgs/msg/Polyline.msg
pkg/trunk/common/robot_msgs/msg/VisualizationMarker.msg
Property Changed:
----------------
pkg/trunk/visualization/
Deleted: pkg/trunk/common/robot_msgs/msg/Polyline.msg
===================================================================
--- pkg/trunk/common/robot_msgs/msg/Polyline.msg 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/common/robot_msgs/msg/Polyline.msg 2009-04-30 20:46:11 UTC (rev 14682)
@@ -1,3 +0,0 @@
-Header header
-Point[] points
-std_msgs/ColorRGBA color
Deleted: pkg/trunk/common/robot_msgs/msg/VisualizationMarker.msg
===================================================================
--- pkg/trunk/common/robot_msgs/msg/VisualizationMarker.msg 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/common/robot_msgs/msg/VisualizationMarker.msg 2009-04-30 20:46:11 UTC (rev 14682)
@@ -1,36 +0,0 @@
-#
-# Visualization Marker message
-# For inserting pre-made objects into the visualization
-#
-
-byte ARROW=0
-byte CUBE=1
-byte SPHERE=2
-byte ROBOT=3
-byte CYLINDER=4
-byte LINE_STRIP=5
-byte LINE_LIST=6
-
-byte ADD=0
-byte MODIFY=0
-byte DELETE=2
-
-Header header # header for time/frame information
-int32 id # object ID useful for manipulating and deleting the object later
-int32 type # Type of object
-int32 action # 0 add/modify an object, 1 (deprecated), 2 deletes an object
-float32 x # x location of the object
-float32 y # y location of the object
-float32 z # z location of the object
-float32 roll # roll of the object
-float32 pitch # pitch of the object
-float32 yaw # yaw of the object
-float32 xScale # scale of the object in the x direction (1 does not change size) (default sizes are usually 1 meter square)
-float32 yScale # scale in y direction
-float32 zScale # scale in z direction
-int32 alpha # alpha (0-255) (amount of transparency, 255 is opaque)
-int32 r # red color value (0-255) (if available)
-int32 g # green color value
-int32 b # blue color value
-#These are only used if the object type is LINE_STRIP or LINE_LIST
-Point[] points
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h 2009-04-30 20:46:11 UTC (rev 14682)
@@ -49,7 +49,7 @@
#include <robot_msgs/JointCmd.h>
#include <robot_srvs/GetJointCmd.h>
#include <robot_msgs/PointStamped.h>
-#include <robot_msgs/VisualizationMarker.h>
+#include <visualization_msgs/VisualizationMarker.h>
// Math utils
#include <math.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2009-04-30 20:46:11 UTC (rev 14682)
@@ -49,7 +49,7 @@
#include <robot_msgs/JointCmd.h>
#include <robot_srvs/GetJointCmd.h>
#include <robot_msgs/PointStamped.h>
-#include <robot_msgs/VisualizationMarker.h>
+#include <visualization_msgs/VisualizationMarker.h>
// Math utils
#include <math.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-04-30 20:46:11 UTC (rev 14682)
@@ -17,6 +17,7 @@
<depend package="deprecated_msgs" />
<depend package="pr2_msgs" />
<depend package="robot_msgs" />
+ <depend package="visualization_msgs" />
<depend package="robot_kinematics" />
<depend package="robot_actions" />
<depend package="rosconsole" />
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2009-04-30 20:46:11 UTC (rev 14682)
@@ -194,7 +194,7 @@
//services
node_->advertiseService(service_prefix_ + "/get_command_array", &HeadPanTiltControllerNode::getJointCmd, this);
guard_get_command_array_.set(service_prefix_ + "/get_command_array");
- node_->advertise<robot_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<visualization_msgs::VisualizationMarker>( "visualizationMarker", 0 );
return true;
}
@@ -340,7 +340,7 @@
c_->setJointCmd(pos,names);
- robot_msgs::VisualizationMarker marker;
+ visualization_msgs::VisualizationMarker marker;
marker.header.frame_id = "stereo";
marker.id = 0;
marker.type = 0;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp 2009-04-30 20:46:11 UTC (rev 14682)
@@ -219,7 +219,7 @@
//services
node_->advertiseService(service_prefix_ + "/get_command_array", &HeadServoingControllerNode::getJointCmd, this);
guard_get_command_array_.set(service_prefix_ + "/get_command_array");
- node_->advertise<robot_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<visualization_msgs::VisualizationMarker>( "visualizationMarker", 0 );
return true;
}
@@ -357,7 +357,7 @@
c_->setJointCmd(pos,names);
- robot_msgs::VisualizationMarker marker;
+ visualization_msgs::VisualizationMarker marker;
marker.header.frame_id = "stereo";
marker.id = 0;
marker.type = 0;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-04-30 20:46:11 UTC (rev 14682)
@@ -49,7 +49,7 @@
#include "joy/Joy.h"
#include "Eigen/LU"
#include "Eigen/Core"
-#include <robot_msgs/VisualizationMarker.h>
+#include <visualization_msgs/VisualizationMarker.h>
namespace controller {
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-04-30 20:46:11 UTC (rev 14682)
@@ -58,7 +58,7 @@
#include "Eigen/LU"
#include "Eigen/Core"
-#include <robot_msgs/VisualizationMarker.h>
+#include <visualization_msgs/VisualizationMarker.h>
namespace controller {
Modified: pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-04-30 20:46:11 UTC (rev 14682)
@@ -15,6 +15,7 @@
<depend package="std_msgs" />
<depend package="robot_srvs" />
<depend package="robot_msgs" />
+ <depend package="visualization_msgs" />
<depend package="tf" />
<depend package="tf_conversions" />
<depend package="kdl" />
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-04-30 20:46:11 UTC (rev 14682)
@@ -40,7 +40,7 @@
#include "tf_conversions/tf_kdl.h"
#include "realtime_tools/realtime_publisher.h"
#include "control_toolbox/pid.h"
-#include "robot_msgs/VisualizationMarker.h"
+#include "visualization_msgs/VisualizationMarker.h"
#include "angles/angles.h"
#include "std_msgs/Float64MultiArray.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-04-30 20:46:11 UTC (rev 14682)
@@ -361,7 +361,7 @@
node_->subscribe(topic_ + "/command", wrench_msg_,
&EndeffectorConstraintControllerNode::command, this, 1);
guard_command_.set(topic_ + "/command");
- node_->advertise<robot_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<visualization_msgs::VisualizationMarker>( "visualizationMarker", 0 );
return true;
@@ -379,10 +379,10 @@
// Debugging code. Not currently realtime safe
- robot_msgs::VisualizationMarker marker;
+ visualization_msgs::VisualizationMarker marker;
marker.header.frame_id = "torso_lift_link";
marker.id = 0;
- marker.type = robot_msgs::VisualizationMarker::CUBE;
+ marker.type = visualization_msgs::VisualizationMarker::CUBE;
marker.action = 0;
marker.x = 0.6;
marker.y = 0;
@@ -399,10 +399,10 @@
marker.b = 0;
node_->publish("visualizationMarker", marker );
- robot_msgs::VisualizationMarker marker;
+ visualization_msgs::VisualizationMarker marker;
marker.header.frame_id = "torso_lift_link";
marker.id = 1;
- marker.type = robot_msgs::VisualizationMarker::SPHERE;
+ marker.type = visualization_msgs::VisualizationMarker::SPHERE;
marker.action = 0;
marker.x = 0.6;
marker.y = 0;
Modified: pkg/trunk/demos/plug_in/manifest.xml
===================================================================
--- pkg/trunk/demos/plug_in/manifest.xml 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/demos/plug_in/manifest.xml 2009-04-30 20:46:11 UTC (rev 14682)
@@ -21,4 +21,6 @@
<depend package="pytf" />
<depend package="tf" />
+
+ <depend package="visualization_msgs" />
</package>
Modified: pkg/trunk/demos/tabletop_manipulation/manifest.xml
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/manifest.xml 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/demos/tabletop_manipulation/manifest.xml 2009-04-30 20:46:11 UTC (rev 14682)
@@ -14,6 +14,7 @@
<depend package="executive_python"/>
<depend package="plan_ompl_path"/>
<depend package="pr2_msgs"/>
+ <depend package="visualization_msgs" />
<depend package="rosviz"/>
<depend package="robot_actions"/>
<depend package="tf"/>
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py 2009-04-30 20:46:11 UTC (rev 14682)
@@ -37,7 +37,8 @@
roslib.load_manifest('tabletop_manipulation')
import rospy
from robot_srvs.srv import FindTable, FindTableRequest
-from robot_msgs.msg import Polygon3D, Point, VisualizationMarker
+from robot_msgs.msg import Polygon3D, Point
+from visualization_msgs.msg import VisualizationMarker
from deprecated_msgs.msg import RobotBase2DOdom
from tf.msg import tfMessage
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/executive.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/executive.py 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/executive.py 2009-04-30 20:46:11 UTC (rev 14682)
@@ -125,7 +125,7 @@
import rospy
import random
import sys
-from robot_msgs.msg import VisualizationMarker
+from visualization_msgs.msg import VisualizationMarker
from robot_msgs.msg import AttachedObject, PoseConstraint
from robot_srvs.srv import FindTable, FindTableRequest, SubtractObjectFromCollisionMap, SubtractObjectFromCollisionMapRequest, RecordStaticMapTrigger, RecordStaticMapTriggerRequest
from pr2_mechanism_controllers.srv import SetProfile, SetProfileRequest
Modified: pkg/trunk/deprecated/old_costmap_2d/manifest.xml
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/manifest.xml 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/deprecated/old_costmap_2d/manifest.xml 2009-04-30 20:46:11 UTC (rev 14682)
@@ -14,6 +14,7 @@
<depend package="robot_filter" />
<depend package="robot_srvs" />
<depend package="angles" />
+<depend package="visualization_msgs"/>
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lold_costmap_2d"/>
</export>
Modified: pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/deprecated/old_costmap_2d/src/costmap_2d_ros.cpp 2009-04-30 20:46:11 UTC (rev 14682)
@@ -53,7 +53,7 @@
#include <old_costmap_2d/costmap_2d.h>
// For GUI debug
-#include <robot_msgs/Polyline.h>
+#include <visualization_msgs/Polyline.h>
//window length for remembering laser data (seconds)
static const double WINDOW_LENGTH = 1.0;
@@ -90,7 +90,7 @@
//laser scan message
laser_scan::LaserScan laser_msg_;
- robot_msgs::Polyline pointcloud_msg_;
+ visualization_msgs::Polyline pointcloud_msg_;
//projector for the laser
laser_scan::LaserProjection projector_;
Modified: pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp
===================================================================
--- pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/deprecated/old_costmap_2d/src/costmap_node.cpp 2009-04-30 20:46:11 UTC (rev 14682)
@@ -63,8 +63,8 @@
* - @b "obstacle_cloud"/robot_msgs::PointCloud : low obstacles near the ground
* Publishes to (name / type):
- * - @b "raw_obstacles"robot_msgs::Polyline : contains all workspace obstacles in a window around the robot
- * - @b "inflated_obstacles"/robot_msgs::Polyline : contains c-space expansion, up to the inscribed radius of the robot
+ * - @b "raw_obstacles"visualization_msgs::Polyline : contains all workspace obstacles in a window around the robot
+ * - @b "inflated_obstacles"/visualization_msgs::Polyline : contains c-space expansion, up to the inscribed radius of the robot
* <hr>
*
* @section parameters ROS parameters (in addition to base class parameters):
@@ -104,7 +104,7 @@
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/PointCloud.h>
#include <deprecated_msgs/Pose2DFloat32.h>
-#include <robot_msgs/Polyline.h>
+#include <visualization_msgs/Polyline.h>
#include <robot_srvs/StaticMap.h>
#include <robot_msgs/PointStamped.h>
#include <algorithm>
@@ -307,8 +307,8 @@
local_map_accessor_ = new CostMapAccessor(*costMap_, local_access_mapsize_, 0.0, 0.0);
// Advertize messages to publish cost map updates
- ros::Node::instance()->advertise<robot_msgs::Polyline>("raw_obstacles", 1);
- ros::Node::instance()->advertise<robot_msgs::Polyline>("inflated_obstacles", 1);
+ ros::Node::instance()->advertise<visualization_msgs::Polyline>("raw_obstacles", 1);
+ ros::Node::instance()->advertise<visualization_msgs::Polyline>("inflated_obstacles", 1);
// Advertise costmap service
// Might be worth eventually having a dedicated node provide this service and all
@@ -522,7 +522,7 @@
}
// First publish raw obstacles in red
- robot_msgs::Polyline pointCloudMsg;
+ visualization_msgs::Polyline pointCloudMsg;
pointCloudMsg.header.frame_id = global_frame_;
unsigned int pointCount = rawObstacles.size();
pointCloudMsg.set_points_size(pointCount);
@@ -590,7 +590,7 @@
}
// First publish raw obstacles in red
- robot_msgs::Polyline pointCloudMsg;
+ visualization_msgs::Polyline pointCloudMsg;
pointCloudMsg.header.frame_id = global_frame_;
unsigned int pointCount = rawObstacles.size();
pointCloudMsg.set_points_size(pointCount);
Modified: pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2009-04-30 20:46:11 UTC (rev 14682)
@@ -13,6 +13,7 @@
<depend package='roscpp' />
<depend package='realtime_tools' />
<depend package='robot_msgs' />
+<depend package="visualization_msgs" />
<depend package='rospy' />
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lethercat_hardware -Wl,-rpath,${prefix}/lib -lloki"/>
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/pressure_node.py
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/pressure_node.py 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/pressure_node.py 2009-04-30 20:46:11 UTC (rev 14682)
@@ -44,7 +44,7 @@
import rospy
from ethercat_hardware.msg import PressureState
-from robot_msgs.msg import VisualizationMarker
+from visualization_msgs.msg import VisualizationMarker
positions = [ # x, y, z, xscale, yscale, zscale
( 0.026, 0.007, 0.000, 0.010, 0.010, 0.015),
Modified: pkg/trunk/drivers/robot/pr2/fingertip_pressure/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/fingertip_pressure/manifest.xml 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/drivers/robot/pr2/fingertip_pressure/manifest.xml 2009-04-30 20:46:11 UTC (rev 14682)
@@ -10,5 +10,6 @@
<depend package='roscpp' />
<depend package='rospy' />
<depend package='robot_msgs' />
+<depend package="visualization_msgs" />
<repository>http://pr.willowgarage.com/repos</repository>
</package>
Modified: pkg/trunk/drivers/robot/pr2/fingertip_pressure/scripts/pressure_visualization_marker.py
===================================================================
--- pkg/trunk/drivers/robot/pr2/fingertip_pressure/scripts/pressure_visualization_marker.py 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/drivers/robot/pr2/fingertip_pressure/scripts/pressure_visualization_marker.py 2009-04-30 20:46:11 UTC (rev 14682)
@@ -44,7 +44,7 @@
import rospy
from ethercat_hardware.msg import PressureState
-from robot_msgs.msg import VisualizationMarker
+from visualization_msgs.msg import VisualizationMarker
positions = [ # x, y, z, xscale, yscale, zscale
( 0.026, 0.007, 0.000, 0.010, 0.010, 0.015),
Modified: pkg/trunk/highlevel/doors/doors_core/manifest.xml
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/manifest.xml 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/highlevel/doors/doors_core/manifest.xml 2009-04-30 20:46:11 UTC (rev 14682)
@@ -21,6 +21,7 @@
<depend package="tf"/>
<depend package="costmap_2d"/>
<depend package="base_local_planner"/>
+ <depend package="visualization_msgs"/>
<export>
<cpp cflags="-I${prefix}/msg/cpp -I${prefix}/srv/cpp -I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lexecutive_functions"/>
Modified: pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-04-30 20:46:11 UTC (rev 14682)
@@ -59,9 +59,9 @@
ros_node_.param("diagnostics_expected_publish_time",diagnostics_expected_publish_time_,0.2);
ros_node_.param("~control_topic_name", control_topic_name_, std::string("/base/trajectory_controller/command"));
//for display purposes
- ros_node_.advertise<robot_msgs::Polyline>("~gui_path", 1);
- ros_node_.advertise<robot_msgs::Polyline>("~local_path", 1);
- ros_node_.advertise<robot_msgs::Polyline>("~robot_footprint", 1);
+ ros_node_.advertise<visualization_msgs::Polyline>("~gui_path", 1);
+ ros_node_.advertise<visualization_msgs::Polyline>("~local_path", 1);
+ ros_node_.advertise<visualization_msgs::Polyline>("~robot_footprint", 1);
ros_node_.advertise<robot_msgs::DiagnosticMessage> ("/diagnostics", 1) ;
//pass on some parameters to the components of the move base node if they are not explicitly overridden
@@ -361,7 +361,7 @@
std::vector<robot_msgs::Point> footprint;
planner_->computeOrientedFootprint(getPose2D(global_pose_), planner_->footprint_, footprint);
- robot_msgs::Polyline footprint_msg;
+ visualization_msgs::Polyline footprint_msg;
footprint_msg.header.frame_id = global_frame_;
footprint_msg.set_points_size(footprint.size());
footprint_msg.color.r = 1.0;
@@ -379,7 +379,7 @@
void MoveBaseDoorAction::publishPath(const std::vector<pr2_robot_actions::Pose2D>& path, std::string topic, double r, double g, double b, double a){
// Extract the plan in world co-ordinates
- robot_msgs::Polyline gui_path_msg;
+ visualization_msgs::Polyline gui_path_msg;
gui_path_msg.header.frame_id = global_frame_;
gui_path_msg.set_points_size(path.size());
for(unsigned int i=0; i < path.size(); i++){
Modified: pkg/trunk/mapping/collision_map/manifest.xml
===================================================================
--- pkg/trunk/mapping/collision_map/manifest.xml 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/mapping/collision_map/manifest.xml 2009-04-30 20:46:11 UTC (rev 14682)
@@ -13,5 +13,6 @@
<depend package="tf" />
<depend package="robot_msgs" />
<depend package="robot_srvs" />
+ <depend package="visualization_msgs" />
<depend package="point_cloud_mapping" />
</package>
Modified: pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp 2009-04-30 20:36:55 UTC (rev 14681)
+++ pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp 2009-04-30 20:46:11 UTC (rev 14682)
@@ -46,7 +46,7 @@
#include <robot_msgs/Point.h>
#include <robot_msgs/PointCloud.h>
#include <robot_msgs/PoseStamped.h>
-#include <robot_msgs/VisualizationMarker.h>
+#include <visualization_msgs/VisualizationMarker.h>
#include <Eigen/Core>
#include <point_cloud_mapping/geometry/point.h>
@@ -66,6 +66,7 @@
using namespace robot_msgs;
using namespace robot_msgs;
using namespace robot_srvs;
+using namespace visualization_msgs;
struct Leaf
{
@@ -187,7 +188,7 @@
subtract_object_ = false;
m_id_ = 0;
- node_.advertise<robot_msgs::VisualizationMarker>("visualizationMarker", 100);
+ node_.advertise<visualization_msgs::VisualizationMarker>("visu...
[truncated message content] |
|
From: <jfa...@us...> - 2009-05-01 04:47:54
|
Revision: 14717
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14717&view=rev
Author: jfaustwg
Date: 2009-05-01 04:47:46 +0000 (Fri, 01 May 2009)
Log Message:
-----------
Rename and refactor visualization markers:
* VisualizationMarker->Marker
* x/y/z/yaw/pitch/roll -> Pose pose
* alpha/r/g/b -> ColorRGBA color
* xScale/yScale/zScale -> Vector3 scale
* Added ns field (namespace)
* Added lifetime field
* visualizationMarker topic -> visualization_marker
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py
pkg/trunk/demos/tabletop_manipulation/scripts/executive.py
pkg/trunk/drivers/motor/ethercat_hardware/src/pressure_node.py
pkg/trunk/drivers/robot/pr2/fingertip_pressure/scripts/pressure_visualization_marker.py
pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h
pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp
pkg/trunk/mapping/door_handle_detector/src/handle_detector.cpp
pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
pkg/trunk/mapping/door_tracker/src/door_database.cpp
pkg/trunk/mapping/door_tracker/src/door_tracker.cpp
pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp
pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp
pkg/trunk/vision/outlet_detection/src/outlet_spotting.cpp
pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
pkg/trunk/vision/visual_odometry/nodes/camview_py.py
pkg/trunk/vision/visual_odometry/nodes/corrector.py
pkg/trunk/vision/visual_odometry/nodes/lineperf.py
pkg/trunk/vision/visual_odometry/src/marker.py
pkg/trunk/visualization/rviz/src/rviz/displays/marker_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/marker_display.h
pkg/trunk/visualization/rviz/src/test/marker_test.cpp
pkg/trunk/world_models/test_collision_space/src/test_cs_point_inclusion.cpp
Added Paths:
-----------
pkg/trunk/visualization_core/visualization_msgs/msg/Marker.msg
Removed Paths:
-------------
pkg/trunk/visualization_core/visualization_msgs/msg/VisualizationMarker.msg
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h 2009-05-01 04:47:46 UTC (rev 14717)
@@ -49,7 +49,7 @@
#include <robot_msgs/JointCmd.h>
#include <robot_srvs/GetJointCmd.h>
#include <robot_msgs/PointStamped.h>
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
// Math utils
#include <math.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h 2009-05-01 04:47:46 UTC (rev 14717)
@@ -49,7 +49,7 @@
#include <robot_msgs/JointCmd.h>
#include <robot_srvs/GetJointCmd.h>
#include <robot_msgs/PointStamped.h>
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
// Math utils
#include <math.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -194,7 +194,7 @@
//services
node_->advertiseService(service_prefix_ + "/get_command_array", &HeadPanTiltControllerNode::getJointCmd, this);
guard_get_command_array_.set(service_prefix_ + "/get_command_array");
- node_->advertise<visualization_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
return true;
}
@@ -340,44 +340,36 @@
c_->setJointCmd(pos,names);
- visualization_msgs::VisualizationMarker marker;
+ visualization_msgs::Marker marker;
marker.header.frame_id = "stereo";
+ marker.ns = "head_pan_tilt_controller";
marker.id = 0;
marker.type = 0;
marker.action = 0;
- marker.x = 0;
- marker.y = 0;
- marker.z = 0;
- marker.yaw = 0;
- marker.pitch = 0;
- marker.roll = 0.0;
- marker.xScale = 2;
- marker.yScale = 0.05;
- marker.zScale = 0.05;
- marker.alpha = 255;
- marker.r = 0;
- marker.g = 255;
- marker.b = 0;
- node_->publish("visualizationMarker", marker );
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 2;
+ marker.scale.y = 0.05;
+ marker.scale.z = 0.05;
+ marker.color.g = 1.0;
+ marker.color.a = 1.0;
+ node_->publish("visualization_marker", marker );
marker.header.frame_id = "head_pan_link";
+ marker.ns = "head_pan_tilt_controller";
marker.id = 1;
marker.type = 2;
marker.action = 0;
- marker.x = pan_point.x();
- marker.y = pan_point.y();
- marker.z = pan_point.z();
- marker.yaw = 0;
- marker.pitch = 0;
- marker.roll = 0.0;
- marker.xScale = 0.1;
- marker.yScale = 0.1;
- marker.zScale = 0.1;
- marker.alpha = 255;
- marker.r = 255;
- marker.g = 0;
- marker.b = 0;
- node_->publish("visualizationMarker", marker );
+ marker.pose.position.x = pan_point.x();
+ marker.pose.position.y = pan_point.y();
+ marker.pose.position.z = pan_point.z();
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 0.1;
+ marker.scale.y = 0.1;
+ marker.scale.z = 0.1;
+ marker.color.r = 1.0;
+ marker.color.g = 0.0;
+ marker.color.a = 1.0;
+ node_->publish("visualization_marker", marker );
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -73,15 +73,15 @@
elt = elt->NextSiblingElement("controller");
}
-
+
TiXmlElement *cd = config->FirstChildElement("controller_defaults");
if (cd)
{
-
- //TODO: make init function for this
+
+ //TODO: make init function for this
max_velocity_ = atof(cd->Attribute("max_velocity"));
gain_ = atof(cd->Attribute("gain"));
-
+
}
else
{
@@ -93,7 +93,7 @@
fprintf(stderr,"HeadServoingController:: num_joints_: %d\n",num_joints_);
-
+
set_pts_.resize(num_joints_);
for(unsigned int i=0; i < num_joints_;++i)
{
@@ -158,8 +158,8 @@
void HeadServoingController::update(void)
{
double error(0);
-
-
+
+
for(unsigned int i=0; i < num_joints_;++i)
{
angles::shortest_angular_distance_with_limits(joint_velocity_controllers_[i]->joint_state_->position_,set_pts_[i], joint_velocity_controllers_[i]->joint_state_->joint_->joint_limit_min_, joint_velocity_controllers_[i]->joint_state_->joint_->joint_limit_max_, error);
@@ -219,7 +219,7 @@
//services
node_->advertiseService(service_prefix_ + "/get_command_array", &HeadServoingControllerNode::getJointCmd, this);
guard_get_command_array_.set(service_prefix_ + "/get_command_array");
- node_->advertise<visualization_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
return true;
}
@@ -357,44 +357,36 @@
c_->setJointCmd(pos,names);
- visualization_msgs::VisualizationMarker marker;
+ visualization_msgs::Marker marker;
marker.header.frame_id = "stereo";
+ marker.ns = "head_servoing_controller";
marker.id = 0;
- marker.type = 0;
+ marker.type = 0;
marker.action = 0;
- marker.x = 0;
- marker.y = 0;
- marker.z = 0;
- marker.yaw = 0;
- marker.pitch = 0;
- marker.roll = 0.0;
- marker.xScale = 2;
- marker.yScale = 0.05;
- marker.zScale = 0.05;
- marker.alpha = 255;
- marker.r = 0;
- marker.g = 255;
- marker.b = 0;
- node_->publish("visualizationMarker", marker );
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 2;
+ marker.scale.y = 0.05;
+ marker.scale.z = 0.05;
+ marker.color.g = 1.0;
+ marker.color.a = 1.0;
+ node_->publish("visualization_marker", marker );
- marker.header.frame_id = "head_pan";
+ marker.header.frame_id = "head_pan_link";
+ marker.ns = "head_servoing_controller";
marker.id = 1;
- marker.type = 2;
+ marker.type = 2;
marker.action = 0;
- marker.x = pan_point.x();
- marker.y = pan_point.y();
- marker.z = pan_point.z();
- marker.yaw = 0;
- marker.pitch = 0;
- marker.roll = 0.0;
- marker.xScale = 0.1;
- marker.yScale = 0.1;
- marker.zScale = 0.1;
- marker.alpha = 255;
- marker.r = 255;
- marker.g = 0;
- marker.b = 0;
- node_->publish("visualizationMarker", marker );
+ marker.pose.position.x = pan_point.x();
+ marker.pose.position.y = pan_point.y();
+ marker.pose.position.z = pan_point.z();
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 0.1;
+ marker.scale.y = 0.1;
+ marker.scale.z = 0.1;
+ marker.color.r = 1.0;
+ marker.color.g = 0.0;
+ marker.color.a = 1.0;
+ node_->publish("visualization_marker", marker );
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-05-01 04:47:46 UTC (rev 14717)
@@ -49,7 +49,7 @@
#include "joy/Joy.h"
#include "Eigen/LU"
#include "Eigen/Core"
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
namespace controller {
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-05-01 04:47:46 UTC (rev 14717)
@@ -58,7 +58,7 @@
#include "Eigen/LU"
#include "Eigen/Core"
-#include <visualization_msgs/VisualizationMarker.h>
+#include <visualization_msgs/Marker.h>
namespace controller {
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -40,7 +40,7 @@
#include "tf_conversions/tf_kdl.h"
#include "realtime_tools/realtime_publisher.h"
#include "control_toolbox/pid.h"
-#include "visualization_msgs/VisualizationMarker.h"
+#include "visualization_msgs/Marker.h"
#include "angles/angles.h"
#include "std_msgs/Float64MultiArray.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-05-01 04:47:46 UTC (rev 14717)
@@ -361,7 +361,7 @@
node_->subscribe(topic_ + "/command", wrench_msg_,
&EndeffectorConstraintControllerNode::command, this, 1);
guard_command_.set(topic_ + "/command");
- node_->advertise<visualization_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+ node_->advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
return true;
@@ -379,45 +379,43 @@
// Debugging code. Not currently realtime safe
- visualization_msgs::VisualizationMarker marker;
- marker.header.frame_id = "torso_lift_link";
- marker.id = 0;
- marker.type = visualization_msgs::VisualizationMarker::CUBE;
- marker.action = 0;
- marker.x = 0.6;
- marker.y = 0;
- marker.z = 0;
- marker.yaw = 0;
- marker.pitch = 0;
- marker.roll = 0.0;
- marker.xScale = 0.01;
- marker.yScale = 10;
- marker.zScale = 10;
- marker.alpha = 200;
- marker.r = 0;
- marker.g = 255;
- marker.b = 0;
- node_->publish("visualizationMarker", marker );
+ {
+ visualization_msgs::Marker marker;
+ marker.header.frame_id = "torso_lift_link";
+ marker.ns = "endeffector_constraint_controller";
+ marker.id = 0;
+ marker.type = visualization_msgs::Marker::CUBE;
+ marker.action = 0;
+ marker.pose.position.x = 0.6;
+ marker.pose.position.y = 0;
+ marker.pose.position.z = 0;
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 0.01;
+ marker.scale.y = 10;
+ marker.scale.z = 10;
+ marker.color.g = 1.0;
+ marker.color.a = 0.7;
+ node_->publish("visualization_marker", marker );
+ }
- visualization_msgs::VisualizationMarker marker;
- marker.header.frame_id = "torso_lift_link";
- marker.id = 1;
- marker.type = visualization_msgs::VisualizationMarker::SPHERE;
- marker.action = 0;
- marker.x = 0.6;
- marker.y = 0;
- marker.z = 1;
- marker.yaw = 0;
- marker.pitch = 0;
- marker.roll = 0.0;
- marker.xScale = 0.01;
- marker.yScale = 0.4;
- marker.zScale = 0.4;
- marker.alpha = 200;
- marker.r = 255;
- marker.g = 0;
- marker.b = 0;
- node_->publish("visualizationMarker", marker );
+ {
+ visualization_msgs::Marker marker;
+ marker.header.frame_id = "torso_lift_link";
+ marker.ns = "endeffector_constraint_controller";
+ marker.id = 1;
+ marker.type = visualization_msgs::Marker::SPHERE;
+ marker.action = 0;
+ marker.pose.position.x = 0.6;
+ marker.pose.position.y = 0;
+ marker.pose.position.z = 1;
+ marker.pose.orientation.w = 1.0;
+ marker.scale.x = 0.01;
+ marker.scale.y = 0.4;
+ marker.scale.z = 0.4;
+ marker.color.r = 1.0;
+ marker.color.a = 0.7;
+ node_->publish("visualization_marker", marker );
+ }
#endif
}
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py 2009-05-01 04:47:46 UTC (rev 14717)
@@ -38,9 +38,11 @@
import rospy
from robot_srvs.srv import FindTable, FindTableRequest
from robot_msgs.msg import Polygon3D, Point
-from visualization_msgs.msg import VisualizationMarker
+from visualization_msgs.msg import Marker
from deprecated_msgs.msg import RobotBase2DOdom
from tf.msg import tfMessage
+import bullet
+import tf
from movebase import MoveBase
from detecttable import DetectTable
@@ -59,7 +61,7 @@
self.vm = None
rospy.Subscriber('odom', RobotBase2DOdom, self.odomCallback)
rospy.Subscriber('tf_message', tfMessage, self.tfCallback)
- self.pub_vis = rospy.Publisher("visualizationMarker", VisualizationMarker)
+ self.pub_vis = rospy.Publisher("visualization_marker", Marker)
self.global_frame = rospy.get_param('/global_frame_id')
# Hack to get around the fact that we don't have pytf
@@ -85,9 +87,10 @@
print '[ApproachTable] Failed to detect table'
return False
- self.vm = VisualizationMarker()
+ self.vm = Marker()
self.vm.header.frame_id = resp.table.header.frame_id
- self.vm.z = resp.table.table.points[0].z
+ self.vm.ns = "approachtable";
+ self.vm.pose.position.z = resp.table.table.points[0].z
approach_pose = self.computeApproachPose(self.robot_position, resp.table.table, standoff, True)
@@ -95,21 +98,17 @@
return False
self.vm.id = 1000
- self.vm.type = VisualizationMarker.ARROW
- self.vm.action = VisualizationMarker.ADD
- self.vm.x = approach_pose[0]
- self.vm.y = approach_pose[1]
+ self.vm.type = Marker.ARROW
+ self.vm.action = Marker.ADD
+ self.vm.pose.position.x = approach_pose[0]
+ self.vm.pose.position.y = approach_pose[1]
#self.vm.z = 0.0
- self.vm.roll = 0.0
- self.vm.pitch = 0.0
- self.vm.yaw = approach_pose[2]
- self.vm.xScale = 0.6
- self.vm.yScale = 0.25
- self.vm.zScale = 0.1
- self.vm.alpha = 128
- self.vm.r = 255
- self.vm.g = 0
- self.vm.b = 0
+ self.vm.pose.orientation = tf.quaternion_bt_to_msg(bullet.Quaternion(approach_pose[2], 0.0, 0.0))
+ self.vm.scale.x = 0.6
+ self.vm.scale.y = 0.25
+ self.vm.scale.z = 0.1
+ self.vm.color.a = 0.5
+ self.vm.color.r = 1.0
self.vm.points = []
self.pub_vis.publish(self.vm)
@@ -209,78 +208,58 @@
goala = atan2(sin(goala),cos(goala))
self.vm.id = 1001
- self.vm.type = VisualizationMarker.SPHERE
- self.vm.action = VisualizationMarker.ADD
- self.vm.x = midp.x
- self.vm.y = midp.y
+ self.vm.type = Marker.SPHERE
+ self.vm.action = Marker.ADD
+ self.vm.pose.position.x = midp.x
+ self.vm.pose.position.y = midp.y
#self.vm.z = 0.0
- self.vm.roll = 0.0
- self.vm.pitch = 0.0
- self.vm.yaw = 0.0
- self.vm.xScale = 0.05
- self.vm.yScale = 0.05
- self.vm.zScale = 0.05
- self.vm.alpha = 128
- self.vm.r = 0
- self.vm.g = 0
- self.vm.b = 255
+ self.vm.pose.orientation.x = 0.0
+ self.vm.pose.orientation.y = 0.0
+ self.vm.pose.orientation.z = 0.0
+ self.vm.pose.orientation.w = 1.0
+ self.vm.scale.x = 0.05
+ self.vm.scale.y = 0.05
+ self.vm.scale.z = 0.05
+ self.vm.color.a = 0.5
+ self.vm.color.r = 0.0
+ self.vm.color.g = 0.0
+ self.vm.color.b = 1.0
self.vm.points = []
self.pub_vis.publish(self.vm)
self.vm.id = 1002
- self.vm.type = VisualizationMarker.SPHERE
- self.vm.action = VisualizationMarker.ADD
- self.vm.x = closestp[0].x
- self.vm.y = closestp[0].y
+ self.vm.type = Marker.SPHERE
+ self.vm.action = Marker.ADD
+ self.vm.pose.position.x = closestp[0].x
+ self.vm.pose.position.y = closestp[0].y
#self.vm.z = 0.0
- self.vm.roll = 0.0
- self.vm.pitch = 0.0
- self.vm.yaw = 0.0
- self.vm.xScale = 0.05
- self.vm.yScale = 0.05
- self.vm.zScale = 0.05
- self.vm.alpha = 128
- self.vm.r = 0
- self.vm.g = 255
- self.vm.b = 0
+ self.vm.color.r = 0.0
+ self.vm.color.g = 1.0
+ self.vm.color.b = 0.0
self.vm.points = []
self.pub_vis.publish(self.vm)
self.vm.id = 1003
- self.vm.type = VisualizationMarker.SPHERE
- self.vm.action = VisualizationMarker.ADD
- self.vm.x = closestp[1].x
- self.vm.y = closestp[1].y
+ self.vm.type = Marker.SPHERE
+ self.vm.action = Marker.ADD
+ self.vm.pose.position.x = closestp[1].x
+ self.vm.pose.position.y = closestp[1].y
#self.vm.z = 0.0
- self.vm.roll = 0.0
- self.vm.pitch = 0.0
- self.vm.yaw = 0.0
- self.vm.xScale = 0.05
- self.vm.yScale = 0.05
- self.vm.zScale = 0.05
- self.vm.alpha = 128
- self.vm.r = 255
- self.vm.g = 0
- self.vm.b = 255
+ self.vm.color.r = 1.0
+ self.vm.color.g = 0.0
+ self.vm.color.b = 1.0
self.vm.points = []
self.pub_vis.publish(self.vm)
self.vm.id = 1004
- self.vm.type = VisualizationMarker.SPHERE
- self.vm.action = VisualizationMarker.ADD
- self.vm.x = farthestp.x
- self.vm.y = farthestp.y
+ self.vm.type = Marker.SPHERE
+ self.vm.action = Marker.ADD
+ self.vm.pose.position.x = farthestp.x
+ self.vm.pose.position.y = farthestp.y
#self.vm.z = 0.0
- self.vm.roll = 0.0
- self.vm.pitch = 0.0
- self.vm.yaw = 0.0
- self.vm.xScale = 0.05
- self.vm.yScale = 0.05
- self.vm.zScale = 0.05
- self.vm.alpha = 128
- self.vm.r = 255
- self.vm.g = 255
- self.vm.b = 255
+ self.vm.color.r = 1.0
+ self.vm.color.g = 01
+ self.vm.color.b = 1.0
self.vm.points = []
self.pub_vis.publish(self.vm)
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/executive.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/executive.py 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/executive.py 2009-05-01 04:47:46 UTC (rev 14717)
@@ -125,7 +125,7 @@
import rospy
import random
import sys
-from visualization_msgs.msg import VisualizationMarker
+from visualization_msgs.msg import Marker
from robot_msgs.msg import AttachedObject, PoseConstraint
from robot_srvs.srv import FindTable, FindTableRequest, SubtractObjectFromCollisionMap, SubtractObjectFromCollisionMapRequest, RecordStaticMapTrigger, RecordStaticMapTriggerRequest
from pr2_mechanism_controllers.srv import SetProfile, SetProfileRequest
@@ -165,7 +165,7 @@
self.laser_tilt_profile_amplitude = 0.75
self.laser_tilt_profile_offset = 0.30
- self.vis_pub = rospy.Publisher("visualizationMarker", VisualizationMarker)
+ self.vis_pub = rospy.Publisher("visualization_marker", Marker)
self.attached_obj_pub = rospy.Publisher("attach_object", AttachedObject)
print 'Waiting for service: ' + self.laser_tilt_profile_controller + '/set_profile'
@@ -239,22 +239,22 @@
def drawObjectVisMarker(self, obj):
# TODO: use bounds instead of this hardcoded radius
radius = 2.0
-
- mk = VisualizationMarker()
+ mk = Marker()
mk.header.frame_id = obj.frame_id
+ mk.ns = "executive"
mk.id = self.vmk_id
#self.vmk_id += 1
- mk.type = VisualizationMarker.SPHERE
- mk.action = VisualizationMarker.ADD # same as modify
- mk.x = obj.center.x
- mk.y = obj.center.y
- mk.z = obj.center.z
- mk.roll = mk.pitch = mk.yaw = 0
- mk.xScale = mk.yScale = mk.zScale = radius * 2.0
- mk.alpha = 255
- mk.r = 255
- mk.g = 10
- mk.b = 10
+ mk.type = Marker.SPHERE
+ mk.action = Marker.ADD # same as modify
+ mk.pose.position.x = obj.center.x
+ mk.pose.position.y = obj.center.y
+ mk.pose.position.z = obj.center.z
+ mk.pose.orientation.w = 1.0
+ mk.scale.x = mk.scale.y = mk.scale.z = radius * 2.0
+ mk.color.a = 1.0
+ mk.color.r = 1.0
+ mk.color.g = 0.04
+ mk.color.b = 0.04
vis_pub.publish(mk)
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/pressure_node.py
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/pressure_node.py 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/pressure_node.py 2009-05-01 04:47:46 UTC (rev 14717)
@@ -37,14 +37,14 @@
# Author: Blaise Gassend
# Reads fingertip pressure data from /pressure and publishes it as a
-# visualizationMarker
+# visualization_marker
import roslib
roslib.load_manifest('ethercat_hardware')
import rospy
from ethercat_hardware.msg import PressureState
-from visualization_msgs.msg import VisualizationMarker
+from visualization_msgs.msg import Marker
positions = [ # x, y, z, xscale, yscale, zscale
( 0.026, 0.007, 0.000, 0.010, 0.010, 0.015),
@@ -77,14 +77,14 @@
return (0,0,0)
if data < 3000:
x = (data-1000)/2000.
- return 0,0,255*x
+ return 0,0,x
if data < 6000:
x = (data-3000)/3000.
- return 255*x,0,255*(1-x)
+ return x,0,(1-x)
if data < 10000:
x = (data-6000)/4000.
- return 255,255*x,255*x
- return 255,255,255
+ return 1.0,x,x
+ return 1.0,1.0,1.0
class pressureVisualizer:
def callback(self, pressurestate):
@@ -101,21 +101,22 @@
self.makeVisualization(self.data1, self.frame1, self.startid + numsensors,-1)
def makeVisualization(self, data, frame, sensorstartid, ydir):
- mk = VisualizationMarker()
+ mk = Marker()
mk.header.frame_id = frame
# @todo Make timestamp come from when data is collected in hardware
- mk.header.stamp = rospy.get_rostime()
- mk.type = VisualizationMarker.SPHERE
- mk.action = VisualizationMarker.ADD
+ mk.header.stamp = rospy.get_rostime()
+ mk.ns = "pressure_node"
+ mk.type = Marker.SPHERE
+ mk.action = Marker.ADD
mk.points = []
for i in range(0,numsensors):
mk.id = sensorstartid + i
- (mk.x, mk.y, mk.z, mk.xScale, mk.yScale, mk.zScale) = positions[i]
- mk.y = mk.y * ydir
- mk.z = mk.z * ydir
- mk.roll = mk.pitch = mk.yaw = 0
- mk.alpha = 255
- (mk.r, mk.g, mk.b) = color(data[i])
+ (mk.pose.positions.x, mk.pose.positions.y, mk.pose.positions.z, mk.scale.x, mk.scale.y, mk.scale.z) = positions[i]
+ mk.pose.positions.y = mk.pose.positions.y * ydir
+ mk.pose.positions.z = mk.pose.positions.z * ydir
+ mk.pose.orientation.w = 1.0
+ mk.color.a = 1.0
+ (mk.color.r, mk.color.g, mk.color.b) = color(data[i])
self.vis_pub.publish(mk)
def __init__(self, source, frame0, frame1, startid):
@@ -126,8 +127,8 @@
rospy.init_node('pressureVisualizer', anonymous=True)
- self.vis_pub = rospy.Publisher('visualizationMarker',
- VisualizationMarker)
+ self.vis_pub = rospy.Publisher('visualization_marker',
+ Marker)
rospy.Subscriber(source, PressureState, self.callback)
while not rospy.is_shutdown():
Modified: pkg/trunk/drivers/robot/pr2/fingertip_pressure/scripts/pressure_visualization_marker.py
===================================================================
--- pkg/trunk/drivers/robot/pr2/fingertip_pressure/scripts/pressure_visualization_marker.py 2009-05-01 03:43:19 UTC (rev 14716)
+++ pkg/trunk/drivers/robot/pr2/fingertip_pressure/scripts/pressure_visualization_marker.py 2009-05-01 04:47:46 UTC (rev 14717)
@@ -37,14 +37,14 @@
# Author: Blaise Gassend
# Reads fingertip pressure data from /pressure and publishes it as a
-# visualizationMarker
+# visualization_marker
import roslib
roslib.load_manifest('fingertip_pressure')
import rospy
from ethercat_hardware.msg import PressureState
-from visualization_msgs.msg import VisualizationMarker
+from visualization_msgs.msg import Marker
positions = [ # x, y, z, xscale, yscale, zscale
( 0.026, 0.007, 0.000, 0.010, 0.010, 0.015),
@@ -77,14 +77,14 @@
return (0,0,0)
if data < 3000:
x = (data-1000)/2000.
- return 0,0,255*x
+ return 0,0,x
if data < 6000:
x = (data-3000)/3000.
- return 255*x,0,255*(1-x)
+ return x,0,(1-x)
if data < 10000:
x = (data-6000)/4000.
- return 255,255*x,255*x
- return 255,255,255
+ return 1.0,x,x
+ return 1.0,1.0,1.0
class pressureVisualizer:
def callback(self, pressurestate):
@@ -101,21 +101,22 @@
self.makeVisualization(self.data1, self.frame1, self.startid + numsensors,-1)
def makeVisualization(self, data, frame, sensorstartid, ydir):
- mk = VisualizationMarker()
+ mk = Marker()
mk.header.frame_id = frame
# @todo Make timestamp come from when data is collected in hardware
- mk.header.stamp = rospy.get_rostime()
- mk.type = VisualizationMarker.SPHERE
- mk.action = VisualizationMarker.ADD
+ mk.header.stamp = rospy.get_rostime()
+ mk.ns = "pressure_node"
+ mk.type = Marker.SPHERE
+ mk.action = Marker.ADD
mk.points = []
for i in range(0,numsensors):
mk.id = sensorstartid + i
- (mk.x, mk.y, mk.z, mk.xScale, mk.yScale, mk.zScale) = positions[i]
- mk.y = mk.y * ydir
- mk.z = mk.z * ydir
- mk.roll = mk.pitch = mk.yaw = 0
- mk.alpha = 255
- (mk.r, mk.g, mk.b) = color(data[i])
+ (mk.pose.positions.x, mk.pose.positions.y, mk.pose.positions.z, mk.scale.x, mk.scale.y, mk.scale.z) = positions[i]
+ mk.pose.positions.y = mk.pose.positions.y * ydir
+ mk.pose.positions.z = mk.pose.positions.z * ydir
+ mk.pose.orientation.w = 1.0
+ mk.color.a = 1.0
+ (mk.color.r, mk.color.g, mk.color.b) = color(data[i])
...
[truncated message content] |
|
From: <stu...@us...> - 2009-05-01 18:52:14
|
Revision: 14753
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14753&view=rev
Author: stuglaser
Date: 2009-05-01 18:52:11 +0000 (Fri, 01 May 2009)
Log Message:
-----------
Moved the plug detector outside of the actions.
Modified Paths:
--------------
pkg/trunk/demos/plug_in/integration/controllers.launch
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h
pkg/trunk/highlevel/plugs/plugs_core/src/action_localize_plug_in_gripper.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_hybrid.cpp
Added Paths:
-----------
pkg/trunk/highlevel/plugs/plugs_core/msg/
pkg/trunk/highlevel/plugs/plugs_core/msg/PlugInState.msg
Modified: pkg/trunk/demos/plug_in/integration/controllers.launch
===================================================================
--- pkg/trunk/demos/plug_in/integration/controllers.launch 2009-05-01 18:51:49 UTC (rev 14752)
+++ pkg/trunk/demos/plug_in/integration/controllers.launch 2009-05-01 18:52:11 UTC (rev 14753)
@@ -1,5 +1,5 @@
<launch>
-
+
<group ns="prosilica">
<include file="$(find prosilica_cam)/cam_settings.xml"/>
<param name="acquisition_mode" type="str" value="Triggered"/>
@@ -11,26 +11,26 @@
<group ns="r_arm_joint_trajectory_controller" clear_params="true">
<param name="velocity_scaling_factor" value="0.25" type="double" />
<param name="trajectory_wait_timeout" value="10.0" type="double" />
-
+
<param name="r_shoulder_pan_joint/goal_reached_threshold" value="0.05" type="double" />
<param name="r_shoulder_lift_joint/goal_reached_threshold" value="0.05" type="double" />
<param name="r_upper_arm_roll_joint/goal_reached_threshold" value="0.05" type="double" />
<param name="r_elbow_flex_joint/goal_reached_threshold" value="0.05" type="double" />
<param name="r_forearm_roll_joint/goal_reached_threshold" value="0.05" type="double" />
<param name="r_wrist_flex_joint/goal_reached_threshold" value="0.05" type="double" />
- <param name="r_wrist_roll_joint/goal_reached_threshold" value="0.05" type="double" />
- <param name="autostart" value="false" type="bool" />
+ <param name="r_wrist_roll_joint/goal_reached_threshold" value="0.05" type="double" />
+ <param name="autostart" value="false" type="bool" />
</group>
-
-
+
+
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/r_arm_joint_trajectory_controller.xml" />
-
+
<!-- Controller and Node Required for plug on base detection -->
-
-
- <param name="laser_tilt_controller/autostart" value="false" type="bool" />
+
+
+ <param name="laser_tilt_controller/autostart" value="false" type="bool" />
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_experimental_controllers)/laser_tilt/laser_tilt_controller.xml" />
-
+
<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="tf_cache_time_secs" type="double" value="10.0" />
@@ -44,14 +44,14 @@
<remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
<remap from="build_cloud" to="laser_scan_assembler/build_cloud" />
<remap from="full_cloud" to="snapshot_cloud" />
- </node>
-
+ </node>
+
<!-- Controller and Node Required for move and grasp -->
<!-- Cartesian wrench controller -->
<group ns="r_arm_cartesian_wrench_controller" clear_params="true">
<param name="root_name" type="string" value="torso_lift_link" />
<param name="tip_name" type="string" value="r_gripper_tool_frame" />
- <param name="autostart" value="false" type="bool" />
+ <param name="autostart" value="false" type="bool" />
</group>
<!-- Cartesian twist controller -->
@@ -72,7 +72,7 @@
<param name="fb_rot/i" value="0.1" />
<param name="fb_rot/d" value="0.0" />
<param name="fb_rot/i_clamp" value="0.2" />
- <param name="autostart" value="false" type="bool" />
+ <param name="autostart" value="false" type="bool" />
</group>
<!-- Cartesian pose controller -->
@@ -85,7 +85,7 @@
<param name="i" value="0.1" />
<param name="d" value="0.0" />
<param name="i_clamp" value="0.5" />
- <param name="autostart" value="false" type="bool" />
+ <param name="autostart" value="false" type="bool" />
</group>
<!-- Cartesian trajectory controller -->
@@ -98,17 +98,17 @@
<param name="max_vel_rot" value="1.0" />
<param name="max_acc_trans" value="0.2" />
<param name="max_acc_rot" value="0.4" />
- <param name="autostart" value="false" type="bool" />
+ <param name="autostart" value="false" type="bool" />
</group>
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/r_arm_cartesian_wrench_controller.xml $(find pr2_default_controllers)/r_arm_cartesian_twist_controller.xml $(find pr2_default_controllers)/r_arm_cartesian_pose_controller.xml $(find pr2_default_controllers)/r_arm_cartesian_trajectory_controller.xml" />
<!--Gripper position controller-->
- <param name="r_gripper_position_controller/autostart" value="false" type="bool" />
+ <param name="r_gripper_position_controller/autostart" value="false" type="bool" />
<node pkg="mechanism_control" type="spawner.py" args="$(find plug_in)/gripper_controller.xml"/>
-
-
+
+
<!-- Parameters for the controllers used in plugging in -->
<group ns="r_arm_hybrid_controller" clear_params="true">
@@ -123,10 +123,14 @@
<rosparam file="$(find pr2_default_controllers)/gains/arm_twist.yaml" command="load" />
</group>
<param name="twist_filter" command="./../make_lowpass.m twist_filter 0.01" />
- <param name="saturated_velocity" value="0.5" />
+ <param name="saturated_velocity" value="0.4" />
+ <!--<param name="saturated_rot_velocity" value="0.8" />-->
<param name="autostart" value="false" type="bool" />
</group>
<node pkg="mechanism_control" type="spawner.py" args="$(find plug_in)/integration/r_arm_hybrid_controller.xml"/>
+ <!-- Plug tracking node -->
+ <include file="$(find outlet_detection)/launch/plug.launch" />
+
</launch>
Modified: pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h 2009-05-01 18:51:49 UTC (rev 14752)
+++ pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h 2009-05-01 18:52:11 UTC (rev 14753)
@@ -46,6 +46,7 @@
#include "robot_msgs/PoseStamped.h"
#include "robot_msgs/CartesianState.h"
#include "robot_msgs/TaskFrameFormalism.h"
+#include "plugs_core/PlugInState.h"
//TF
#include <tf/transform_listener.h>
@@ -70,7 +71,7 @@
robot_actions::ResultStatus execute(const std_msgs::Empty& empty, std_msgs::Empty& feedback);
private:
-
+
void reset();
void plugMeasurementCallback(const tf::MessageNotifier<robot_msgs::PoseStamped>::MessagePtr &msg);
void measure();
@@ -79,28 +80,27 @@
void force();
void insert();
-
+
std::string action_name_;
ros::Node& node_;
std::string arm_controller_;
-
+
PlugTracker::PlugTracker* detector_;
std_msgs::Empty empty_;
-
+
boost::scoped_ptr<tf::MessageNotifier<robot_msgs::PoseStamped> > notifier_;
boost::scoped_ptr<tf::TransformListener> TF_;
-
- double last_standoff_;
+
+ double last_standoff_;
ros::Time g_started_inserting_, g_started_forcing_, g_stopped_forcing_;
int g_state_;
int prev_state_;
-
+
tf::Stamped<tf::Transform> mech_offset_;
tf::Pose mech_offset_desi_;
- robot_msgs::TaskFrameFormalism tff_msg_;
-
+ robot_msgs::TaskFrameFormalism tff_msg_;
};
}
Added: pkg/trunk/highlevel/plugs/plugs_core/msg/PlugInState.msg
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/msg/PlugInState.msg (rev 0)
+++ pkg/trunk/highlevel/plugs/plugs_core/msg/PlugInState.msg 2009-05-01 18:52:11 UTC (rev 14753)
@@ -0,0 +1,6 @@
+Header header
+int32 state
+int32 next_state
+
+robot_msgs/Twist viz_offset
+robot_msgs/Twist viz_error
\ No newline at end of file
Modified: pkg/trunk/highlevel/plugs/plugs_core/src/action_localize_plug_in_gripper.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/src/action_localize_plug_in_gripper.cpp 2009-05-01 18:51:49 UTC (rev 14752)
+++ pkg/trunk/highlevel/plugs/plugs_core/src/action_localize_plug_in_gripper.cpp 2009-05-01 18:52:11 UTC (rev 14753)
@@ -43,6 +43,7 @@
robot_actions::Action<std_msgs::Empty, std_msgs::Empty>("localize_plug_in_gripper"),
action_name_("localize_plug_in_gripper"),
node_(node),
+ detector_(NULL),
arm_controller_("r_arm_cartesian_trajectory_controller"),
servoing_controller_("r_arm_hybrid_controller"),
TF(*ros::Node::instance(),false, ros::Duration(10))
@@ -59,24 +60,24 @@
node_.param(action_name_ + "/arm_controller", arm_controller_, arm_controller_);
if(arm_controller_ == "" )
- {
- ROS_ERROR("%s: Aborted, arm controller param was not set.", action_name_.c_str());
- terminate();
- return;
- }
+ {
+ ROS_ERROR("%s: Aborted, arm controller param was not set.", action_name_.c_str());
+ terminate();
+ return;
+ }
node_.param(action_name_ + "/servoing_controller", servoing_controller_, servoing_controller_);
if(servoing_controller_ == "" )
- {
- ROS_ERROR("%s: Aborted, servoing controller param was not set.", action_name_.c_str());
- terminate();
- return;
- }
+ {
+ ROS_ERROR("%s: Aborted, servoing controller param was not set.", action_name_.c_str());
+ terminate();
+ return;
+ }
- detector_ = new PlugTracker::PlugTracker(node);
- detector_->deactivate();
- node_.subscribe("~plug_pose", plug_pose_msg_, &LocalizePlugInGripperAction::setToolFrame, this, 1);
+ //detector_ = new PlugTracker::PlugTracker(node);
+ //detector_->deactivate();
+ node_.subscribe("/plug_detector/plug_pose", plug_pose_msg_, &LocalizePlugInGripperAction::setToolFrame, this, 1);
};
LocalizePlugInGripperAction::~LocalizePlugInGripperAction()
@@ -156,7 +157,8 @@
return;
}
- detector_->activate();
+ if (detector_)
+ detector_->activate();
return;
}
@@ -169,7 +171,8 @@
if (isPreemptRequested())
{
- detector_->deactivate();
+ if (detector_)
+ detector_->deactivate();
deactivate(robot_actions::PREEMPTED, std_msgs::Empty());
return;
}
@@ -183,8 +186,8 @@
return;
}
-
- detector_->deactivate();
+ if (detector_)
+ detector_->deactivate();
ROS_INFO("%s: succeeded.", action_name_.c_str());
deactivate(robot_actions::SUCCESS, empty_);
}
Modified: pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in.cpp 2009-05-01 18:51:49 UTC (rev 14752)
+++ pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in.cpp 2009-05-01 18:52:11 UTC (rev 14753)
@@ -27,7 +27,7 @@
* 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 INeco
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING INeco
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
@@ -46,7 +46,8 @@
robot_actions::Action<std_msgs::Empty, std_msgs::Empty>("plug_in"),
action_name_("plug_in"),
node_(node),
- arm_controller_("r_arm_hybrid_controller")
+ arm_controller_("r_arm_hybrid_controller"),
+ detector_(NULL)
{
node_.setParam("~roi_policy", "LastImageLocation");
node_.setParam("~display", 0);
@@ -57,28 +58,26 @@
node_.param(action_name_ + "/arm_controller", arm_controller_, arm_controller_);
if(arm_controller_ == "" )
- {
- ROS_ERROR("%s: Aborted, arm controller param was not set.", action_name_.c_str());
- terminate();
- return;
- }
+ {
+ ROS_ERROR("%s: Aborted, arm controller param was not set.", action_name_.c_str());
+ terminate();
+ return;
+ }
- node_.advertise<robot_msgs::TaskFrameFormalism>(arm_controller_ + "/command", 2);
+ node_.advertise<robot_msgs::TaskFrameFormalism>(arm_controller_ + "/command", 2);
- detector_ = new PlugTracker::PlugTracker(node);
- detector_->deactivate();
+ //detector_ = new PlugTracker::PlugTracker(node);
+ //detector_->deactivate();
- TF_.reset(new tf::TransformListener(node));
-// notifier_.reset(new tf::MessageNotifier<robot_msgs::PoseStamped>(
-// TF_.get(), &node, PlugInAction::plugMeasurementCallback, "~pose", "outlet_pose", 100));
- notifier_.reset(new tf::MessageNotifier<robot_msgs::PoseStamped>(
- TF_.get(), &node,\
-boost::bind(&PlugInAction::plugMeasurementCallback, this, _1),
-//PlugInAction::plugMeasurementCallback,
-"~plug_pose", "outlet_pose", 100));
+ TF_.reset(new tf::TransformListener(node));
+ notifier_.reset(new tf::MessageNotifier<robot_msgs::PoseStamped>(
+ TF_.get(), &node,
+ boost::bind(&PlugInAction::plugMeasurementCallback, this, _1),
+ "/plug_detector/plug_pose", "outlet_pose", 100));
tff_msg_.header.frame_id = "outlet_pose";
+ node_.advertise<plugs_core::PlugInState>(action_name_ + "/state", 10);
};
PlugInAction::~PlugInAction()
@@ -89,7 +88,8 @@
robot_actions::ResultStatus PlugInAction::execute(const std_msgs::Empty& empty, std_msgs::Empty& feedback)
{
reset();
- detector_->activate();
+ if (detector_)
+ detector_->activate();
return waitForDeactivation(feedback);
}
@@ -103,18 +103,27 @@
g_stopped_forcing_ = ros::Time::now();
}
+void PoseTFToMsg(const tf::Pose &p, robot_msgs::Twist &t)
+{
+ t.vel.x = p.getOrigin().x();
+ t.vel.y = p.getOrigin().y();
+ t.vel.z = p.getOrigin().z();
+ btMatrix3x3(p.getRotation()).getEulerYPR(t.rot.x, t.rot.y, t.rot.z);
+}
void PlugInAction::plugMeasurementCallback(const tf::MessageNotifier<robot_msgs::PoseStamped>::MessagePtr &msg)
{
+ plugs_core::PlugInState state_msg;
ROS_INFO("recieved plug_pose Msg in callback");
if (!isActive())
return;
-
+
if (isPreemptRequested()){
deactivate(robot_actions::PREEMPTED, empty_);
- detector_->deactivate();
+ if (detector_)
+ detector_->deactivate();
return;
}
tff_msg_.header.stamp = msg->header.stamp;
@@ -133,22 +142,25 @@
tf::Pose viz_offset;
tf::PoseMsgToTF(viz_offset_msg.pose, viz_offset);
- double standoff = std::max(MIN_STANDOFF, viz_offset.getOrigin().length() * 2.5/4.0);
-
+ PoseTFToMsg(viz_offset, state_msg.viz_offset);
+ double standoff = std::max(MIN_STANDOFF, viz_offset.getOrigin().length() * 2.5/4.0);
+
// Computes the offset for movement
tf::Pose viz_offset_desi;
viz_offset_desi.setIdentity();
viz_offset_desi.getOrigin().setX(-standoff);
+ viz_offset_desi.getOrigin().setY(-0.003);
viz_offset_desi.getOrigin().setZ(-0.004);
+ PoseTFToMsg(viz_offset.inverse() * viz_offset_desi, state_msg.viz_error);
mech_offset_desi_ = viz_offset.inverse() * viz_offset_desi * mech_offset_;
prev_state_ = g_state_;
switch (g_state_) {
- case MEASURING:
+ case MEASURING:
{
if (viz_offset.getOrigin().length() > 0.5 ||
- viz_offset.getRotation().getAngle() > (M_PI/4.0))
+ viz_offset.getRotation().getAngle() > (M_PI/6.0))
{
double ypr[3];
btMatrix3x3(viz_offset.getRotation()).getEulerYPR(ypr[2], ypr[1], ypr[0]);
@@ -160,7 +172,7 @@
double error = sqrt(pow(viz_offset.getOrigin().y() - viz_offset_desi.getOrigin().y(), 2) +
pow(viz_offset.getOrigin().z() - viz_offset_desi.getOrigin().z(), 2));
- ROS_DEBUG("%s: Error = %0.6lf.", action_name_.c_str(), error);
+ ROS_DEBUG("%s: Error = %0.6lf.", action_name_.c_str(), error);
if (error < 0.002 && last_standoff_ < MIN_STANDOFF + 0.002)
g_state_ = INSERTING;
else
@@ -174,7 +186,7 @@
break;
}
- case INSERTING:
+ case INSERTING:
{
tf::Vector3 offset = viz_offset.getOrigin() - viz_offset_desi.getOrigin();
ROS_DEBUG("%s: Offset: (% 0.3lf, % 0.3lf, % 0.3lf)", action_name_.c_str(), offset.x(), offset.y(), offset.z());
@@ -191,22 +203,25 @@
}
break;
}
- case FORCING:
+ case FORCING:
{
if (ros::Time::now() > g_started_forcing_ + ros::Duration(1.0))
g_state_ = HOLDING;
break;
}
- case HOLDING:
+ case HOLDING:
{
break;
}
}
+ state_msg.state = prev_state_;
+ state_msg.next_state = g_state_;
+
if (g_state_ != prev_state_)
{
switch (g_state_) {
- case MEASURING:
+ case MEASURING:
{
ROS_DEBUG("MEASURING");
if (prev_state_ == INSERTING || prev_state_ == FORCING)
@@ -215,36 +230,37 @@
}
break;
}
- case MOVING:
+ case MOVING:
{
ROS_DEBUG("MOVING");
move();
break;
}
- case INSERTING:
+ case INSERTING:
{
ROS_DEBUG("INSERTING");
insert();
break;
}
- case FORCING:
+ case FORCING:
{
ROS_DEBUG("FORCING");
force();
break;
}
- case HOLDING:
+ case HOLDING:
{
ROS_DEBUG("HOLDING");
hold();
deactivate(robot_actions::SUCCESS, empty_);
- detector_->deactivate();
+ if (detector_)
+ detector_->deactivate();
break;
}
}
}
-
+
last_standoff_ = standoff;
return;
}
@@ -316,7 +332,7 @@
{
if (!isActive())
return;
-
+
g_started_forcing_ = ros::Time::now();
tff_msg_.mode.vel.x = 1;
@@ -335,6 +351,19 @@
tff_msg_.mode.vel.z = 2;
tff_msg_.value.vel.y = 0.0;
tff_msg_.value.vel.z = 0.0;
+
+
+ tff_msg_.mode.vel.x = 1;
+ tff_msg_.mode.vel.y = 3;
+ tff_msg_.mode.vel.z = 3;
+ tff_msg_.mode.rot.x = 3;
+ tff_msg_.mode.rot.y = 3;
+ tff_msg_.mode.rot.z = 3;
+ tff_msg_.value.vel.x = 30;
+ tff_msg_.value.vel.y = mech_offset_.getOrigin().y();
+ tff_msg_.value.vel.z = mech_offset_.getOrigin().z();
+ mech_offset_desi_.getBasis().getEulerZYX(tff_msg_.value.rot.z, tff_msg_.value.rot.y, tff_msg_.value.rot.x);
+
node_.publish(arm_controller_ + "/command", tff_msg_);
return;
}
@@ -343,7 +372,7 @@
{
if (!isActive())
return;
-
+
tff_msg_.mode.vel.x = 1;
tff_msg_.mode.vel.y = 3;
tff_msg_.mode.vel.z = 3;
Modified: pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_hybrid.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_hybrid.cpp 2009-05-01 18:51:49 UTC (rev 14752)
+++ pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_hybrid.cpp 2009-05-01 18:52:11 UTC (rev 14753)
@@ -1,13 +1,13 @@
/*********************************************************************
* 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
@@ -17,7 +17,7 @@
* * Neither the name of 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
@@ -83,14 +83,14 @@
ros::Node node("test_executive");
boost::thread_group threads_;
-
+
pr2_robot_actions::SwitchControllers switchlist;
std_msgs::Empty empty;
- robot_msgs::PlugStow plug_stow;
+ robot_msgs::PlugStow plug_stow;
robot_msgs::PointStamped point;
robot_msgs::PoseStamped pose;
-
+
point.header.frame_id = "torso_lift_link";
point.point.x=0;
point.point.y=0;
@@ -112,7 +112,7 @@
robot_actions::ActionClient< robot_msgs::PlugStow, pr2_robot_actions::StowPlugState, std_msgs::Empty> stow_plug("stow_plug");
- timeout_medium.sleep();
+ timeout_short.sleep();
#if 0
// tuck arm
// switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
@@ -143,7 +143,7 @@
// detect outlet fine
if (detect_outlet_fine.execute(point, pose, timeout_long) != robot_actions::SUCCESS) return -1;
-
+
// localize plug in gripper
if (localize_plug_in_gripper.execute(empty, empty, timeout_long) != robot_actions::SUCCESS) return -1;
@@ -157,11 +157,12 @@
if (switch_controllers.execute(switchlist, empty, timeout_short) != robot_actions::SUCCESS) return -1;
if (plug_in.execute(empty, empty, timeout_long) != robot_actions::SUCCESS) return -1;
- timeout_long.sleep();
-#if 0
+ Duration().fromSec(5.0).sleep();
+
//unplug
if (unplug.execute(empty, empty, timeout_long) != robot_actions::SUCCESS) return -1;
+#if 0
//stow plug
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
switchlist.stop_controllers.push_back("r_arm_hybrid_controller");
@@ -175,6 +176,7 @@
#endif
+ timeout_long.sleep();
// stop remaining controllers
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
switchlist.stop_controllers.push_back("r_arm_hybrid_controller");
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2009-05-05 06:10:44
|
Revision: 14879
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14879&view=rev
Author: jfaustwg
Date: 2009-05-05 06:10:34 +0000 (Tue, 05 May 2009)
Log Message:
-----------
#if'd fixes against ros-trunk
Modified Paths:
--------------
pkg/trunk/nav/map_server/src/main.cpp
pkg/trunk/nav/wavefront/src/cli.cpp
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
Modified: pkg/trunk/nav/map_server/src/main.cpp
===================================================================
--- pkg/trunk/nav/map_server/src/main.cpp 2009-05-05 03:54:44 UTC (rev 14878)
+++ pkg/trunk/nav/map_server/src/main.cpp 2009-05-05 06:10:34 UTC (rev 14879)
@@ -118,7 +118,11 @@
*/
robot_srvs::StaticMap::Response map_resp_;
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ void metadataSubscriptionCallback(const ros::SingleSubscriberPublisher& pub)
+#else
void metadataSubscriptionCallback(const ros::PublisherPtr& pub)
+#endif
{
publish( "map_metadata", meta_data_message_ );
}
@@ -164,7 +168,7 @@
{
fprintf(stderr, "%s\n", e.what());
}
-
+
return 0;
}
Modified: pkg/trunk/nav/wavefront/src/cli.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/cli.cpp 2009-05-05 03:54:44 UTC (rev 14878)
+++ pkg/trunk/nav/wavefront/src/cli.cpp 2009-05-05 06:10:34 UTC (rev 14879)
@@ -36,7 +36,12 @@
publish("goal", wf_goal);
ros::Duration().fromSec(0.5).sleep(); // hack to try and wait for the message to go
}
+
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ void goal_subscriber_callback(const ros::SingleSubscriberPublisher& pub)
+#else
void goal_subscriber_callback(const ros::PublisherPtr& pub)
+#endif
{
publish("goal", wf_goal);
}
@@ -66,6 +71,6 @@
}
else
printf("success\n");
-
+
return 0;
}
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-05-05 03:54:44 UTC (rev 14878)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-05-05 06:10:34 UTC (rev 14879)
@@ -30,6 +30,8 @@
#include <pr2_mechanism_controllers/TrajectoryWait.h>
#include <pr2_mechanism_controllers/TrajectoryQuery.h>
+#include "ros/node.h"
+
class ROSRobotController : public ControllerBase
{
enum ControllerState {
@@ -51,20 +53,24 @@
{
assert(pnode != NULL);
Destroy();
-
+
_srvTrajectoryStart = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryStart::Request, pr2_mechanism_controllers::TrajectoryStart::Response>(_strTrajectoryServiceDir+"TrajectoryStart", true);
_srvTrajectoryCancel = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryCancel::Request, pr2_mechanism_controllers::TrajectoryCancel::Response>(_strTrajectoryServiceDir+"TrajectoryCancel", true);
_srvTrajectoryWait = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryWait::Request, pr2_mechanism_controllers::TrajectoryWait::Response>(_strTrajectoryServiceDir+"TrajectoryWait", true);
_srvTrajectoryQuery = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryQuery::Request, pr2_mechanism_controllers::TrajectoryQuery::Response>(_strTrajectoryServiceDir+"TrajectoryQuery", true);
-
+
if( !_srvTrajectoryQuery ) {
RAVELOG_ERRORA("failed to find %s service\n", (_strTrajectoryServiceDir+"TrajectoryQuery").c_str());
return false;
}
-
+
pr2_mechanism_controllers::TrajectoryQuery::Request req;
pr2_mechanism_controllers::TrajectoryQuery::Response res;
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ if( !_srvTrajectoryQuery.call(req,res) ) {
+#else
if( !_srvTrajectoryQuery->call(req,res) ) {
+#endif
RAVELOG_ERRORA("failed to query trajectory service %s\n", _strTrajectoryServiceDir.c_str());
return false;
}
@@ -83,7 +89,7 @@
_vjointmap.reserve(res.jointnames.size());
FOREACH(itname, res.jointnames) {
vector<string>::iterator itindex = find(vrobotjoints.begin(), vrobotjoints.end(), *itname);
- if( itindex == vrobotjoints.end() ) {
+ if( itindex == vrobotjoints.end() ) {
RAVELOG_ERRORA("failed to find joint %s\n", itname->c_str());
Destroy();
return false;
@@ -102,10 +108,18 @@
{
_vjointmap.clear();
_listTrajectories.clear();
+
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ _srvTrajectoryStart = ros::ServiceClient();
+ _srvTrajectoryCancel = ros::ServiceClient();
+ _srvTrajectoryWait = ros::ServiceClient();
+ _srvTrajectoryQuery = ros::ServiceClient();
+#else
_srvTrajectoryStart.reset();
_srvTrajectoryCancel.reset();
_srvTrajectoryWait.reset();
_srvTrajectoryQuery.reset();
+#endif
return true;
}
@@ -120,7 +134,11 @@
// trajectory services
RobotBase* _probot;
string _strTrajectoryServiceDir;
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ ros::ServiceClient _srvTrajectoryStart, _srvTrajectoryCancel, _srvTrajectoryWait, _srvTrajectoryQuery;
+#else
service::ServiceHandlePtr _srvTrajectoryStart, _srvTrajectoryCancel, _srvTrajectoryWait, _srvTrajectoryQuery;
+#endif
vector<int> _vjointmap;
list<uint32_t> _listTrajectories; ///< trajectories currently pending for completion
};
@@ -235,9 +253,9 @@
bool bSuccess = true;
pr2_mechanism_controllers::TrajectoryStart::Request req;
pr2_mechanism_controllers::TrajectoryStart::Response res;
-
+
vector<dReal> vnewvalues;
-
+
{
RobotBase::RobotStateSaver saver(_probot);
_probot->SetJointValues(NULL, NULL, pValues,true);
@@ -276,7 +294,11 @@
(*ittrajcontroller)->GetTrajPoint(_vcurvalues, req.traj.points[0]);
(*ittrajcontroller)->GetTrajPoint(vnewvalues, req.traj.points[1]);
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ if( (*ittrajcontroller)->_srvTrajectoryStart.call(req,res) )
+#else
if( (*ittrajcontroller)->_srvTrajectoryStart->call(req,res) )
+#endif
(*ittrajcontroller)->_listTrajectories.push_back(res.trajectoryid);
else {
RAVELOG_ERRORA("failed to start trajectory\n");
@@ -301,7 +323,7 @@
bool bSuccess = true;
pr2_mechanism_controllers::TrajectoryStart::Request req;
pr2_mechanism_controllers::TrajectoryStart::Response res;
-
+
FOREACH(ittrajcontroller, _listControllers) {
if( !(*ittrajcontroller)->_srvTrajectoryStart ) {
RAVELOG_ERRORA("no start trajectory service\n");
@@ -313,11 +335,15 @@
_bIsDone = false;
req.hastiming = 0;
req.traj.points.resize(ptraj->GetPoints().size());
- typeof(req.traj.points.begin()) ittraj = req.traj.points.begin();
+ typeof(req.traj.points.begin()) ittraj = req.traj.points.begin();
FOREACHC(itpoint, ptraj->GetPoints())
(*ittrajcontroller)->GetTrajPoint(itpoint->q, *ittraj++);
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ if( (*ittrajcontroller)->_srvTrajectoryStart.call(req,res) )
+#else
if( (*ittrajcontroller)->_srvTrajectoryStart->call(req,res) )
+#endif
(*ittrajcontroller)->_listTrajectories.push_back(res.trajectoryid);
else {
RAVELOG_ERRORA("failed to start trajectory\n");
@@ -415,7 +441,7 @@
FOREACHC(itj, _mapjoints)
vel[itj->second] = _mstate.joint_states[itj->first].velocity;
}
-
+
virtual void GetTorque(std::vector<dReal>& torque) const
{
torque.resize(0);
@@ -485,7 +511,7 @@
break;
}
}
-
+
if( !bAdded ) {
RAVELOG_WARNA("could not find robot joint %s in mechanism state\n", itj->first.c_str());
break;
@@ -510,7 +536,7 @@
_mstate = _mstate_cb;
}
-
+
// do some monitoring of the joint state (try to look for stalls)
}
@@ -523,7 +549,7 @@
// check if the first trajectory is done
boost::mutex::scoped_lock lock(_mutexTrajectories);
-
+
if( _listControllers.size() > 0 ) {
bool bPopTrajectory = true;
@@ -535,11 +561,15 @@
break;
}
req.trajectoryid = (*ittraj)->_listTrajectories.front();
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ if( !(*ittraj)->_srvTrajectoryQuery.call(req,res) ) {
+#else
if( !(*ittraj)->_srvTrajectoryQuery->call(req,res) ) {
+#endif
RAVELOG_ERRORA("trajectory query failed\n");
bPopTrajectory = false;
}
-
+
if( !(res.done == pr2_mechanism_controllers::TrajectoryQuery::Response::State_Done ||
res.done == pr2_mechanism_controllers::TrajectoryQuery::Response::State_Deleted ||
res.done == pr2_mechanism_controllers::TrajectoryQuery::Response::State_Failed ||
@@ -568,7 +598,7 @@
RobotBase* _probot; ///< robot owning this controller
string _topic;
-
+
robot_msgs::MechanismState _mstate_cb, _mstate;
vector<dReal> _vecdesired;
set< pair<string, int> > _setEnabledJoints; // set of enabled joints and their indices
@@ -577,7 +607,7 @@
ofstream flog;
int logid;
-
+
map<int, int> _mapjoints; ///< maps mechanism state joints to robot joints
list<boost::shared_ptr<TrajectoryController> > _listControllers;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-05-06 03:11:44
|
Revision: 14924
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14924&view=rev
Author: gerkey
Date: 2009-05-06 03:11:41 +0000 (Wed, 06 May 2009)
Log Message:
-----------
Fixed build for moved mocap messages
Modified Paths:
--------------
pkg/trunk/calibration/kinematic_calibration/manifest.xml
pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg
pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg
pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
pkg/trunk/drivers/phase_space/manifest.xml
pkg/trunk/drivers/phase_space/phase_space_node.cpp
pkg/trunk/drivers/phase_space/phase_space_node.h
Modified: pkg/trunk/calibration/kinematic_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/manifest.xml 2009-05-06 02:53:38 UTC (rev 14923)
+++ pkg/trunk/calibration/kinematic_calibration/manifest.xml 2009-05-06 03:11:41 UTC (rev 14924)
@@ -6,6 +6,7 @@
<depend package="kdl" />
<depend package="phase_space" />
<depend package="robot_msgs" />
+ <depend package="mocap_msgs" />
<depend package="std_msgs" />
<depend package="robot_kinematics" />
<depend package="topic_synchronizer" />
Modified: pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg 2009-05-06 02:53:38 UTC (rev 14923)
+++ pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg 2009-05-06 03:11:41 UTC (rev 14924)
@@ -14,4 +14,4 @@
robot_msgs/PointCloud laser_cloud
# Data that we captured from phasespace
-robot_msgs/MocapSnapshot mocap_snapshot
+mocap_msgs/MocapSnapshot mocap_snapshot
Modified: pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg 2009-05-06 02:53:38 UTC (rev 14923)
+++ pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg 2009-05-06 03:11:41 UTC (rev 14924)
@@ -14,8 +14,8 @@
robot_msgs/PointCloud laser_cloud
# Data that we captured from phasespace
-robot_msgs/MocapSnapshot mocap_snapshot
+mocap_msgs/MocapSnapshot mocap_snapshot
# Data from multiple monocular cameras
image_msgs/Image[] image
-image_msgs/CamInfo[] cam_info
\ No newline at end of file
+image_msgs/CamInfo[] cam_info
Modified: pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-05-06 02:53:38 UTC (rev 14923)
+++ pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-05-06 03:11:41 UTC (rev 14924)
@@ -19,4 +19,4 @@
robot_msgs/PointCloud laser_cloud
# Data that we captured from phasespace
-robot_msgs/MocapSnapshot phase_space_snapshot
+mocap_msgs/MocapSnapshot phase_space_snapshot
Modified: pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp 2009-05-06 02:53:38 UTC (rev 14923)
+++ pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp 2009-05-06 03:11:41 UTC (rev 14924)
@@ -37,7 +37,7 @@
#include "ros/node.h"
#include "boost/thread/mutex.hpp"
#include "robot_msgs/MechanismState.h"
-#include "robot_msgs/MocapSnapshot.h"
+#include "mocap_msgs/MocapSnapshot.h"
#include "robot_kinematics/robot_kinematics.h"
#include "kdl/chain.hpp"
@@ -111,7 +111,7 @@
case ' ':
{
printf("Capturing...\n") ;
- robot_msgs::MocapMarker cur_marker ;
+ mocap_msgs::MocapMarker cur_marker ;
//GetMarker(cur_marker, marker_id_) ;
@@ -238,7 +238,7 @@
return true ;
}
- void GetMarker(robot_msgs::MocapMarker& marker, int id)
+ void GetMarker(mocap_msgs::MocapMarker& marker, int id)
{
bool marker_found = false ;
@@ -304,10 +304,10 @@
}
private:
- robot_msgs::MocapSnapshot snapshot_ ;
+ mocap_msgs::MocapSnapshot snapshot_ ;
robot_msgs::MechanismState mech_state_ ;
- robot_msgs::MocapSnapshot safe_snapshot_ ;
+ mocap_msgs::MocapSnapshot safe_snapshot_ ;
int marker_id_ ;
robot_msgs::MechanismState safe_mech_state_ ;
Modified: pkg/trunk/drivers/phase_space/manifest.xml
===================================================================
--- pkg/trunk/drivers/phase_space/manifest.xml 2009-05-06 02:53:38 UTC (rev 14923)
+++ pkg/trunk/drivers/phase_space/manifest.xml 2009-05-06 03:11:41 UTC (rev 14924)
@@ -5,7 +5,7 @@
<review status="unreviewed" notes=""/>
<depend package="roscpp" />
<depend package="std_msgs" />
- <depend package="robot_msgs" />
+ <depend package="mocap_msgs" />
<depend package="rospy" />
<sysdepend os="ubuntu" version="8.04-hardy" package="libstdc++5"/>
Modified: pkg/trunk/drivers/phase_space/phase_space_node.cpp
===================================================================
--- pkg/trunk/drivers/phase_space/phase_space_node.cpp 2009-05-06 02:53:38 UTC (rev 14923)
+++ pkg/trunk/drivers/phase_space/phase_space_node.cpp 2009-05-06 03:11:41 UTC (rev 14924)
@@ -57,7 +57,7 @@
PhaseSpaceNode::PhaseSpaceNode() : ros::Node("phase_space")
{
- advertise<robot_msgs::MocapSnapshot>("phase_space_snapshot", 1) ;
+ advertise<mocap_msgs::MocapSnapshot>("phase_space_snapshot", 1) ;
}
PhaseSpaceNode::~PhaseSpaceNode()
@@ -88,7 +88,7 @@
while (ok()) // While the node has not been shutdown
{
usleep(1) ;
- robot_msgs::MocapSnapshot snapshot ;
+ mocap_msgs::MocapSnapshot snapshot ;
snapshot.header.frame_id = "phase_space" ;
@@ -112,7 +112,7 @@
return true ;
}
-int PhaseSpaceNode::grabMarkers(robot_msgs::MocapSnapshot& snapshot)
+int PhaseSpaceNode::grabMarkers(mocap_msgs::MocapSnapshot& snapshot)
{
OWLMarker markers[MAX_NUM_MARKERS] ;
@@ -147,7 +147,7 @@
return n ;
}
-int PhaseSpaceNode::grabBodies(robot_msgs::MocapSnapshot& snapshot)
+int PhaseSpaceNode::grabBodies(mocap_msgs::MocapSnapshot& snapshot)
{
OWLRigid bodies[MAX_NUM_BODIES] ;
int n = owlGetRigids(bodies, MAX_NUM_BODIES) ;
@@ -173,7 +173,7 @@
return n ;
}
-void PhaseSpaceNode::grabTime(robot_msgs::MocapSnapshot& snapshot)
+void PhaseSpaceNode::grabTime(mocap_msgs::MocapSnapshot& snapshot)
{
int timeVal[3] ;
timeVal[0] = 0 ;
@@ -193,7 +193,7 @@
owlDone() ; // OWL API-call to perform system cleanup before client termination
}
-void PhaseSpaceNode::copyMarkerToMessage(const OWLMarker& owl_marker, robot_msgs::MocapMarker& msg_marker)
+void PhaseSpaceNode::copyMarkerToMessage(const OWLMarker& owl_marker, mocap_msgs::MocapMarker& msg_marker)
{
msg_marker.id = owl_marker.id ;
@@ -204,7 +204,7 @@
msg_marker.condition = owl_marker.cond ;
}
-void PhaseSpaceNode::copyBodyToMessage(const OWLRigid& owl_body, robot_msgs::MocapBody& msg_body)
+void PhaseSpaceNode::copyBodyToMessage(const OWLRigid& owl_body, mocap_msgs::MocapBody& msg_body)
{
msg_body.id = owl_body.id ;
@@ -236,7 +236,7 @@
printf("%s: 0x%x\n", s, n);
}
-void PhaseSpaceNode::dispSnapshot(const robot_msgs::MocapSnapshot& s)
+void PhaseSpaceNode::dispSnapshot(const mocap_msgs::MocapSnapshot& s)
{
printf("rosTF_frame: %s Frame #%u\n", s.header.frame_id.c_str(), s.frameNum) ;
printf(" Time: %lf\n", s.header.stamp.toSec()) ;
Modified: pkg/trunk/drivers/phase_space/phase_space_node.h
===================================================================
--- pkg/trunk/drivers/phase_space/phase_space_node.h 2009-05-06 02:53:38 UTC (rev 14923)
+++ pkg/trunk/drivers/phase_space/phase_space_node.h 2009-05-06 03:11:41 UTC (rev 14924)
@@ -48,9 +48,9 @@
#include "owl.h"
// Messages
-#include "robot_msgs/MocapSnapshot.h"
-#include "robot_msgs/MocapMarker.h"
-#include "robot_msgs/MocapBody.h"
+#include "mocap_msgs/MocapSnapshot.h"
+#include "mocap_msgs/MocapMarker.h"
+#include "mocap_msgs/MocapBody.h"
namespace phase_space
{
@@ -68,13 +68,13 @@
bool spin() ;
private :
- int grabMarkers(robot_msgs::MocapSnapshot& snapshot) ;
- int grabBodies (robot_msgs::MocapSnapshot& snapshot) ;
- void grabTime (robot_msgs::MocapSnapshot& snapshot) ;
- void copyMarkerToMessage(const OWLMarker& owl_marker, robot_msgs::MocapMarker& msg_marker) ;
- void copyBodyToMessage(const OWLRigid& owl_body, robot_msgs::MocapBody& msg_body) ;
+ int grabMarkers(mocap_msgs::MocapSnapshot& snapshot) ;
+ int grabBodies (mocap_msgs::MocapSnapshot& snapshot) ;
+ void grabTime (mocap_msgs::MocapSnapshot& snapshot) ;
+ void copyMarkerToMessage(const OWLMarker& owl_marker, mocap_msgs::MocapMarker& msg_marker) ;
+ void copyBodyToMessage(const OWLRigid& owl_body, mocap_msgs::MocapBody& msg_body) ;
void owlPrintError(const char *s, int n) ;
- void dispSnapshot(const robot_msgs::MocapSnapshot& snapshot) ;
+ void dispSnapshot(const mocap_msgs::MocapSnapshot& snapshot) ;
} ;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-05-07 02:11:56
|
Revision: 15019
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=15019&view=rev
Author: vijaypradeep
Date: 2009-05-07 02:11:53 +0000 (Thu, 07 May 2009)
Log Message:
-----------
Moving ImagePoint messages into kinematic_calibration package
Modified Paths:
--------------
pkg/trunk/calibration/kinematic_calibration/scripts/new_cmd_node.py
Added Paths:
-----------
pkg/trunk/calibration/kinematic_calibration/msg/ImagePoint.msg
pkg/trunk/calibration/kinematic_calibration/msg/ImagePointStamped.msg
Removed Paths:
-------------
pkg/trunk/common/image_msgs/msg/ImagePoint.msg
pkg/trunk/common/image_msgs/msg/ImagePointStamped.msg
Copied: pkg/trunk/calibration/kinematic_calibration/msg/ImagePoint.msg (from rev 15014, pkg/trunk/common/image_msgs/msg/ImagePoint.msg)
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/ImagePoint.msg (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/msg/ImagePoint.msg 2009-05-07 02:11:53 UTC (rev 15019)
@@ -0,0 +1,3 @@
+# Stores the xy location of a point in an image in pixel coordinates
+float64 x
+float64 y
Copied: pkg/trunk/calibration/kinematic_calibration/msg/ImagePointStamped.msg (from rev 15014, pkg/trunk/common/image_msgs/msg/ImagePointStamped.msg)
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/ImagePointStamped.msg (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/msg/ImagePointStamped.msg 2009-05-07 02:11:53 UTC (rev 15019)
@@ -0,0 +1,2 @@
+Header header
+ImagePoint image_point
\ No newline at end of file
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/new_cmd_node.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/new_cmd_node.py 2009-05-07 02:11:35 UTC (rev 15018)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/new_cmd_node.py 2009-05-07 02:11:53 UTC (rev 15019)
@@ -9,7 +9,7 @@
from pr2_mechanism_controllers.srv import *
from pr2_mechanism_controllers.msg import *
from robot_msgs.msg import *
-from image_msgs.msg import *
+from kinematic_calibration.msg import *
from kinematic_calibration.arm_commander import ArmCommander
from kinematic_calibration.settler import *
Deleted: pkg/trunk/common/image_msgs/msg/ImagePoint.msg
===================================================================
--- pkg/trunk/common/image_msgs/msg/ImagePoint.msg 2009-05-07 02:11:35 UTC (rev 15018)
+++ pkg/trunk/common/image_msgs/msg/ImagePoint.msg 2009-05-07 02:11:53 UTC (rev 15019)
@@ -1,3 +0,0 @@
-# Stores the xy location of a point in an image in pixel coordinates
-float64 x
-float64 y
Deleted: pkg/trunk/common/image_msgs/msg/ImagePointStamped.msg
===================================================================
--- pkg/trunk/common/image_msgs/msg/ImagePointStamped.msg 2009-05-07 02:11:35 UTC (rev 15018)
+++ pkg/trunk/common/image_msgs/msg/ImagePointStamped.msg 2009-05-07 02:11:53 UTC (rev 15019)
@@ -1,2 +0,0 @@
-Header header
-ImagePoint image_point
\ No newline at end of file
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