Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7186] pkg/trunk
|
From: <tpr...@us...> - 2008-11-22 02:29:31
|
Revision: 7186
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7186&view=rev
Author: tpratkanis
Date: 2008-11-22 02:29:25 +0000 (Sat, 22 Nov 2008)
Log Message:
-----------
Massive commit to port trajectory rollout and highlevel_controllers to the new transform lib.
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
pkg/trunk/highlevel/highlevel_controllers/include/VelocityControllers.hh
pkg/trunk/highlevel/highlevel_controllers/manifest.xml
pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/highlevel/highlevel_controllers/src/VelocityControllers.cc
pkg/trunk/trajectory_rollout/include/trajectory_rollout/governor_node.h
pkg/trunk/trajectory_rollout/include/trajectory_rollout/trajectory_controller.h
pkg/trunk/trajectory_rollout/manifest.xml
pkg/trunk/trajectory_rollout/src/governor_node.cpp
pkg/trunk/trajectory_rollout/src/trajectory_controller.cpp
Modified: pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-22 01:35:23 UTC (rev 7185)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-11-22 02:29:25 UTC (rev 7186)
@@ -50,7 +50,8 @@
#include <std_msgs/RobotBase2DOdom.h>
// For transform support
-#include <rosTF/rosTF.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
// Laser projection
#include "laser_scan_utils/laser_scan.h"
@@ -201,7 +202,7 @@
std_msgs::RobotBase2DOdom odomMsg_; /**< Odometry in the odom frame picked up by subscription */
laser_scan::LaserProjection projector_; /**< Used to project laser scans */
- rosTFClient tf_; /**< Used to do transforms */
+ tf::TransformListener tf_; /**< Used to do transforms */
// Observation Buffers are dynamically allocated since their constructors take
// arguments bound by lookup to the param server. This could be chnaged with some reworking of how paramaters
@@ -219,7 +220,7 @@
CostMapAccessor* ma_; /**< Sliding read-only window on the cost map */
- libTF::TFPose2D global_pose_; /**< The global pose in the map frame */
+ tf::Stamped<tf::Pose> global_pose_; /**< The global pose in the map frame */
std_msgs::RobotBase2DOdom base_odom_; /**< Odometry in the base frame */
Modified: pkg/trunk/highlevel/highlevel_controllers/include/VelocityControllers.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/VelocityControllers.hh 2008-11-22 01:35:23 UTC (rev 7185)
+++ pkg/trunk/highlevel/highlevel_controllers/include/VelocityControllers.hh 2008-11-22 02:29:25 UTC (rev 7186)
@@ -11,7 +11,7 @@
#include <trajectory_rollout/trajectory_controller.h>
// For transform support
-#include <rosTF/rosTF.h>
+#include <tf/transform_listener.h>
using namespace costmap_2d;
@@ -30,7 +30,7 @@
* @brief Compute velocities for x, y and theta based on an obstacle map, and a current path
*/
virtual bool computeVelocityCommands(const std::list<std_msgs::Pose2DFloat32>& globalPlan,
- const libTF::TFPose2D& pose,
+ const tf::Stamped<tf::Pose>& pose,
const std_msgs::BaseVel& currentVel,
std_msgs::BaseVel& cmdVel,
std::list<std_msgs::Pose2DFloat32>& localPlan) = 0;
@@ -44,7 +44,7 @@
*/
class TrajectoryRolloutController: public VelocityController {
public:
- TrajectoryRolloutController(rosTFClient* tf, const CostMapAccessor& ma,
+ TrajectoryRolloutController(tf::TransformListener* tf, const CostMapAccessor& ma,
double sim_time, int sim_steps, int samples_per_dim,
double pdist_scale, double gdist_scale, double dfast_scale, double occdist_scale,
double acc_lim_x, double acc_lim_y, double acc_lim_th, std::vector<std_msgs::Point2DFloat32> footprint_spec);
@@ -52,7 +52,7 @@
virtual ~TrajectoryRolloutController(){}
virtual bool computeVelocityCommands(const std::list<std_msgs::Pose2DFloat32>& globalPlan,
- const libTF::TFPose2D& pose,
+ const tf::Stamped<tf::Pose>& pose,
const std_msgs::BaseVel& currentVel,
std_msgs::BaseVel& cmdVel,
std::list<std_msgs::Pose2DFloat32>& localPlan);
@@ -63,7 +63,7 @@
private:
//transform client
- rosTFClient* tf_;
+ tf::TransformListener* tf_;
// Cost map accessor
const costmap_2d::ObstacleMapAccessor& ma_;
Modified: pkg/trunk/highlevel/highlevel_controllers/manifest.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2008-11-22 01:35:23 UTC (rev 7185)
+++ pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2008-11-22 02:29:25 UTC (rev 7186)
@@ -20,7 +20,6 @@
<depend package="trajectory_rollout"/>
<depend package="laser_scan_utils" />
<depend package="costmap_2d"/>
- <depend package="rosTF"/>
<depend package="tf"/>
<depend package="ompl"/>
<depend package="sbpl"/>
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp 2008-11-22 01:35:23 UTC (rev 7185)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp 2008-11-22 02:29:25 UTC (rev 7186)
@@ -80,7 +80,7 @@
#include <robot_msgs/DisplayKinematicPath.h>
#include <robot_srvs/NamedKinematicPlanState.h>
#include <robot_srvs/PlanNames.h>
-#include <rosTF/rosTF.h>
+#include "tf/transform_listener.h"
static const unsigned int RIGHT_ARM_JOINTS_BASE_INDEX = 11;
static const unsigned int LEFT_ARM_JOINTS_BASE_INDEX = 12;
@@ -120,7 +120,7 @@
robot_msgs::MechanismState mechanismState;
robot_srvs::NamedKinematicPlanState::response plan;
unsigned int currentWaypoint; /*!< The waypoint in the plan that we are targetting */
- rosTFClient tf_; /**< Used to do transforms */
+ tf::TransformListener tf_; /**< Used to do transforms */
protected:
std::vector<std::string> jointNames_; /*< The collection of joint names of interest. Instantiate in the derived class.*/
@@ -217,28 +217,25 @@
//Get the pose of the robot:
- libTF::TFPose2D robotPose, globalPose;
- globalPose.x = robotPose.x = 0;
- globalPose.y = robotPose.y = 0;
- globalPose.yaw = robotPose.yaw = 0;
- robotPose.frame = "base_link";
- globalPose.time = robotPose.time = 0;
+ tf::Stamped<tf::Pose> robotPose, globalPose;
+ robotPose.setIdentity();
+ robotPose.frame_id_ = "base_link";
+ robotPose.stamp_ = ros::Time((uint64_t)0ULL);
+
try{
- globalPose = this->tf_.transformPose2D("map", robotPose);
+ tf_.transformPose("map", robotPose, globalPose);
}
- catch(libTF::TransformReference::LookupException& ex){
- std::cout << "No Transform available Error\n";
+ catch(tf::LookupException& ex) {
+ ROS_INFO("No Transform available Error\n");
}
- catch(libTF::TransformReference::ConnectivityException& ex){
- std::cout << "Connectivity Error\n";
+ catch(tf::ConnectivityException& ex) {
+ ROS_INFO("Connectivity Error\n");
}
- catch(libTF::TransformReference::ExtrapolateException& ex){
- std::cout << "Extrapolation Error\n";
+ catch(tf::ExtrapolationException& ex) {
+ ROS_INFO("Extrapolation Error\n");
}
-
-
//initializing full value state
req.start_state.set_names_size(names.get_names_size());
req.start_state.set_joints_size(names.get_names_size());
@@ -247,10 +244,12 @@
//std::cout << req.start_state.names[i] << ": " << names.num_values[i] << std::endl;
req.start_state.joints[i].set_vals_size(names.num_values[i]);
if (names.names[i] == "base_joint") {
- std::cout << "Base: " << i << ", " << globalPose.x << ", " << globalPose.y << ", " << globalPose.yaw << std::endl;
- req.start_state.joints[i].vals[0] = globalPose.x;
- req.start_state.joints[i].vals[1] = globalPose.y;
- req.start_state.joints[i].vals[2] = globalPose.yaw;
+ double yaw, pitch, roll;
+ globalPose.getBasis().getEulerZYX(yaw, pitch, roll);
+ std::cout << "Base: " << i << ", " << globalPose.getOrigin().getX() << ", " << globalPose.getOrigin().getY() << ", " << yaw << std::endl;
+ req.start_state.joints[i].vals[0] = globalPose.getOrigin().getX();
+ req.start_state.joints[i].vals[1] = globalPose.getOrigin().getY();
+ req.start_state.joints[i].vals[2] = yaw;
} else {
for (int k = 0 ; k < names.num_values[i]; k++) {
req.start_state.joints[i].vals[k] = 0;
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-22 01:35:23 UTC (rev 7185)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-11-22 02:29:25 UTC (rev 7186)
@@ -47,7 +47,7 @@
MoveBase::MoveBase()
: HighlevelController<std_msgs::Planner2DState, std_msgs::Planner2DGoal>("move_base", "state", "goal"),
- tf_(*this, true, 10000000000ULL, 0ULL), // cache for 10 sec, no extrapolation
+ tf_(*this, true, 10000000000ULL), // cache for 10 sec, no extrapolation
controller_(NULL),
costMap_(NULL),
ma_(NULL),
@@ -56,9 +56,7 @@
minZ_(0.03), maxZ_(2.0), robotWidth_(0.0), active_(true) , map_update_frequency_(10.0)
{
// Initialize global pose. Will be set in control loop based on actual data.
- global_pose_.x = 0;
- global_pose_.y = 0;
- global_pose_.yaw = 0;
+ global_pose_.setIdentity();
// Initialize odometry
base_odom_.vel.x = 0;
@@ -117,10 +115,9 @@
robotWidth_ = inscribedRadius * 2;
// Allocate observation buffers
- tf::TransformListener *tempTf_ = new tf::TransformListener(*this, true, (uint64_t)10000000000ULL);
- baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), *tempTf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
- tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), *tempTf_, ros::Duration(1, 0), inscribedRadius, minZ_, maxZ_);
- stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo"), *tempTf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
+ baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
+ tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), tf_, ros::Duration(1, 0), inscribedRadius, minZ_, maxZ_);
+ stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo"), tf_, ros::Duration(0, 0), inscribedRadius, minZ_, maxZ_);
// get map via RPC
std_srvs::StaticMap::request req;
@@ -261,58 +258,67 @@
delete stereoCloudBuffer_;
}
- void MoveBase::updateGlobalPose(){
- libTF::TFPose2D robotPose;
- robotPose.x = 0;
- robotPose.y = 0;
- robotPose.yaw = 0;
- robotPose.frame = "base_link";
- robotPose.time = 0;
+ void MoveBase::updateGlobalPose(){
+ tf::Stamped<tf::Pose> robotPose;
+ robotPose.setIdentity();
+ robotPose.frame_id_ = "base_link";
+ robotPose.stamp_ = ros::Time((uint64_t)0ULL);
try{
- global_pose_ = this->tf_.transformPose2D("map", robotPose);
+ tf_.transformPose("map", robotPose, global_pose_);
}
- catch(libTF::TransformReference::LookupException& ex){
+ catch(tf::LookupException& ex) {
ROS_INFO("No Transform available Error\n");
}
- catch(libTF::TransformReference::ConnectivityException& ex){
- ROS_INFO("Connectivity Error\n");
+ catch(tf::ConnectivityException& ex) {
+ ROS_INFO("Connectivity Error\n");
}
- catch(libTF::TransformReference::ExtrapolateException& ex){
- ROS_INFO("Extrapolation Error\n");
- }
+ catch(tf::ExtrapolationException& ex) {
+ ROS_INFO("Extrapolation Error\n");
+ }
+
+ // Update the cost map window
+
+
+ double uselessPitch, uselessRoll, yaw;
+ global_pose_.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
+ printf("Received new position (x=%f, y=%f, th=%f)\n", global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), yaw);
+
+ ma_->updateForRobotPosition(global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
}
void MoveBase::updateGoalMsg(){
- libTF::TFPose2D goalPose, transformedGoalPose;
- goalPose.x = goalMsg.goal.x;
- goalPose.y = goalMsg.goal.y;
- goalPose.yaw = goalMsg.goal.th;
- goalPose.frame = goalMsg.header.frame_id;
- goalPose.time = 0;
-
+ tf::Stamped<tf::Pose> goalPose, transformedGoalPose;
+ btQuaternion qt;
+ qt.setEulerZYX(goalMsg.goal.th, 0, 0);
+ goalPose.setData(btTransform(qt, btVector3(goalMsg.goal.x, goalMsg.goal.y, 0)));
+ goalPose.frame_id_ = goalMsg.header.frame_id;
+ goalPose.stamp_ = ros::Time((uint64_t)0ULL);
+
try{
- transformedGoalPose = this->tf_.transformPose2D("map", goalPose);
+ tf_.transformPose("map", goalPose, transformedGoalPose);
}
- catch(libTF::TransformReference::LookupException& ex){
+ catch(tf::LookupException& ex){
ROS_ERROR("No transform available from %s to map. This may be because the frame_id of the goalMsg is wrong.\n", goalMsg.header.frame_id.c_str());
ROS_ERROR("The details of the LookupException are: %s\n", ex.what());
}
- catch(libTF::TransformReference::ConnectivityException& ex){
+ catch(tf::ConnectivityException& ex){
ROS_ERROR("No transform available from %s to map. This may be because the frame_id of the goalMsg is wrong.\n", goalMsg.header.frame_id.c_str());
ROS_ERROR("The details of the LookupException are: %s\n", ex.what());
}
- catch(libTF::TransformReference::ExtrapolateException& ex){
+ catch(tf::ExtrapolationException& ex){
ROS_ERROR("No transform available from %s to map. This may be because the frame_id of the goalMsg is wrong.\n", goalMsg.header.frame_id.c_str());
ROS_ERROR("The details of the LookupException are: %s\n", ex.what());
}
- stateMsg.goal.x = goalPose.x;
- stateMsg.goal.y = goalPose.y;
- stateMsg.goal.th = goalPose.yaw;
-
- ROS_DEBUG("Received new goal (x=%f, y=%f, th=%f)\n", goalMsg.goal.x, goalMsg.goal.y, goalMsg.goal.th);
+ stateMsg.goal.x = transformedGoalPose.getOrigin().x();
+ stateMsg.goal.y = transformedGoalPose.getOrigin().y();
+ double uselessPitch, uselessRoll, yaw;
+ transformedGoalPose.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
+ stateMsg.goal.th = (float)yaw;
+
+ ROS_DEBUG("Received new goal (x=%f, y=%f, th=%f)\n", stateMsg.goal.x, stateMsg.goal.y, stateMsg.goal.th);
}
void MoveBase::updateStateMsg(){
@@ -320,9 +326,11 @@
updateGlobalPose();
// Assign state data
- stateMsg.pos.x = global_pose_.x;
- stateMsg.pos.y = global_pose_.y;
- stateMsg.pos.th = global_pose_.yaw;
+ stateMsg.pos.x = global_pose_.getOrigin().x();
+ stateMsg.pos.y = global_pose_.getOrigin().y();
+ double uselessPitch, uselessRoll, yaw;
+ global_pose_.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
+ stateMsg.pos.th = (float)yaw;
}
void MoveBase::baseScanCallback()
@@ -366,28 +374,23 @@
try
{
- libTF::TFVector v_in, v_out;
- v_in.x = odomMsg_.vel.x;
- v_in.y = odomMsg_.vel.y;
- v_in.z = odomMsg_.vel.th;
- v_in.time = 0; // Gets the latest
- v_in.frame = "odom";
- v_out = tf_.transformVector("base_link", v_in);
- base_odom_.vel.x = v_in.x;
- base_odom_.vel.y = v_in.y;
- base_odom_.vel.th = v_in.z;
+ tf::Stamped<btVector3> v_in(btVector3(odomMsg_.vel.x, odomMsg_.vel.y, odomMsg_.vel.th), ros::Time((uint64_t)0ULL), "odom"), v_out;
+ tf_.transformVector("base_link", ros::Time((uint64_t)0ULL), v_in, "odom", v_out);
+ base_odom_.vel.x = v_in.x();
+ base_odom_.vel.y = v_in.y();
+ base_odom_.vel.th = v_in.z();
}
- catch(libTF::TransformReference::LookupException& ex)
+ catch(tf::LookupException& ex)
{
puts("no odom->base Tx yet");
ROS_DEBUG("%s\n", ex.what());
}
- catch(libTF::TransformReference::ConnectivityException& ex)
+ catch(tf::ConnectivityException& ex)
{
puts("no odom->base Tx yet");
ROS_DEBUG("%s\n", ex.what());
}
- catch(libTF::TransformReference::ExtrapolateException& ex)
+ catch(tf::ExtrapolationException& ex)
{
puts("Extrapolation exception");
}
@@ -485,12 +488,14 @@
// If the plan has been executed (i.e. empty) and we are within a required distance of the target orientation,
// and we have stopped the robot, then we are done
+ double uselessPitch, uselessRoll, yaw;
+ global_pose_.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
if(plan_.empty() &&
- fabs(global_pose_.yaw - stateMsg.goal.th) < 0.1){
+ fabs(yaw - stateMsg.goal.th) < 0.1){
ROS_DEBUG("Goal achieved at: (%f, %f, %f) for (%f, %f, %f)\n",
- global_pose_.x, global_pose_.y, global_pose_.yaw,
- stateMsg.goal.x, stateMsg.goal.y, stateMsg.goal.th);
+ global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), yaw,
+ stateMsg.goal.x, stateMsg.goal.y, stateMsg.goal.th);
// The last act will issue stop command
stopRobot();
@@ -500,11 +505,11 @@
// If we have reached the end of the path then clear the plan
if(!plan_.empty() &&
- withinDistance(global_pose_.x, global_pose_.y, global_pose_.yaw,
- stateMsg.goal.x, stateMsg.goal.y, global_pose_.yaw)){
+ withinDistance(global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), yaw,
+ stateMsg.goal.x, stateMsg.goal.y, yaw)){
ROS_DEBUG("Last waypoint achieved at: (%f, %f, %f) for (%f, %f, %f)\n",
- global_pose_.x, global_pose_.y, global_pose_.yaw,
- stateMsg.goal.x, stateMsg.goal.y, stateMsg.goal.th);
+ global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), yaw,
+ stateMsg.goal.x, stateMsg.goal.y, stateMsg.goal.th);
plan_.clear();
}
@@ -550,15 +555,17 @@
std_msgs::BaseVel cmdVel; // Commanded velocities
// Update the cost map window
- ma_->updateForRobotPosition(global_pose_.x, global_pose_.y);
+ ma_->updateForRobotPosition(global_pose_.getOrigin().getX(), global_pose_.getOrigin().getY());
// if we have achieved all our waypoints but have yet to achieve the goal, then we know that we wish to accomplish our desired
// orientation
if(plan_.empty()){
+ double uselessPitch, uselessRoll, yaw;
+ global_pose_.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
ROS_DEBUG("Moving to desired goal orientation\n");
cmdVel.vx = 0;
cmdVel.vy = 0;
- cmdVel.vw = stateMsg.goal.th - global_pose_.yaw;
+ cmdVel.vw = stateMsg.goal.th - yaw;
cmdVel.vw = cmdVel.vw >= 0.0 ? cmdVel.vw + .4 : cmdVel.vw - .4;
}
else {
@@ -570,8 +577,8 @@
while(it != plan_.end()){
const std_msgs::Pose2DFloat32& w = *it;
// Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
- if(fabs(global_pose_.x - w.x) < 2 && fabs(global_pose_.y - w.y) < 2){
- ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose_.x, global_pose_.y, w.x, w.y);
+ if(fabs(global_pose_.getOrigin().x() - w.x) < 2 && fabs(global_pose_.getOrigin().y() - w.y) < 2){
+ ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), w.x, w.y);
break;
}
@@ -619,7 +626,9 @@
}
publish("cmd_vel", cmdVel);
- publishFootprint(global_pose_.x, global_pose_.y, global_pose_.yaw);
+ double uselessPitch, uselessRoll, yaw;
+ global_pose_.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
+ publishFootprint(global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), yaw);
//publish a point that the head can track
double ptx, pty;
@@ -651,7 +660,7 @@
*/
void MoveBase::publishLocalCostMap() {
double mapSize = std::min(costMap_->getWidth()/2, costMap_->getHeight()/2);
- CostMapAccessor cm(*costMap_, std::min(10.0, mapSize), global_pose_.x, global_pose_.y);
+ CostMapAccessor cm(*costMap_, std::min(10.0, mapSize), global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
// Publish obstacle data for each obstacle cell
std::vector< std::pair<double, double> > rawObstacles, inflatedObstacles;
@@ -710,7 +719,7 @@
*/
void MoveBase::publishFreeSpaceAndObstacles() {
double mapSize = std::min(costMap_->getWidth()/2, costMap_->getHeight()/2);
- CostMapAccessor cm(*costMap_, std::min(10.0, mapSize), global_pose_.x, global_pose_.y);
+ CostMapAccessor cm(*costMap_, std::min(10.0, mapSize), global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
// Publish obstacle data for each obstacle cell
std::vector< std::pair<double, double> > rawObstacles, inflatedObstacles;
@@ -806,7 +815,7 @@
gettimeofday(&curr,NULL);
double curr_t, last_t, t_diff;
curr_t = curr.tv_sec + curr.tv_usec / 1e6;
- costMap_->updateDynamicObstacles(global_pose_.x, global_pose_.y, observations);
+ costMap_->updateDynamicObstacles(global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), observations);
gettimeofday(&curr,NULL);
last_t = curr.tv_sec + curr.tv_usec / 1e6;
t_diff = last_t - curr_t;
Modified: pkg/trunk/highlevel/highlevel_controllers/src/VelocityControllers.cc
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/VelocityControllers.cc 2008-11-22 01:35:23 UTC (rev 7185)
+++ pkg/trunk/highlevel/highlevel_controllers/src/VelocityControllers.cc 2008-11-22 02:29:25 UTC (rev 7186)
@@ -4,7 +4,7 @@
namespace ros {
namespace highlevel_controllers {
- TrajectoryRolloutController::TrajectoryRolloutController(rosTFClient* tf, const CostMapAccessor& ma,
+ TrajectoryRolloutController::TrajectoryRolloutController(tf::TransformListener* tf, const CostMapAccessor& ma,
double sim_time, int sim_steps, int samples_per_dim,
double pdist_scale, double gdist_scale, double dfast_scale, double occdist_scale,
double acc_lim_x, double acc_lim_y, double acc_lim_th, std::vector<std_msgs::Point2DFloat32> footprint_spec)
@@ -17,20 +17,20 @@
}
bool TrajectoryRolloutController::computeVelocityCommands(const std::list<std_msgs::Pose2DFloat32>& globalPlan,
- const libTF::TFPose2D& pose,
+ const tf::Stamped<tf::Pose>& pose,
const std_msgs::BaseVel& currentVel,
std_msgs::BaseVel& cmdVel,
std::list<std_msgs::Pose2DFloat32>& localPlan){
localPlan.clear();
- libTF::TFPose2D drive_cmds;
+ tf::Stamped<tf::Pose> drive_cmds;
+ drive_cmds.frame_id_ = "base_link";
- libTF::TFPose2D robot_vel;
- robot_vel.x = currentVel.vx;
- robot_vel.y = currentVel.vy;
- robot_vel.yaw = currentVel.vw;
- robot_vel.frame = "base_link";
- robot_vel.time = 0;
+ tf::Stamped<tf::Pose> robot_vel;
+ btQuaternion qt(currentVel.vw, 0, 0);
+ robot_vel.setData(btTransform(qt, btVector3(currentVel.vx, currentVel.vy, 0)));
+ robot_vel.frame_id_ = "base_link";
+ robot_vel.stamp_ = ros::Time((uint64_t)0ULL);
//do we need to resize our map?
double origin_x, origin_y;
@@ -47,14 +47,20 @@
}
tc_.updatePlan(copiedGlobalPlan);
-
+
+
+
+
//compute what trajectory to drive along
Trajectory path = tc_.findBestPath(pose, robot_vel, drive_cmds);
//pass along drive commands
- cmdVel.vx = drive_cmds.x;
- cmdVel.vy = drive_cmds.y;
- cmdVel.vw = drive_cmds.yaw;
+ cmdVel.vx = drive_cmds.getOrigin().getX();
+ cmdVel.vy = drive_cmds.getOrigin().getY();
+ double uselessPitch, uselessRoll, yaw;
+ drive_cmds.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
+
+ cmdVel.vw = yaw;
//if we cannot move... tell someone
if(path.cost_ < 0)
Modified: pkg/trunk/trajectory_rollout/include/trajectory_rollout/governor_node.h
===================================================================
--- pkg/trunk/trajectory_rollout/include/trajectory_rollout/governor_node.h 2008-11-22 01:35:23 UTC (rev 7185)
+++ pkg/trunk/trajectory_rollout/include/trajectory_rollout/governor_node.h 2008-11-22 02:29:25 UTC (rev 7186)
@@ -52,7 +52,7 @@
#include <sys/time.h>
//for transform support
-#include <rosTF/rosTF.h>
+#include "tf/transform_listener.h"
//the trajectory controller
#include <trajectory_rollout/trajectory_controller.h>
@@ -145,7 +145,7 @@
MapGrid map_;
//transform client
- rosTFClient tf_;
+ tf::TransformListener tf_;
//map accessor
WavefrontMapAccessor ma_;
@@ -168,7 +168,7 @@
ros::thread::mutex map_lock;
//keep track of the robot's velocity
- libTF::TFPose2D robot_vel_;
+ tf::Stamped<tf::Pose> robot_vel_;
//how long for each cycle
double cycle_time_;
Modified: pkg/trunk/trajectory_rollout/include/trajectory_rollout/trajectory_controller.h
===================================================================
--- pkg/trunk/trajectory_rollout/include/trajectory_rollout/trajectory_controller.h 2008-11-22 01:35:23 UTC (rev 7185)
+++ pkg/trunk/trajectory_rollout/include/trajectory_rollout/trajectory_controller.h 2008-11-22 02:29:25 UTC (rev 7186)
@@ -45,7 +45,7 @@
#include <algorithm>
//For transform support
-#include <rosTF/rosTF.h>
+#include "tf/transform_listener.h"
#include <trajectory_rollout/map_cell.h>
#include <trajectory_rollout/map_grid.h>
@@ -70,12 +70,12 @@
//create a controller given a map and a path
TrajectoryController(MapGrid& mg, double sim_time, int num_steps, int samples_per_dim,
double pdist_scale, double gdist_scale, double dfast_scale, double occdist_scale,
- double acc_lim_x, double acc_lim_y, double acc_lim_theta, rosTFClient* tf,
+ double acc_lim_x, double acc_lim_y, double acc_lim_theta, tf::TransformListener* tf,
const costmap_2d::ObstacleMapAccessor& ma, std::vector<std_msgs::Point2DFloat32> footprint_spec);
//given the current state of the robot, find a good trajectory
- Trajectory findBestPath(libTF::TFPose2D global_pose, libTF::TFPose2D global_vel,
- libTF::TFPose2D& drive_velocities);
+ Trajectory findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
+ tf::Stamped<tf::Pose>& drive_velocities);
//compute the distance from each cell in the map grid to the planned path
void computePathDistance(std::queue<MapCell*>& dist_queue);
@@ -83,7 +83,7 @@
void computeGoalDistance(std::queue<MapCell*>& dist_queue);
//given a trajectory in map space get the drive commands to send to the robot
- libTF::TFPose2D getDriveVelocities(int t_num);
+ tf::Stamped<tf::Pose> getDriveVelocities(int t_num);
//create the trajectories we wish to score
Trajectory createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta,
@@ -142,7 +142,7 @@
double prev_x_, prev_y_;
//transform client
- rosTFClient* tf_;
+ tf::TransformListener* tf_;
//so that we can access obstacle information
const costmap_2d::ObstacleMapAccessor& ma_;
Modified: pkg/trunk/trajectory_rollout/manifest.xml
===================================================================
--- pkg/trunk/trajectory_rollout/manifest.xml 2008-11-22 01:35:23 UTC (rev 7185)
+++ pkg/trunk/trajectory_rollout/manifest.xml 2008-11-22 02:29:25 UTC (rev 7186)
@@ -12,7 +12,7 @@
<!-- <depend package="newmat10" /> -->
<!-- <depend package="boost" /> -->
<!-- <depend package="xmlparam" /> -->
- <depend package="rosTF" />
+ <depend package="tf" />
<depend package="rospy" />
<depend package="costmap_2d" />
<url>http://pr.willowgarage.com</url>
Modified: pkg/trunk/trajectory_rollout/src/governor_node.cpp
===================================================================
--- pkg/trunk/trajectory_rollout/src/governor_node.cpp 2008-11-22 01:35:23 UTC (rev 7185)
+++ pkg/trunk/trajectory_rollout/src/governor_node.cpp 2008-11-22 02:29:25 UTC (rev 7186)
@@ -40,17 +40,15 @@
GovernorNode::GovernorNode(std::vector<std_msgs::Point2DFloat32> footprint_spec) :
ros::node("governor_node"), map_(MAP_SIZE_X, MAP_SIZE_Y),
- tf_(*this, true, 1 * 1000000000ULL, 100000000ULL),
+ tf_(*this, true, (uint64_t)10000000000ULL),
ma_(map_, OUTER_RADIUS),
tc_(map_, SIM_TIME, SIM_STEPS, VEL_SAMPLES,
PDIST_SCALE, GDIST_SCALE, OCCDIST_SCALE, DFAST_SCALE, MAX_ACC_X, MAX_ACC_Y, MAX_ACC_THETA, &tf_, ma_, footprint_spec),
cycle_time_(0.1)
{
- robot_vel_.x = 0.0;
- robot_vel_.y = 0.0;
- robot_vel_.yaw = 0.0;
- robot_vel_.frame = "base";
- robot_vel_.time = 0;
+ robot_vel_.setIdentity();
+ robot_vel_.frame_id_ = "base_link";
+ robot_vel_.stamp_ = (ros::Time)0ULL;
//so we can draw the local path
advertise<std_msgs::Polyline2D>("local_path", 10);
@@ -66,11 +64,12 @@
void GovernorNode::odomReceived(){
//we want to make sure that processPlan isn't using robot_vel_
vel_lock.lock();
- robot_vel_.x = odom_msg_.vel.x;
- robot_vel_.y = odom_msg_.vel.y;
- robot_vel_.yaw = odom_msg_.vel.th;
- robot_vel_.frame = "base";
- robot_vel_.time = 0;
+
+ btQuaternion qt(odom_msg_.vel.th, 0, 0);
+ robot_vel_.setData(btTransform(qt, btVector3(odom_msg_.vel.x, odom_msg_.vel.y, 0)));
+ robot_vel_.frame_id_ = "base_link";
+ robot_vel_.stamp_ = ros::Time((uint64_t)0ULL);
+
//give robot_vel_ back
vel_lock.unlock();
}
@@ -97,44 +96,42 @@
}
void GovernorNode::processPlan(){
- libTF::TFPose2D robot_pose, global_pose;
- robot_pose.x = 0.0;
- robot_pose.y = 0.0;
- robot_pose.yaw = 0.0;
- robot_pose.frame = "base";
- robot_pose.time = 0;
+ tf::Stamped<tf::Pose> robot_pose, global_pose;
+ robot_pose.setIdentity();
+ robot_pose.frame_id_ = "base_link";
+ robot_pose.stamp_ = ros::Time((uint64_t)0ULL);
try
{
- global_pose = tf_.transformPose2D("map", robot_pose);
+ tf_.transformPose("map", robot_pose, global_pose);
}
- catch(libTF::TransformReference::LookupException& ex)
+ catch(tf::LookupException& ex)
{
puts("no global->local Tx yet");
printf("%s\n", ex.what());
return;
}
- catch(libTF::TransformReference::ConnectivityException& ex)
+ catch(tf::ConnectivityException& ex)
{
puts("no global->local Tx yet");
printf("%s\n", ex.what());
return;
}
- catch(libTF::TransformReference::ExtrapolateException& ex)
+ catch(tf::ExtrapolationException& ex)
{
// puts("extrapolation required");
// printf("%s\n", ex.what());
return;
}
- libTF::TFPose2D robot_vel;
+ tf::Stamped<tf::Pose> robot_vel;
//we need robot_vel_ to compute global_vel so we'll lock
vel_lock.lock();
robot_vel = robot_vel_;
//give robot_vel_ back
vel_lock.unlock();
- libTF::TFPose2D drive_cmds;
+ tf::Stamped<tf::Pose> drive_cmds;
struct timeval start;
struct timeval end;
double start_t, end_t, t_diff;
@@ -153,8 +150,11 @@
//give map_ back
map_lock.unlock();
- printf("Robot Vel - vx: %.2f, vy: %.2f, vth: %.2f\n", robot_vel.x, robot_vel.y, robot_vel.yaw);
+ double uselessPitch, uselessRoll, yaw;
+ robot_vel.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
+ printf("Robot Vel - vx: %.2f, vy: %.2f, vth: %.2f\n", robot_vel.getOrigin().getX(), robot_vel.getOrigin().getY(), yaw);
+
if(path.cost_ >= 0){
//let's print debug output to the screen
path_msg.set_points_size(tc_.num_steps_);
@@ -197,9 +197,10 @@
}
//drive the robot!
- cmd_vel_msg_.vx = drive_cmds.x;
- cmd_vel_msg_.vy = drive_cmds.y;
- cmd_vel_msg_.vw = drive_cmds.yaw;
+ cmd_vel_msg_.vx = drive_cmds.getOrigin().getX();
+ cmd_vel_msg_.vy = drive_cmds.getOrigin().getY();
+ drive_cmds.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
+ cmd_vel_msg_.vw = yaw;
if(path.cost_ < 0)
Modified: pkg/trunk/trajectory_rollout/src/trajectory_controller.cpp
===================================================================
--- pkg/trunk/trajectory_rollout/src/trajectory_controller.cpp 2008-11-22 01:35:23 UTC (rev 7185)
+++ pkg/trunk/trajectory_rollout/src/trajectory_controller.cpp 2008-11-22 02:29:25 UTC (rev 7186)
@@ -38,7 +38,7 @@
TrajectoryController::TrajectoryController(MapGrid& mg, double sim_time, int num_steps, int samples_per_dim,
double pdist_scale, double gdist_scale,
- double dfast_scale, double occdist_scale, double acc_lim_x, double acc_lim_y, double acc_lim_theta, rosTFClient* tf,
+ double dfast_scale, double occdist_scale, double acc_lim_x, double acc_lim_y, double acc_lim_theta, tf::TransformListener* tf,
const costmap_2d::ObstacleMapAccessor& ma, vector<std_msgs::Point2DFloat32> footprint_spec)
: map_(mg), num_steps_(num_steps), sim_time_(sim_time), samples_per_dim_(samples_per_dim),
pdist_scale_(pdist_scale), gdist_scale_(gdist_scale), dfast_scale_(dfast_scale), occdist_scale_(occdist_scale),
@@ -517,13 +517,22 @@
}
//given the current state of the robot, find a good trajectory
-Trajectory TrajectoryController::findBestPath(libTF::TFPose2D global_pose, libTF::TFPose2D global_vel,
- libTF::TFPose2D& drive_velocities){
+Trajectory TrajectoryController::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
+ tf::Stamped<tf::Pose>& drive_velocities){
+
+
+
//reset the map for new operations
map_.resetPathDist();
+
+ double uselessPitch, uselessRoll, yaw, velYaw;
+ global_pose.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
+
+
+
//temporarily remove obstacles that are within the footprint of the robot
- vector<std_msgs::Position2DInt> footprint_list = getFootprintCells(global_pose.x, global_pose.y, global_pose.yaw, true);
+ vector<std_msgs::Position2DInt> footprint_list = getFootprintCells(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), yaw, true);
//mark cells within the initial footprint of the robot
for(unsigned int i = 0; i < footprint_list.size(); ++i){
@@ -535,8 +544,13 @@
printf("Path/Goal distance computed\n");
+ global_pose.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
+ global_vel.getBasis().getEulerZYX(velYaw, uselessPitch, uselessRoll);
+
+
//rollout trajectories and find the minimum cost one
- Trajectory best = createTrajectories(global_pose.x, global_pose.y, global_pose.yaw, global_vel.x, global_vel.y, global_vel.yaw,
+ Trajectory best = createTrajectories(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), yaw,
+ global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), velYaw,
acc_lim_x_, acc_lim_y_, acc_lim_theta_);
printf("Trajectories created\n");
@@ -560,14 +574,14 @@
*/
if(best.cost_ < 0){
- drive_velocities.x = 0;
- drive_velocities.y = 0;
- drive_velocities.yaw = 0;
+ drive_velocities.setIdentity();
}
else{
- drive_velocities.x = best.xv_;
- drive_velocities.y = best.yv_;
- drive_velocities.yaw = best.thetav_;
+ btVector3 start(best.xv_, best.yv_, 0);
+ drive_velocities.setOrigin(start);
+ btMatrix3x3 matrix;
+ matrix.setRotation(btQuaternion(best.thetav_, 0, 0));
+ drive_velocities.setBasis(matrix);
}
return best;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
