Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[7796] pkg/trunk
|
From: <tf...@us...> - 2008-12-09 08:19:09
|
Revision: 7796
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7796&view=rev
Author: tfoote
Date: 2008-12-09 08:19:00 +0000 (Tue, 09 Dec 2008)
Log Message:
-----------
removing rosTF from planning_node_util and disabling build of world_3d_map for now until it can be ported to the new tf
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
pkg/trunk/motion_planning/planning_node_util/manifest.xml
pkg/trunk/world_models/world_3d_map/manifest.xml
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Added Paths:
-----------
pkg/trunk/world_models/world_3d_map/ROS_BUILD_BLACKLIST
Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-12-09 04:36:16 UTC (rev 7795)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-12-09 08:19:00 UTC (rev 7796)
@@ -48,7 +48,7 @@
#include <urdf/URDF.h>
#include <planning_models/kinematic.h>
#include <std_msgs/RobotBase2DOdom.h>
-#include <rosTF/rosTF.h>
+#include <tf/transform_listener.h>
#include <cmath>
#include <robot_msgs/MechanismState.h>
@@ -88,8 +88,9 @@
public:
- NodeRobotModel(ros::node *node, const std::string &robot_model) : m_tf(*node, true, 1 * 1000000000ULL, 1000000000ULL)
+ NodeRobotModel(ros::node *node, const std::string &robot_model) : m_tf(*node, true, 1000000000ULL)
{
+ m_tf.setExtrapolationLimit(ros::Duration().fromSec(10));
m_urdf = NULL;
m_kmodel = NULL;
m_kmodelSimple = NULL;
@@ -198,46 +199,46 @@
void localizedPoseCallback(void)
{
bool success = true;
- libTF::TFPose2D pose;
- pose.x = m_localizedPose.pos.x;
- pose.y = m_localizedPose.pos.y;
- pose.yaw = m_localizedPose.pos.th;
- pose.time = m_localizedPose.header.stamp.to_ull();
- pose.frame = m_localizedPose.header.frame_id;
-
+ tf::Stamped<tf::Pose> pose (tf::Transform(tf::Quaternion(m_localizedPose.pos.th, 0, 0),
+ tf::Point(m_localizedPose.pos.x,
+ m_localizedPose.pos.y, 0)),
+ m_localizedPose.header.stamp,
+ m_localizedPose.header.frame_id);
try
{
- pose = m_tf.transformPose2D("map", pose);
+ m_tf.transformPose("map", pose, pose);
}
- catch(libTF::TransformReference::LookupException& ex)
+ catch(tf::LookupException& ex)
{
- fprintf(stderr, "Discarding pose: from %s, Transform reference lookup exception: %s\n", pose.frame.c_str(), ex.what());
+ fprintf(stderr, "Discarding pose: from %s, Transform reference lookup exception: %s\n", pose.frame_id_.c_str(), ex.what());
success = false;
}
- catch(libTF::TransformReference::ExtrapolateException& ex)
+ catch(tf::ExtrapolationException& ex)
{
- fprintf(stderr, "Discarding pose: from %s, Extrapolation exception: %s\n", pose.frame.c_str(), ex.what());
+ fprintf(stderr, "Discarding pose: from %s, Extrapolation exception: %s\n", pose.frame_id_.c_str(), ex.what());
success = false;
}
- catch(libTF::TransformReference::ConnectivityException& ex)
+ catch(tf::ConnectivityException& ex)
{
- fprintf(stderr, "Discarding pose: from %s, Connectivity exception: %s\n", pose.frame.c_str(), ex.what());
+ fprintf(stderr, "Discarding pose: from %s, Connectivity exception: %s\n", pose.frame_id_.c_str(), ex.what());
success = false;
}
- catch(...)
+ catch(tf::TransformException)
{
- fprintf(stderr, "Discarding pose: from %s, Exception in pose computation\n", pose.frame.c_str());
+ fprintf(stderr, "Discarding pose: from %s, Exception in pose computation\n", pose.frame_id_.c_str());
success = false;
}
if (success)
{
- if (isfinite(pose.x))
- m_basePos[0] = pose.x;
- if (isfinite(pose.y))
- m_basePos[1] = pose.y;
- if (isfinite(pose.yaw))
- m_basePos[2] = pose.yaw;
+ if (isfinite(pose.getOrigin().x()))
+ m_basePos[0] = pose.getOrigin().x();
+ if (isfinite(pose.getOrigin().y()))
+ m_basePos[1] = pose.getOrigin().y();
+ double yaw, pitch, roll;
+ pose.getBasis().getEulerZYX(yaw, pitch, roll);
+ if (isfinite(yaw))
+ m_basePos[2] = yaw;
m_haveBasePos = true;
baseUpdate();
}
@@ -292,7 +293,7 @@
m_haveState = m_haveBasePos && m_haveMechanismState;
}
- rosTFClient m_tf;
+ tf::TransformListener m_tf;
ros::node *m_node;
std_msgs::RobotBase2DOdom m_localizedPose;
bool m_haveBasePos;
Modified: pkg/trunk/motion_planning/planning_node_util/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/manifest.xml 2008-12-09 04:36:16 UTC (rev 7795)
+++ pkg/trunk/motion_planning/planning_node_util/manifest.xml 2008-12-09 08:19:00 UTC (rev 7796)
@@ -11,7 +11,7 @@
<depend package="roscpp"/>
<depend package="std_msgs"/>
<depend package="robot_msgs"/>
- <depend package="rosTF"/>
+ <depend package="tf"/>
<depend package="wg_robot_description_parser"/>
<depend package="planning_models"/>
<depend package="collision_space"/>
Added: pkg/trunk/world_models/world_3d_map/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/world_models/world_3d_map/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/world_models/world_3d_map/ROS_BUILD_BLACKLIST 2008-12-09 08:19:00 UTC (rev 7796)
@@ -0,0 +1,3 @@
+blacklisting this for I am trying to convert to the new tf. This is on the list to be reworked with a better sense of persistance. If you need this back soon let me know.
+
+--Tully
\ No newline at end of file
Modified: pkg/trunk/world_models/world_3d_map/manifest.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/manifest.xml 2008-12-09 04:36:16 UTC (rev 7795)
+++ pkg/trunk/world_models/world_3d_map/manifest.xml 2008-12-09 08:19:00 UTC (rev 7796)
@@ -5,7 +5,7 @@
<review status="unreviewed" notes=""/>
<depend package="roscpp" />
<depend package="std_msgs" />
-<depend package="rosTF" />
+<depend package="tf" /> <!-- Code not changed from rosTF yet -->
<depend package="laser_scan" />
<depend package="random_utils" />
<depend package="wg_robot_description_parser" />
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-12-09 04:36:16 UTC (rev 7795)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-12-09 08:19:00 UTC (rev 7796)
@@ -140,15 +140,6 @@
m_acceptScans = false;
random_utils::init(&m_rng);
- /// @todo Find a way to make this work for pr2, with mechanism control,
- /// but not break for STAIR. Somebody needs to be periodically
- /// publishing the base->base_laser Tx. Or else we need a more standard
- /// way of retrieving such Txs;
- /* Set up the transform client */
- double laser_x_offset;
- param("laser_x_offset", laser_x_offset, 0.275);
- m_tf.setWithEulers("base_laser", "base_link", laser_x_offset, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
-
/* create a thread that handles the publishing of the data */
m_publishingThread = ros::thread::member_thread::startMemberFunctionThread<World3DMap>(this, &World3DMap::publishDataThread);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
