Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18540] pkg/trunk
|
From: <is...@us...> - 2009-07-09 02:25:33
|
Revision: 18540
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18540&view=rev
Author: isucan
Date: 2009-07-09 02:25:32 +0000 (Thu, 09 Jul 2009)
Log Message:
-----------
TF is now a constructor argument
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/util/self_watch/self_watch.cpp
Modified: pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
===================================================================
--- pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-07-09 02:25:32 UTC (rev 18540)
@@ -93,7 +93,8 @@
planning_environment::CollisionModels *collisionModels_;
planning_environment::PlanningMonitor *planningMonitor_;
-
+ tf::TransformListener tf_;
+
bool getControlJointNames(std::vector<std::string> &joint_names);
bool fillStartState(std::vector<motion_planning_msgs::KinematicJoint> &start);
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -188,8 +188,8 @@
return 0;
boost::thread th(&spinThread);
-
- planning_environment::KinematicModelStateMonitor km(&rm);
+ tf::TransformListener tf;
+ planning_environment::KinematicModelStateMonitor km(&rm, &tf);
km.waitForState();
std::cout << std::endl << std::endl << "Using joints:" << std::endl;
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -65,7 +65,7 @@
// monitor robot
collisionModels_ = new planning_environment::CollisionModels("robot_description");
- planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_);
+ planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, &tf_);
if (collisionModels_->getKinematicModel()->getGroupID(arm_) < 0)
{
Modified: pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -59,7 +59,7 @@
id_ = 0;
visualizationMarkerPublisher_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
collisionModels_ = new planning_environment::CollisionModels("robot_description");
- collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_);
+ collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_, &tf_);
if (collisionModels_->loadedModels())
{
collisionSpaceMonitor_->setOnAfterMapUpdateCallback(boost::bind(&DisplayPlannerCollisionModel::afterWorldUpdate, this, _1));
@@ -144,6 +144,7 @@
int id_;
ros::Publisher visualizationMarkerPublisher_;
ros::NodeHandle nh_;
+ tf::TransformListener tf_;
planning_environment::CollisionModels *collisionModels_;
planning_environment::CollisionSpaceMonitor *collisionSpaceMonitor_;
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -53,10 +53,10 @@
if (nodeHandle_.hasParam("~planning_frame_id"))
{
std::string frame; nodeHandle_.param("~planning_frame_id", frame, std::string(""));
- planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, frame);
+ planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, &tf_, frame);
}
else
- planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, "torso_lift_link");
+ planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, &tf_);
planKinematicPathService_ = nodeHandle_.advertiseService("plan_kinematic_path", &OMPLPlanning::planToGoal, this);
}
@@ -211,6 +211,7 @@
ros::NodeHandle nodeHandle_;
planning_environment::CollisionModels *collisionModels_;
planning_environment::PlanningMonitor *planningMonitor_;
+ tf::TransformListener tf_;
ros::ServiceServer planKinematicPathService_;
Modified: pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -57,7 +57,7 @@
{
stateValidityPublisher_ = nh_.advertise<std_msgs::Byte>("state_validity", 1);
collisionModels_ = new planning_environment::CollisionModels("robot_description");
- collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_);
+ collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_, &tf_);
if (collisionModels_->loadedModels())
{
collisionSpaceMonitor_->setOnAfterMapUpdateCallback(boost::bind(&StateValidityMonitor::afterWorldUpdate, this, _1));
@@ -103,6 +103,7 @@
int last_;
ros::NodeHandle nh_;
ros::Publisher stateValidityPublisher_;
+ tf::TransformListener tf_;
planning_environment::CollisionModels *collisionModels_;
planning_environment::CollisionSpaceMonitor *collisionSpaceMonitor_;
};
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -62,7 +62,7 @@
plan_id_ = -1;
robot_stopped_ = true;
rm_ = new planning_environment::RobotModels("robot_description");
- kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_);
+ kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_, &tf_);
// we use the topic for sending commands to the controller, so we need to advertise it
jointCommandPublisher_ = nh_.advertise<manipulation_msgs::JointTraj>("right_arm/trajectory_controller/trajectory_command", 1);
@@ -229,6 +229,7 @@
ros::Publisher displayPathPublisher_;
planning_environment::RobotModels *rm_;
planning_environment::KinematicModelStateMonitor *kmsm_;
+ tf::TransformListener tf_;
};
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -52,7 +52,7 @@
TestExecutionPath(void)
{
rm_ = new planning_environment::RobotModels("robot_description");
- kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_);
+ kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_, &tf_);
jointCommandPublisher_ = nh_.advertise<manipulation_msgs::JointTraj>("right_arm/trajectory_controller/trajectory_command", 1);
sleep_duration_ = 4;
use_topic_ = false;
@@ -183,6 +183,8 @@
ros::Publisher jointCommandPublisher_;
planning_environment::RobotModels *rm_;
planning_environment::KinematicModelStateMonitor *kmsm_;
+ tf::TransformListener tf_;
+
};
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-07-09 02:25:32 UTC (rev 18540)
@@ -56,13 +56,13 @@
{
public:
- CollisionSpaceMonitor(CollisionModels *cm, std::string frame_id) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), frame_id)
+ CollisionSpaceMonitor(CollisionModels *cm, tf::TransformListener *tf, std::string frame_id) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), tf, frame_id)
{
cm_ = cm;
setupCSM();
}
- CollisionSpaceMonitor(CollisionModels *cm) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm))
+ CollisionSpaceMonitor(CollisionModels *cm, tf::TransformListener *tf) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), tf)
{
cm_ = cm;
setupCSM();
@@ -90,12 +90,6 @@
return cm_;
}
- /** \brief Return the transform listener */
- tf::TransformListener *getTransformListener(void) const
- {
- return tf_;
- }
-
/** \brief Return the scaling employed when creating spheres
from boxes in a collision map. The radius of a sphere is
this scaling multiplied by the largest extent of the box */
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-07-09 02:25:32 UTC (rev 18540)
@@ -56,16 +56,18 @@
{
public:
- KinematicModelStateMonitor(RobotModels *rm)
+ KinematicModelStateMonitor(RobotModels *rm, tf::TransformListener *tf)
{
rm_ = rm;
+ tf_ = tf;
includePose_ = false;
setupRSM();
}
- KinematicModelStateMonitor(RobotModels *rm, const std::string &frame_id)
+ KinematicModelStateMonitor(RobotModels *rm, tf::TransformListener *tf, const std::string &frame_id)
{
rm_ = rm;
+ tf_ = tf;
includePose_ = true;
frame_id_ = frame_id;
setupRSM();
@@ -75,8 +77,6 @@
{
if (robotState_)
delete robotState_;
- if (tf_)
- delete tf_;
}
/** \brief Define a callback for when the state is changed */
@@ -103,6 +103,12 @@
return robotState_;
}
+ /** \brief Return the transform listener */
+ tf::TransformListener *getTransformListener(void) const
+ {
+ return tf_;
+ }
+
/** \brief Return the frame id of the state */
const std::string& getFrameId(void) const
{
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h 2009-07-09 02:25:32 UTC (rev 18540)
@@ -54,12 +54,12 @@
{
public:
- PlanningMonitor(CollisionModels *cm, std::string frame_id) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), frame_id)
+ PlanningMonitor(CollisionModels *cm, tf::TransformListener *tf, std::string frame_id) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), tf, frame_id)
{
loadParams();
}
- PlanningMonitor(CollisionModels *cm) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm))
+ PlanningMonitor(CollisionModels *cm, tf::TransformListener *tf) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), tf)
{
loadParams();
}
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -60,9 +60,6 @@
haveMap_ = false;
- if (!tf_)
- tf_ = new tf::TransformListener();
-
collisionSpace_ = cm_->getODECollisionModel().get();
// the factor by which a boxes maximum extent is multiplied to get the radius of the sphere to be placed in the collision space
Modified: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -43,7 +43,6 @@
kmodel_ = NULL;
robotState_ = NULL;
onStateUpdate_ = NULL;
- tf_ = NULL;
tfWait_ = ros::Duration(0.1);
havePose_ = haveMechanismState_ = false;
if (rm_->loadedModels())
@@ -68,10 +67,7 @@
ROS_DEBUG("Robot frame is '%s'", robot_frame_.c_str());
if (includePose_)
- {
- tf_ = new tf::TransformListener();
ROS_DEBUG("Maintaining robot pose in frame '%s'", frame_id_.c_str());
- }
else
frame_id_ = robot_frame_;
}
Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp 2009-07-09 02:18:30 UTC (rev 18539)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-07-09 02:25:32 UTC (rev 18540)
@@ -73,7 +73,7 @@
// create a state that can be used to monitor the
// changes in the joints of the kinematic model
- m_stateMonitor = boost::shared_ptr<planning_environment::KinematicModelStateMonitor>(new planning_environment::KinematicModelStateMonitor(m_envModels.get()));
+ m_stateMonitor = boost::shared_ptr<planning_environment::KinematicModelStateMonitor>(new planning_environment::KinematicModelStateMonitor(m_envModels.get(), &m_tf));
m_robotState = m_stateMonitor->getRobotState();
m_stateMonitor->setOnStateUpdateCallback(boost::bind(&SelfWatch::stateUpdate, this));
@@ -143,7 +143,8 @@
}
ros::NodeHandle m_nodeHandle;
-
+ tf::TransformListener m_tf;
+
// we don't want to detect a collision after it happened, but this
// is what collision checkers do, so we scale the robot up by a
// small factor; when a collision is found between the inflated
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
