Menu
▾
▴
personalrobots-commit
|
From: <is...@us...> - 2009-06-16 23:57:41
|
Revision: 17184
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17184&view=rev
Author: isucan
Date: 2009-06-16 23:57:35 +0000 (Tue, 16 Jun 2009)
Log Message:
-----------
more launch files
Added Paths:
-----------
pkg/trunk/demos/tabletop_manipulation/launch/larm.launch
pkg/trunk/demos/tabletop_manipulation/launch/ompl_planning.launch
pkg/trunk/demos/tabletop_manipulation/launch/rarm.launch
pkg/trunk/highlevel/move_arm/move_larm.launch
pkg/trunk/highlevel/move_arm/move_rarm.launch
pkg/trunk/util/pr2_ik/pr2_ik_larm_node.launch
pkg/trunk/util/pr2_ik/pr2_ik_rarm_node.launch
Removed Paths:
-------------
pkg/trunk/highlevel/move_arm/move_arm.launch
pkg/trunk/util/pr2_ik/pr2_ik_node.launch
Added: pkg/trunk/demos/tabletop_manipulation/launch/larm.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/larm.launch (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/launch/larm.launch 2009-06-16 23:57:35 UTC (rev 17184)
@@ -0,0 +1,5 @@
+<launch>
+ <include file="$(find pr2_ik)/pr2_ik_larm_node.launch"/>
+ <include file="$(find move_arm)/move_larm.launch"/>
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/l_arm_joint_trajectory_controller.xml" />
+</launch>
Added: pkg/trunk/demos/tabletop_manipulation/launch/ompl_planning.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/ompl_planning.launch (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/launch/ompl_planning.launch 2009-06-16 23:57:35 UTC (rev 17184)
@@ -0,0 +1,3 @@
+<launch>
+ <include file="$(find ompl_planning)/motion_planning.launch"/>
+</launch>
Added: pkg/trunk/demos/tabletop_manipulation/launch/rarm.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/rarm.launch (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/launch/rarm.launch 2009-06-16 23:57:35 UTC (rev 17184)
@@ -0,0 +1,5 @@
+<launch>
+ <include file="$(find pr2_ik)/pr2_ik_rarm_node.launch"/>
+ <include file="$(find move_arm)/move_rarm.launch"/>
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/r_arm_joint_trajectory_controller.xml" />
+</launch>
Deleted: pkg/trunk/highlevel/move_arm/move_arm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/move_arm.launch 2009-06-16 23:50:31 UTC (rev 17183)
+++ pkg/trunk/highlevel/move_arm/move_arm.launch 2009-06-16 23:57:35 UTC (rev 17184)
@@ -1,6 +0,0 @@
-<launch>
- <node pkg="move_arm" type="move_arm" name="move_arm" args="robot_description:=robotdesc/pr2" output="screen">
- <param name="ik_service_name" value="pr2_ik_node/ik_service" />
- <param name="ik_query_name" value="pr2_ik_node/ik_query" />
- </node>
-</launch>
Copied: pkg/trunk/highlevel/move_arm/move_larm.launch (from rev 17182, pkg/trunk/highlevel/move_arm/move_arm.launch)
===================================================================
--- pkg/trunk/highlevel/move_arm/move_larm.launch (rev 0)
+++ pkg/trunk/highlevel/move_arm/move_larm.launch 2009-06-16 23:57:35 UTC (rev 17184)
@@ -0,0 +1,7 @@
+<launch>
+ <node pkg="move_arm" type="move_arm" name="move_arm" args="robot_description:=robotdesc/pr2" output="screen">
+ <param name="ik_service_name" value="pr2_ik_node/ik_service" />
+ <param name="ik_query_name" value="pr2_ik_node/ik_query" />
+ <param name="arm" value="left_arm" />
+ </node>
+</launch>
Property changes on: pkg/trunk/highlevel/move_arm/move_larm.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/highlevel/move_arm/move_arm.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/highlevel/move_arm/move_rarm.launch (from rev 17182, pkg/trunk/highlevel/move_arm/move_arm.launch)
===================================================================
--- pkg/trunk/highlevel/move_arm/move_rarm.launch (rev 0)
+++ pkg/trunk/highlevel/move_arm/move_rarm.launch 2009-06-16 23:57:35 UTC (rev 17184)
@@ -0,0 +1,7 @@
+<launch>
+ <node pkg="move_arm" type="move_arm" name="move_arm" args="robot_description:=robotdesc/pr2" output="screen">
+ <param name="ik_service_name" value="pr2_ik_node/ik_service" />
+ <param name="ik_query_name" value="pr2_ik_node/ik_query" />
+ <param name="arm" value="right_arm" />
+ </node>
+</launch>
Property changes on: pkg/trunk/highlevel/move_arm/move_rarm.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/highlevel/move_arm/move_arm.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Added: pkg/trunk/util/pr2_ik/pr2_ik_larm_node.launch
===================================================================
--- pkg/trunk/util/pr2_ik/pr2_ik_larm_node.launch (rev 0)
+++ pkg/trunk/util/pr2_ik/pr2_ik_larm_node.launch 2009-06-16 23:57:35 UTC (rev 17184)
@@ -0,0 +1,6 @@
+<launch>
+ <node pkg="pr2_ik" type="pr2_ik_node" name="pr2_ik_node">
+ <param name="tip_name" value="l_wrist_roll_link" />
+ <param name="root_name" value="torso_lift_link" />
+ </node>
+</launch>
Deleted: pkg/trunk/util/pr2_ik/pr2_ik_node.launch
===================================================================
--- pkg/trunk/util/pr2_ik/pr2_ik_node.launch 2009-06-16 23:50:31 UTC (rev 17183)
+++ pkg/trunk/util/pr2_ik/pr2_ik_node.launch 2009-06-16 23:57:35 UTC (rev 17184)
@@ -1,7 +0,0 @@
-<launch>
- <node pkg="pr2_ik" type="pr2_ik_node" name="pr2_ik_node">
- <param name="tip_name" value="r_wrist_roll_link" />
- <param name="root_name" value="torso_lift_link" />
- <param name="dimension" type="int" value="7" />
- </node>
-</launch>
\ No newline at end of file
Copied: pkg/trunk/util/pr2_ik/pr2_ik_rarm_node.launch (from rev 17182, pkg/trunk/util/pr2_ik/pr2_ik_node.launch)
===================================================================
--- pkg/trunk/util/pr2_ik/pr2_ik_rarm_node.launch (rev 0)
+++ pkg/trunk/util/pr2_ik/pr2_ik_rarm_node.launch 2009-06-16 23:57:35 UTC (rev 17184)
@@ -0,0 +1,6 @@
+<launch>
+ <node pkg="pr2_ik" type="pr2_ik_node" name="pr2_ik_node">
+ <param name="tip_name" value="r_wrist_roll_link" />
+ <param name="root_name" value="torso_lift_link" />
+ </node>
+</launch>
Property changes on: pkg/trunk/util/pr2_ik/pr2_ik_rarm_node.launch
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/util/pr2_ik/pr2_ik_node.launch:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-06-17 05:44:44
|
Revision: 17206
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17206&view=rev
Author: isucan
Date: 2009-06-17 05:44:42 +0000 (Wed, 17 Jun 2009)
Log Message:
-----------
fixing move_arm
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/RKPGoalDefinitions.cpp
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-17 05:10:53 UTC (rev 17205)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-17 05:44:42 UTC (rev 17206)
@@ -74,7 +74,7 @@
ROS_ERROR("Arm '%s' is not known", arm_.c_str());
}
else
- ROS_INFO("Starting move_arm for '%s'", arm_.c_str());
+ ROS_INFO("Starting move_arm for '%s' (IK is %senabled)", arm_.c_str(), perform_ik_ ? "" : "not ");
if (valid_)
valid_ = collisionModels_->loadedModels() && getControlJointNames(arm_joint_names_);
@@ -200,7 +200,9 @@
{
if (res.approximate)
ROS_INFO("Approximate path was found. Distance to goal is: %f", res.distance);
+ ROS_INFO("Received path with %u states from motion planner", (unsigned int)res.path.states.size());
currentPath_ = res.path;
+ displayPathPublisher_.publish(currentPath_);
feedback = pr2_robot_actions::MoveArmState::MOVING;
update(feedback);
}
@@ -210,7 +212,7 @@
}
else
{
- ROS_ERROR("Service 'plan_kinematic_path' failed");
+ ROS_ERROR("Motion planning service failed");
result = robot_actions::ABORTED;
break;
}
@@ -328,15 +330,11 @@
}
eps.sleep();
}
-
- return result;
-
- /*
-
- return robot_actions::SUCCESS;
-
- */
+ if (result == robot_actions::SUCCESS)
+ ROS_INFO("Trajectory execution is complete");
+
+ return result;
}
void MoveArm::fillTrajectoryPath(const motion_planning_msgs::KinematicPath &path, robot_msgs::JointTraj &traj)
Modified: pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp 2009-06-17 05:10:53 UTC (rev 17205)
+++ pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp 2009-06-17 05:44:42 UTC (rev 17206)
@@ -81,12 +81,17 @@
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1;
+ goal.goal_constraints.pose_constraint[0].position_distance = 0.01;
+ goal.goal_constraints.pose_constraint[0].orientation_distance = 0.01;
+ goal.goal_constraints.pose_constraint[0].orientation_importance = 0.1;
+ goal.goal_constraints.pose_constraint[0].type = motion_planning_msgs::PoseConstraint::POSITION_XYZ +
+ motion_planning_msgs::PoseConstraint::ORIENTATION_RPY;
// switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
// switchlist.start_controllers.push_back("r_arm_joint_trajectory_controller");
// if(switch_controllers.execute(switchlist, empty, timeout_medium) != robot_actions::SUCCESS) return -1;
- ROS_INFO("Done switching controllers");
+ // ROS_INFO("Done switching controllers");
if(move_arm.execute(goal,feedback,timeout_long) != robot_actions::SUCCESS) return -1;
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h 2009-06-17 05:10:53 UTC (rev 17205)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h 2009-06-17 05:44:42 UTC (rev 17206)
@@ -38,8 +38,6 @@
#define KINEMATIC_PLANNING_KINEMATIC_RKP_MODEL_BASE_
#include <planning_environment/planning_monitor.h>
-#include <boost/thread/mutex.hpp>
-#include <boost/shared_ptr.hpp>
#include <string>
namespace kinematic_planning
@@ -59,7 +57,6 @@
{
}
- boost::mutex lock;
planning_environment::PlanningMonitor *planningMonitor;
collision_space::EnvironmentModel *collisionSpace;
planning_models::KinematicModel *kmodel;
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/RKPGoalDefinitions.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/RKPGoalDefinitions.cpp 2009-06-17 05:10:53 UTC (rev 17205)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/RKPGoalDefinitions.cpp 2009-06-17 05:44:42 UTC (rev 17206)
@@ -167,7 +167,6 @@
double kinematic_planning::GoalToPosition::evaluateGoalAux(const ompl::sb::State *state, std::vector<bool> *decision) const
{
- model_->kmodel->lock();
update(state);
if (decision)
@@ -181,7 +180,6 @@
(*decision)[i] = pce_[i]->decide(dPos, dAng);
distance += dPos + pce_[i]->getConstraintMessage().orientation_importance * dAng;
}
- model_->kmodel->unlock();
return distance;
}
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-17 05:10:53 UTC (rev 17205)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-17 05:44:42 UTC (rev 17206)
@@ -54,7 +54,7 @@
haveMap_ = false;
if (!tf_)
- tf_ = new tf::TransformListener(*ros::Node::instance());
+ tf_ = new tf::TransformListener();
collisionSpace_ = cm_->getODECollisionModel().get();
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-06-17 05:10:53 UTC (rev 17205)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-17 05:44:42 UTC (rev 17206)
@@ -68,7 +68,7 @@
if (includePose_)
{
- tf_ = new tf::TransformListener(*ros::Node::instance());
+ tf_ = new tf::TransformListener();
tf_->setExtrapolationLimit(ros::Duration(1.0));
ROS_DEBUG("Maintaining robot pose in frame '%s'", frame_id_.c_str());
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mar...@us...> - 2009-06-17 17:39:43
|
Revision: 17223
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17223&view=rev
Author: mariusmuja
Date: 2009-06-17 17:39:30 +0000 (Wed, 17 Jun 2009)
Log Message:
-----------
Temporarely switched the tests from outlet_detector and door_handle_detector to future tests, ticket #1609
Modified Paths:
--------------
pkg/trunk/mapping/door_handle_detector/CMakeLists.txt
pkg/trunk/vision/outlet_detection/CMakeLists.txt
Modified: pkg/trunk/mapping/door_handle_detector/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/door_handle_detector/CMakeLists.txt 2009-06-17 17:36:18 UTC (rev 17222)
+++ pkg/trunk/mapping/door_handle_detector/CMakeLists.txt 2009-06-17 17:39:30 UTC (rev 17223)
@@ -35,7 +35,7 @@
# Helper macro
macro(rostest_door_handle _test_name)
rospack_download_test_data(http://pr.willowgarage.com/data/door_handle_detector/${_test_name}.bag test/${_test_name}.bag)
- rospack_add_rostest(test/${_test_name}.launch)
+ rospack_add_rostest_future(test/${_test_name}.launch)
endmacro(rostest_door_handle)
# We use the point_cloud_cropper in the tests, to simulate a narrow scan of
Modified: pkg/trunk/vision/outlet_detection/CMakeLists.txt
===================================================================
--- pkg/trunk/vision/outlet_detection/CMakeLists.txt 2009-06-17 17:36:18 UTC (rev 17222)
+++ pkg/trunk/vision/outlet_detection/CMakeLists.txt 2009-06-17 17:39:30 UTC (rev 17223)
@@ -51,7 +51,7 @@
macro(rostest_outlet_coarse _test_name)
rospack_download_test_data(http://pr.willowgarage.com/data/outlet_detection/outlet_test_${_test_name}.bag test/outlet_test_${_test_name}.bag)
rospack_download_test_data(http://pr.willowgarage.com/data/outlet_detection/outlet_test_${_test_name}.pose test/outlet_test_${_test_name}.pose)
- rospack_add_rostest(test/outlet_test_${_test_name}.launch)
+ rospack_add_rostest_future(test/outlet_test_${_test_name}.launch)
endmacro(rostest_outlet_coarse)
rostest_outlet_coarse(1)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-06-17 18:45:52
|
Revision: 17234
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17234&view=rev
Author: isucan
Date: 2009-06-17 18:45:10 +0000 (Wed, 17 Jun 2009)
Log Message:
-----------
fixes for move_arm
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp
pkg/trunk/motion_planning/ompl_planning/motion_planning.launch
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-17 18:31:54 UTC (rev 17233)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-17 18:45:10 UTC (rev 17234)
@@ -30,7 +30,7 @@
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE. return robot_actions::PREEMPTED;
+* POSSIBILITY OF SUCH DAMAGE.
*
* Authors: Sachin Chitta, Ioan Sucan
@@ -91,7 +91,7 @@
delete planningMonitor_;
delete collisionModels_;
}
-
+
robot_actions::ResultStatus MoveArm::execute(const pr2_robot_actions::MoveArmGoal& goal, int32_t& feedback)
{
if (!valid_)
@@ -121,40 +121,41 @@
req.goal_constraints.pose_constraint[0].type == motion_planning_msgs::PoseConstraint::POSITION_XYZ +
motion_planning_msgs::PoseConstraint::ORIENTATION_RPY && // that is active on all 6 DOFs
req.goal_constraints.pose_constraint[0].position_distance < 0.1 && // and the desired position and
- req.goal_constraints.pose_constraint[0].orientation_distance < 0.1 && // orientation distances are small,
- ((arm_ == "left_arm" && req.goal_constraints.pose_constraint[0].link_name == arm_joint_names_.back())
- || (arm_ == "right_arm" && req.goal_constraints.pose_constraint[0].link_name == arm_joint_names_.back()))) // and acts on the last link of the arm
+ req.goal_constraints.pose_constraint[0].orientation_distance < 0.1) // orientation distances are small
{
- // we can do ik can turn the pose constraint into a joint one
- ROS_INFO("Converting pose constraint to joint constraint using IK...");
-
- std::vector<double> solution;
- if (computeIK(req.goal_constraints.pose_constraint[0].pose, solution))
+ planning_models::KinematicModel::Link *link = planningMonitor_->getKinematicModel()->getLink(req.goal_constraints.pose_constraint[0].link_name);
+ if (link && link->before && link->before->name == arm_joint_names_.back())
{
- req.goal_constraints.joint_constraint.resize(1);
- req.goal_constraints.joint_constraint[0].header.frame_id = req.goal_constraints.pose_constraint[0].pose.header.frame_id;
- req.goal_constraints.joint_constraint[0].header.stamp = planningMonitor_->lastStateUpdate();
- unsigned int n = 0;
- for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
+ // we can do ik can turn the pose constraint into a joint one
+ ROS_INFO("Converting pose constraint to joint constraint using IK...");
+
+ std::vector<double> solution;
+ if (computeIK(req.goal_constraints.pose_constraint[0].pose, solution))
{
- motion_planning_msgs::JointConstraint jc;
- jc.joint_name = arm_joint_names_[i];
- unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
- for (unsigned int j = 0 ; j < u ; ++j)
+ unsigned int n = 0;
+ for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
{
- jc.value.push_back(solution[n + j]);
- jc.toleranceAbove.push_back(0.0);
- jc.toleranceBelow.push_back(0.0);
+ motion_planning_msgs::JointConstraint jc;
+ jc.joint_name = arm_joint_names_[i];
+ jc.header.frame_id = req.goal_constraints.pose_constraint[0].pose.header.frame_id;
+ jc.header.stamp = planningMonitor_->lastStateUpdate();
+ unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
+ for (unsigned int j = 0 ; j < u ; ++j)
+ {
+ jc.value.push_back(solution[n + j]);
+ jc.toleranceAbove.push_back(0.0);
+ jc.toleranceBelow.push_back(0.0);
+ }
+ n += u;
+ req.goal_constraints.joint_constraint.push_back(jc);
}
- n += u;
- req.goal_constraints.joint_constraint.push_back(jc);
+ req.goal_constraints.pose_constraint.clear();
}
- req.goal_constraints.pose_constraint.clear();
+ else
+ ROS_WARN("Unable to compute IK");
}
- else
- ROS_WARN("Unable to compute IK");
}
-
+
req.times = 1;
req.allowed_time = 0.5;
@@ -226,12 +227,17 @@
// stop the robot if we need to
if (feedback == pr2_robot_actions::MoveArmState::MOVING)
{
- if (result == robot_actions::PREEMPTED || !planningMonitor_->isPathValid(currentPath_))
+ bool safe = planningMonitor_->isEnvironmentSafe();
+ bool valid = planningMonitor_->isPathValid(currentPath_);
+ if (result == robot_actions::PREEMPTED || !safe || !valid)
{
if (result == robot_actions::PREEMPTED)
ROS_INFO("Preempt requested. Stopping arm.");
else
- ROS_INFO("Current path is no longer valid. Stopping & replanning...");
+ if (safe)
+ ROS_WARN("Environment is no longer safe. Cannot decide if path is valid. Stopping & replanning...");
+ else
+ ROS_INFO("Current path is no longer valid. Stopping & replanning...");
if (trajectoryId != -1)
{
@@ -354,8 +360,19 @@
pr2_mechanism_controllers::TrajectoryQuery::Response res_query;
req_query.trajectoryid = -1;
- if (!client_query.call(req_query, res_query))
+ bool result = client_query.call(req_query, res_query);
+
+ if (!result)
{
+ ROS_WARN("Unable to retrieve controller joint names from control query service. Waiting a bit and retrying...");
+ ros::Duration(1.0).sleep();
+ result = client_query.call(req_query, res_query);
+ if (result)
+ ROS_WARN("Retrieved controller joints on second attempt");
+ }
+
+ if (!result)
+ {
ROS_ERROR("Unable to retrieve controller joint names from control query service");
return false;
}
Modified: pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp 2009-06-17 18:31:54 UTC (rev 17233)
+++ pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp 2009-06-17 18:45:10 UTC (rev 17234)
@@ -73,6 +73,7 @@
goal.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
goal.goal_constraints.pose_constraint[0].pose.header.frame_id = "torso_lift_link";
+ goal.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
goal.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.75;
goal.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.188;
goal.goal_constraints.pose_constraint[0].pose.pose.position.z = 0;
Modified: pkg/trunk/motion_planning/ompl_planning/motion_planning.launch
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/motion_planning.launch 2009-06-17 18:31:54 UTC (rev 17233)
+++ pkg/trunk/motion_planning/ompl_planning/motion_planning.launch 2009-06-17 18:45:10 UTC (rev 17234)
@@ -3,7 +3,5 @@
<node pkg="ompl_planning" type="motion_planner" args="robot_description:=robotdesc/pr2 collision_map:=collision_map_buffer" respawn="true" output="screen">
<param name="~refresh_interval_collision_map" type="double" value="0.0" />
<param name="~refresh_interval_kinematic_state" type="double" value="0.0" />
- <param name="~refresh_interval_base_pose" type="double" value="0.0" />
- <param name="~kinematic_planning_status_interval" type="double" value="0.02" />
</node>
</launch>
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-17 18:31:54 UTC (rev 17233)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-17 18:45:10 UTC (rev 17234)
@@ -56,10 +56,6 @@
planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_);
planKinematicPathService_ = nodeHandle_.advertiseService("plan_kinematic_path", &KinematicPlanning::planToGoal, this);
-
- // determine intervals; a value of 0 means forever
- nodeHandle_.param("~refresh_interval_collision_map", intervalCollisionMap_, 3.0);
- nodeHandle_.param("~refresh_interval_kinematic_state", intervalState_, 0.5);
}
/** Free the memory */
@@ -99,25 +95,6 @@
ROS_ERROR("No robot model loaded. OMPL planning node cannot start.");
}
- bool isSafeToPlan(bool report)
- {
- if (!planningMonitor_->isMapUpdated(intervalCollisionMap_))
- {
- if (report)
- ROS_WARN("Planning is not safe: map is not up to date");
- return false;
- }
-
- if (!planningMonitor_->isStateUpdated(intervalState_))
- {
- if (report)
- ROS_WARN("Planning is not safe: robot state is not up to date");
- return false;
- }
-
- return true;
- }
-
bool planToGoal(motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
{
ROS_INFO("Received request for planning");
@@ -130,7 +107,7 @@
res.path.model_id = req.params.model_id;
res.path.header.frame_id = planningMonitor_->getFrameId();
res.path.header.stamp = planningMonitor_->lastMapUpdate();
- res.unsafe = isSafeToPlan(true) ? 0 : 1;
+ res.unsafe = planningMonitor_->isEnvironmentSafe() ? 0 : 1;
res.distance = -1.0;
res.approximate = 0;
Modified: pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp 2009-06-17 18:31:54 UTC (rev 17233)
+++ pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp 2009-06-17 18:45:10 UTC (rev 17234)
@@ -39,17 +39,19 @@
bool planning_environment::PlanningMonitor::isEnvironmentSafe(void) const
{
- if (isMapUpdated(intervalCollisionMap_))
- {
- ROS_WARN("Planning is not safe: map is not up to date");
- return false;
- }
+ if (intervalCollisionMap_ > 0.0)
+ if (isMapUpdated(intervalCollisionMap_))
+ {
+ ROS_WARN("Planning is not safe: map is not up to date");
+ return false;
+ }
- if (isStateUpdated(intervalState_))
- {
- ROS_WARN("Planning is not safe: robot state is not up to date");
- return false;
- }
+ if (intervalState_ > 0.0)
+ if (isStateUpdated(intervalState_))
+ {
+ ROS_WARN("Planning is not safe: robot state is not up to date");
+ return false;
+ }
return true;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-06-17 18:48:52
|
Revision: 17235
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17235&view=rev
Author: tfoote
Date: 2009-06-17 18:47:51 +0000 (Wed, 17 Jun 2009)
Log Message:
-----------
moving nav_srvs into common stack from highlevel stack getting ready for nav-0.1 release
Added Paths:
-----------
pkg/trunk/common/nav_srvs/
Removed Paths:
-------------
pkg/trunk/highlevel/nav_srvs/
Property changes on: pkg/trunk/common/nav_srvs
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/highlevel/nav_srvs:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-06-17 20:40:05
|
Revision: 17237
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17237&view=rev
Author: stuglaser
Date: 2009-06-17 20:39:57 +0000 (Wed, 17 Jun 2009)
Log Message:
-----------
Removed robot_msgs/CartesianState
Modified Paths:
--------------
pkg/trunk/demos/plug_in/src/servo.cpp
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_unplug.h
Removed Paths:
-------------
pkg/trunk/common/robot_msgs/msg/CartesianState.msg
Deleted: pkg/trunk/common/robot_msgs/msg/CartesianState.msg
===================================================================
--- pkg/trunk/common/robot_msgs/msg/CartesianState.msg 2009-06-17 20:39:51 UTC (rev 17236)
+++ pkg/trunk/common/robot_msgs/msg/CartesianState.msg 2009-06-17 20:39:57 UTC (rev 17237)
@@ -1,7 +0,0 @@
-Header header
-robot_msgs/PoseStamped last_pose_meas
-robot_msgs/Twist last_pose_desi
-robot_msgs/Twist last_twist_meas
-#last_pose_desi
-robot_msgs/Twist last_twist_desi
-robot_msgs/Wrench last_wrench_desi
Modified: pkg/trunk/demos/plug_in/src/servo.cpp
===================================================================
--- pkg/trunk/demos/plug_in/src/servo.cpp 2009-06-17 20:39:51 UTC (rev 17236)
+++ pkg/trunk/demos/plug_in/src/servo.cpp 2009-06-17 20:39:57 UTC (rev 17237)
@@ -33,7 +33,6 @@
#include "tf/message_notifier.h"
#include "robot_msgs/PoseStamped.h"
-#include "robot_msgs/CartesianState.h"
#include "manipulation_msgs/TaskFrameFormalism.h"
const double MIN_STANDOFF = 0.035;
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-06-17 20:39:51 UTC (rev 17236)
+++ pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h 2009-06-17 20:39:57 UTC (rev 17237)
@@ -45,7 +45,6 @@
#include <std_msgs/Empty.h>
#include <std_msgs/Int32.h>
#include "robot_msgs/PoseStamped.h"
-#include "robot_msgs/CartesianState.h"
#include "manipulation_msgs/TaskFrameFormalism.h"
#include "plugs_core/PlugInState.h"
Modified: pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_unplug.h
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_unplug.h 2009-06-17 20:39:51 UTC (rev 17236)
+++ pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_unplug.h 2009-06-17 20:39:57 UTC (rev 17237)
@@ -44,7 +44,6 @@
// Msgs
#include <std_msgs/Empty.h>
#include "robot_msgs/PoseStamped.h"
-#include "robot_msgs/CartesianState.h"
#include "manipulation_msgs/TaskFrameFormalism.h"
#include "robot_mechanism_controllers/CartesianHybridState.h"
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-06-17 23:25:45
|
Revision: 17257
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17257&view=rev
Author: stuglaser
Date: 2009-06-17 23:24:43 +0000 (Wed, 17 Jun 2009)
Log Message:
-----------
Migrated code to use the mechanism messages in mechanism_msgs instead of robot_msgs
Modified Paths:
--------------
pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/settler.py
pkg/trunk/calibration/kinematic_calibration/manifest.xml
pkg/trunk/calibration/kinematic_calibration/msg/CalSnapshot.msg
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/scripts/auto_arm_commander.py
pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py
pkg/trunk/calibration/kinematic_calibration/scripts/new_cmd_node.py
pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/settler.py
pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp
pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
pkg/trunk/calibration/laser_camera_calibration/gatherdata.py
pkg/trunk/calibration/sandbox/laser_head_updater/manifest.xml
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
pkg/trunk/controllers/robot_mechanism_controllers/scripts/wxeffect.py
pkg/trunk/demos/plug_in/fake_camera_frame.py
pkg/trunk/demos/plug_in/fake_vision.py
pkg/trunk/demos/plug_in/plug_in.py
pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/scripts/assembler_node.py
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/arm_state_adapter.cpp
pkg/trunk/mechanism/mechanism_msgs/msg/JointStates.msg
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/manifest.xml
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
pkg/trunk/pr2/cb_characterization/scripts/cmd_node.py
pkg/trunk/pr2/life_test/caster_life_test/periodic_drive.py
pkg/trunk/pr2/life_test/scripts/periodic_drive.py
pkg/trunk/pr2/motorconf_gui/src/motorconf_gui/ui.py
pkg/trunk/pr2/pr2_msgs/manifest.xml
pkg/trunk/pr2/pr2_msgs/msg/MoveEndEffectorGoal.msg
pkg/trunk/pr2/pr2_msgs/msg/MoveEndEffectorState.msg
pkg/trunk/pr2/qualification/scripts/wait_for_stationary.py
pkg/trunk/sandbox/robot_state_publisher/manifest.xml
pkg/trunk/sandbox/robot_state_publisher/src/main.cpp
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/vision/outlet_detection/scripts/fake_gripper_frame.py
pkg/trunk/visualization/rviz/manifest.xml
pkg/trunk/visualization/rviz/src/rviz/displays/robot_model_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/robot_model_display.h
pkg/trunk/visualization/rviz/src/rviz/robot/robot.cpp
pkg/trunk/visualization/rviz/src/rviz/robot/robot.h
pkg/trunk/visualization/rviz/src/rviz/robot/robot_link.cpp
pkg/trunk/visualization/rviz/src/rviz/robot/robot_link.h
Modified: pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/settler.py
===================================================================
--- pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/settler.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/settler.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -39,7 +39,8 @@
import sys
import threading
-from pr2_mechanism_controllers.srv import *
+from pr2_mechanism_controllers.srv import *\
+from mechanism_msgs.msg import MechanismState
from robot_msgs.msg import *
from roslib import rostime
from auto_arm_commander.msg_cache import MsgCache
@@ -65,7 +66,7 @@
def get_stats_interval(self, joints, start, end) :
seg = self._cache.get_segment_interval(start, end)
return self._process_stats(joints, seg)
-
+
def get_stats_latest(self, joints, N) :
seg = self._cache.get_segment_latest(N)
return self._process_stats(joints, seg)
Modified: pkg/trunk/calibration/kinematic_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/manifest.xml 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/manifest.xml 2009-06-17 23:24:43 UTC (rev 17257)
@@ -6,6 +6,7 @@
<depend package="kdl" />
<depend package="phase_space" />
<depend package="robot_msgs" />
+ <depend package="mechanism_msgs" />
<depend package="mocap_msgs" />
<depend package="std_msgs" />
<depend package="topic_synchronizer" />
Modified: pkg/trunk/calibration/kinematic_calibration/msg/CalSnapshot.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/CalSnapshot.msg 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/msg/CalSnapshot.msg 2009-06-17 23:24:43 UTC (rev 17257)
@@ -1,3 +1,3 @@
Header header
-robot_msgs/MechanismState mech_state
+mechanism_msgs/MechanismState mech_state
SensorSample[] samples
\ No newline at end of file
Modified: pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg 2009-06-17 23:24:43 UTC (rev 17257)
@@ -8,7 +8,7 @@
image_msgs/RawStereo raw_stereo
# Mechanism State - Stores the robots joint angles
-robot_msgs/MechanismState mechanism_state
+mechanism_msgs/MechanismState mechanism_state
# PointCloud generated from the tilt laser
robot_msgs/PointCloud laser_cloud
Modified: pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg 2009-06-17 23:24:43 UTC (rev 17257)
@@ -8,7 +8,7 @@
image_msgs/RawStereo raw_stereo
# Mechanism State - Stores the robots joint angles
-robot_msgs/MechanismState mechanism_state
+mechanism_msgs/MechanismState mechanism_state
# PointCloud generated from the tilt laser
robot_msgs/PointCloud laser_cloud
Modified: pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-06-17 23:24:43 UTC (rev 17257)
@@ -13,7 +13,7 @@
image_msgs/CamInfo right_info
# Mechanism State - Stores the robots joint angles
-robot_msgs/MechanismState mechanism_state
+mechanism_msgs/MechanismState mechanism_state
# PointCloud generated from the tilt laser
robot_msgs/PointCloud laser_cloud
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -9,6 +9,7 @@
from pr2_mechanism_controllers.srv import *
from pr2_mechanism_controllers.msg import *
from robot_msgs.msg import *
+from mechanism_msgs.msg import MechanismState
from kinematic_calibration.msg import *
arm_mapping = [ ]
@@ -25,7 +26,7 @@
capture_publisher = [ ] ;
def mech_state_callback(data, interval_publisher):
-
+
#print "Callback Called"
global arm_mapping
global headers
@@ -41,7 +42,7 @@
ms_joint_names = [x.name for x in data.joint_states]
ms_joint_mapping = [ms_joint_names.index(x) for x in traj_joint_names]
- joint_state_hist.append([data.header.stamp, [data.joint_states[x].position for x in ms_joint_mapping ]] )
+ joint_state_hist.append([data.header.stamp, [data.joint_states[x].position for x in ms_joint_mapping ]] )
while (len(joint_state_hist) > N) :
joint_state_hist.pop(0)
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -9,6 +9,7 @@
from pr2_mechanism_controllers.srv import *
from pr2_mechanism_controllers.msg import *
from robot_msgs.msg import *
+from mechanism_msgs.msg import MechanismState
from kinematic_calibration.msg import *
traj_actuator_names = ['r_shoulder_pan_motor',
@@ -32,7 +33,7 @@
print_count = 0
def mech_state_callback(data, interval_publisher):
-
+
#print "Callback Called"
global arm_mapping
global headers
@@ -51,7 +52,7 @@
ms_actuator_mapping = [ms_actuator_names.index(x) for x in traj_actuator_names]
ms_joint_mapping = [ms_joint_names.index(x) for x in traj_joint_names]
- joint_state_hist.append([data.header.stamp, [data.joint_states[x].position for x in ms_joint_mapping ]] )
+ joint_state_hist.append([data.header.stamp, [data.joint_states[x].position for x in ms_joint_mapping ]] )
while (len(joint_state_hist) > N) :
joint_state_hist.pop(0)
@@ -76,8 +77,8 @@
joint_state_hist[0][0], joint_state_hist[-1][0]))
ready_to_capture = False
done_capturing = True
-
+
if __name__ == '__main__':
print "Running python code"
rospy.init_node('auto_arm_commander', sys.argv, anonymous=False)
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/new_cmd_node.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/new_cmd_node.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/new_cmd_node.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -9,6 +9,7 @@
from pr2_mechanism_controllers.srv import *
from pr2_mechanism_controllers.msg import *
from robot_msgs.msg import *
+from mechanism_msgs.msg import MechanismState
from kinematic_calibration.msg import *
from kinematic_calibration.arm_commander import ArmCommander
@@ -69,7 +70,7 @@
for x in arm_stats.ranges :
arm_settled = arm_settled and (x < .0001)
time.sleep(.1)
- print " Arm is settled!"
+ print " Arm is settled!"
# Grab a stereotypical MechanismState that's close to the middle of the interval
cur_mech_state = arm_stats.seg[len(arm_stats.seg)/2]
@@ -94,7 +95,7 @@
print " [" + ', '.join( ['%.3f'%x for x in arm_stats.ranges] ) + "]"
for s in msg.samples :
print " [%3.2f, %3.2f]" % (s.m[0], s.m[1])
-
+
else :
print " Found no leds...sleeping...",
sys.stdout.flush()
Modified: pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp 2009-06-17 23:24:43 UTC (rev 17257)
@@ -36,7 +36,7 @@
#include "ros/node.h"
#include "boost/thread/mutex.hpp"
-#include "robot_msgs/MechanismState.h"
+#include "mechanism_msgs/MechanismState.h"
#include "mocap_msgs/MocapSnapshot.h"
#include "robot_kinematics/robot_kinematics.h"
@@ -271,7 +271,7 @@
{
// Grab mechanism state and put it in a local copy
mech_lock_.lock() ;
- robot_msgs::MechanismState mech_state = safe_mech_state_ ;
+ mechanism_msgs::MechanismState mech_state = safe_mech_state_ ;
mech_lock_.unlock() ;
angles.resize(names.size()) ;
@@ -305,12 +305,12 @@
private:
mocap_msgs::MocapSnapshot snapshot_ ;
- robot_msgs::MechanismState mech_state_ ;
+ mechanism_msgs::MechanismState mech_state_ ;
mocap_msgs::MocapSnapshot safe_snapshot_ ;
int marker_id_ ;
- robot_msgs::MechanismState safe_mech_state_ ;
+ mechanism_msgs::MechanismState safe_mech_state_ ;
boost::mutex mech_lock_ ;
boost::mutex snapshot_lock_ ;
@@ -327,7 +327,7 @@
ArmPhaseSpaceGrabber grabber ;
grabber.spin() ;
-
+
return 0 ;
}
Modified: pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/settler.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/settler.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/settler.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -40,6 +40,7 @@
import threading
from pr2_mechanism_controllers.srv import *
+from mechanism_msgs.msg import MechanismState
from robot_msgs.msg import *
from roslib import rostime
from kinematic_calibration.msg_cache import MsgCache
@@ -65,7 +66,7 @@
def get_stats_interval(self, joints, start, end) :
seg = self._cache.get_segment_interval(start, end)
return self._process_stats(joints, seg)
-
+
def get_stats_latest(self, joints, N) :
seg = self._cache.get_segment_latest(N)
return self._process_stats(joints, seg)
Modified: pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp 2009-06-17 23:24:43 UTC (rev 17257)
@@ -36,7 +36,7 @@
#include "ros/node.h"
#include "boost/thread/mutex.hpp"
-#include "robot_msgs/MechanismState.h"
+#include "mechanism_msgs/MechanismState.h"
#include "topic_synchronizer/topic_synchronizer.h"
@@ -82,7 +82,7 @@
// Mechanism State
boost::mutex mech_state_lock_ ;
- ADD_MSG(robot_msgs::MechanismState, mech_state_) ;
+ ADD_MSG(mechanism_msgs::MechanismState, mech_state_) ;
// Stereo Messages
boost::mutex stereo_lock_ ;
Modified: pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-06-17 23:24:43 UTC (rev 17257)
@@ -36,7 +36,7 @@
#include "ros/node.h"
#include "boost/thread/mutex.hpp"
-#include "robot_msgs/MechanismState.h"
+#include "mechanism_msgs/MechanismState.h"
#include "mocap_msgs/MocapSnapshot.h"
#include "topic_synchronizer/topic_synchronizer.h"
@@ -79,8 +79,8 @@
TopicSynchronizer<SensorKinematicsGrabber> sync_ ;
// Mechanism State Messages
- robot_msgs::MechanismState mech_state_ ;
- robot_msgs::MechanismState safe_mech_state_ ;
+ mechanism_msgs::MechanismState mech_state_ ;
+ mechanism_msgs::MechanismState safe_mech_state_ ;
boost::mutex mech_state_lock_ ;
// Empty message used for callbacks
Modified: pkg/trunk/calibration/laser_camera_calibration/gatherdata.py
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/gatherdata.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/laser_camera_calibration/gatherdata.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -38,7 +38,7 @@
PKG = 'laser_camera_calibration' # this package name
NAME = 'lasercamera_gatherer'
-import roslib; roslib.load_manifest(PKG)
+import roslib; roslib.load_manifest(PKG)
import sys
import thread
@@ -47,7 +47,7 @@
import rospy
from roslib import rostime
from laser_scan.msg import LaserScan
-from robot_msgs.msg import MechanismState
+from mechanism_msgs.msg import MechanismState
from checkerboard_detector.msg import ObjectDetection
import copy
@@ -70,7 +70,7 @@
rospy.Subscriber("tilt_scan", LaserScan, self.callback_laser, 1)
rospy.Subscriber("mechanism_state", MechanismState, self.callback_robot, 1)
rospy.init_node(NAME, anonymous=True)
-
+
def pose_dist(self, pose1, pose2):
t1 = array([pose1.position.x, pose1.position.y, pose1.position.z])
q1 = array([pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w])
@@ -129,7 +129,7 @@
if len(self.laserqueue) < 2 or len(self.robotqueue) < 2:
return
-
+
stamp = min(self.objdetqueue[-1].header.stamp, self.laserqueue[-1].header.stamp, self.robotqueue[-1].header.stamp)
# interpolate scanlines, find two scanlines that are between stamp
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/manifest.xml
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/manifest.xml 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/manifest.xml 2009-06-17 23:24:43 UTC (rev 17257)
@@ -6,6 +6,7 @@
<review status="unreviewed" notes=""/>
<depend package="kdl" />
<depend package="robot_msgs" />
+ <depend package="mechanism_msgs" />
<depend package="std_msgs" />
<depend package="tf" />
<depend package="laser_scan" />
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp 2009-06-17 23:24:43 UTC (rev 17257)
@@ -38,7 +38,7 @@
#include "ros/ros.h"
#include "tf/tfMessage.h"
#include "tf/transform_datatypes.h"
-#include "robot_msgs/MechanismState.h"
+#include "mechanism_msgs/MechanismState.h"
#include "laser_scan/LaserScan.h"
#include "image_msgs/RawStereo.h"
@@ -128,7 +128,7 @@
after_tilt_T_ = buildTransform(after_tilt) ;
}
- double getJointPos(const robot_msgs::MechanismStateConstPtr& mech_state,
+ double getJointPos(const mechanism_msgs::MechanismStateConstPtr& mech_state,
const std::string& name)
{
for(unsigned int i=0; i<mech_state->joint_states.size(); i++)
@@ -142,7 +142,7 @@
}
- void updateTf(const robot_msgs::MechanismStateConstPtr& mech_state)
+ void updateTf(const mechanism_msgs::MechanismStateConstPtr& mech_state)
{
//printf("In callback\n") ;
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp 2009-06-17 23:24:43 UTC (rev 17257)
@@ -38,7 +38,7 @@
#include "ros/ros.h"
#include "tf/tfMessage.h"
#include "tf/transform_datatypes.h"
-#include "robot_msgs/MechanismState.h"
+#include "mechanism_msgs/MechanismState.h"
#include "laser_scan/LaserScan.h"
#include "image_msgs/RawStereo.h"
@@ -132,7 +132,7 @@
after_tilt_T_ = buildTransform(after_tilt) ;
}
- double getJointPos(const robot_msgs::MechanismStateConstPtr& mech_state,
+ double getJointPos(const mechanism_msgs::MechanismStateConstPtr& mech_state,
const std::string& name)
{
for(unsigned int i=0; i<mech_state->joint_states.size(); i++)
@@ -146,7 +146,7 @@
}
- void updateTf(const robot_msgs::MechanismStateConstPtr& mech_state)
+ void updateTf(const mechanism_msgs::MechanismStateConstPtr& mech_state)
{
//printf("In callback\n") ;
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp 2009-06-17 23:24:43 UTC (rev 17257)
@@ -38,7 +38,7 @@
#include "ros/ros.h"
#include "tf/tfMessage.h"
#include "tf/transform_datatypes.h"
-#include "robot_msgs/MechanismState.h"
+#include "mechanism_msgs/MechanismState.h"
#include "laser_scan/LaserScan.h"
#include "image_msgs/RawStereo.h"
@@ -131,7 +131,7 @@
after_tilt_T_ = buildTransform(after_tilt) ;
}
- double getJointPos(const robot_msgs::MechanismStateConstPtr& mech_state,
+ double getJointPos(const mechanism_msgs::MechanismStateConstPtr& mech_state,
const std::string& name)
{
for(unsigned int i=0; i<mech_state->joint_states.size(); i++)
@@ -145,7 +145,7 @@
}
- void updateTf(const robot_msgs::MechanismStateConstPtr& mech_state)
+ void updateTf(const mechanism_msgs::MechanismStateConstPtr& mech_state)
{
//printf("In callback\n") ;
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-06-17 23:24:43 UTC (rev 17257)
@@ -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:
-*
+*
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
@@ -16,7 +16,7 @@
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
-*
+*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
@@ -45,7 +45,7 @@
#include "deprecated_msgs/RobotBase2DOdom.h"
#include "robot_msgs/PoseDot.h"
#include "robot_msgs/PoseWithRatesStamped.h"
-#include "robot_msgs/MechanismState.h"
+#include "mechanism_msgs/MechanismState.h"
namespace calibration
{
@@ -79,12 +79,12 @@
void AngleOverflowCorrect(double& a, double ref);
// messages to receive
- deprecated_msgs::RobotBase2DOdom _odom;
- robot_msgs::PoseWithRatesStamped _imu;
- robot_msgs::MechanismState _mech;
+ deprecated_msgs::RobotBase2DOdom _odom;
+ robot_msgs::PoseWithRatesStamped _imu;
+ mechanism_msgs::MechanismState _mech;
// estimated robot pose message to send
- robot_msgs::PoseDot _vel;
+ robot_msgs::PoseDot _vel;
// service messages
pr2_mechanism_controllers::WheelRadiusMultiplier::Request _srv_snd, _srv_rsp;
@@ -101,7 +101,7 @@
std::vector<double> _mech_begin, _mech_end;
// mutex
- boost::mutex _odom_mutex, _imu_mutex, _mech_mutex;
+ boost::mutex _odom_mutex, _imu_mutex, _mech_mutex;
}; // class
Modified: pkg/trunk/controllers/robot_mechanism_controllers/scripts/wxeffect.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/wxeffect.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/wxeffect.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -5,6 +5,7 @@
import rospy
from robot_msgs.msg import *
+from mechanism_msgs.msg import MechanismState
import wx
from wx import xrc
Modified: pkg/trunk/demos/plug_in/fake_camera_frame.py
===================================================================
--- pkg/trunk/demos/plug_in/fake_camera_frame.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/demos/plug_in/fake_camera_frame.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -1,10 +1,10 @@
#! /usr/bin/env python
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
-#
+#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
-#
+#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
@@ -13,7 +13,7 @@
# * Neither the name of the Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
-#
+#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@@ -61,11 +61,6 @@
def callback(self, msg):
self.msg = msg
-mechanism_state = Tracker('/mechanism_state', MechanismState)
-def last_time():
- global mechanism_state
- return mechanism_state.msg.header.stamp
-
t = TransformStamped()
t.header.frame_id = 'high_def_frame'
t.header.seq = 0
@@ -77,7 +72,7 @@
pub_tf = rospy.Publisher('/tf_message', tfMessage)
while not rospy.is_shutdown():
- t.header.stamp = last_time()
+ t.header.stamp = rospy.get_rostime()
msg = tfMessage([t])
pub_tf.publish(msg)
time.sleep(0.1)
Modified: pkg/trunk/demos/plug_in/fake_vision.py
===================================================================
--- pkg/trunk/demos/plug_in/fake_vision.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/demos/plug_in/fake_vision.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -57,16 +57,8 @@
def callback(self, msg):
self.msg = msg
-mechanism_state = Tracker('/mechanism_state', MechanismState)
-def last_time():
- return rospy.get_rostime()
- global mechanism_state
- if mechanism_state.msg:
- return mechanism_state.msg.header.stamp
- return 0
-
pub_outlet = rospy.Publisher('/outlet_detector/pose', PoseStamped)
pub_plug = rospy.Publisher('/plug_detector/pose', PoseStamped)
rospy.init_node('fake_vision', anonymous=True)
@@ -74,13 +66,13 @@
def send():
op = PoseStamped()
- op.header.stamp = last_time()
+ op.header.stamp = rospy.get_rostime()
op.header.frame_id = 'torso_lift_link'
op.pose.position = xyz(0.7, -0.4, -0.4)
op.pose.orientation = rpy(0, 0, 0)
pp = PoseStamped()
- pp.header.stamp = last_time()
+ pp.header.stamp = rospy.get_rostime()
pp.header.frame_id = 'r_gripper_tool_frame'
pp.pose.position = xyz(0.0, 0.0, 0.0)
pp.pose.orientation = rpy(0,-pi/6,0)
Modified: pkg/trunk/demos/plug_in/plug_in.py
===================================================================
--- pkg/trunk/demos/plug_in/plug_in.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/demos/plug_in/plug_in.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -46,15 +46,7 @@
def callback(self, msg):
self.msg = msg
-mechanism_state = Tracker('/mechanism_state', MechanismState)
-def last_time():
- global mechanism_state
- if mechanism_state.msg:
- return mechanism_state.msg.header.stamp
- return 0
-
-
def main():
rospy.init_node('plug_in')
@@ -100,7 +92,7 @@
cnt = 0
while not rospy.is_shutdown():
cnt += 1
- outlet_pose.header.stamp = last_time()
+ outlet_pose.header.stamp = rospy.get_rostime()
pub_command.publish(outlet_pose)
if cnt % 3 == 0:
try:
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -37,7 +37,8 @@
roslib.load_manifest('tabletop_manipulation')
import rospy
from pr2_msgs.msg import MoveArmGoal, MoveArmState
-from robot_msgs.msg import JointState, PoseConstraint, ControllerStatus
+from robot_msgs.msg import PoseConstraint, ControllerStatus
+from mechanism_msgs.msg import JointState
import sys
@@ -129,7 +130,7 @@
rospy.sleep(1.0)
return self.status.value == ControllerStatus.SUCCESS
-
+
USAGE = 'movearm.py {left|right} <shoulder_lift> <shoulder_pan>'
if __name__ == '__main__':
if len(sys.argv) != 4 or (sys.argv[1] != 'left' and sys.argv[1] != 'right'):
@@ -137,7 +138,7 @@
sys.exit(-1)
side = sys.argv[1]
- joints = {}
+ joints = {}
joints[side[0] + '_shoulder_lift_joint'] = float(sys.arg...
[truncated message content] |
|
From: <stu...@us...> - 2009-06-17 23:26:30
|
Revision: 17258
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17258&view=rev
Author: stuglaser
Date: 2009-06-17 23:25:09 +0000 (Wed, 17 Jun 2009)
Log Message:
-----------
Killed robot_msgs/(Mechanism|Joint|Actuator)State
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
Removed Paths:
-------------
pkg/trunk/common/robot_msgs/msg/ActuatorState.msg
pkg/trunk/common/robot_msgs/msg/JointState.msg
pkg/trunk/common/robot_msgs/msg/MechanismState.msg
Deleted: pkg/trunk/common/robot_msgs/msg/ActuatorState.msg
===================================================================
--- pkg/trunk/common/robot_msgs/msg/ActuatorState.msg 2009-06-17 23:24:43 UTC (rev 17257)
+++ pkg/trunk/common/robot_msgs/msg/ActuatorState.msg 2009-06-17 23:25:09 UTC (rev 17258)
@@ -1,23 +0,0 @@
-string name
-int32 device_id
-int32 encoder_count
-float64 position
-float64 timestamp
-float64 encoder_velocity
-float64 velocity
-byte calibration_reading
-byte calibration_rising_edge_valid
-byte calibration_falling_edge_valid
-float64 last_calibration_rising_edge
-float64 last_calibration_falling_edge
-byte is_enabled
-byte run_stop_hit
-float64 last_requested_current
-float64 last_commanded_current
-float64 last_measured_current
-float64 last_requested_effort
-float64 last_commanded_effort
-float64 last_measured_effort
-float64 motor_voltage
-int32 num_encoder_errors
-
Deleted: pkg/trunk/common/robot_msgs/msg/JointState.msg
===================================================================
--- pkg/trunk/common/robot_msgs/msg/JointState.msg 2009-06-17 23:24:43 UTC (rev 17257)
+++ pkg/trunk/common/robot_msgs/msg/JointState.msg 2009-06-17 23:25:09 UTC (rev 17258)
@@ -1,6 +0,0 @@
-string name
-float64 position
-float64 velocity
-float64 applied_effort
-float64 commanded_effort
-byte is_calibrated
\ No newline at end of file
Deleted: pkg/trunk/common/robot_msgs/msg/MechanismState.msg
===================================================================
--- pkg/trunk/common/robot_msgs/msg/MechanismState.msg 2009-06-17 23:24:43 UTC (rev 17257)
+++ pkg/trunk/common/robot_msgs/msg/MechanismState.msg 2009-06-17 23:25:09 UTC (rev 17258)
@@ -1,4 +0,0 @@
-Header header
-float64 time
-ActuatorState[] actuator_states
-JointState[] joint_states
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2009-06-17 23:24:43 UTC (rev 17257)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2009-06-17 23:25:09 UTC (rev 17258)
@@ -16,6 +16,7 @@
<depend package="prosilica_cam"/>
<depend package="rospy"/>
<depend package="std_msgs" />
+ <depend package="mechanism_msgs" />
<depend package="image_msgs" />
<depend package="opencv_latest" />
<depend package="laser_scan" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-06-18 00:57:11
|
Revision: 17268
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17268&view=rev
Author: isucan
Date: 2009-06-18 00:55:11 +0000 (Thu, 18 Jun 2009)
Log Message:
-----------
added stereo perception launch file
Added Paths:
-----------
pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch
pkg/trunk/demos/tabletop_manipulation/launch/stereo-perception.launch
Removed Paths:
-------------
pkg/trunk/demos/tabletop_manipulation/launch/perception.launch
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/point_cloud_assembler.launch
Added: pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch 2009-06-18 00:55:11 UTC (rev 17268)
@@ -0,0 +1,27 @@
+<launch>
+
+ <include file="$(find pr2_defs)/pr2_planning_environment.launch" />
+
+ <node pkg="point_cloud_assembler" type="laser_scan_assembler_srv" output="screen" name="laser_scan_assembler">
+ <remap from="scan_in" to="tilt_scan"/>
+ <param name="tf_cache_time_secs" type="double" value="10.0" />
+ <param name="tf_tolerance_secs" type="double" value="0.0" />
+ <param name="max_scans" type="int" value="400" />
+ <param name="ignore_laser_skew" type="bool" value="true" />
+ <param name="fixed_frame" type="string" value="torso_lift_link" />
+ <param name="downsample_factor" type="int" value="2" />
+ </node>
+
+ <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
+ <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>
+
+ <include file="$(find pr2_laser_tilt)/laser_tilt_controller.launch" />
+
+ <node pkg="new_robot_self_filter" type="self_filter" args="robot_description:=robotdesc/pr2 full_cloud:=snapshot_cloud" respawn="true" output="screen" />
+
+ <include file="$(find collision_map)/collision_map.launch" />
+
+</launch>
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/perception.launch 2009-06-18 00:55:06 UTC (rev 17267)
+++ pkg/trunk/demos/tabletop_manipulation/launch/perception.launch 2009-06-18 00:55:11 UTC (rev 17268)
@@ -1,7 +0,0 @@
-<launch>
- <include file="$(find pr2_defs)/pr2_planning_environment.launch" />
- <include file="$(find pr2_laser_tilt)/laser_tilt_controller.launch" />
- <include file="$(find point_cloud_assembler)/point_cloud_assembler.launch" />
- <include file="$(find new_robot_self_filter)/self_filter.launch" />
- <include file="$(find collision_map)/collision_map.launch" />
-</launch>
Added: pkg/trunk/demos/tabletop_manipulation/launch/stereo-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/stereo-perception.launch (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/launch/stereo-perception.launch 2009-06-18 00:55:11 UTC (rev 17268)
@@ -0,0 +1,10 @@
+<launch>
+
+ <include file="$(find pr2_defs)/pr2_planning_environment.launch" />
+ <include file="$(find stereo_image_proc)/stereo.launch" />
+
+ <node pkg="new_robot_self_filter" type="self_filter" args="robot_description:=robotdesc/pr2 full_cloud:=stereo/cloud" respawn="true" output="screen" />
+
+ <include file="$(find collision_map)/collision_map.launch" />
+
+</launch>
Deleted: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/point_cloud_assembler.launch
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/point_cloud_assembler.launch 2009-06-18 00:55:06 UTC (rev 17267)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/point_cloud_assembler.launch 2009-06-18 00:55:11 UTC (rev 17268)
@@ -1,19 +0,0 @@
-<launch>
-
- <node pkg="point_cloud_assembler" type="laser_scan_assembler_srv" output="screen" name="laser_scan_assembler">
- <remap from="scan_in" to="tilt_scan"/>
- <param name="tf_cache_time_secs" type="double" value="10.0" />
- <param name="tf_tolerance_secs" type="double" value="0.0" />
- <param name="max_scans" type="int" value="400" />
- <param name="ignore_laser_skew" type="bool" value="true" />
- <param name="fixed_frame" type="string" value="torso_lift_link" />
- <param name="downsample_factor" type="int" value="2" />
- </node>
-
- <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
- <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>
-
-</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tpr...@us...> - 2009-06-18 01:23:05
|
Revision: 17266
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17266&view=rev
Author: tpratkanis
Date: 2009-06-18 00:40:41 +0000 (Thu, 18 Jun 2009)
Log Message:
-----------
Fix some things in robot actions.
Modified Paths:
--------------
pkg/trunk/common/robot_actions/include/robot_actions/action.h
pkg/trunk/common/robot_actions/msg/ActionStatus.msg
pkg/trunk/highlevel/robot_actions_tools/list_action_state.py
pkg/trunk/highlevel/test_robot_actions/test/utest.cc
Modified: pkg/trunk/common/robot_actions/include/robot_actions/action.h
===================================================================
--- pkg/trunk/common/robot_actions/include/robot_actions/action.h 2009-06-18 00:40:41 UTC (rev 17265)
+++ pkg/trunk/common/robot_actions/include/robot_actions/action.h 2009-06-18 00:40:41 UTC (rev 17266)
@@ -51,6 +51,7 @@
#include <boost/thread.hpp>
#include <ros/node.h>
#include <diagnostic_msgs/DiagnosticMessage.h>
+#include "ros/assert.h"
namespace robot_actions {
@@ -77,13 +78,6 @@
* @param feedback The feedback message. At the end of the function, this will be published.
*/
virtual ResultStatus execute(const Goal& goal, Feedback& feedback) = 0;
-
- /**
- * @brief Allows the user to check if the action is active. This is primarily used to condition behavior
- * for low duty cycles when the action is inactive
- * @return True if the action is active, false otherwise.
- */
- bool isActive() const { return _status.value == _status.ACTIVE; }
/**
* @brief Called by the user to check if the action has a new goal.
@@ -107,6 +101,13 @@
public:
/**
+ * @brief Allows the user to check if the action is active. This is primarily used to condition behavior
+ * for low duty cycles when the action is inactive
+ * @return True if the action is active, false otherwise.
+ */
+ bool isActive() const { return _status.value == _status.ACTIVE; }
+
+ /**
* @brief Accessor for the action name
*/
const std::string& getName() const { return _name; }
@@ -184,12 +185,12 @@
*/
Action(const std::string& name)
: _name(name), _preempt_request(false), _result_status(SUCCESS), _terminated(false), _action_thread(NULL), _callback(NULL),_diagnostics_statuses(1){
- _status.value = ActionStatus::UNDEFINED;
+ _status.value = ActionStatus::RESET;
ros::Node::instance()->advertise<diagnostic_msgs::DiagnosticMessage> ("/diagnostics", 1) ;
}
virtual ~Action(){
- terminate();
+ ROS_ASSERT_MSG(_terminated, "Actions must be terminated before they are deleted, or there will be a segfault. In this case, action '%s' terminated was terminated before it was deleted.", _name.c_str());
if(_action_thread != NULL){
_action_thread->join();
delete _action_thread;
@@ -270,8 +271,8 @@
else
diagnostics_status.message = "Active";
}
- else if (_status.value == ActionStatus::UNDEFINED)
- diagnostics_status.message = "Undefined";
+ else if (_status.value == ActionStatus::RESET)
+ diagnostics_status.message = "Reset";
else if (_status.value == ActionStatus::PREEMPTED)
diagnostics_status.message = "Preempted";
else if (_status.value == ActionStatus::ABORTED)
Modified: pkg/trunk/common/robot_actions/msg/ActionStatus.msg
===================================================================
--- pkg/trunk/common/robot_actions/msg/ActionStatus.msg 2009-06-18 00:40:41 UTC (rev 17265)
+++ pkg/trunk/common/robot_actions/msg/ActionStatus.msg 2009-06-18 00:40:41 UTC (rev 17266)
@@ -1,8 +1,8 @@
# This message defines the expected format for robot action status messages
# Embed this in the feedback state message of robot actions
-# The action is inactive, and its status with respect to a prior request is undefined.
-byte UNDEFINED=0
+# The action is inactive, and has just been reset.
+byte RESET=0
# The action has successfuly completed and is now inactive
byte SUCCESS=1
Modified: pkg/trunk/highlevel/robot_actions_tools/list_action_state.py
===================================================================
--- pkg/trunk/highlevel/robot_actions_tools/list_action_state.py 2009-06-18 00:40:41 UTC (rev 17265)
+++ pkg/trunk/highlevel/robot_actions_tools/list_action_state.py 2009-06-18 00:40:41 UTC (rev 17266)
@@ -103,8 +103,8 @@
def set_topic_key(topic, msg):
global controllers
- if (msg.status.value == msg.status.UNDEFINED):
- controllers[topic] = "Undefined"
+ if (msg.status.value == msg.status.RESET):
+ controllers[topic] = "Reset"
elif (msg.status.value == msg.status.SUCCESS):
controllers[topic] = "Success"
elif (msg.status.value == msg.status.ABORTED):
Modified: pkg/trunk/highlevel/test_robot_actions/test/utest.cc
===================================================================
--- pkg/trunk/highlevel/test_robot_actions/test/utest.cc 2009-06-18 00:40:41 UTC (rev 17265)
+++ pkg/trunk/highlevel/test_robot_actions/test/utest.cc 2009-06-18 00:40:41 UTC (rev 17266)
@@ -1,4 +1,3 @@
-#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
#include <robot_actions/action_client.h>
#include <robot_actions/message_adapter.h>
@@ -18,7 +17,7 @@
class MyAction: public robot_actions::Action<Float32, Float32> {
public:
- MyAction(): robot_actions::Action<Float32, Float32>("my_action"), _use_deactivate_wait(false) {}
+ MyAction(std::string name): robot_actions::Action<Float32, Float32>(name), _use_deactivate_wait(false) {}
bool _use_deactivate_wait;
@@ -80,7 +79,7 @@
MySimpleContainer(MyAction& action){
_value = 0;
action.connect(boost::bind(&MySimpleContainer::notify, this, _1, _2, _3));
- _status.value = robot_actions::ActionStatus::UNDEFINED;
+ _status.value = robot_actions::ActionStatus::RESET;
}
ActionStatus _status;
@@ -89,7 +88,7 @@
// Hack to fordce linkage - gtest macro issue
-const int8_t robot_actions::ActionStatus::UNDEFINED;
+const int8_t robot_actions::ActionStatus::RESET;
const int8_t robot_actions::ActionStatus::SUCCESS;
const int8_t robot_actions::ActionStatus::PREEMPTED;
const int8_t robot_actions::ActionStatus::ABORTED;
@@ -100,7 +99,7 @@
* Test - just exercises compilation and demonstrates use
*/
TEST(robot_actions, action_with_simple_container){
- MyAction a;
+ MyAction a("my_action_test_action_with_simple_container");
MySimpleContainer c(a);
Float32 g;
g.data = 10.0;
@@ -131,13 +130,14 @@
robot_actions::AbstractAdapter& abstract_adapter(adapter);
abstract_adapter.initialize();
abstract_adapter.terminate();
+ a.terminate();
}
/**
* Test - Run give a number of goals.
*/
TEST(robot_actions, many_goals) {
- MyAction a;
+ MyAction a("my_action_test_many_goals");
MySimpleContainer c(a);
Float32 g;
robot_actions::ActionStatus foo;
@@ -165,14 +165,14 @@
ASSERT_EQ(i < c._value, true);
}
}
-
+ a.terminate();
}
/**
* Test - Run give a number of goals. Use waitForDeactivation().
*/
TEST(robot_actions, many_goals_wait_for_deactivation) {
- MyAction a;
+ MyAction a("robot_actions_test_many_goals_wait_for_deactivation");
MySimpleContainer c(a);
Float32 g;
robot_actions::ActionStatus foo;
@@ -201,6 +201,7 @@
ASSERT_EQ(i < c._value, true);
}
}
+ a.terminate();
}
/**
@@ -209,7 +210,7 @@
TEST(robot_actions, action_with_action_runner){
// Now connect actions
- MyAction a;
+ MyAction a("my_action_test_action_with_action_runner");
Float32 g;
robot_actions::ActionStatus foo;
@@ -230,7 +231,7 @@
TEST(robot_actions, action_client){
// Now connect actions
- MyAction action;
+ MyAction action("my_action");
// Now run it.
robot_actions::ActionRunner runner(10.0);
@@ -244,7 +245,7 @@
Float32 g, f;
g.data = 5;
- robot_actions::ResultStatus result = client.execute(g, f, ros::Duration(1));
+ robot_actions::ResultStatus result = client.execute(g, f, ros::Duration().fromSec(10));
ASSERT_EQ(result, robot_actions::SUCCESS);
result = client.execute(g, f, ros::Duration().fromSec(0.0001));
@@ -265,7 +266,9 @@
// First allocate it with an update rate of 10 Hz
robot_actions::ActionRunner runner(10.0);
for (int i = 0; i < 100; i++) {
- MyAction* a = new MyAction();
+ std::stringstream buff;
+ buff << "my_action_test_scalability" << i;
+ MyAction* a = new MyAction(buff.str());
actions.push_back(a);
runner.connect<Float32, TestState, Float32>(*a);
}
@@ -278,6 +281,7 @@
duration.sleep();
while(!actions.size()) {
+ actions[0]->terminate();
delete actions[0];
actions.erase(actions.begin());
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-06-18 03:09:39
|
Revision: 17273
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17273&view=rev
Author: isucan
Date: 2009-06-18 03:09:27 +0000 (Thu, 18 Jun 2009)
Log Message:
-----------
some bugfixes
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/CMakeLists.txt
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
Added Paths:
-----------
pkg/trunk/highlevel/move_arm/test/move_to_position.cpp
pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp
Removed Paths:
-------------
pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp
Modified: pkg/trunk/highlevel/move_arm/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/move_arm/CMakeLists.txt 2009-06-18 01:55:03 UTC (rev 17272)
+++ pkg/trunk/highlevel/move_arm/CMakeLists.txt 2009-06-18 03:09:27 UTC (rev 17273)
@@ -4,6 +4,6 @@
rospack(move_arm)
rospack_add_executable(move_arm src/move_arm.cpp)
-rospack_add_executable(move_arm_action_test test/test_move_arm.cpp)
-
+rospack_add_executable(move_to_position test/move_to_position.cpp)
+rospack_add_executable(oscillate_move test/oscillate_move.cpp)
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-18 01:55:03 UTC (rev 17272)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-18 03:09:27 UTC (rev 17273)
@@ -103,18 +103,26 @@
motion_planning_srvs::KinematicPlan::Request req;
motion_planning_srvs::KinematicPlan::Response res;
- req.params.model_id = arm_;
- req.params.planner_id = "KPIECE";
- req.params.distance_metric = "L2Square";
+ req.params.model_id = arm_; // the model to plan for (should be defined in planning.yaml)
+ req.params.planner_id = "KPIECE"; // this is optional; the planning node should be able to pick a planner
+ req.params.distance_metric = "L2Square"; // the metric to be used in the robot's state space
+
// this volume is only needed if planar or floating joints move in the space
req.params.volumeMin.x = req.params.volumeMin.y = req.params.volumeMin.z = 0.0;
req.params.volumeMax.x = req.params.volumeMax.y = req.params.volumeMax.z = 0.0;
+ // forward the goal & path constraints
req.goal_constraints = goal.goal_constraints;
req.path_constraints = goal.path_constraints;
-
+ // compute the path once
+ req.times = 1;
+
+ // do not spend more than this amount of time
+ req.allowed_time = 0.5;
+
+ // change pose constraints to joint constraints, if possible and so desired
if (perform_ik_ && // IK is enabled,
req.goal_constraints.joint_constraint.empty() && // we have no joint constraints on the goal,
req.goal_constraints.pose_constraint.size() == 1 && // we have a single pose constraint on the goal
@@ -156,9 +164,6 @@
}
}
- req.times = 1;
- req.allowed_time = 0.5;
-
ResultStatus result = robot_actions::SUCCESS;
feedback = pr2_robot_actions::MoveArmState::PLANNING;
@@ -180,6 +185,20 @@
// if we have to plan, do so
if (result == robot_actions::SUCCESS && feedback == pr2_robot_actions::MoveArmState::PLANNING)
{
+
+ // fill in start state with current one
+ std::vector<planning_models::KinematicModel::Joint*> joints;
+ planningMonitor_->getKinematicModel()->getJoints(joints);
+
+ req.start_state.resize(joints.size());
+ for (unsigned int i = 0 ; i < joints.size() ; ++i)
+ {
+ req.start_state[i].header.frame_id = planningMonitor_->getFrameId();
+ req.start_state[i].header.stamp = planningMonitor_->lastStateUpdate();
+ req.start_state[i].joint_name = joints[i]->name;
+ planningMonitor_->getRobotState()->copyParamsJoint(req.start_state[i].value, joints[i]->name);
+ }
+
// call the planner and decide whether to use the path
if (clientPlan.call(req, res))
{
Copied: pkg/trunk/highlevel/move_arm/test/move_to_position.cpp (from rev 17262, pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp)
===================================================================
--- pkg/trunk/highlevel/move_arm/test/move_to_position.cpp (rev 0)
+++ pkg/trunk/highlevel/move_arm/test/move_to_position.cpp 2009-06-18 03:09:27 UTC (rev 17273)
@@ -0,0 +1,100 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: test_executive.cpp 16122 2009-05-27 00:28:28Z meeussen $
+ *
+ *********************************************************************/
+
+/* Author: Sachin Chitta */
+
+
+#include <ros/node.h>
+#include <boost/thread/thread.hpp>
+#include <robot_actions/action_client.h>
+
+#include <pr2_robot_actions/MoveArmGoal.h>
+#include <pr2_robot_actions/MoveArmState.h>
+#include <pr2_robot_actions/SwitchControllersState.h>
+
+using namespace ros;
+using namespace std;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ ros::Node node("test_move_arm");
+ // boost::thread* thread;
+
+ pr2_robot_actions::SwitchControllers switchlist;
+ std_msgs::Empty empty;
+
+ Duration timeout_short = Duration().fromSec(2.0);
+ Duration timeout_medium = Duration().fromSec(10.0);
+ Duration timeout_long = Duration().fromSec(40.0);
+
+ // robot_actions::ActionClient<pr2_robot_actions::SwitchControllers, pr2_robot_actions::SwitchControllersState, std_msgs::Empty> switch_controllers("switch_controllers");
+ robot_actions::ActionClient<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t> move_arm("move_arm");
+
+ int32_t feedback;
+ pr2_robot_actions::MoveArmGoal goal;
+ pr2_robot_actions::MoveArmState state;
+
+ goal.goal_constraints.set_pose_constraint_size(1);
+ goal.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
+ goal.goal_constraints.pose_constraint[0].pose.header.frame_id = "torso_lift_link";
+
+ goal.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
+ goal.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.75;
+ goal.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.188;
+ goal.goal_constraints.pose_constraint[0].pose.pose.position.z = 0;
+
+ goal.goal_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
+ goal.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
+ goal.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
+ goal.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1;
+ goal.goal_constraints.pose_constraint[0].position_distance = 0.01;
+ goal.goal_constraints.pose_constraint[0].orientation_distance = 0.01;
+ goal.goal_constraints.pose_constraint[0].orientation_importance = 0.1;
+ goal.goal_constraints.pose_constraint[0].type = motion_planning_msgs::PoseConstraint::POSITION_XYZ +
+ motion_planning_msgs::PoseConstraint::ORIENTATION_RPY;
+
+ // switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
+ // switchlist.start_controllers.push_back("r_arm_joint_trajectory_controller");
+
+ // if(switch_controllers.execute(switchlist, empty, timeout_medium) != robot_actions::SUCCESS) return -1;
+ // ROS_INFO("Done switching controllers");
+
+ if(move_arm.execute(goal,feedback,timeout_long) != robot_actions::SUCCESS) return -1;
+
+ return (0);
+}
Property changes on: pkg/trunk/highlevel/move_arm/test/move_to_position.cpp
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/highlevel/move_arm/test/test_move_arm.cpp:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Copied: pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp (from rev 17262, pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp)
===================================================================
--- pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp (rev 0)
+++ pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp 2009-06-18 03:09:27 UTC (rev 17273)
@@ -0,0 +1,122 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *
+ *********************************************************************/
+
+/* Author: Ioan Sucan */
+
+#include <ros/ros.h>
+#include <robot_actions/action_client.h>
+
+#include <pr2_robot_actions/MoveArmGoal.h>
+#include <pr2_robot_actions/MoveArmState.h>
+
+#include <vector>
+#include <string>
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "test_move_arm");
+
+ ros::Node hack; // hack
+
+ robot_actions::ActionClient<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t> move_arm("move_arm");
+
+ int32_t feedback;
+ pr2_robot_actions::MoveArmGoal goalA;
+ pr2_robot_actions::MoveArmGoal goalB;
+ pr2_robot_actions::MoveArmState state;
+
+ goalA.goal_constraints.set_pose_constraint_size(1);
+ goalA.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
+ goalA.goal_constraints.pose_constraint[0].pose.header.frame_id = "torso_lift_link";
+
+ goalA.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
+ goalA.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.75;
+ goalA.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.188;
+ goalA.goal_constraints.pose_constraint[0].pose.pose.position.z = 0;
+
+ goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
+ goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
+ goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
+ goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1;
+ goalA.goal_constraints.pose_constraint[0].position_distance = 0.01;
+ goalA.goal_constraints.pose_constraint[0].orientation_distance = 0.01;
+ goalA.goal_constraints.pose_constraint[0].orientation_importance = 0.1;
+ goalA.goal_constraints.pose_constraint[0].type =
+ motion_planning_msgs::PoseConstraint::POSITION_XYZ +
+ motion_planning_msgs::PoseConstraint::ORIENTATION_RPY;
+
+
+ std::vector<std::string> names(7);
+ names[0] = "r_shoulder_pan_joint";
+ names[1] = "r_shoulder_lift_joint";
+ names[2] = "r_upper_arm_roll_joint";
+ names[3] = "r_elbow_flex_joint";
+ names[4] = "r_forearm_roll_joint";
+ names[5] = "r_wrist_flex_joint";
+ names[6] = "r_wrist_roll_joint";
+
+ goalB.goal_constraints.joint_constraint.resize(names.size());
+ for (unsigned int i = 0 ; i < goalB.goal_constraints.joint_constraint.size(); ++i)
+ {
+ goalB.goal_constraints.joint_constraint[i].header.stamp = ros::Time::now();
+ goalB.goal_constraints.joint_constraint[i].header.frame_id = "base_link";
+ goalB.goal_constraints.joint_constraint[i].joint_name = names[i];
+ goalB.goal_constraints.joint_constraint[i].value.resize(1);
+ goalB.goal_constraints.joint_constraint[i].toleranceAbove.resize(1);
+ goalB.goal_constraints.joint_constraint[i].toleranceBelow.resize(1);
+ goalB.goal_constraints.joint_constraint[i].value[0] = 0.0;
+ goalB.goal_constraints.joint_constraint[i].toleranceBelow[0] = 0.0;
+ goalB.goal_constraints.joint_constraint[i].toleranceAbove[0] = 0.0;
+ }
+
+ goalB.goal_constraints.joint_constraint[0].value[0] = -2.0;
+ goalB.goal_constraints.joint_constraint[3].value[0] = -0.1;
+ goalB.goal_constraints.joint_constraint[5].value[0] = 0.15;
+
+ ros::NodeHandle nh;
+
+ while (nh.ok())
+ {
+ ros::spinOnce();
+ if (move_arm.execute(goalA, feedback, ros::Duration(10.0)) != robot_actions::SUCCESS)
+ return -1;
+ ros::spinOnce();
+ if (move_arm.execute(goalB, feedback, ros::Duration(10.0)) != robot_actions::SUCCESS)
+ return -1;
+ }
+
+ return 0;
+}
Property changes on: pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/highlevel/move_arm/test/test_move_arm.cpp:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Deleted: pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp 2009-06-18 01:55:03 UTC (rev 17272)
+++ pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp 2009-06-18 03:09:27 UTC (rev 17273)
@@ -1,100 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- * $Id: test_executive.cpp 16122 2009-05-27 00:28:28Z meeussen $
- *
- *********************************************************************/
-
-/* Author: Sachin Chitta */
-
-
-#include <ros/node.h>
-#include <boost/thread/thread.hpp>
-#include <robot_actions/action_client.h>
-
-#include <pr2_robot_actions/MoveArmGoal.h>
-#include <pr2_robot_actions/MoveArmState.h>
-#include <pr2_robot_actions/SwitchControllersState.h>
-
-using namespace ros;
-using namespace std;
-
-int main(int argc, char **argv)
-{
- ros::init(argc, argv);
- ros::Node node("test_move_arm");
- // boost::thread* thread;
-
- pr2_robot_actions::SwitchControllers switchlist;
- std_msgs::Empty empty;
-
- Duration timeout_short = Duration().fromSec(2.0);
- Duration timeout_medium = Duration().fromSec(10.0);
- Duration timeout_long = Duration().fromSec(40.0);
-
- // robot_actions::ActionClient<pr2_robot_actions::SwitchControllers, pr2_robot_actions::SwitchControllersState, std_msgs::Empty> switch_controllers("switch_controllers");
- robot_actions::ActionClient<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t> move_arm("move_arm");
-
- int32_t feedback;
- pr2_robot_actions::MoveArmGoal goal;
- pr2_robot_actions::MoveArmState state;
-
- goal.goal_constraints.set_pose_constraint_size(1);
- goal.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
- goal.goal_constraints.pose_constraint[0].pose.header.frame_id = "torso_lift_link";
-
- goal.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
- goal.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.75;
- goal.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.188;
- goal.goal_constraints.pose_constraint[0].pose.pose.position.z = 0;
-
- goal.goal_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
- goal.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
- goal.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
- goal.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1;
- goal.goal_constraints.pose_constraint[0].position_distance = 0.01;
- goal.goal_constraints.pose_constraint[0].orientation_distance = 0.01;
- goal.goal_constraints.pose_constraint[0].orientation_importance = 0.1;
- goal.goal_constraints.pose_constraint[0].type = motion_planning_msgs::PoseConstraint::POSITION_XYZ +
- motion_planning_msgs::PoseConstraint::ORIENTATION_RPY;
-
- // switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
- // switchlist.start_controllers.push_back("r_arm_joint_trajectory_controller");
-
- // if(switch_controllers.execute(switchlist, empty, timeout_medium) != robot_actions::SUCCESS) return -1;
- // ROS_INFO("Done switching controllers");
-
- if(move_arm.execute(goal,feedback,timeout_long) != robot_actions::SUCCESS) return -1;
-
- return (0);
-}
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-18 01:55:03 UTC (rev 17272)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-18 03:09:27 UTC (rev 17273)
@@ -117,7 +117,7 @@
planningMonitor_->getRobotState()->copyParams(res.path.start_state.vals);
// apply changes as indicated in request
- for (unsigned int i = 0 ; req.start_state.size() ; ++i)
+ for (unsigned int i = 0 ; i < req.start_state.size() ; ++i)
{
if (!planningMonitor_->getTransformListener()->frameExists(req.start_state[i].header.frame_id))
{
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06-18 01:55:03 UTC (rev 17272)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06-18 03:09:27 UTC (rev 17273)
@@ -531,9 +531,12 @@
/** Copy all parameters to a destination address */
void copyParams(std::vector<double> ¶ms) const;
- /** Copy the parameters describen a given joint */
+ /** Copy the parameters describing a given joint */
void copyParamsJoint(double *params, const std::string &name) const;
+ /** Copy the parameters describing a given joint */
+ void copyParamsJoint(std::vector<double> ¶ms, const std::string &name) const;
+
/** Check if all params in a group were seen */
bool seenAllGroup(const std::string &group) const;
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-06-18 01:55:03 UTC (rev 17272)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-06-18 03:09:27 UTC (rev 17273)
@@ -472,13 +472,19 @@
void planning_models::KinematicModel::getJoints(std::vector<Joint*> &joints) const
{
- joints.resize(m_jointMap.size());
+ std::vector<Joint*> jn(m_mi.stateDimension, NULL);
for (std::map<std::string, Joint*>::const_iterator it = m_jointMap.begin() ; it != m_jointMap.end() ; ++it)
{
+ if (it->second->usedParams == 0)
+ continue;
std::map<std::string, unsigned int>::const_iterator p = m_mi.parameterIndex.find(it->first);
assert(p != m_mi.parameterIndex.end());
- joints[p->second] = it->second;
+ jn[p->second] = it->second;
}
+ joints.clear();
+ for (unsigned int i = 0 ; i < jn.size() ; ++i)
+ if (jn[i])
+ joints.push_back(jn[i]);
}
unsigned int planning_models::KinematicModel::getGroupDimension(int groupID) const
@@ -504,10 +510,9 @@
for (std::map<std::string, Joint*>::const_iterator it = m_jointMap.begin() ; it != m_jointMap.end() ; ++it)
if (it->second->inGroup[groupID] && it->second->usedParams > 0)
nm.push_back(it->first);
- unsigned int start = names.size();
- names.resize(start + nm.size());
+ names.resize(nm.size());
for (unsigned int i = 0 ; i < nm.size() ; ++i)
- names[start + getJointIndexInGroup(nm[i], groupID)] = nm[i];
+ names[getJointIndexInGroup(nm[i], groupID)] = nm[i];
}
void planning_models::KinematicModel::buildChainJ(Robot *robot, Link *parent, Joint* joint, const robot_desc::URDF::Link* urdfLink, const robot_desc::URDF &model)
@@ -895,6 +900,24 @@
m_msg.error("Unknown joint: '" + name + "'");
}
+void planning_models::KinematicModel::StateParams::copyParamsJoint(std::vector<double> ¶ms, const std::string &name) const
+{
+ Joint *joint = m_owner->getJoint(name);
+ if (joint)
+ {
+ params.resize(joint->usedParams);
+ std::map<std::string, unsigned int>::const_iterator it = m_mi.parameterIndex.find(name);
+ if (it != m_mi.parameterIndex.end())
+ {
+ unsigned int pos = it->second;
+ for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
+ params[i] = m_params[pos + i];
+ return;
+ }
+ }
+ m_msg.error("Unknown joint: '" + name + "'");
+}
+
void planning_models::KinematicModel::StateParams::copyParams(double *params) const
{
for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sac...@us...> - 2009-06-18 10:06:45
|
Revision: 17279
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17279&view=rev
Author: sachinchitta
Date: 2009-06-18 10:06:30 +0000 (Thu, 18 Jun 2009)
Log Message:
-----------
moved JointTraj and JointTrajPoint messages from robot_msgs to manipulation_msgs
Modified Paths:
--------------
pkg/trunk/calibration/auto_arm_commander/manifest.xml
pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/arm_commander.py
pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py
pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py
pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/arm_commander.py
pkg/trunk/calibration/optical_flag_calibration/manifest.xml
pkg/trunk/calibration/optical_flag_calibration/scripts/flag_tester.py
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/tuckarm.py
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/srv/GraspPointSrv.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_joint_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_whole_body_controller.cpp
pkg/trunk/demos/plug_in/move_to_pickup.py
pkg/trunk/demos/tabletop_manipulation/scripts/tuckarm.py
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/move_arm/include/move_arm/move_arm.h
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_untuck_arms.h
pkg/trunk/highlevel/safety/safety_core/include/safety_core/action_tuck_arms.h
pkg/trunk/motion_planning/mpglue/include/mpglue/plan.h
pkg/trunk/motion_planning/mpglue/manifest.xml
pkg/trunk/motion_planning/mpglue/src/plan.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.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_research/sbpl_arm_executive/include/sbpl_arm_executive/pr2_arm_node.h
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_check_path.h
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/include/sbpl_arm_planner_node/sbpl_arm_planner_node.h
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/srv/CheckPathSrv.srv
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner_node/srv/PlanPathSrv.srv
pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/include/sbpl_door_planner_action/sbpl_door_planner_action.h
pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/src/sbpl_door_planner_action.cpp
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/include/sbpl_planner_node/sbpl_planner_node.h
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/src/sbpl_planner_node.cpp
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/srv/PlanPathSrv.srv
pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/scripts/tuckarm.py
pkg/trunk/util/pr2_ik/include/pr2_ik/pr2_ik_controller.h
pkg/trunk/util/pr2_ik/include/pr2_ik/pr2_ik_node.h
pkg/trunk/util/pr2_ik/src/pr2_ik_controller.cpp
Added Paths:
-----------
pkg/trunk/common/manipulation_msgs/msg/JointTraj.msg
pkg/trunk/common/manipulation_msgs/msg/JointTrajPoint.msg
Removed Paths:
-------------
pkg/trunk/common/robot_msgs/msg/JointTraj.msg
pkg/trunk/common/robot_msgs/msg/JointTrajPoint.msg
pkg/trunk/common/robot_srvs/srv/IKService.srv
Modified: pkg/trunk/calibration/auto_arm_commander/manifest.xml
===================================================================
--- pkg/trunk/calibration/auto_arm_commander/manifest.xml 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/calibration/auto_arm_commander/manifest.xml 2009-06-18 10:06:30 UTC (rev 17279)
@@ -10,6 +10,7 @@
<depend package="pr2_mechanism_controllers" />
<depend package="std_msgs" />
<depend package="robot_msgs" />
+ <depend package="manipulation_msgs" />
</package>
Modified: pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/arm_commander.py
===================================================================
--- pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/arm_commander.py 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/arm_commander.py 2009-06-18 10:06:30 UTC (rev 17279)
@@ -41,8 +41,8 @@
from pr2_mechanism_controllers.srv import *
#from pr2_mechanism_controllers.msg import
from robot_msgs.msg import *
+from manipulation_msgs.msg import *
-
class ArmCommander() :
def __init__(self, arm_controller_name):
print "Initializing Object"
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py 2009-06-18 10:06:30 UTC (rev 17279)
@@ -11,6 +11,7 @@
from robot_msgs.msg import *
from mechanism_msgs.msg import MechanismState
from kinematic_calibration.msg import *
+from manipulation_msgs.msg import *
arm_mapping = [ ]
headers = [ ]
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py 2009-06-18 10:06:30 UTC (rev 17279)
@@ -11,6 +11,7 @@
from robot_msgs.msg import *
from mechanism_msgs.msg import MechanismState
from kinematic_calibration.msg import *
+from manipulation_msgs.msg import *
traj_actuator_names = ['r_shoulder_pan_motor',
'r_shoulder_lift_motor',
Modified: pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/arm_commander.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/arm_commander.py 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/arm_commander.py 2009-06-18 10:06:30 UTC (rev 17279)
@@ -41,6 +41,7 @@
from pr2_mechanism_controllers.srv import *
#from pr2_mechanism_controllers.msg import
from robot_msgs.msg import *
+from manipulation_msgs.msg import *
class ArmCommander() :
Modified: pkg/trunk/calibration/optical_flag_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/optical_flag_calibration/manifest.xml 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/calibration/optical_flag_calibration/manifest.xml 2009-06-18 10:06:30 UTC (rev 17279)
@@ -12,6 +12,7 @@
<!-- depend package="image_msgs" / -->
<!-- depend package="newmat10"/ -->
<depend package="pr2_mechanism_controllers" />
+ <depend package="manipulation_msgs" />
<export>
<cpp cflags="-I${prefix}/msg/cpp"/>
Modified: pkg/trunk/calibration/optical_flag_calibration/scripts/flag_tester.py
===================================================================
--- pkg/trunk/calibration/optical_flag_calibration/scripts/flag_tester.py 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/calibration/optical_flag_calibration/scripts/flag_tester.py 2009-06-18 10:06:30 UTC (rev 17279)
@@ -9,6 +9,7 @@
from pr2_mechanism_controllers.srv import *
from pr2_mechanism_controllers.msg import *
from robot_msgs.msg import *
+from manipulation_msgs.msg import *
cmd_count = 0
Added: pkg/trunk/common/manipulation_msgs/msg/JointTraj.msg
===================================================================
--- pkg/trunk/common/manipulation_msgs/msg/JointTraj.msg (rev 0)
+++ pkg/trunk/common/manipulation_msgs/msg/JointTraj.msg 2009-06-18 10:06:30 UTC (rev 17279)
@@ -0,0 +1 @@
+JointTrajPoint[] points
\ No newline at end of file
Added: pkg/trunk/common/manipulation_msgs/msg/JointTrajPoint.msg
===================================================================
--- pkg/trunk/common/manipulation_msgs/msg/JointTrajPoint.msg (rev 0)
+++ pkg/trunk/common/manipulation_msgs/msg/JointTrajPoint.msg 2009-06-18 10:06:30 UTC (rev 17279)
@@ -0,0 +1,2 @@
+float64[] positions
+float64 time
\ No newline at end of file
Deleted: pkg/trunk/common/robot_msgs/msg/JointTraj.msg
===================================================================
--- pkg/trunk/common/robot_msgs/msg/JointTraj.msg 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/common/robot_msgs/msg/JointTraj.msg 2009-06-18 10:06:30 UTC (rev 17279)
@@ -1 +0,0 @@
-JointTrajPoint[] points
\ No newline at end of file
Deleted: pkg/trunk/common/robot_msgs/msg/JointTrajPoint.msg
===================================================================
--- pkg/trunk/common/robot_msgs/msg/JointTrajPoint.msg 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/common/robot_msgs/msg/JointTrajPoint.msg 2009-06-18 10:06:30 UTC (rev 17279)
@@ -1,2 +0,0 @@
-float64[] positions
-float64 time
\ No newline at end of file
Deleted: pkg/trunk/common/robot_srvs/srv/IKService.srv
===================================================================
--- pkg/trunk/common/robot_srvs/srv/IKService.srv 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/common/robot_srvs/srv/IKService.srv 2009-06-18 10:06:30 UTC (rev 17279)
@@ -1,6 +0,0 @@
-Header header
-robot_msgs/Pose pose
-robot_msgs/JointTrajPoint joint_pos
-duration timeout
----
-robot_msgs/JointTraj traj
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-06-18 10:06:30 UTC (rev 17279)
@@ -56,8 +56,8 @@
#include <pr2_mechanism_controllers/SetJointTarget.h>
#include <pr2_mechanism_controllers/JointPosCmd.h>
-#include <robot_msgs/JointTraj.h>
-#include <robot_msgs/JointTrajPoint.h>
+#include <manipulation_msgs/JointTraj.h>
+#include <manipulation_msgs/JointTrajPoint.h>
#include <diagnostic_msgs/DiagnosticMessage.h>
#include <pr2_mechanism_controllers/TrajectoryStart.h>
@@ -228,9 +228,9 @@
void deleteTrajectoryFromQueue(int id);
- void addTrajectoryToQueue(robot_msgs::JointTraj new_traj, int id);
+ void addTrajectoryToQueue(manipulation_msgs::JointTraj new_traj, int id);
- int createTrajectory(const robot_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
+ int createTrajectory(const manipulation_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
void updateTrajectoryQueue(int last_trajectory_finish_status);
@@ -247,7 +247,7 @@
*/
boost::mutex ros_lock_;
- robot_msgs::JointTraj traj_msg_;
+ manipulation_msgs::JointTraj traj_msg_;
pr2_mechanism_controllers::JointPosCmd msg_; //The message used by the ROS callback
ArmTrajectoryController *c_;
@@ -280,11 +280,11 @@
int trajectory_id_;
- std::vector<robot_msgs::JointTraj> joint_trajectory_vector_;
+ std::vector<manipulation_msgs::JointTraj> joint_trajectory_vector_;
std::vector<int> joint_trajectory_id_;
- void setTrajectoryCmdFromMsg(robot_msgs::JointTraj traj_msg);
+ void setTrajectoryCmdFromMsg(manipulation_msgs::JointTraj traj_msg);
int request_trajectory_id_;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_trajectory_controller.h 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_trajectory_controller.h 2009-06-18 10:06:30 UTC (rev 17279)
@@ -37,8 +37,8 @@
#include <robot_msgs/PoseDot.h>
#include <pr2_robot_actions/Pose2D.h>
-#include <robot_msgs/JointTrajPoint.h>
-#include <robot_msgs/JointTraj.h>
+#include <manipulation_msgs/JointTrajPoint.h>
+#include <manipulation_msgs/JointTraj.h>
#include <trajectory/trajectory.h>
#include <tf/tf.h>
@@ -86,8 +86,8 @@
double controller_frequency_;
std::string control_topic_name_, path_input_topic_name_;
std::string trajectory_type_;
- robot_msgs::JointTraj path_msg_in_;
- robot_msgs::JointTraj path_msg_;
+ manipulation_msgs::JointTraj path_msg_in_;
+ manipulation_msgs::JointTraj path_msg_;
ros::Node& ros_node_;
tf::TransformListener& tf_;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h 2009-06-18 10:06:30 UTC (rev 17279)
@@ -41,8 +41,8 @@
#include <robot_mechanism_controllers/joint_pd_controller.h>
// Services
-#include <robot_msgs/JointTraj.h>
-#include <robot_msgs/JointTrajPoint.h>
+#include <manipulation_msgs/JointTraj.h>
+#include <manipulation_msgs/JointTrajPoint.h>
#include <diagnostic_msgs/DiagnosticMessage.h>
#include <pr2_mechanism_controllers/TrajectoryStart.h>
@@ -189,7 +189,7 @@
*/
boost::mutex ros_lock_;
- robot_msgs::JointTraj traj_msg_; /**< The trajectory message received over ROS */
+ manipulation_msgs::JointTraj traj_msg_; /**< The trajectory message received over ROS */
/*!
* \brief node name
@@ -206,7 +206,7 @@
*/
ros::Node * const node_;
- std::vector<robot_msgs::JointTraj> joint_trajectory_vector_; /**< Vector of trajectory requests */
+ std::vector<manipulation_msgs::JointTraj> joint_trajectory_vector_; /**< Vector of trajectory requests */
std::vector<int> joint_trajectory_id_; /**< Vector of ids for trajectory requests */
@@ -366,7 +366,7 @@
* @param traj_msg Trajectory message received on a topic or a service
* @param id The associated id for the trajectory command
*/
- void setTrajectoryCmdFromMsg(robot_msgs::JointTraj traj_msg, int id);
+ void setTrajectoryCmdFromMsg(manipulation_msgs::JointTraj traj_msg, int id);
/**
* @brief Callback when a trajectory message is received on a topic
@@ -406,28 +406,28 @@
* @param new_traj The trajectory message that needs to be converted
* @param tp The resultant vector of TPoints
*/
- bool createTrajectoryPointsVectorFromMsg(const robot_msgs::JointTraj &new_traj, std::vector<trajectory::Trajectory::TPoint> &tp);
+ bool createTrajectoryPointsVectorFromMsg(const manipulation_msgs::JointTraj &new_traj, std::vector<trajectory::Trajectory::TPoint> &tp);
/**
* @brief Create a trajectory object from a trajectory message
* @param new_traj The trajectory message that needs to be converted
* @param return_trajectory The resultant trajectory object
*/
- bool createTrajectoryFromMsg(const robot_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
+ bool createTrajectoryFromMsg(const manipulation_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
/**
* @brief Add a new trajectory request to the queue of trajectories that need to be sent out
* @param new_traj The trajectory message that needs to be queued
* @param id The id of the trajectory to be queued
*/
- void addTrajectoryToQueue(robot_msgs::JointTraj new_traj, int id);
+ void addTrajectoryToQueue(manipulation_msgs::JointTraj new_traj, int id);
/**
* @brief Preempt the current trajectory queue
* @param new_traj The trajectory message that needs to executed
* @param id The id of the trajectory
*/
- void preemptTrajectoryQueue(robot_msgs::JointTraj new_traj, int id);
+ void preemptTrajectoryQueue(manipulation_msgs::JointTraj new_traj, int id);
/**
* @brief Delete a trajectory from the queue of trajectories
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h 2009-06-18 10:06:30 UTC (rev 17279)
@@ -56,8 +56,8 @@
#include <pr2_mechanism_controllers/SetJointTarget.h>
#include <pr2_mechanism_controllers/JointPosCmd.h>
-#include <robot_msgs/JointTraj.h>
-#include <robot_msgs/JointTrajPoint.h>
+#include <manipulation_msgs/JointTraj.h>
+#include <manipulation_msgs/JointTrajPoint.h>
#include <diagnostic_msgs/DiagnosticMessage.h>
#include <pr2_mechanism_controllers/TrajectoryStart.h>
@@ -271,9 +271,9 @@
void deleteTrajectoryFromQueue(int id);
- void addTrajectoryToQueue(robot_msgs::JointTraj new_traj, int id);
+ void addTrajectoryToQueue(manipulation_msgs::JointTraj new_traj, int id);
- int createTrajectory(const robot_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
+ int createTrajectory(const manipulation_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
void updateTrajectoryQueue(int last_trajectory_finish_status);
@@ -294,7 +294,7 @@
*/
boost::mutex ros_lock_;
- robot_msgs::JointTraj traj_msg_;
+ manipulation_msgs::JointTraj traj_msg_;
pr2_mechanism_controllers::JointPosCmd msg_; //The message used by the ROS callback
WholeBodyTrajectoryController *c_;
@@ -327,11 +327,11 @@
int trajectory_id_;
- std::vector<robot_msgs::JointTraj> joint_trajectory_vector_;
+ std::vector<manipulation_msgs::JointTraj> joint_trajectory_vector_;
std::vector<int> joint_trajectory_id_;
- void setTrajectoryCmdFromMsg(robot_msgs::JointTraj traj_msg);
+ void setTrajectoryCmdFromMsg(manipulation_msgs::JointTraj traj_msg);
int request_trajectory_id_;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-06-18 10:06:30 UTC (rev 17279)
@@ -25,6 +25,8 @@
<depend package="angles" />
<depend package="control_toolbox" />
<depend package="filters" />
+ <depend package="manipulation_msgs" />
+
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/tuckarm.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/tuckarm.py 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/tuckarm.py 2009-06-18 10:06:30 UTC (rev 17279)
@@ -39,7 +39,7 @@
import rospy
-from robot_msgs.msg import JointTraj, JointTrajPoint
+from manipulation_msgs.msg import JointTraj, JointTrajPoint
from mechanism_control import mechanism
from robot_mechanism_controllers.srv import *
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-06-18 10:06:30 UTC (rev 17279)
@@ -435,7 +435,7 @@
}
}
-void ArmTrajectoryControllerNode::setTrajectoryCmdFromMsg(robot_msgs::JointTraj traj_msg)
+void ArmTrajectoryControllerNode::setTrajectoryCmdFromMsg(manipulation_msgs::JointTraj traj_msg)
{
std::vector<trajectory::Trajectory::TPoint> tp;
int msg_size = std::max<int>((int)traj_msg.get_points_size(),1);
@@ -614,7 +614,7 @@
return true;
}
-int ArmTrajectoryControllerNode::createTrajectory(const robot_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory)
+int ArmTrajectoryControllerNode::createTrajectory(const manipulation_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory)
{
std::vector<trajectory::Trajectory::TPoint> tp;
@@ -654,7 +654,7 @@
return 1;
}
-void ArmTrajectoryControllerNode::addTrajectoryToQueue(robot_msgs::JointTraj new_traj, int id)
+void ArmTrajectoryControllerNode::addTrajectoryToQueue(manipulation_msgs::JointTraj new_traj, int id)
{
joint_trajectory_vector_.push_back(new_traj);
joint_trajectory_id_.push_back(id);
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp 2009-06-18 10:06:30 UTC (rev 17279)
@@ -741,7 +741,7 @@
}
-void JointTrajectoryController::setTrajectoryCmdFromMsg(robot_msgs::JointTraj traj_msg, int id)
+void JointTrajectoryController::setTrajectoryCmdFromMsg(manipulation_msgs::JointTraj traj_msg, int id)
{
std::vector<trajectory::Trajectory::TPoint> tp;
int msg_size = std::max<int>((int)traj_msg.get_points_size(),1);
@@ -894,7 +894,7 @@
}
-bool JointTrajectoryController::createTrajectoryPointsVectorFromMsg(const robot_msgs::JointTraj &new_traj, std::vector<trajectory::Trajectory::TPoint> &tp)
+bool JointTrajectoryController::createTrajectoryPointsVectorFromMsg(const manipulation_msgs::JointTraj &new_traj, std::vector<trajectory::Trajectory::TPoint> &tp)
{
if(new_traj.get_points_size() > 0)
{
@@ -925,7 +925,7 @@
return true;
}
-bool JointTrajectoryController::createTrajectoryFromMsg(const robot_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory)
+bool JointTrajectoryController::createTrajectoryFromMsg(const manipulation_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory)
{
std::vector<trajectory::Trajectory::TPoint> tp;
@@ -945,7 +945,7 @@
return true;
}
-void JointTrajectoryController::addTrajectoryToQueue(robot_msgs::JointTraj new_traj, int id)
+void JointTrajectoryController::addTrajectoryToQueue(manipulation_msgs::JointTraj new_traj, int id)
{
trajectory_queue_.lock();
@@ -958,7 +958,7 @@
trajectory_queue_.unlock();
}
-void JointTrajectoryController::preemptTrajectoryQueue(robot_msgs::JointTraj new_traj, int id)
+void JointTrajectoryController::preemptTrajectoryQueue(manipulation_msgs::JointTraj new_traj, int id)
{
int index = std::max(current_trajectory_index_,0);
trajectory_queue_.try_lock();
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp 2009-06-18 10:06:30 UTC (rev 17279)
@@ -666,7 +666,7 @@
}
}
-void WholeBodyTrajectoryControllerNode::setTrajectoryCmdFromMsg(robot_msgs::JointTraj traj_msg)
+void WholeBodyTrajectoryControllerNode::setTrajectoryCmdFromMsg(manipulation_msgs::JointTraj traj_msg)
{
std::vector<trajectory::Trajectory::TPoint> tp;
int msg_size = std::max<int>((int)traj_msg.get_points_size(),1);
@@ -839,7 +839,7 @@
return true;
}
-int WholeBodyTrajectoryControllerNode::createTrajectory(const robot_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory)
+int WholeBodyTrajectoryControllerNode::createTrajectory(const manipulation_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory)
{
std::vector<trajectory::Trajectory::TPoint> tp;
@@ -879,7 +879,7 @@
return 1;
}
-void WholeBodyTrajectoryControllerNode::addTrajectoryToQueue(robot_msgs::JointTraj new_traj, int id)
+void WholeBodyTrajectoryControllerNode::addTrajectoryToQueue(manipulation_msgs::JointTraj new_traj, int id)
{
joint_trajectory_vector_.push_back(new_traj);
joint_trajectory_id_.push_back(id);
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/srv/GraspPointSrv.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/GraspPointSrv.srv 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/GraspPointSrv.srv 2009-06-18 10:06:30 UTC (rev 17279)
@@ -1,3 +1,3 @@
robot_msgs/PoseStamped transform
---
-robot_msgs/JointTraj traj
\ No newline at end of file
+manipulation_msgs/JointTraj traj
\ No newline at end of file
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv 2009-06-18 10:06:30 UTC (rev 17279)
@@ -1,5 +1,5 @@
## Starts a trajectory
-robot_msgs/JointTraj traj
+manipulation_msgs/JointTraj traj
uint8 hastiming # if 1, use timestamps of trajectory points, otherwise do internal retiming
uint8 requesttiming # if 1, send back the timestamps for every trajectory point (first point starts at 0)
---
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp 2009-06-18 10:06:30 UTC (rev 17279)
@@ -33,7 +33,7 @@
*********************************************************************/
#include <ros/node.h>
-#include <robot_msgs/JointTraj.h>
+#include <manipulation_msgs/JointTraj.h>
static int done = 0;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_joint_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_joint_trajectory_controller.cpp 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_joint_trajectory_controller.cpp 2009-06-18 10:06:30 UTC (rev 17279)
@@ -33,7 +33,7 @@
*********************************************************************/
#include <ros/node.h>
-#include <robot_msgs/JointTraj.h>
+#include <manipulation_msgs/JointTraj.h>
static int done = 0;
@@ -55,7 +55,7 @@
/*********** Start moving the robot ************/
- robot_msgs::JointTraj cmd;
+ manipulation_msgs::JointTraj cmd;
int num_points = 1;
int num_joints = 3;
@@ -70,7 +70,7 @@
cmd.points[0].positions[2] = 0.0;
cmd.points[0].time = 0.0;
- node->advertise<robot_msgs::JointTraj>("base/trajectory_controller/command",1);
+ node->advertise<manipulation_msgs::JointTraj>("base/trajectory_controller/command",1);
sleep(1);
node->publish("base/trajectory_controller/command",cmd);
sleep(4);
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_whole_body_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_whole_body_controller.cpp 2009-06-18 09:27:04 UTC (rev 17278)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_whole_body_controller.cpp 2009-06-18 10:06:30 UTC (rev 17279)
@@ -33,7 +33,7 @@
*********************************************************************/
#include <ros/node.h>
-#include <robot_msgs/JointTraj.h>
+#include <manipulation_msgs/JointTraj.h>
static int done = 0;
@@ -55,7 +55,7 @@
/*********** Start moving the robot ************/
- robot_msgs::JointTraj cmd;
+ manipulation_msgs::JointTraj cmd;
int num_points = 1;
int num_joints = 10;
@@ -77,7 +77,7 @@
cmd.points[0].positions[9] =...
[truncated message content] |
|
From: <is...@us...> - 2009-06-18 22:01:31
|
Revision: 17313
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17313&view=rev
Author: isucan
Date: 2009-06-18 22:01:25 +0000 (Thu, 18 Jun 2009)
Log Message:
-----------
changing member in KinematicPath, at Ben's request. also split kinematic model into two files (one for maintaining state)
Modified Paths:
--------------
pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPRequestHandler.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.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/include/planning_environment/robot_models.h
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
pkg/trunk/motion_planning/planning_models/CMakeLists.txt
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg 2009-06-18 22:01:25 UTC (rev 17313)
@@ -2,8 +2,7 @@
Header header
# The full state of the robot at the start of the path
-# As described by KinematicModel::StateParams
-KinematicState start_state
+KinematicJoint[] start_state
# The model for which this plan was computed
string model_id
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPRequestHandler.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPRequestHandler.h 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPRequestHandler.h 2009-06-18 22:01:25 UTC (rev 17313)
@@ -59,17 +59,17 @@
{
}
- bool isRequestValid(ModelMap &models, motion_planning_msgs::KinematicState &startState, motion_planning_srvs::KinematicPlan::Request &req);
+ bool isRequestValid(ModelMap &models, motion_planning_srvs::KinematicPlan::Request &req);
/* Check and compute a motion plan. Return true if the plan was succesfully computed */
- bool computePlan(ModelMap &models, motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res);
+ bool computePlan(ModelMap &models, const planning_models::StateParams *start, motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res);
protected:
/** Set up all the data needed by motion planning based on a request and lock the planner setup
* using this data */
- void configure(const motion_planning_msgs::KinematicState &startState, motion_planning_srvs::KinematicPlan::Request &req, RKPPlannerSetup *psetup);
+ void configure(const planning_models::StateParams *startState, motion_planning_srvs::KinematicPlan::Request &req, RKPPlannerSetup *psetup);
/** Compute the actual motion plan. Return true if computed plan was trivial (start state already in goal region) */
bool callPlanner(RKPPlannerSetup *psetup, int times, double allowed_time, bool interpolate,
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp 2009-06-18 22:01:25 UTC (rev 17313)
@@ -39,7 +39,7 @@
#include <sstream>
/** Check if request looks valid */
-bool kinematic_planning::RKPRequestHandler::isRequestValid(ModelMap &models, motion_planning_msgs::KinematicState &startState, motion_planning_srvs::KinematicPlan::Request &req)
+bool kinematic_planning::RKPRequestHandler::isRequestValid(ModelMap &models, motion_planning_srvs::KinematicPlan::Request &req)
{
ModelMap::const_iterator pos = models.find(req.params.model_id);
@@ -76,12 +76,6 @@
return false;
}
- if (m->kmodel->getModelInfo().stateDimension != startState.get_vals_size())
- {
- ROS_ERROR("Dimension of start state expected to be %d but was received as %d", m->kmodel->getModelInfo().stateDimension, startState.get_vals_size());
- return false;
- }
-
// check headers
for (unsigned int i = 0 ; i < req.goal_constraints.pose_constraint.size() ; ++i)
if (!m->planningMonitor->getTransformListener()->frameExists(req.goal_constraints.pose_constraint[i].pose.header.frame_id))
@@ -114,7 +108,7 @@
return true;
}
-void kinematic_planning::RKPRequestHandler::configure(const motion_planning_msgs::KinematicState &startState, motion_planning_srvs::KinematicPlan::Request &req, RKPPlannerSetup *psetup)
+void kinematic_planning::RKPRequestHandler::configure(const planning_models::StateParams *startState, motion_planning_srvs::KinematicPlan::Request &req, RKPPlannerSetup *psetup)
{
/* clear memory */
psetup->si->clearGoal();
@@ -139,19 +133,15 @@
if (psetup->model->groupID >= 0)
{
/* set the pose of the whole robot */
- psetup->model->kmodel->computeTransforms(&startState.vals[0]);
+ psetup->model->kmodel->computeTransforms(startState->getParams());
psetup->model->collisionSpace->updateRobotModel();
/* extract the components needed for the start state of the desired group */
- for (unsigned int i = 0 ; i < dim ; ++i)
- start->values[i] = startState.vals[psetup->model->kmodel->getModelInfo().groupStateIndexList[psetup->model->groupID][i]];
+ startState->copyParamsGroup(start->values, psetup->model->groupID);
}
else
- {
/* the start state is the complete state */
- for (unsigned int i = 0 ; i < dim ; ++i)
- start->values[i] = startState.vals[i];
- }
+ startState->copyParams(start->values);
psetup->si->addStartState(start);
@@ -186,9 +176,10 @@
ROS_DEBUG("=======================================");
}
-bool kinematic_planning::RKPRequestHandler::computePlan(ModelMap &models, motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
+bool kinematic_planning::RKPRequestHandler::computePlan(ModelMap &models, const planning_models::StateParams *start,
+ motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
{
- if (!isRequestValid(models, res.path.start_state, req))
+ if (!isRequestValid(models, req))
return false;
ROS_INFO("Selected motion planner: '%s'", req.params.planner_id.c_str());
@@ -200,7 +191,7 @@
RKPPlannerSetup *psetup = m->planners[req.params.planner_id];
// configure the planner
- configure(res.path.start_state, req, psetup);
+ configure(start, req, psetup);
/* compute actual motion plan */
ompl::sb::PathKinematic *bestPath = NULL;
@@ -221,6 +212,15 @@
/* copy the solution to the result */
if (bestPath)
{
+ std::vector<planning_models::KinematicModel::Joint*> joints;
+ psetup->model->kmodel->getJoints(joints);
+ for (unsigned int i = 0 ; i < joints.size() ; ++i)
+ {
+ res.path.start_state[i].header = res.path.header;
+ res.path.start_state[i].joint_name = joints[i]->name;
+ start->copyParamsJoint(res.path.start_state[i].value, joints[i]->name);
+ }
+
res.path.states.resize(bestPath->states.size());
res.path.times.resize(bestPath->states.size());
res.path.names.clear();
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-18 22:01:25 UTC (rev 17313)
@@ -95,6 +95,40 @@
ROS_ERROR("No robot model loaded. OMPL planning node cannot start.");
}
+ planning_models::StateParams* fillStartState(const std::vector<motion_planning_msgs::KinematicJoint> &given)
+ {
+ planning_models::StateParams *s = planningMonitor_->getKinematicModel()->newStateParams();
+ for (unsigned int i = 0 ; i < given.size() ; ++i)
+ {
+ if (!planningMonitor_->getTransformListener()->frameExists(given[i].header.frame_id))
+ {
+ ROS_ERROR("Frame '%s' for joint '%s' in starting state is unknown", given[i].header.frame_id.c_str(), given[i].joint_name.c_str());
+ continue;
+ }
+
+ motion_planning_msgs::KinematicJoint kj = given[i];
+ if (planningMonitor_->transformJointToFrame(kj, planningMonitor_->getFrameId()))
+ s->setParamsJoint(kj.value, kj.joint_name);
+ }
+
+ if (s->seenAll())
+ return s;
+ else
+ {
+ if (planningMonitor_->haveState())
+ {
+ std::vector<planning_models::KinematicModel::Joint*> joints;
+ planningMonitor_->getKinematicModel()->getJoints(joints);
+ for (unsigned int i = 0 ; i < joints.size() ; ++i)
+ if (!s->seenJoint(joints[i]->name))
+ s->setParamsJoint(planningMonitor_->getRobotState()->getParamsJoint(joints[i]->name), joints[i]->name);
+ return s;
+ }
+ }
+ delete s;
+ return NULL;
+ }
+
bool planToGoal(motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
{
ROS_INFO("Received request for planning");
@@ -103,7 +137,7 @@
res.path.states.clear();
res.path.names.clear();
res.path.times.clear();
- res.path.start_state.vals.clear();
+ res.path.start_state.clear();
res.path.model_id = req.params.model_id;
res.path.header.frame_id = planningMonitor_->getFrameId();
res.path.header.stamp = planningMonitor_->lastMapUpdate();
@@ -111,34 +145,12 @@
res.distance = -1.0;
res.approximate = 0;
- if (planningMonitor_->haveState())
+ planning_models::StateParams *startState = fillStartState(req.start_state);
+
+ if (startState)
{
- // get current state
- planningMonitor_->getRobotState()->copyParams(res.path.start_state.vals);
-
- // apply changes as indicated in request
- for (unsigned int i = 0 ; i < req.start_state.size() ; ++i)
- {
- if (!planningMonitor_->getTransformListener()->frameExists(req.start_state[i].header.frame_id))
- {
- ROS_ERROR("Frame '%s' for joint '%s' in starting state is unknown", req.start_state[i].header.frame_id.c_str(), req.start_state[i].joint_name.c_str());
- continue;
- }
-
- planningMonitor_->transformJointToFrame(req.start_state[i], planningMonitor_->getFrameId());
- int idx = planningMonitor_->getKinematicModel()->getJointIndex(req.start_state[i].joint_name);
- if (idx >= 0)
- {
- unsigned int u = planningMonitor_->getKinematicModel()->getJoint(req.start_state[i].joint_name)->usedParams;
- if (u == req.start_state[i].value.size())
- for (unsigned int j = 0 ; j < u ; ++j)
- res.path.start_state.vals[idx + j] = req.start_state[i].value[j];
- else
- ROS_ERROR("Incorrect number of parameters for joint '%s': expected %u, got %u", req.start_state[i].joint_name.c_str(), u, (unsigned int)req.start_state[i].value.size());
- }
- }
-
- st = requestHandler_.computePlan(models_, req, res);
+ st = requestHandler_.computePlan(models_, startState, req, res);
+ delete startState;
}
else
ROS_ERROR("Starting robot state is unknown. Cannot start plan.");
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-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp 2009-06-18 22:01:25 UTC (rev 17313)
@@ -95,7 +95,7 @@
fprintf(stderr, "%s:\n", joint->name.c_str());
// check the value of the joint at small increments
- planning_models::KinematicModel::StateParams *sp = kmsm_->getKinematicModel()->newStateParams();
+ planning_models::StateParams *sp = kmsm_->getKinematicModel()->newStateParams();
for (double val = joint->limit[0] ; val <= joint->limit[1] ; val += 0.1)
{
double to_send[controllerDim];
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-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-18 22:01:25 UTC (rev 17313)
@@ -118,7 +118,7 @@
return haveMap_;
}
- /** \brief Return true if a map update has been received in the last sec seconds */
+ /** \brief Return true if a map update has been received in the last sec seconds. If sec < 10us, this function always returns true. */
bool isMapUpdated(double sec) const;
/** \brief Return the last update time for the map */
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-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-18 22:01:25 UTC (rev 17313)
@@ -106,7 +106,7 @@
}
/** \brief Return a pointer to the maintained robot state */
- const planning_models::KinematicModel::StateParams* getRobotState(void) const
+ const planning_models::StateParams* getRobotState(void) const
{
return robotState_;
}
@@ -138,7 +138,7 @@
/** \brief Wait until a full mechanism state is received */
void waitForState(void) const;
- /** \brief Return true if a full mechanism state has been received in the last sec seconds */
+ /** \brief Return true if a full mechanism state has been received in the last sec seconds. If sec < 10us, this function always returns true. */
bool isStateUpdated(double sec) const;
/** \brief Return true if the pose is included in the state */
@@ -156,29 +156,28 @@
void mechanismStateCallback(const mechanism_msgs::MechanismStateConstPtr &mechanismState);
- RobotModels *rm_;
- bool includePose_;
- planning_models::KinematicModel *kmodel_;
- std::string planarJoint_;
- std::string floatingJoint_;
+ RobotModels *rm_;
+ bool includePose_;
+ planning_models::KinematicModel *kmodel_;
+ std::string planarJoint_;
+ std::string floatingJoint_;
- ros::NodeHandle nh_;
- ros::Subscriber mechanismStateSubscriber_;
- tf::TransformListener *tf_;
+ ros::NodeHandle nh_;
+ ros::Subscriber mechanismStateSubscriber_;
+ tf::TransformListener *tf_;
- planning_models::KinematicModel::StateParams *robotState_;
- tf::Pose pose_;
- std::string robot_frame_;
- std::string frame_id_;
+ planning_models::StateParams *robotState_;
+ tf::Pose pose_;
+ std::string robot_frame_;
+ std::string frame_id_;
- boost::function<void(void)> onStateUpdate_;
+ boost::function<void(void)> onStateUpdate_;
- bool havePose_;
- bool haveMechanismState_;
- ros::Time lastStateUpdate_;
+ bool havePose_;
+ bool haveMechanismState_;
+ ros::Time lastStateUpdate_;
};
}
#endif
-
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-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h 2009-06-18 22:01:25 UTC (rev 17313)
@@ -72,10 +72,10 @@
bool isEnvironmentSafe(void) const;
/** Check if the full state of the robot is valid */
- bool isStateValidOnPath(const planning_models::KinematicModel::StateParams *state) const;
+ bool isStateValidOnPath(const planning_models::StateParams *state) const;
/** Check if the full state of the robot is valid */
- bool isStateValidAtGoal(const planning_models::KinematicModel::StateParams *state) const;
+ bool isStateValidAtGoal(const planning_models::StateParams *state) const;
/** Check if the path is valid */
bool isPathValid(const motion_planning_msgs::KinematicPath &path) const;
@@ -87,18 +87,18 @@
void setGoalConstraints(const motion_planning_msgs::KinematicConstraints &kc);
/** Transform the frames in which constraints are specified to the one requested */
- void transformConstraintsToFrame(motion_planning_msgs::KinematicConstraints &kc, const std::string &target) const;
+ bool transformConstraintsToFrame(motion_planning_msgs::KinematicConstraints &kc, const std::string &target) const;
/** Transform the kinematic path to the frame requested */
- void transformPathToFrame(motion_planning_msgs::KinematicPath &kp, const std::string &target) const;
+ bool transformPathToFrame(motion_planning_msgs::KinematicPath &kp, const std::string &target) const;
/** Transform the kinematic joint to the frame requested */
- void transformJointToFrame(motion_planning_msgs::KinematicJoint &kj, const std::string &target) const;
+ bool transformJointToFrame(motion_planning_msgs::KinematicJoint &kj, const std::string &target) const;
protected:
/** Transform the joint parameters (if needed) to a target frame */
- void transformJoint(const std::string &name, unsigned int index, std::vector<double> ¶ms, roslib::Header &header, const std::string& target) const;
+ bool transformJoint(const std::string &name, unsigned int index, std::vector<double> ¶ms, roslib::Header &header, const std::string& target) const;
/** Check the path assuming it is in the frame of the model */
bool isPathValidAux(const motion_planning_msgs::KinematicPath &path) const;
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-18 22:01:25 UTC (rev 17313)
@@ -38,6 +38,7 @@
#define PLANNING_ENVIRONMENT_ROBOT_MODELS_
#include <planning_models/kinematic.h>
+#include <planning_models/kinematic_state_params.h>
#include <ros/ros.h>
#include <ros/node.h>
#include <boost/shared_ptr.hpp>
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-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-18 22:01:25 UTC (rev 17313)
@@ -69,7 +69,6 @@
if (includePose_)
{
tf_ = new tf::TransformListener();
- tf_->setExtrapolationLimit(ros::Duration(1.0));
ROS_DEBUG("Maintaining robot pose in frame '%s'", frame_id_.c_str());
}
else
Modified: pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp 2009-06-18 22:01:25 UTC (rev 17313)
@@ -36,6 +36,7 @@
#include "planning_environment/planning_monitor.h"
#include "planning_environment/kinematic_state_constraint_evaluator.h"
+#include <boost/scoped_ptr.hpp>
bool planning_environment::PlanningMonitor::isEnvironmentSafe(void) const
{
@@ -65,8 +66,9 @@
transformConstraintsToFrame(kcPath_, getFrameId());
}
-void planning_environment::PlanningMonitor::transformConstraintsToFrame(motion_planning_msgs::KinematicConstraints &kc, const std::string &target) const
+bool planning_environment::PlanningMonitor::transformConstraintsToFrame(motion_planning_msgs::KinematicConstraints &kc, const std::string &target) const
{
+ bool res = true;
for (unsigned int i = 0; i < kc.pose_constraint.size() ; ++i)
tf_->transformPose(target, kc.pose_constraint[i].pose, kc.pose_constraint[i].pose);
@@ -74,41 +76,44 @@
if (getKinematicModel()->getModelInfo().planarJoints.size() > 0 || getKinematicModel()->getModelInfo().floatingJoints.size() > 0)
{
for (unsigned int i = 0; i < kc.joint_constraint.size() ; ++i)
- transformJoint(kc.joint_constraint[i].joint_name, 0, kc.joint_constraint[i].value, kc.joint_constraint[i].header, target);
+ if (!transformJoint(kc.joint_constraint[i].joint_name, 0, kc.joint_constraint[i].value, kc.joint_constraint[i].header, target))
+ res = false;
}
+ return res;
}
-void planning_environment::PlanningMonitor::transformPathToFrame(motion_planning_msgs::KinematicPath &kp, const std::string &target) const
+bool planning_environment::PlanningMonitor::transformPathToFrame(motion_planning_msgs::KinematicPath &kp, const std::string &target) const
{
// if there are no planar of floating transforms, there is nothing to do
if (getKinematicModel()->getModelInfo().planarJoints.empty() && getKinematicModel()->getModelInfo().floatingJoints.empty())
{
kp.header.frame_id = target;
- return;
+ return true;
}
roslib::Header updatedHeader = kp.header;
-
+ updatedHeader.frame_id = target;
+
// transform start state
- std::vector<planning_models::KinematicModel::Joint*> joints;
- getKinematicModel()->getJoints(joints);
- unsigned int u = 0;
- for (unsigned int i = 0 ; i < joints.size() ; ++i)
- if (joints[i]->usedParams > 0)
- {
- roslib::Header header = kp.header;
- transformJoint(joints[i]->name, u, kp.start_state.vals, header, target);
- updatedHeader = header;
- u += joints[i]->usedParams;
- }
-
-
+ for (unsigned int i = 0 ; i < kp.start_state.size() ; ++i)
+ if (!transformJointToFrame(kp.start_state[i], target))
+ return false;
+
+
// transform the rest of the states
// get the joints this path is for
+ std::vector<planning_models::KinematicModel::Joint*> joints;
joints.resize(kp.names.size());
for (unsigned int j = 0 ; j < joints.size() ; ++j)
+ {
joints[j] = getKinematicModel()->getJoint(kp.names[j]);
+ if (joints[j] == NULL)
+ {
+ ROS_ERROR("Unknown joint '%s' found on path", kp.names[j].c_str());
+ return false;
+ }
+ }
// iterate through the states
for (unsigned int i = 0 ; i < kp.states.size() ; ++i)
@@ -117,24 +122,37 @@
for (unsigned int j = 0 ; j < joints.size() ; ++j)
{
roslib::Header header = kp.header;
- transformJoint(joints[j]->name, u, kp.states[i].vals, header, target);
+ if (!transformJoint(joints[j]->name, u, kp.states[i].vals, header, target))
+ return false;
updatedHeader = header;
u += joints[j]->usedParams;
}
}
kp.header = updatedHeader;
+ return true;
}
-void planning_environment::PlanningMonitor::transformJointToFrame(motion_planning_msgs::KinematicJoint &kj, const std::string &target) const
+bool planning_environment::PlanningMonitor::transformJointToFrame(motion_planning_msgs::KinematicJoint &kj, const std::string &target) const
{
- transformJoint(kj.joint_name, 0, kj.value, kj.header, target);
+ return transformJoint(kj.joint_name, 0, kj.value, kj.header, target);
}
-void planning_environment::PlanningMonitor::transformJoint(const std::string &name, unsigned int index, std::vector<double> ¶ms, roslib::Header &header, const std::string& target) const
+bool planning_environment::PlanningMonitor::transformJoint(const std::string &name, unsigned int index, std::vector<double> ¶ms, roslib::Header &header, const std::string& target) const
{
// planar joints and floating joints may need to be transformed
planning_models::KinematicModel::Joint *joint = getKinematicModel()->getJoint(name);
+ if (joint == NULL)
+ {
+ ROS_ERROR("Unknown joint '%s'", name.c_str());
+ return false;
+ }
+ else
+ if (params.size() < index + joint->usedParams)
+ {
+ ROS_ERROR("Insufficient parameters for joint '%s'", name.c_str());
+ return false;
+ }
if (dynamic_cast<planning_models::KinematicModel::PlanarJoint*>(joint))
{
@@ -174,9 +192,10 @@
}
else
header.frame_id = target;
+ return true;
}
-bool planning_environment::PlanningMonitor::isStateValidOnPath(const planning_models::KinematicModel::StateParams *state) const
+bool planning_environment::PlanningMonitor::isStateValidOnPath(const planning_models::StateParams *state) const
{
getEnvironmentModel()->lock();
getKinematicModel()->lock();
@@ -203,7 +222,7 @@
return valid;
}
-bool planning_environment::PlanningMonitor::isStateValidAtGoal(const planning_models::KinematicModel::StateParams *state) const
+bool planning_environment::PlanningMonitor::isStateValidAtGoal(const planning_models::StateParams *state) const
{
getEnvironmentModel()->lock();
getKinematicModel()->lock();
@@ -238,8 +257,10 @@
if (path.header.frame_id != getFrameId())
{
motion_planning_msgs::KinematicPath pathT = path;
- transformPathToFrame(pathT, getFrameId());
- return isPathValidAux(pathT);
+ if (transformPathToFrame(pathT, getFrameId()))
+ return isPathValidAux(pathT);
+ else
+ return false;
}
else
return isPathValidAux(path);
@@ -247,9 +268,17 @@
bool planning_environment::PlanningMonitor::isPathValidAux(const motion_planning_msgs::KinematicPath &path) const
{
- planning_models::KinematicModel::StateParams *sp = getKinematicModel()->newStateParams();
- sp->setParams(path.start_state.vals);
-
+ boost::scoped_ptr<planning_models::StateParams> sp(getKinematicModel()->newStateParams());
+
+ for (unsigned int i = 0 ; i < path.start_state.size() ; ++i)
+ sp->setParamsJoint(path.start_state[i].value, path.start_state[i].joint_name);
+
+ if (!sp->seenAll())
+ {
+ ROS_WARN("Incomplete start state specification in path");
+ return false;
+ }
+
KinematicConstraintEvaluatorSet ks;
ks.add(getKinematicModel(), kcPath_.joint_constraint);
ks.add(getKinematicModel(), kcPath_.pose_constraint);
@@ -267,7 +296,15 @@
// get the joints this path is for
std::vector<planning_models::KinematicModel::Joint*> joints(path.names.size());
for (unsigned int j = 0 ; j < joints.size() ; ++j)
+ {
joints[j] = getKinematicModel()->getJoint(path.names[j]);
+ if (joints[j] == NULL)
+ {
+ ROS_ERROR("Unknown joint '%s' found on path", path.names[j].c_str());
+ valid = false;
+ break;
+ }
+ }
// check every state
for (unsigned int i = 0 ; valid && i < path.states.size() ; ++i)
@@ -275,6 +312,13 @@
unsigned int u = 0;
for (unsigned int j = 0 ; j < joints.size() ; ++j)
{
+ if (path.states[i].vals.size() < u + joints[j]->usedParams)
+ {
+ ROS_ERROR("Incorrect state specification on path");
+ valid = false;
+ break;
+ }
+
/* copy the parameters that describe the joint */
std::vector<double> params;
for (unsigned int k = 0 ; k < joints[j]->usedParams ; ++k)
@@ -306,6 +350,5 @@
getKinematicModel()->unlock();
getEnvironmentModel()->unlock();
- delete sp;
return valid;
}
Modified: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-06-18 21:54:43 UTC (rev 17312)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-06-18 22:01:25 UTC (rev 17313)
@@ -6,6 +6,7 @@
set(ROS_BUILD_TYPE Debug)
rospack_add_library(planning_models src/kinematic.cpp
+ src/kinematic_state_params.cpp
src/load_mesh.cpp
src/output.cpp)
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06...
[truncated message content] |
|
From: <tf...@us...> - 2009-06-19 01:13:32
|
Revision: 17332
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17332&view=rev
Author: tfoote
Date: 2009-06-19 01:06:32 +0000 (Fri, 19 Jun 2009)
Log Message:
-----------
bye bye libTF. Last dependency gone. I had to copy Pose3D into pr2_mechanism_controllers to do this, but it's not my problem anymore. I'll switch the ticket:832 to reflect the new location.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
Added Paths:
-----------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/Pose3D.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/Pose3D.cpp
Removed Paths:
-------------
pkg/trunk/deprecated/libTF/
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-06-19 01:03:23 UTC (rev 17331)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2009-06-19 01:06:32 UTC (rev 17332)
@@ -31,6 +31,7 @@
src/pr2_odometry.cpp
src/pr2_base_controller.cpp
src/base_kinematics.cpp
+ src/Pose3D.cpp
)
rospack_link_boost(pr2_mechanism_controllers thread)
Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/Pose3D.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/Pose3D.h (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/Pose3D.h 2009-06-19 01:06:32 UTC (rev 17332)
@@ -0,0 +1,355 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef LIBTF_POSE3D_HH
+#define LIBTF_POSE3D_HH
+
+#include <iostream>
+#include <vector>
+#include <newmat10/newmat.h>
+#include <newmat10/newmatio.h>
+#include <pthread.h>
+#include <cmath>
+
+
+namespace libTF
+{
+/** \brief A struct to represent the translational component (a point) */
+struct Position
+{
+ double x,y,z;
+ /** \brief Constructor */
+ Position():x(0),y(0),z(0){;};
+ /** \brief Constructor */
+ Position(double x, double y, double z):x(x),y(y),z(z){;};
+ /** \brief operator overloading for the + operator */
+ Position operator+(const Position &rhs){
+ Position result;
+ result.x = x + rhs.x;
+ result.y = y + rhs.y;
+ result.z = z + rhs.z;
+ return result;
+ }
+
+ /** \brief operator overloading for the += operator */
+ Position & operator+=(const Position &rhs){
+ x += rhs.x;
+ y += rhs.y;
+ z += rhs.z;
+ return *this;
+ }
+
+ /** \brief operator overloading for the - operator */
+ Position operator-(const Position &rhs){
+ Position result;
+ result.x = x - rhs.x;
+ result.y = y - rhs.y;
+ result.z = z - rhs.z;
+ return result;
+ }
+
+ /** \brief operator overloading for the -= operator */
+ Position & operator-=(const Position &rhs){
+ x -= rhs.x;
+ y -= rhs.y;
+ z -= rhs.z;
+ return *this;
+ }
+
+ /** \brief operator overloading for the *= operator */
+ Position & operator*=(double rhs){
+ x *= rhs;
+ y *= rhs;
+ z *= rhs;
+ return *this;
+ }
+
+ /** \brief operator overloading for the * operator */
+ Position operator*(double rhs){
+ Position result;
+ result.x = x*rhs;
+ result.y = y*rhs;
+ result.z = z*rhs;
+ return result;
+ }
+
+ /** \brief Rotate a position about the z-axis */
+ Position rot2D(double angle){
+ Position result;
+ double cosa = cos(angle);
+ double sina = sin(angle);
+ result.x = cosa*x - sina*y;
+ result.y = sina*x + cosa*y;
+ result.z = z;
+ return result;
+
+
+
+ }
+};
+
+/** \brief A struct to represent vectors */
+struct Vector
+{
+ double x,y,z;
+
+ /** \brief Constructor */
+ Vector():x(0),y(0),z(0){;};
+ /** \brief Constructor */
+ Vector(double x, double y, double z):x(x),y(y),z(z){;};
+
+ /** \brief operator overloading for the + operator */
+ Vector operator+(const Vector &rhs){
+ Vector result;
+ result.x = x + rhs.x;
+ result.y = y + rhs.y;
+ result.z = z + rhs.z;
+ return result;
+ }
+
+ /** \brief operator overloading for the += operator */
+ Vector & operator+=(const Vector &rhs){
+ x += rhs.x;
+ y += rhs.y;
+ z += rhs.z;
+ return *this;
+ }
+
+ /** \brief operator overloading for the - operator */
+ Vector operator-(const Vector &rhs){
+ Vector result;
+ result.x = x - rhs.x;
+ result.y = y - rhs.y;
+ result.z = z - rhs.z;
+ return result;
+ }
+
+ /** \brief operator overloading for the -= operator */
+ Vector & operator-=(const Vector &rhs){
+ x -= rhs.x;
+ y -= rhs.y;
+ z -= rhs.z;
+ return *this;
+ }
+
+ /** \brief operator overloading for the *= operator */
+ Vector & operator*=(double rhs){
+ x *= rhs;
+ y *= rhs;
+ z *= rhs;
+ return *this;
+ }
+
+ /** \brief operator overloading for the * operator */
+ Vector operator*(double rhs){
+ Vector result;
+ result.x = x*rhs;
+ result.y = y*rhs;
+ result.z = z*rhs;
+ return result;
+ }
+
+ /** \brief Rotate a vector about the z-axis */
+ Vector rot2D(double angle){
+ Vector result;
+ double cosa = cos(angle);
+ double sina = sin(angle);
+ result.x = cosa*x - sina*y;
+ result.y = sina*x + cosa*y;
+ result.z = z;
+ return result;
+ }
+
+};
+
+/** \brief A struct to represent the quaternion component */
+struct Quaternion
+{
+ double x,y,z,w;
+
+ /** \brief Constructor */
+ Quaternion():x(0),y(0),z(0),w(1){;};
+ /** \brief Constructor */
+ Quaternion(double x, double y, double z, double w):x(x),y(y),z(z),w(w){;};
+
+};
+/** \brief A struct to represent Euler angles */
+struct Euler
+{
+ double yaw, pitch, roll;
+ /** \brief Constructor */
+ Euler():yaw(0),pitch(0),roll(0){;};
+ /** \brief Constructor */
+ Euler(double yaw, double pitch, double roll):yaw(yaw),pitch(pitch),roll(roll){;};
+
+};
+
+/** \brief A class used to store and do basic minipulations of 3D transformations
+ *
+ */
+ class Pose3D
+ {
+ friend class Pose3DCache;
+
+ public:
+
+ /* Constructors */
+ /** \brief Empty Constructor initialize to zero */
+ Pose3D();
+ /** \brief Translation only constructor */
+ Pose3D(double xt, double yt, double zt);
+ /** \brief Quaternion only constructor */
+ Pose3D(double xr, double yt, double zt, double w);
+ /** \brief Translation and Quaternion constructor */
+ Pose3D(Position &pos, Quaternion &quat);
+ /** \brief Translation and Quaternion constructor */
+ Pose3D(double xt, double yt, double zt,
+ double xr, double yr, double zr, double w);
+
+ /** \brief Destructor */
+ virtual ~Pose3D(void)
+ {
+ }
+
+ // Utility functions to normalize and get magnitude.
+ /** \brief Normalize the quaternion */
+ void normalize(void);
+ /** \brief Get the magnitude of the Quaternion
+ * used for normalization */
+ double getMagnitude(void);
+ /** \brief Assignment operator overload*/
+ Pose3D & operator=(const Pose3D & input);
+
+
+ /* Accessors */
+ /** \brief Return the transform as a matrix */
+ NEWMAT::Matrix asMatrix() const;
+ /** \brief Return the inverse of the transform as a matrix */
+ NEWMAT::Matrix getInverseMatrix(void) const;
+ /** \brief Return the rotation as a quaternion */
+ Quaternion getQuaternion(void) const;
+ /** \brief Return the rotation as a quaternion */
+ void getQuaternion(Quaternion &quat) const;
+ /** \brief Return the rotation as a Euler angles */
+ Euler getEuler(void) const;
+ /** \brief Return the rotation as a Euler angles */
+ void getEuler(Euler &eu) const;
+ /** \brief Return the translation as a position */
+ void getPosition(Position &pos) const;
+ /** \brief Return the translation as a position */
+ Position getPosition(void) const;
+ /** \brief Return the rotation as an axis angle pair */
+ void getAxisAngle(double axis[3], double *angle) const;
+
+
+ /** Mutators **/
+ /** \brief Set the values to the identity transform */
+ void setIdentity(void);
+ /** \brief Set the values from a matrix */
+ void setFromMatrix(const NEWMAT::Matrix& matIn);
+ /** \brief Set the values using Euler angles */
+ void setFromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll);
+ /** \brief Set the values using Euler angles */
+ void setFromEuler(Position &pos, Euler &euler);
+ /** \brief Set the values using DH Parameters */
+ void setFromDH(double length, double alpha, double offset, double theta);
+
+
+
+ /** \brief Set the translational components */
+ void setPosition(double x, double y, double z);
+ /** \brief Set the translational components */
+ void setPosition(Position &pos);
+ /** \brief Set the rotational components */
+ void setQuaternion(double x, double y, double z, double w);
+ /** \brief Set the rotational components */
+ void setQuaternion(Quaternion &quat);
+ /** \brief Set the quaterion from an axis-angle representation */
+ void setAxisAngle(double ax, double ay, double az, double angle);
+ /** \brief Set the quaterion from an axis-angle representation */
+ void setAxisAngle(double axis[3], double angle);
+
+ /** \brief Set the translational components */
+ void addPosition(double x, double y, double z);
+ /** \brief Set the translational components */
+ void addPosition(Position &pos);
+ /** \brief Set the rotational components */
+ void multiplyQuaternion(double x, double y, double z, double w);
+ /** \brief Set the rotational components */
+ void multiplyQuaternion(Quaternion &quat);
+
+ /** \brief Apply another pose to the transform contained in the current pose (transform multiplication) */
+ void multiplyPose(Pose3D &pose);
+
+ /** \brief Invert this transfrom */
+ void invert(void);
+
+ /** Application of the transform **/
+ /** \brief Apply the stored transform to a point */
+ void applyToPosition(Position &pos) const;
+ /** \brief Apply the stored transform to a vector of points */
+ void applyToPositions(std::vector<Position*> &posv) const;
+ /** \brief Apply the stored transform to a point */
+ void applyToVector(Vector &pos) const;
+ /** \brief Apply the stored transform to a vector of points */
+ void applyToVectors(std::vector<Vector*> &posv) const;
+
+ /**************** Static Helper Functions ***********************/
+ /** \brief Convert DH Parameters to a Homogeneous Transformation Matrix */
+ static NEWMAT::Matrix matrixFromDH(double length, double alpha, double offset, double theta);
+ /** \brief Convert Euler Angles to a Homogeneous Transformation Matrix */
+ static NEWMAT::Matrix matrixFromEuler(double ax, double ay, double az,
+ double yaw, double pitch, double roll);
+ /** \brief isolate Euler Angles from a homogenous transform matrix */
+ static Euler eulerFromMatrix(const NEWMAT::Matrix & matrix_in, unsigned int solution_number=1);
+ /** \brief isolate translational change from a homogeneous transform matrix */
+ static Position positionFromMatrix(const NEWMAT::Matrix & matrix_in);
+
+ protected:
+
+ /** Internal Data Storage*/
+ double xt, yt, zt, xr, yr, zr, w;
+
+ };
+
+
+ /** \brief A namespace ostream overload for displaying poses */
+ std::ostream & operator<<(std::ostream& mystream, const Pose3D &pose);
+
+ std::ostream & operator<<(std::ostream& mystream, const libTF::Vector &p);
+
+}
+
+
+#endif //LIBTF_POSE3D_HH
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h 2009-06-19 01:03:23 UTC (rev 17331)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h 2009-06-19 01:06:32 UTC (rev 17332)
@@ -46,7 +46,7 @@
#include <pr2_mechanism_controllers/GetBaseCommand.h>
#include <pr2_mechanism_controllers/WheelRadiusMultiplier.h>
-#include <libTF/Pose3D.h>
+#include <pr2_mechanism_controllers/Pose3D.h>
#include <urdf/URDF.h>
#include <newmat10/newmat.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h 2009-06-19 01:03:23 UTC (rev 17331)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h 2009-06-19 01:06:32 UTC (rev 17332)
@@ -48,7 +48,7 @@
#include <pr2_mechanism_controllers/WheelRadiusMultiplier.h>
#include <pr2_mechanism_controllers/OdometryResiduals.h>
-#include <libTF/Pose3D.h>
+#include <pr2_mechanism_controllers/Pose3D.h>
#include <urdf/URDF.h>
#include <newmat10/newmat.h>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h 2009-06-19 01:03:23 UTC (rev 17331)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h 2009-06-19 01:06:32 UTC (rev 17332)
@@ -53,7 +53,6 @@
//Kinematics
#include <robot_kinematics/robot_kinematics.h>
-// #include <libTF/Pose3D.h>
// #include <urdf/URDF.h>
namespace controller
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-06-19 01:03:23 UTC (rev 17331)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-06-19 01:06:32 UTC (rev 17332)
@@ -12,7 +12,6 @@
<depend package="realtime_tools" />
<depend package="robot_mechanism_controllers" />
<depend package="rospy" />
- <depend package="libTF" />
<depend package="std_msgs" />
<depend package="deprecated_msgs" />
<depend package="pr2_msgs" />
@@ -26,6 +25,7 @@
<depend package="control_toolbox" />
<depend package="filters" />
<depend package="manipulation_msgs" />
+ <depend package="newmat10" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/Pose3D.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/Pose3D.cpp (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/Pose3D.cpp 2009-06-19 01:06:32 UTC (rev 17332)
@@ -0,0 +1,585 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "pr2_mechanism_controllers/Pose3D.h"
+#include <cmath>
+#include <cassert>
+
+using namespace libTF;
+
+Pose3D::Pose3D()
+{
+ setIdentity();
+};
+
+
+///Translation only constructor
+Pose3D::Pose3D(double xt, double yt, double zt):
+ xt(xt), yt(yt),zt(zt),
+ xr(0), yr(0), zr(0),w(1)
+{
+};
+/// Quaternion only constructor
+Pose3D::Pose3D(double xr, double yr, double zr, double w):
+ xt(0), yt(0),zt(0),
+ xr(xr), yr(yr), zr(zr),w(w)
+{
+};
+/// Trans and Quat constructor
+Pose3D::Pose3D(double xt, double yt, double zt,
+ double xr, double yr, double zr, double w):
+ xt(xt), yt(yt), zt(zt),
+ xr(xr), yr(yr), zr(zr), w(w)
+{
+};
+
+Pose3D::Pose3D(Position &pos, Quaternion &quat)
+{
+ xt = pos.x; yt = pos.y; zt = pos.z;
+ xr = quat.x;yr = quat.y;zr = quat.z;
+ w = quat.w;
+}
+
+void Pose3D::setIdentity(void)
+{
+ xt = yt = zt = 0.0;
+ xr = yr = zr = 0.0;
+ w = 1.0;
+}
+
+void Pose3D::setAxisAngle(double axis[3], double angle)
+{
+ setAxisAngle(axis[0], axis[1], axis[2], angle);
+}
+
+void Pose3D::setAxisAngle(double ax, double ay, double az, double angle)
+{
+ double h = angle / 2.0;
+ double s = sin(h);
+
+ xr = ax * s;
+ yr = ay * s;
+ zr = az * s;
+ w = cos(h);
+
+ normalize();
+}
+
+void Pose3D::setFromMatrix(const NEWMAT::Matrix& matIn)
+{
+ // math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+
+ // std::cout <<std::endl<<"Matrix in"<<std::endl<<matIn;
+
+
+ const double * mat = matIn.Store();
+ //Get the translations
+ xt = mat[3];
+ yt = mat[7];
+ zt = mat[11];
+
+ //TODO ASSERT others are zero and one as they should be
+
+
+ double T = mat[0] + mat[5] + mat[10];
+
+ // If the trace of the matrix is greater than zero, then
+ // perform an "instant" calculation.
+ // Important note wrt. rouning errors:
+
+ if ( T > 0.00000001 ) //to avoid large distortions!
+ {
+ double S = sqrt(T + 1) * 2;
+ xr = ( mat[9] - mat[6] ) / S;
+ yr = ( mat[2] - mat[8] ) / S;
+ zr = ( mat[4] - mat[1] ) / S;
+ w = 0.25 * S;
+ }
+ //If the trace of the matrix is equal to zero then identify
+ // which major diagonal element has the greatest value.
+ // Depending on this, calculate the following:
+ else if ( mat[0] > mat[5] && mat[0] > mat[10] ) {// Column 0:
+ double S = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
+ xr = 0.25 * S;
+ yr = (mat[4] + mat[1] ) / S;
+ zr = (mat[2] + mat[8] ) / S;
+ w = (mat[9] - mat[6] ) / S;
+ } else if ( mat[5] > mat[10] ) {// Column 1:
+ double S = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
+ xr = (mat[4] + mat[1] ) / S;
+ yr = 0.25 * S;
+ zr = (mat[9] + mat[6] ) / S;
+ w = (mat[2] - mat[8] ) / S;
+ } else {// Column 2:
+ double S = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
+ xr = (mat[2] + mat[8] ) / S;
+ yr = (mat[9] + mat[6] ) / S;
+ zr = 0.25 * S;
+ w = (mat[4] - mat[1] ) / S;
+ }
+
+ // std::cout << "setFromMatrix" << xt <<" "<< yt <<" "<< zt <<" "<< xr <<" "<<yr <<" "<<zr <<" "<<w <<std::endl;
+ //std::cout << "asMatrix" <<std::endl<< asMatrix();
+
+};
+
+void Pose3D::setFromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll)
+{
+ setFromMatrix(Pose3D::matrixFromEuler(_x,_y,_z,_yaw,_pitch,_roll));
+};
+
+void Pose3D::setFromEuler(Position &pos, Euler &euler)
+{
+ setFromEuler(pos.x, pos.y, pos.z, euler.yaw, euler.pitch, euler.roll);
+}
+
+void Pose3D::setFromDH(double length, double alpha, double offset, double theta)
+{
+ setFromMatrix(Pose3D::matrixFromDH(length, alpha, offset, theta));
+};
+
+
+NEWMAT::Matrix Pose3D::matrixFromEuler(double ax,
+ double ay, double az, double yaw,
+ double pitch, double roll)
+{
+ NEWMAT::Matrix matrix(4,4);
+ double ca = cos(yaw);
+ double sa = sin(yaw);
+ double cb = cos(pitch);
+ double sb = sin(pitch);
+ double cg = cos(roll);
+ double sg = sin(roll);
+ double sbsg = sb*sg;
+ double sbcg = sb*cg;
+
+
+ double* matrix_pointer = matrix.Store();
+
+ matrix_pointer[0] = ca*cb;
+ matrix_pointer[1] = (ca*sbsg)-(sa*cg);
+ matrix_pointer[2] = (ca*sbcg)+(sa*sg);
+ matrix_pointer[3] = ax;
+
+ matrix_pointer[4] = sa*cb;
+ matrix_pointer[5] = (sa*sbsg)+(ca*cg);
+ matrix_pointer[6] = (sa*sbcg)-(ca*sg);
+ matrix_pointer[7] = ay;
+
+ matrix_pointer[8] = -sb;
+ matrix_pointer[9] = cb*sg;
+ matrix_pointer[10] = cb*cg;
+ matrix_pointer[11] = az;
+
+ matrix_pointer[12] = 0.0;
+ matrix_pointer[13] = 0.0;
+ matrix_pointer[14] = 0.0;
+ matrix_pointer[15] = 1.0;
+
+ return matrix;
+};
+
+
+// Math from http://en.wikipedia.org/wiki/Robotics_conventions
+NEWMAT::Matrix Pose3D::matrixFromDH(double length,
+ double alpha, double offset, double theta)
+{
+ NEWMAT::Matrix matrix(4,4);
+
+ double ca = cos(alpha);
+ double sa = sin(alpha);
+ double ct = cos(theta);
+ double st = sin(theta);
+
+ double* matrix_pointer = matrix.Store();
+
+ matrix_pointer[0] = ct;
+ matrix_pointer[1] = -st*ca;
+ matrix_pointer[2] = st*sa;
+ matrix_pointer[3] = length * ct;
+ matrix_pointer[4] = st;
+ matrix_pointer[5] = ct*ca;
+ matrix_pointer[6] = -ct*sa;
+ matrix_pointer[7] = length*st;
+ matrix_pointer[8] = 0;
+ matrix_pointer[9] = sa;
+ matrix_pointer[10] = ca;
+ matrix_pointer[11] = offset;
+ matrix_pointer[12] = 0.0;
+ matrix_pointer[13] = 0.0;
+ matrix_pointer[14] = 0.0;
+ matrix_pointer[15] = 1.0;
+
+ return matrix;
+};
+
+
+Pose3D& Pose3D::operator=(const Pose3D & input)
+{
+ xt = input.xt;
+ yt = input.yt;
+ zt = input.zt;
+ xr = input.xr;
+ yr = input.yr;
+ zr = input.zr;
+ w = input.w ;
+
+ return *this;
+};
+
+
+
+Euler Pose3D::eulerFromMatrix(const NEWMAT::Matrix & matrix_in, unsigned int solution_number)
+{
+
+ Euler euler_out;
+ Euler euler_out2; //second solution
+ //get the pointer to the raw data
+ double* matrix_pointer = matrix_in.Store();
+
+ // Check that pitch is not at a singularity
+ if (fabs(matrix_pointer[8]) >= 1)
+ {
+ euler_out.yaw = 0;
+ euler_out2.yaw = 0;
+
+ // From difference of angles formula
+ double delta = atan2(matrix_pointer[1],matrix_pointer[2]);
+ if (matrix_pointer[8] > 0) //gimbal locked up
+ {
+ euler_out.pitch = M_PI / 2.0;
+ euler_out2.pitch = M_PI / 2.0;
+ euler_out.roll = euler_out.pitch + delta;
+ euler_out2.roll = euler_out.pitch + delta;
+ }
+ else // gimbal locked down
+ {
+ euler_out.pitch = -M_PI / 2.0;
+ euler_out2.pitch = -M_PI / 2.0;
+ euler_out.roll = -euler_out.pitch + delta;
+ euler_out2.roll = -euler_out.pitch + delta;
+ }
+ }
+ else
+ {
+ euler_out.pitch = - asin(matrix_pointer[8]);
+ euler_out2.pitch = M_PI - euler_out.pitch;
+
+ euler_out.roll = atan2(matrix_pointer[9]/cos(euler_out.pitch),
+ matrix_pointer[10]/cos(euler_out.pitch));
+ euler_out2.roll = atan2(matrix_pointer[9]/cos(euler_out2.pitch),
+ matrix_pointer[10]/cos(euler_out2.pitch));
+
+ euler_out.yaw = atan2(matrix_pointer[4]/cos(euler_out.pitch),
+ matrix_pointer[0]/cos(euler_out.pitch));
+ euler_out2.yaw = atan2(matrix_pointer[4]/cos(euler_out2.pitch),
+ matrix_pointer[0]/cos(euler_out2.pitch));
+ }
+
+ if (solution_number == 1)
+ return euler_out;
+ else
+ return euler_out2;
+};
+
+Position Pose3D::positionFromMatrix(const NEWMAT::Matrix & matrix_in)
+{
+ Position position;
+ //get the pointer to the raw data
+ double* matrix_pointer = matrix_in.Store();
+ //Pass through translations
+ position.x = matrix_pointer[3];
+ position.y = matrix_pointer[7];
+ position.z = matrix_pointer[11];
+ return position;
+};
+
+void Pose3D::normalize()
+{
+ double mag = getMagnitude();
+ xr /= mag;
+ yr /= mag;
+ zr /= mag;
+ w /= mag;
+};
+
+double Pose3D::getMagnitude()
+{
+ return sqrt(xr*xr + yr*yr + zr*zr + w*w);
+};
+
+NEWMAT::Matrix Pose3D::asMatrix() const
+{
+
+ NEWMAT::Matrix outMat(4,4);
+
+ double * mat = outMat.Store();
+
+ // math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+ double xx = xr * xr;
+ double xy = xr * yr;
+ double xz = xr * zr;
+ double xw = xr * w;
+ double yy = yr * yr;
+ double yz = yr * zr;
+ double yw = yr * w;
+ double zz = zr * zr;
+ double zw = zr * w;
+ mat[0] = 1 - 2 * ( yy + zz );
+ mat[1] = 2 * ( xy - zw );
+ mat[2] = 2 * ( xz + yw );
+ mat[4] = 2 * ( xy + zw );
+ mat[5] = 1 - 2 * ( xx + zz );
+ mat[6] = 2 * ( yz - xw );
+ mat[8] = 2 * ( xz - yw );
+ mat[9] = 2 * ( yz + xw );
+ mat[10] = 1 - 2 * ( xx + yy );
+ mat[12] = mat[13] = mat[14] = 0;
+ mat[3] = xt;
+ mat[7] = yt;
+ mat[11] = zt;
+ mat[15] = 1;
+
+
+ return outMat;
+};
+
+NEWMAT::Matrix Pose3D::getInverseMatrix(void) const
+{
+ return asMatrix().i();
+};
+
+void Pose3D::invert(void) // not very fast :(
+{
+ setFromMatrix(getInverseMatrix());
+}
+
+void Pose3D::getAxisAngle(double axis[3], double *angle) const
+{
+ *angle = 2.0 * acos(w);
+ double d = sqrt(1.0 - w*w);
+ // there could be singularities ....
+ axis[0] = xr / d;
+ axis[1] = yr / d;
+ axis[2] = zr / d;
+}
+
+Euler Pose3D::getEuler(void) const
+{
+ return eulerFromMatrix(asMatrix());
+}
+
+void Pose3D::getEuler(Euler &eu) const
+{
+ eu = eulerFromMatrix(asMatrix());
+}
+
+Quaternion Pose3D::getQuaternion(void) const
+{
+ Quaternion quat;
+ quat.x = xr;
+ quat.y = yr;
+ quat.z = zr;
+ quat.w = w;
+ return quat;
+};
+
+Position Pose3D::getPosition(void) const
+{
+ Position pos;
+ pos.x = xt;
+ pos.y = yt;
+ pos.z = zt;
+ return pos;
+};
+
+void Pose3D::getQuaternion(Quaternion &quat) const
+{
+ quat.x = xr;
+ quat.y = yr;
+ quat.z = zr;
+ quat.w = w;
+}
+
+void Pose3D::getPosition(Position &pos) const
+{
+ pos.x = xt;
+ pos.y = yt;
+ pos.z = zt;
+}
+
+
+void Pose3D::setPosition(double x, double y, double z)
+{
+ xt = x;
+ yt = y;
+ zt = z;
+}
+
+void Pose3D::setPosition(Position &pos)
+{
+ xt = pos.x;
+ yt = pos.y;
+ zt = pos.z;
+}
+
+void Pose3D::setQuaternion(double x, double y, double z, double _w)
+{
+ xr = x;
+ yr = y;
+ zr = z;
+ w = _w;
+}
+
+void Pose3D::setQuaternion(Quaternion &quat)
+{
+ xr = quat.x;
+ yr = quat.y;
+ zr = quat.z;
+ w = quat.w;
+}
+
+void Pose3D::applyToPosition(Position &pos) const
+{
+ double xx = 2.0 * xr * xr, x...
[truncated message content] |
|
From: <is...@us...> - 2009-06-19 03:02:21
|
Revision: 17341
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17341&view=rev
Author: isucan
Date: 2009-06-19 03:02:19 +0000 (Fri, 19 Jun 2009)
Log Message:
-----------
a number of fixes
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_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/motion_planning/planning_models/src/kinematic_state_params.cpp
pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h
pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -77,8 +77,15 @@
ROS_INFO("Starting move_arm for '%s' (IK is %senabled)", arm_.c_str(), perform_ik_ ? "" : "not ");
if (valid_)
- valid_ = collisionModels_->loadedModels() && getControlJointNames(arm_joint_names_);
+ valid_ = collisionModels_->loadedModels();
+ if (valid_)
+ {
+ planningMonitor_->waitForState();
+ planningMonitor_->waitForMap();
+ valid_ = getControlJointNames(arm_joint_names_);
+ }
+
if (!valid_)
ROS_ERROR("Move arm action is invalid");
@@ -138,8 +145,33 @@
ROS_INFO("Converting pose constraint to joint constraint using IK...");
std::vector<double> solution;
- if (computeIK(req.goal_constraints.pose_constraint[0].pose, solution))
+ bool foundIKsolution = false;
+ unsigned int IKsteps = 0;
+ while (IKsteps < 5 && !foundIKsolution)
{
+ if (!computeIK(req.goal_constraints.pose_constraint[0].pose, solution))
+ break;
+
+ // if IK did not fail, check if the state is valid
+ planning_models::StateParams spTest(*planningMonitor_->getRobotState());
+ unsigned int n = 0;
+ for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
+ {
+ unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
+ for (unsigned int j = 0 ; j < u ; ++j)
+ {
+ std::vector<double> params(solution.begin() + n, solution.begin() + n + u);
+ spTest.setParamsJoint(params, arm_joint_names_[i]);
+ }
+ n += u;
+ }
+ if (planningMonitor_->isStateValidAtGoal(&spTest))
+ foundIKsolution = true;
+ IKsteps++;
+ }
+
+ if (foundIKsolution)
+ {
unsigned int n = 0;
for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
{
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -148,6 +148,10 @@
{
std::stringstream ss;
printState(st, ss);
+ ss << " within margins [ ";
+ for (unsigned int j = 0 ; j < rhoStart.size() ; ++j)
+ ss << rhoStart[j] << " ";
+ ss << "]";
m_msg.message("Attempting to fix initial state %s", ss.str().c_str());
State temp(m_stateDimension);
if (searchValidNearby(&temp, st, rhoStart, attempts))
@@ -181,6 +185,10 @@
std::stringstream ss;
printState(st, ss);
+ ss << " within margins [ ";
+ for (unsigned int i = 0 ; i < rhoGoal.size() ; ++i)
+ ss << rhoGoal[i] << " ";
+ ss << "]";
m_msg.message("Attempting to fix goal state %s", ss.str().c_str());
State temp(m_stateDimension);
if (searchValidNearby(&temp, st, rhoGoal, attempts))
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -214,6 +214,7 @@
{
std::vector<planning_models::KinematicModel::Joint*> joints;
psetup->model->kmodel->getJoints(joints);
+ res.path.start_state.resize(joints.size());
for (unsigned int i = 0 ; i < joints.size() ; ++i)
{
res.path.start_state[i].header = res.path.header;
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -105,7 +105,6 @@
ROS_ERROR("Frame '%s' for joint '%s' in starting state is unknown", given[i].header.frame_id.c_str(), given[i].joint_name.c_str());
continue;
}
-
motion_planning_msgs::KinematicJoint kj = given[i];
if (planningMonitor_->transformJointToFrame(kj, planningMonitor_->getFrameId()))
s->setParamsJoint(kj.value, kj.joint_name);
@@ -117,6 +116,7 @@
{
if (planningMonitor_->haveState())
{
+ ROS_INFO("Using the current state to fill in the starting state for the motion plan");
std::vector<planning_models::KinematicModel::Joint*> joints;
planningMonitor_->getKinematicModel()->getJoints(joints);
for (unsigned int i = 0 ; i < joints.size() ; ++i)
@@ -149,6 +149,9 @@
if (startState)
{
+ std::stringstream ss;
+ startState->print(ss);
+ ROS_DEBUG("Complete starting state:\n%s", ss.str().c_str());
st = requestHandler_.computePlan(models_, startState, req, res);
delete startState;
}
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-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-19 03:02:19 UTC (rev 17341)
@@ -121,6 +121,10 @@
/** \brief Return true if a map update has been received in the last sec seconds. If sec < 10us, this function always returns true. */
bool isMapUpdated(double sec) const;
+
+ /** \brief Wait until a map is received */
+ void waitForMap(void) const;
+
/** \brief Return the last update time for the map */
const ros::Time& lastMapUpdate(void) const
{
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -78,6 +78,21 @@
return true;
}
+void planning_environment::CollisionSpaceMonitor::waitForMap(void) const
+{
+ int s = 0;
+ while (nh_.ok() && !haveMap())
+ {
+ if (s == 0)
+ ROS_INFO("Waiting for map ...");
+ s = (s + 1) % 40;
+ ros::spinOnce();
+ ros::Duration().fromSec(0.05).sleep();
+ }
+ if (haveMap())
+ ROS_INFO("Map received!");
+}
+
void planning_environment::CollisionSpaceMonitor::collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap)
{
int n = collisionMap->get_boxes_size();
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-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -168,9 +168,12 @@
void planning_environment::KinematicModelStateMonitor::waitForState(void) const
{
+ int s = 0;
while (nh_.ok() && !haveState())
{
- ROS_INFO("Waiting for mechanism state ...");
+ if (s == 0)
+ ROS_INFO("Waiting for mechanism state ...");
+ s = (s + 1) % 40;
ros::spinOnce();
ros::Duration().fromSec(0.05).sleep();
}
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -191,6 +191,7 @@
if (joint)
{
double *dparams = new double[joint->usedParams];
+ std::copy(params.begin(), params.end(), dparams);
result = setParamsJoint(dparams, name);
delete[] dparams;
}
@@ -223,8 +224,7 @@
bool planning_models::StateParams::setParams(const std::vector<double> ¶ms)
{
double *dparams = new double[m_mi.stateDimension];
- for (unsigned int i = 0 ; i < params.size() ; ++i)
- dparams[i] = params[i];
+ std::copy(params.begin(), params.end(), dparams);
bool result = setParams(dparams);
delete[] dparams;
return result;
@@ -251,8 +251,7 @@
bool planning_models::StateParams::setParamsGroup(const std::vector<double> ¶ms, int groupID)
{
double *dparams = new double[m_owner->getGroupDimension(groupID)];
- for (unsigned int i = 0 ; i < params.size() ; ++i)
- dparams[i] = params[i];
+ std::copy(params.begin(), params.end(), dparams);
bool result = setParamsGroup(dparams, groupID);
delete[] dparams;
return result;
Modified: pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h
===================================================================
--- pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h 2009-06-19 03:02:19 UTC (rev 17341)
@@ -36,8 +36,10 @@
#include <planning_environment/robot_models.h>
#include <collision_space/bodies.h>
#include <tf/transform_listener.h>
+#include <boost/shared_ptr.hpp>
#include <string>
#include <algorithm>
+#include <climits>
namespace filters
{
@@ -51,14 +53,14 @@
{
protected:
-
+
struct SeeLink
{
std::string name;
collision_space::bodies::Body* body;
btTransform constTransf;
};
-
+
struct SortBodies
{
bool operator()(const SeeLink &b1, const SeeLink &b2)
@@ -68,169 +70,331 @@
};
public:
- /** \brief Construct the filter */
- SelfFilter(void) : rm_("robot_description"), tf_(ros::Duration(10))
- {
- tf_.setExtrapolationLimit(ros::Duration().fromSec(10));
- }
+
+ /** \brief Construct the filter */
+ SelfFilter(void) : rm_("robot_description")
+ {
+ myTf_.reset(new tf::TransformListener());
+ tf_ = myTf_.get();
+ }
+ SelfFilter(tf::TransformListener *tf) : rm_("robot_description")
+ {
+ tf_ = tf;
+ }
+
+ /** \brief Destructor to clean up
+ */
+ ~SelfFilter(void)
+ {
+ for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
+ if (bodies_[i].body)
+ delete bodies_[i].body;
+ }
+
+ virtual bool configure(void)
+ {
+ // keep only the points that are outside of the robot
+ // for testing purposes this may be changed to true
+ nh_.param("~invert", invert_, false);
+
+ invert_ = false;
+
+ if (invert_)
+ ROS_INFO("Inverting filter output");
+
+ nh_.param("~accurate_timing", accurate_, true);
+
+ accurate_ = false;
+
+ if (accurate_)
+ ROS_INFO("Using accurate timing");
+ else
+ ROS_INFO("Using simple timing");
- /** \brief Destructor to clean up
- */
- ~SelfFilter(void)
- {
- for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
- if (bodies_[i].body)
- delete bodies_[i].body;
- }
+ std::vector<std::string> links = rm_.getSelfSeeLinks();
+ double scale = rm_.getSelfSeeScale();
+ double padd = rm_.getSelfSeePadding();
+
+ scale = 1.0;
+ padd = 0.0;
+
+ // from the geometric model, find the shape of each link of interest
+ // and create a body from it, one that knows about poses and can
+ // check for point inclusion
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ {
+ SeeLink sl;
+ sl.body = collision_space::bodies::createBodyFromShape(rm_.getKinematicModel()->getLink(links[i])->shape);
+ if (sl.body)
+ {
+ sl.name = links[i];
+
+ // collision models may have an offset, in addition to what TF gives
+ // so we keep it around
+ sl.constTransf = rm_.getKinematicModel()->getLink(links[i])->constGeomTrans;
+ sl.body->setScale(scale);
+ sl.body->setPadding(padd);
+ bodies_.push_back(sl);
+ }
+ else
+ ROS_WARN("Unable to create point inclusion body for link '%s'", links[i].c_str());
+ }
+
+ if (bodies_.empty())
+ ROS_WARN("No robot links will be checked for self collision");
+
+ // put larger volume bodies first -- higher chances of containing a point
+ std::sort(bodies_.begin(), bodies_.end(), SortBodies());
+
+ bspheres_.resize(bodies_.size());
+ bspheresRadius2_.resize(bodies_.size());
+ bodiesAtIdentity_ = false;
+
+ for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
+ ROS_INFO("Self see link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].body->computeVolume());
+
+ return true;
+ }
- virtual bool configure(void)
- {
- // keep only the points that are outside of the robot
- // for testing purposes this may be changed to true
- invert_ = false;
-
- if (invert_)
- ROS_WARN("Inverting filter output");
-
- std::vector<std::string> links = rm_.getSelfSeeLinks();
- double scale = rm_.getSelfSeeScale();
- double padd = rm_.getSelfSeePadding();
+
+ /** \brief Update the filter and return the data seperately
+ * \param data_in T array with length width
+ * \param data_out T array with length width
+ */
+ virtual bool update(const robot_msgs::PointCloud& data_in, robot_msgs::PointCloud& data_out)
+ {
+ if (bodies_.empty())
+ data_out = data_in;
+ else
+ {
+ std::vector<bool> keep(data_in.pts.size());
+
+ if (accurate_)
+ {
+ int chan = -1;
+ for (unsigned int i = 0 ; i < data_in.chan.size() ; ++i)
+ if (data_in.chan[i].name == "stamps")
+ {
+ chan = i;
+ break;
+ }
+ if (chan < 0)
+ {
+ ROS_WARN("'stamps' channel not available; pruning with simple timing");
+ pruneSimple(data_in, keep);
+ }
+ else
+ {
+ ROS_ASSERT(data_in.chan[chan].vals.size() == data_in.pts.size());
+ pruneAccurate(data_in, data_in.chan[chan], keep);
+ }
+ }
+ else
+ pruneSimple(data_in, keep);
+ fillResult(data_in, keep, data_out);
+ }
+ return true;
+ }
- // from the geometric model, find the shape of each link of interest
- // and create a body from it, one that knows about poses and can
- // check for point inclusion
- for (unsigned int i = 0 ; i < links.size() ; ++i)
- {
- SeeLink sl;
- sl.body = collision_space::bodies::createBodyFromShape(rm_.getKinematicModel()->getLink(links[i])->shape);
- if (sl.body)
- {
- sl.name = links[i];
-
- // collision models may have an offset, in addition to what TF gives
- // so we keep it around
- sl.constTransf = rm_.getKinematicModel()->getLink(links[i])->constGeomTrans;
- sl.body->setScale(scale);
- sl.body->setPadding(padd);
- bodies_.push_back(sl);
- }
- else
- ROS_WARN("Unable to create point inclusion body for link '%s'", links[i].c_str());
- }
-
- if (bodies_.empty())
- ROS_WARN("No robot links will be checked for self collision");
-
- // put larger volume bodies first -- higher chances of containing a point
- std::sort(bodies_.begin(), bodies_.end(), SortBodies());
-
- for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
- ROS_INFO("Self see link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].body->computeVolume());
-
- return true;
- }
+ void identityPoses(void)
+ {
+ if (!bodiesAtIdentity_)
+ {
+ // put all links at origin & lookup the needed transforms
+ const unsigned int bs = bodies_.size();
+ for (unsigned int i = 0 ; i < bs ; ++i)
+ bodies_[i].body->setPose(bodies_[i].constTransf);
+ computeBoundingSpheres();
+ bodiesAtIdentity_ = true;
+ }
+ }
+ void computeBoundingSpheres(void)
+ {
+ const unsigned int bs = bodies_.size();
+ for (unsigned int i = 0 ; i < bs ; ++i)
+ {
+ bodies_[i].body->computeBoundingSphere(bspheres_[i]);
+ bspheresRadius2_[i] = bspheres_[i].radius * bspheres_[i].radius;
+ }
+ }
+
+ void pruneAccurate(const robot_msgs::PointCloud& data_in, const robot_msgs::ChannelFloat32& times, std::vector<bool> &keep)
+ {
+ float maxT = *std::max_element(times.vals.begin(), times.vals.end());
+ if (maxT <= FLT_MIN)
+ {
+ ROS_WARN("'stamp' channel is bogus");
+ pruneSimple(data_in, keep);
+ return;
+ }
- /** \brief Update the filter and return the data seperately
- * \param data_in T array with length width
- * \param data_out T array with length width
- */
- virtual bool update(const robot_msgs::PointCloud& data_in, robot_msgs::PointCloud& data_out)
- {
- if (bodies_.empty())
- data_out = data_in;
- else
- {
- const unsigned int bs = bodies_.size();
- const unsigned int np = data_in.pts.size();
- std::vector<collision_space::bodies::BoundingSphere> bspheres(bs);
-
- // place the links in the frame of the pointcloud
- for (unsigned int i = 0 ; i < bs ; ++i)
- {
- // find the transform between the link's frame and the pointcloud frame
- tf::Stamped<btTransform> transf;
- if (tf_.canTransform(data_in.header.frame_id, bodies_[i].name, data_in.header.stamp))
- tf_.lookupTransform(data_in.header.frame_id, bodies_[i].name, data_in.header.stamp, transf);
- else
- {
- transf.setIdentity();
- ROS_ERROR("Unable to lookup transform from %s to %s", bodies_[i].name.c_str(), data_in.header.frame_id.c_str());
- }
-
- // set it for each body; we also include the offset specified in URDF
- bodies_[i].body->setPose(transf * bodies_[i].constTransf);
- bodies_[i].body->computeBoundingSphere(bspheres[i]);
- }
-
- // compute a sphere that bounds the entire robot
- collision_space::bodies::BoundingSphere bound;
- collision_space::bodies::mergeBoundingSpheres(bspheres, bound);
- btScalar radiusSquared = bound.radius * bound.radius;
-
- // we now decide which points we keep
- std::vector<bool> keep(np);
-
-#pragma omp parallel for
- for (int i = 0 ; i < (int)np ; ++i)
- {
- btVector3 pt = btVector3(btScalar(data_in.pts[i].x), btScalar(data_in.pts[i].y), btScalar(data_in.pts[i].z));
- bool out = true;
- if (bound.center.distance2(pt) < radiusSquared)
- for (unsigned int j = 0 ; out && j < bs ; ++j)
- out = !bodies_[j].body->containsPoint(pt);
-
- keep[i] = invert_ ? !out : out;
- }
-
-
- // fill in output data
- data_out.header = data_in.header;
-
- data_out.pts.resize(0);
- data_out.pts.reserve(np);
-
- data_out.chan.resize(data_in.chan.size());
- for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
- {
- ROS_ASSERT(data_in.chan[i].vals.size() == data_in.pts.size());
- data_out.chan[i].name = data_in.chan[i].name;
- data_out.chan[i].vals.reserve(data_in.chan[i].vals.size());
- }
-
- for (unsigned int i = 0 ; i < np ; ++i)
- if (keep[i])
- {
- data_out.pts.push_back(data_in.pts[i]);
- for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
- data_out.chan[j].vals.push_back(data_in.chan[j].vals[i]);
- }
- }
- return true;
- }
+ // put all links at origin
+ identityPoses();
+
+ const unsigned int bs = bodies_.size();
+ const unsigned int np = data_in.pts.size();
+
+ ros::Time timeStart = data_in.header.stamp;
+ ros::Time timeEnd = timeStart + ros::Duration(maxT);
+ std::vector< std::pair< btVector3, btVector3 > > origs(bs);
+ std::vector< std::pair< btQuaternion, btQuaternion > > quats(bs);
+ tf::Stamped<btTransform> transf;
+
+ // lookup the needed transforms
+ for (unsigned int i = 0 ; i < bs ; ++i)
+ {
+ if (tf_->canTransform(data_in.header.frame_id, bodies_[i].name, timeStart))
+ tf_->lookupTransform(data_in.header.frame_id, bodies_[i].name, timeStart, transf);
+ else
+ {
+ transf.setIdentity();
+ ROS_ERROR("Unable to lookup transform from '%s' to '%s' at start time", data_in.header.frame_id.c_str(), bodies_[i].name.c_str());
+ }
+ quats[i].first = transf.getRotation();
+ origs[i].first = transf.getOrigin();
+
+ if (tf_->canTransform(bodies_[i].name, data_in.header.frame_id, timeEnd))
+ tf_->lookupTransform(bodies_[i].name, data_in.header.frame_id, timeEnd, transf);
+ else
+ {
+ transf.setIdentity();
+ ROS_ERROR("Unable to lookup transform from '%s' to '%s' at end time", data_in.header.frame_id.c_str(), bodies_[i].name.c_str());
+ }
+ quats[i].second = transf.getRotation();
+ origs[i].first = transf.getOrigin();
+ }
+
+ // we now decide which points we keep
+#pragma omp parallel for schedule(dynamic)
+ for (int i = 0 ; i < (int)np ; ++i)
+ {
+ // this point is the cloud's frame
+ btVector3 pt = btVector3(btScalar(data_in.pts[i].x), btScalar(data_in.pts[i].y), btScalar(data_in.pts[i].z));
+ float time01 = times.vals[i] / maxT;
+ float time01i = 1.0f - time01;
+
+ bool out = true;
+ for (unsigned int j = 0 ; out && j < bs ; ++j)
+ {
+ // find the transform to bring the point to the link frame
+ btTransform t(quats[j].first.slerp(quats[j].second, time01i),
+ origs[j].first * time01i + origs[j].second * time01);
+
+ out = !bodies_[j].body->containsPoint(t * pt);
+ }
+
+ keep[i] = invert_ ? !out : out;
+ }
+ }
- virtual bool update(const std::vector<robot_msgs::PointCloud> & data_in, std::vector<robot_msgs::PointCloud>& data_out)
- {
- data_out.resize(data_in.size());
- for (unsigned int i = 0 ; i < data_in.size() ; ++i)
- update(data_in[i], data_out[i]);
- return true;
- }
+ void pruneSimple(const robot_msgs::PointCloud& data_in, std::vector<bool> &keep)
+ {
+ const unsigned int bs = bodies_.size();
+ const unsigned int np = data_in.pts.size();
+
+ // mark that links are no longer at origin
+ bodiesAtIdentity_ = false;
+
+ // place the links in the frame of the pointcloud
+ for (unsigned int i = 0 ; i < bs ; ++i)
+ {
+ // find the transform between the link's frame and the pointcloud frame
+ tf::Stamped<btTransform> transf;
+ if (tf_->canTransform(data_in.header.frame_id, bodies_[i].name, data_in.header.stamp))
+ tf_->lookupTransform(data_in.header.frame_id, bodies_[i].name, data_in.header.stamp, transf);
+ else
+ {
+ transf.setIdentity();
+ ROS_ERROR("Unable to lookup transform from %s to %s", bodies_[i].name.c_str(), data_in.header.frame_id.c_str());
+ }
+
+ // set it for each body; we also include the offset specified in URDF
+ bodies_[i].body->setPose(transf * bodies_[i].constTransf);
+ }
+ computeBoundingSpheres();
+
+ // compute a sphere that bounds the entire robot
+ collision_space::bodies::BoundingSphere bound;
+ collision_space::bodies::mergeBoundingSpheres(bspheres_, bound);
+ btScalar radiusSquared = bound.radius * bound.radius;
+
+ // we now decide which points we keep
+#pragma omp parallel for schedule(dynamic)
+ for (int i = 0 ; i < (int)np ; ++i)
+ {
+ btVector3 pt = btVector3(btScalar(data_in.pts[i].x), btScalar(data_in.pts[i].y), btScalar(data_in.pts[i].z));
+ bool out = true;
+ if (bound.center.distance2(pt) < radiusSquared)
+ for (unsigned int j = 0 ; out && j < bs ; ++j)
+ if (bspheres_[j].center.distance2(pt) < bspheresRadius2_[j])
+ out = !bodies_[j].body->containsPoint(pt);
+
+ keep[i] = invert_ ? !out : out;
+ }
+ }
+ void fillResult(const robot_msgs::PointCloud& data_in, const std::vector<bool> &keep, robot_msgs::PointCloud& data_out)
+ {
+ const unsigned int np = data_in.pts.size();
+
+ // fill in output data
+ data_out.header = data_in.header;
+
+ data_out.pts.resize(0);
+ data_out.pts.reserve(np);
+
+ data_out.chan.resize(data_in.chan.size());
+ for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
+ {
+ ROS_ASSERT(data_in.chan[i].vals.size() == data_in.pts.size());
+ data_out.chan[i].name = data_in.chan[i].name;
+ data_out.chan[i].vals.reserve(data_in.chan[i].vals.size());
+ }
+
+ for (unsigned int i = 0 ; i < np ; ++i)
+ if (keep[i])
+ {
+ data_out.pts.push_back(data_in.pts[i]);
+ for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
+ data_out.chan[j].vals.push_back(data_in.chan[j].vals[i]);
+ }
+ }
+
+ virtual bool update(const std::vector<robot_msgs::PointCloud> & data_in, std::vector<robot_msgs::PointCloud>& data_out)
+ {
+ bool result = true;
+ data_out.resize(data_in.size());
+ for (unsigned int i = 0 ; i < data_in.size() ; ++i)
+ if (!update(data_in[i], data_out[i]))
+ result = false;
+ return true;
+ }
+
protected:
-
- planning_environment::RobotModels rm_;
- ros::NodeHandle nh_;
- tf::TransformListener tf_;
- std::vector<SeeLink> bodies_;
- bool invert_;
-
-};
+
+ planning_environment::RobotModels rm_;
+ tf::TransformListener *tf_;
+ ros::NodeHandle nh_;
+
+ std::vector<SeeLink> bodies_;
+ std::vector<double> bspheresRadius2_;
+ std::vector<collision_space::bodies::BoundingSphere> bspheres_;
+ bool bodiesAtIdentity_;
-typedef robot_msgs::PointCloud robot_msgs_PointCloud;
-FILTERS_REGISTER_FILTER(SelfFilter, robot_msgs_PointCloud);
+ bool accurate_;
+ bool invert_;
+
+private:
+ boost::shared_ptr<tf::TransformListener> myTf_;
+
+};
+
+ typedef robot_msgs::PointCloud robot_msgs_PointCloud;
+ FILTERS_REGISTER_FILTER(SelfFilter, robot_msgs_PointCloud);
+
}
#endif //#ifndef FILTERS_SELF_SEE_H_
Modified: pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -41,7 +41,7 @@
{
public:
- SelfFilter(void)
+ SelfFilter(void) : tf_(ros::Duration(10.0))
{
sf_.configure();
pointCloudSubscriber_ = nh_.subscribe("full_cloud", 1, &SelfFilter::cloudCallback, this);
@@ -59,11 +59,12 @@
}
private:
-
+
+ tf::TransformListener tf_;
+ filters::SelfFilter<robot_msgs::PointCloud> sf_;
ros::Publisher pointCloudPublisher_;
ros::Subscriber pointCloudSubscriber_;
- ros::NodeHandle nh_;
- filters::SelfFilter<robot_msgs::PointCloud> sf_;
+ ros::NodeHandle nh_;
};
Modified: pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp 2009-06-19 02:02:25 UTC (rev 17340)
+++ pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp 2009-06-19 03:02:19 UTC (rev 17341)
@@ -83,16 +83,26 @@
in.header.stamp = ros::Time::now();
in.header.frame_id = "base_link";
+ in.chan.resize(1);
+ in.chan[0].name = "stamps";
const unsigned int N = 100000;
in.pts.resize(N);
+ in.chan[0].vals.resize(N);
for (unsigned int i = 0 ; i < N ; ++i)
{
in.pts[i].x = uniform(1);
in.pts[i].y = uniform(1);
in.pts[i].z = uniform(1);
+ in.chan[0].vals[i] = (double)i/(double)N;
}
+ for (unsigned int i = 0 ; i < 1000 ; ++i)
+ {
+ ros::Duration(0.001).sleep();
+ ros::spinOnce();
+ }
+
ros::WallTime tm = ros::WallTime::now();
sf_.update(in, out);
printf("%f points per second\n", (double)N/(ros::WallTime::now() - tm).toSec());
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-06-19 03:22:14
|
Revision: 17338
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17338&view=rev
Author: eitanme
Date: 2009-06-19 01:34:11 +0000 (Fri, 19 Jun 2009)
Log Message:
-----------
Merging the navigation rework branch back into trunk. I've tested in stage, gazebo, and the robot. Hopefully, not too much breaks.
r25624@att (orig r17263): eitanme | 2009-06-17 17:17:08 -0700
Creating a branch for testing the many changes I'm making to the navstack structure
r25625@att (orig r17264): eitanme | 2009-06-17 17:21:24 -0700
Moved navstack to the NodeHandle API. Changed parameter structure a lot. Changed interface for BaseGlobalPlanner to be more generic. Updates everything above the changes accordingly. Tested in stage, needs testing on a robot.
r25666@att (orig r17299): eitanme | 2009-06-18 11:54:43 -0700
Got new navstack working in gazebo... next up the robot
r25671@att (orig r17304): eitanme | 2009-06-18 13:34:46 -0700
Adding param files that were missing
r25675@att (orig r17308): eitanme | 2009-06-18 14:30:14 -0700
Changing persistance for slower tilt
r25676@att (orig r17309): eitanme | 2009-06-18 14:31:59 -0700
Set intensity explicitly
r25695@att (orig r17323): eitanme | 2009-06-18 16:47:47 -0700
Use a globally namespaced node handle for planning goals
r25696@att (orig r17324): eitanme | 2009-06-18 16:50:49 -0700
Changing error message to be correct
r25709@att (orig r17335): eitanme | 2009-06-18 18:16:29 -0700
Moving robot footprint from action to costmap_ros which can then be queried for it
r25711@att (orig r17337): eitanme | 2009-06-18 18:31:33 -0700
Fixing Costmap2DROS upwards dependencies
Modified Paths:
--------------
pkg/trunk/demos/2dnav_erratic/config/costmap_common_params.yaml
pkg/trunk/demos/2dnav_erratic/move_base/base_local_planner_params.yaml
pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch
pkg/trunk/demos/2dnav_erratic/move_base/navfn_params.yaml
pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.launch
pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.launch
pkg/trunk/demos/2dnav_pr2/config/costmap_common_params.yaml
pkg/trunk/demos/2dnav_pr2/move_base/2dnav_pr2.launch
pkg/trunk/demos/2dnav_pr2/move_base/base_local_planner_params.yaml
pkg/trunk/demos/2dnav_pr2/move_base/move_base.xml
pkg/trunk/demos/2dnav_pr2/move_base/navfn_params.yaml
pkg/trunk/demos/2dnav_pr2/move_base_local/base_local_planner_params.yaml
pkg/trunk/demos/2dnav_pr2/move_base_local/move_base_local.xml
pkg/trunk/demos/2dnav_pr2/nav_view/nav_view_move_base.launch
pkg/trunk/demos/2dnav_pr2/nav_view/nav_view_move_base_local.launch
pkg/trunk/demos/2dnav_pr2/rviz/move_base.vcg
pkg/trunk/demos/2dnav_pr2/rviz/move_base_local.vcg
pkg/trunk/demos/2dnav_pr2/rviz/rviz_move_base.launch
pkg/trunk/demos/2dnav_pr2/rviz/rviz_move_base_local.launch
pkg/trunk/demos/milestone2/milestone2_actions/config/navigation.xml
pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
pkg/trunk/highlevel/move_base/CMakeLists.txt
pkg/trunk/highlevel/move_base/include/move_base/move_base.h
pkg/trunk/highlevel/move_base/src/move_base.cpp
pkg/trunk/highlevel/move_base_stage/config/costmap_common_params.yaml
pkg/trunk/highlevel/move_base_stage/move_base/base_local_planner_params.yaml
pkg/trunk/highlevel/move_base_stage/move_base/move_base.launch
pkg/trunk/highlevel/move_base_stage/move_base/move_base_multi_robot.launch
pkg/trunk/highlevel/move_base_stage/move_base/navfn_params.yaml
pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/include/sbpl_door_planner_action/sbpl_door_planner_action.h
pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/src/sbpl_door_planner_action.cpp
pkg/trunk/motion_planning/planning_research/sbpl_planner_node/src/sbpl_planner_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_robot_actions/include/nav_robot_actions/base_global_planner.h
pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_local_planner.h
pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch
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_publisher.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
Added Paths:
-----------
pkg/trunk/demos/2dnav_erratic/move_base/global_costmap_params.yaml
pkg/trunk/demos/2dnav_erratic/move_base/local_costmap_params.yaml
pkg/trunk/demos/2dnav_pr2/move_base/global_costmap_params.yaml
pkg/trunk/demos/2dnav_pr2/move_base/local_costmap_params.yaml
pkg/trunk/demos/2dnav_pr2/move_base_local/costmap_params.yaml
pkg/trunk/highlevel/move_base_stage/move_base/global_costmap_params.yaml
pkg/trunk/highlevel/move_base_stage/move_base/local_costmap_params.yaml
Removed Paths:
-------------
pkg/trunk/demos/2dnav_pr2/config/voxel_costmap_common_params.yaml
pkg/trunk/demos/2dnav_pr2/move_base/base_local_planner_voxel_params.yaml
pkg/trunk/demos/2dnav_pr2/move_base/move_base_voxel.xml
pkg/trunk/demos/2dnav_pr2/move_base_local/move_base_local_voxel.xml
pkg/trunk/highlevel/move_base/include/move_base/move_base_local.h
pkg/trunk/highlevel/move_base/src/move_base_local.cpp
pkg/trunk/highlevel/move_base_stage/move_base_local/
pkg/trunk/highlevel/move_base_stage/move_base_maze/
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/nav_rework:21482
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/milestone2:16602
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/tags/milestone2_tags/milestone2_final:16577
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/nav_rework:21482
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/milestone2:16602
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/nav_rework:17337
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/tags/milestone2_tags/milestone2_final:16577
Modified: pkg/trunk/demos/2dnav_erratic/config/costmap_common_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_erratic/config/costmap_common_params.yaml 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_erratic/config/costmap_common_params.yaml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -1,12 +1,20 @@
-costmap:
- obstacle_range: 2.5
- max_obstacle_height: 2.0
- raytrace_range: 3.0
- inscribed_radius: 0.20
- circumscribed_radius: 0.25
- inflation_radius: 0.25
- cost_scaling_factor: 1.0
- lethal_cost_threshold: 100
- observation_topics: scan
- scan: {sensor_frame: frame_from_message, data_type: LaserScan, expected_update_rate: 0.2,
- observation_persistance: 0.0, marking: true, clearing: true}
+#START VOXEL STUFF
+map_type: voxel
+origin_z: 0.0
+z_resolution: 0.2
+z_voxels: 10
+unknown_threshold: 10
+mark_threshold: 0
+#END VOXEL STUFF
+transform_tolerance: 0.3
+obstacle_range: 2.5
+max_obstacle_height: 2.0
+raytrace_range: 3.0
+#inscribed_radius: 0.325
+circumscribed_radius: 0.46
+inflation_radius: 0.55
+cost_scaling_factor: 10.0
+lethal_cost_threshold: 100
+observation_topics: scan
+scan: {sensor_frame: frame_from_message, data_type: LaserScan, expected_update_rate: 0.2,
+ observation_persistance: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08}
Modified: pkg/trunk/demos/2dnav_erratic/move_base/base_local_planner_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_erratic/move_base/base_local_planner_params.yaml 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_erratic/move_base/base_local_planner_params.yaml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -1,20 +1,11 @@
-base_local_planner:
+footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.325, -0.325]]
+TrajectoryPlannerROS:
#Independent settings for the local costmap
- costmap:
- global_frame: map
- robot_base_frame: base_link
- update_frequency: 5.0
- publish_frequency: 2.0
- static_map: false
- rolling_window: true
- width: 6.0
- height: 6.0
- resolution: 0.025
- origin_x: 0.0
- origin_y: 0.0
- world_model: freespace
+ transform_tolerance: 0.3
+ costmap_visualization_rate: 0.0
+ world_model: costmap
sim_time: 1.7
- sim_granularity: 0.05
+ sim_granularity: 0.025
dwa: true
vx_samples: 3
vtheta_samples: 20
@@ -27,13 +18,13 @@
yaw_goal_tolerance: 0.05
goal_distance_bias: 0.8
path_distance_bias: 0.6
- occdist_scale: 0.05
+ occdist_scale: 0.01
heading_lookahead: 0.325
oscillation_reset_dist: 0.05
acc_limit_th: 3.2
acc_limit_x: 2.5
- acc_limit_y: 0.0
+ acc_limit_y: 2.5
heading_scoring: false
heading_scoring_timestep: 0.8
- holonomic_robot: false
+ holonomic_robot: true
simple_attractor: false
Added: pkg/trunk/demos/2dnav_erratic/move_base/global_costmap_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_erratic/move_base/global_costmap_params.yaml (rev 0)
+++ pkg/trunk/demos/2dnav_erratic/move_base/global_costmap_params.yaml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -0,0 +1,9 @@
+#Independent settings for the planner's costmap
+global_costmap:
+ inscribed_radius: 0.325
+ global_frame: /map
+ robot_base_frame: base_link
+ update_frequency: 5.0
+ publish_frequency: 0.0
+ static_map: true
+ rolling_window: false
Added: pkg/trunk/demos/2dnav_erratic/move_base/local_costmap_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_erratic/move_base/local_costmap_params.yaml (rev 0)
+++ pkg/trunk/demos/2dnav_erratic/move_base/local_costmap_params.yaml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -0,0 +1,13 @@
+local_costmap:
+ inscribed_radius: 0.325
+ global_frame: /map
+ robot_base_frame: base_link
+ update_frequency: 5.0
+ publish_frequency: 2.0
+ static_map: false
+ rolling_window: true
+ width: 6.0
+ height: 6.0
+ resolution: 0.025
+ origin_x: 0.0
+ origin_y: 0.0
Modified: pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch
===================================================================
--- pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch 2009-06-19 01:34:11 UTC (rev 17338)
@@ -1,16 +1,15 @@
<launch>
<master auto="start"/>
- <group name="wg">
- <node pkg="move_base" 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" />
- <remap from="~base_local_planner/local_plan" to="/local_path" />
- <remap from="~base_local_planner/robot_footprint" to="/robot_footprint" />
+ <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
- <rosparam file="$(find 2dnav_erratic)/config/costmap_common_params.yaml" command="load" ns="navfn" />
- <rosparam file="$(find 2dnav_erratic)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
- <rosparam file="$(find 2dnav_erratic)/move_base/navfn_params.yaml" command="load" />
- <rosparam file="$(find 2dnav_erratic)/move_base/base_local_planner_params.yaml" command="load" />
- </node>
- </group>
+ <param name="footprint_padding" value="0.01" />
+
+ <rosparam file="$(find 2dnav_erratic)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
+ <rosparam file="$(find 2dnav_erratic)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
+ <rosparam file="$(find 2dnav_erratic)/move_base/local_costmap_params.yaml" command="load" />
+ <rosparam file="$(find 2dnav_erratic)/move_base/global_costmap_params.yaml" command="load" />
+ <rosparam file="$(find 2dnav_erratic)/move_base/navfn_params.yaml" command="load" />
+ <rosparam file="$(find 2dnav_erratic)/move_base/base_local_planner_params.yaml" command="load" />
+
+ </node>
</launch>
Modified: pkg/trunk/demos/2dnav_erratic/move_base/navfn_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_erratic/move_base/navfn_params.yaml 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_erratic/move_base/navfn_params.yaml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -1,9 +1,2 @@
-navfn:
- #Independent settings for the planner's costmap
- costmap:
- global_frame: map
- robot_base_frame: base_link
- update_frequency: 5.0
- publish_frequency: 0.0
- static_map: true
- rolling_window: false
+NavfnROS:
+ transform_tolerance: 0.3
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.launch
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.launch 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.launch 2009-06-19 01:34:11 UTC (rev 17338)
@@ -8,5 +8,5 @@
<!--include file="$(find 2dnav_pr2)/config/map_server.launch" /-->
<include file="$(find 2dnav_pr2)/config/ground_plane.xml" />
<!-- The naviagtion stack and associated parameters -->
- <include file="$(find 2dnav_pr2)/move_base/move_base_voxel.xml" />
+ <include file="$(find 2dnav_pr2)/move_base/move_base.xml" />
</launch>
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.launch
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.launch 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.launch 2009-06-19 01:34:11 UTC (rev 17338)
@@ -8,5 +8,5 @@
<!--include file="$(find 2dnav_pr2)/config/map_server.launch" /-->
<include file="$(find 2dnav_pr2)/config/ground_plane.xml" />
<!-- The navigation stack and associated parameters -->
- <include file="$(find 2dnav_pr2)/move_base/move_base_voxel.xml" />
+ <include file="$(find 2dnav_pr2)/move_base/move_base.xml" />
</launch>
Modified: pkg/trunk/demos/2dnav_pr2/config/costmap_common_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/config/costmap_common_params.yaml 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_pr2/config/costmap_common_params.yaml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -1,21 +1,32 @@
-costmap:
- map_type: costmap
- transform_tolerance: 0.2
- obstacle_range: 2.5
- max_obstacle_height: 2.0
- raytrace_range: 3.0
- circumscribed_radius: 0.46
- inflation_radius: 0.55
- cost_scaling_factor: 10.0
- lethal_cost_threshold: 100
+#BEGIN VOXEL STUFF
+map_type: voxel
+origin_z: 0.0
+z_voxels: 6
+z_resolution: 0.23
+unknown_threshold: 7
+mark_threshold: 0
+#END VOXEL STUFF
+transform_tolerance: 0.2
+obstacle_range: 2.5
+raytrace_range: 3.0
+# inscribed_radius: 0.325
+circumscribed_radius: 0.46
+inflation_radius: 0.55
+cost_scaling_factor: 10.0
+lethal_cost_threshold: 100
- observation_topics: base_scan_marking base_scan ground_object_cloud
+# BEGIN VOXEL STUFF
+observation_topics: base_scan_marking base_scan tilt_scan ground_object_cloud
- base_scan_marking: {sensor_frame: base_laser, data_type: PointCloud, expected_update_rate: 0.2,
- observation_persistance: 0.0, marking: true, clearing: false, min_obstacle_height: 0.08, max_obstacle_height: 2.0}
+base_scan_marking: {sensor_frame: base_laser, data_type: PointCloud, expected_update_rate: 0.2,
+ observation_persistance: 0.0, marking: true, clearing: false, min_obstacle_height: 0.08, max_obstacle_height: 2.0}
- base_scan: {sensor_frame: base_laser, data_type: LaserScan, expected_update_rate: 0.2,
- observation_persistance: 0.0, marking: false, clearing: true, min_obstacle_height: -0.10, max_obstacle_height: 2.0}
+base_scan: {sensor_frame: base_laser, data_type: LaserScan, expected_update_rate: 0.2,
+ observation_persistance: 0.0, marking: false, clearing: true, min_obstacle_height: -0.10, max_obstacle_height: 2.0}
- ground_object_cloud: {sensor_frame: laser_tilt_link, data_type: PointCloud, expected_update_rate: 0.2,
- observation_persistance: 2.0, marking: true, clearing: false, min_obstacle_height: -0.10, max_obstacle_height: 2.0}
+tilt_scan: {sensor_frame: laser_tilt_link, data_type: LaserScan, expected_update_rate: 0.2,
+ observation_persistance: 0.2, marking: false, clearing: true, min_obstacle_height: -20.00, max_obstacle_height: 40.0}
+
+ground_object_cloud: {sensor_frame: laser_tilt_link, data_type: PointCloud, expected_update_rate: 0.2,
+ observation_persistance: 3.0, marking: true, clearing: false, min_obstacle_height: -0.10, max_obstacle_height: 2.0}
+# END VOXEL STUFF
Deleted: pkg/trunk/demos/2dnav_pr2/config/voxel_costmap_common_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/config/voxel_costmap_common_params.yaml 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_pr2/config/voxel_costmap_common_params.yaml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -1,33 +0,0 @@
-costmap:
- #BEGIN VOXEL STUFF
- map_type: voxel
- origin_z: 0.0
- z_voxels: 6
- z_resolution: 0.23
- unknown_threshold: 7
- mark_threshold: 0
- #END VOXEL STUFF
- transform_tolerance: 0.2
- obstacle_range: 2.5
- raytrace_range: 3.0
-# inscribed_radius: 0.325
- circumscribed_radius: 0.46
- inflation_radius: 0.55
- cost_scaling_factor: 10.0
- lethal_cost_threshold: 100
-
-# BEGIN VOXEL STUFF
- observation_topics: base_scan_marking base_scan tilt_scan ground_object_cloud
-
- base_scan_marking: {sensor_frame: base_laser, data_type: PointCloud, expected_update_rate: 0.2,
- observation_persistance: 0.0, marking: true, clearing: false, min_obstacle_height: 0.08, max_obstacle_height: 2.0}
-
- base_scan: {sensor_frame: base_laser, data_type: LaserScan, expected_update_rate: 0.2,
- observation_persistance: 0.0, marking: false, clearing: true, min_obstacle_height: -0.10, max_obstacle_height: 2.0}
-
- tilt_scan: {sensor_frame: laser_tilt_link, data_type: LaserScan, expected_update_rate: 0.2,
- observation_persistance: 0.2, marking: false, clearing: true, min_obstacle_height: -20.00, max_obstacle_height: 40.0}
-
- ground_object_cloud: {sensor_frame: laser_tilt_link, data_type: PointCloud, expected_update_rate: 0.2,
- observation_persistance: 2.5, marking: true, clearing: false, min_obstacle_height: -0.10, max_obstacle_height: 2.0}
-# END VOXEL STUFF
Modified: pkg/trunk/demos/2dnav_pr2/move_base/2dnav_pr2.launch
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base/2dnav_pr2.launch 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_pr2/move_base/2dnav_pr2.launch 2009-06-19 01:34:11 UTC (rev 17338)
@@ -8,6 +8,6 @@
<include file="$(find 2dnav_pr2)/config/ground_plane.xml" />
<!-- The naviagtion stack and associated parameters -->
- <include file="$(find 2dnav_pr2)/move_base/move_base_voxel.xml" />
+ <include file="$(find 2dnav_pr2)/move_base/move_base.xml" />
</group>
</launch>
Modified: pkg/trunk/demos/2dnav_pr2/move_base/base_local_planner_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base/base_local_planner_params.yaml 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_pr2/move_base/base_local_planner_params.yaml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -1,19 +1,4 @@
-base_local_planner:
- #Independent settings for the local costmap
- costmap:
- inscribed_radius: 0.325
- publish_voxel_map: true
- global_frame: odom_combined
- robot_base_frame: base_link
- update_frequency: 5.0
- publish_frequency: 2.0
- static_map: false
- rolling_window: true
- width: 6.0
- height: 6.0
- resolution: 0.025
- origin_x: 0.0
- origin_y: 0.0
+TrajectoryPlannerROS:
transform_tolerance: 0.2
costmap_visualization_rate: 0.0
world_model: costmap
@@ -27,8 +12,8 @@
max_vel_th: 1.0
min_vel_th: -1.0
min_in_place_vel_th: 0.4
- xy_goal_tolerance: 0.1
- yaw_goal_tolerance: 0.05
+ xy_goal_tolerance: 0.2
+ yaw_goal_tolerance: 0.17
goal_distance_bias: 0.8
path_distance_bias: 0.6
occdist_scale: 0.01
Deleted: pkg/trunk/demos/2dnav_pr2/move_base/base_local_planner_voxel_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base/base_local_planner_voxel_params.yaml 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_pr2/move_base/base_local_planner_voxel_params.yaml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -1,45 +0,0 @@
-base_local_planner:
- #Independent settings for the local costmap
- costmap:
- inscribed_radius: 0.325
- publish_voxel_map: true
- global_frame: odom_combined
- robot_base_frame: base_link
- update_frequency: 5.0
- publish_frequency: 2.0
- static_map: false
- rolling_window: true
- width: 6.0
- height: 6.0
- resolution: 0.025
- origin_x: 0.0
- origin_y: 0.0
- transform_tolerance: 0.2
- costmap_visualization_rate: 0.0
- world_model: costmap
- sim_time: 1.7
- sim_granularity: 0.025
- dwa: true
- vx_samples: 3
- vtheta_samples: 20
- max_vel_x: 0.45
- min_vel_x: 0.1
- max_vel_th: 1.0
- min_vel_th: -1.0
- min_in_place_vel_th: 0.4
- xy_goal_tolerance: 0.2
- yaw_goal_tolerance: 0.17
- goal_distance_bias: 0.8
- path_distance_bias: 0.6
- occdist_scale: 0.01
- heading_lookahead: 0.325
- oscillation_reset_dist: 0.05
- escape_reset_dist: 0.15
- escape_reset_theta: 0.30
- acc_limit_th: 3.2
- acc_limit_x: 2.5
- acc_limit_y: 2.5
- heading_scoring: false
- heading_scoring_timestep: 0.8
- holonomic_robot: true
- simple_attractor: false
Added: pkg/trunk/demos/2dnav_pr2/move_base/global_costmap_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base/global_costmap_params.yaml (rev 0)
+++ pkg/trunk/demos/2dnav_pr2/move_base/global_costmap_params.yaml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -0,0 +1,9 @@
+#Independent settings for the planner's costmap
+global_costmap:
+ inscribed_radius: 0.325
+ global_frame: /map
+ robot_base_frame: base_link
+ update_frequency: 5.0
+ publish_frequency: 0.0
+ static_map: true
+ rolling_window: false
Added: pkg/trunk/demos/2dnav_pr2/move_base/local_costmap_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base/local_costmap_params.yaml (rev 0)
+++ pkg/trunk/demos/2dnav_pr2/move_base/local_costmap_params.yaml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -0,0 +1,15 @@
+local_costmap:
+ #Independent settings for the local costmap
+ inscribed_radius: 0.325
+ publish_voxel_map: true
+ global_frame: odom_combined
+ robot_base_frame: base_link
+ update_frequency: 5.0
+ publish_frequency: 2.0
+ static_map: false
+ rolling_window: true
+ width: 6.0
+ height: 6.0
+ resolution: 0.025
+ origin_x: 0.0
+ origin_y: 0.0
Modified: pkg/trunk/demos/2dnav_pr2/move_base/move_base.xml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base/move_base.xml 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_pr2/move_base/move_base.xml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -1,6 +1,7 @@
<launch>
<include file="$(find pr2_alpha)/$(env ROBOT).machine" />
+<node pkg="mux" type="throttle" args="3.0 /move_base/local_costmap/voxel_grid /move_base/local_costmap/voxel_grid_throttled" />
<node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen" machine="three">
<!--
<remap from="cmd_vel" to="/bs" />
@@ -10,8 +11,10 @@
<param name="controller_patience" value="15.0" />
<param name="clearing_radius" value="0.59" />
- <rosparam file="$(find 2dnav_pr2)/config/costmap_common_params.yaml" command="load" ns="navfn" />
- <rosparam file="$(find 2dnav_pr2)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
+ <rosparam file="$(find 2dnav_pr2)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
+ <rosparam file="$(find 2dnav_pr2)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
+ <rosparam file="$(find 2dnav_pr2)/move_base/local_costmap_params.yaml" command="load" />
+ <rosparam file="$(find 2dnav_pr2)/move_base/global_costmap_params.yaml" command="load" />
<rosparam file="$(find 2dnav_pr2)/move_base/navfn_params.yaml" command="load" />
<rosparam file="$(find 2dnav_pr2)/move_base/base_local_planner_params.yaml" command="load" />
</node>
Deleted: pkg/trunk/demos/2dnav_pr2/move_base/move_base_voxel.xml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base/move_base_voxel.xml 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_pr2/move_base/move_base_voxel.xml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -1,20 +0,0 @@
-<launch>
-<include file="$(find pr2_alpha)/$(env ROBOT).machine" />
-
-<node pkg="mux" type="throttle" args="3.0 /move_base/base_local_planner/costmap/voxel_grid /move_base/base_local_planner/costmap/voxel_grid_throttled" />
-<node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen" machine="three">
- <!--
- <remap from="cmd_vel" to="/bs" />
- -->
- <param name="controller_frequency" value="10.0" />
- <param name="controller_patience" value="15.0" />
- <param name="planner_patience" value="5.0" />
- <param name="footprint_padding" value="0.015" />
- <param name="clearing_radius" value="0.59" />
-
- <rosparam file="$(find 2dnav_pr2)/config/voxel_costmap_common_params.yaml" command="load" ns="navfn" />
- <rosparam file="$(find 2dnav_pr2)/config/voxel_costmap_common_params.yaml" command="load" ns="base_local_planner" />
- <rosparam file="$(find 2dnav_pr2)/move_base/navfn_params.yaml" command="load" />
- <rosparam file="$(find 2dnav_pr2)/move_base/base_local_planner_voxel_params.yaml" command="load" />
-</node>
-</launch>
Modified: pkg/trunk/demos/2dnav_pr2/move_base/navfn_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base/navfn_params.yaml 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_pr2/move_base/navfn_params.yaml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -1,12 +1,2 @@
navfn:
transform_tolerance: 0.2
- #Independent settings for the planner's costmap
- costmap:
-#inscribed_radius: 0.387
- inscribed_radius: 0.325
- global_frame: /map
- robot_base_frame: base_link
- update_frequency: 5.0
- publish_frequency: 0.0
- static_map: true
- rolling_window: false
Modified: pkg/trunk/demos/2dnav_pr2/move_base_local/base_local_planner_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base_local/base_local_planner_params.yaml 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/trunk/demos/2dnav_pr2/move_base_local/base_local_planner_params.yaml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -1,18 +1,6 @@
-base_local_planner:
- costmap:
- inscribed_radius: 0.325
- publish_voxel_map: true
- global_frame: odom_combined
- robot_base_frame: base_link
- update_frequency: 5.0
- publish_frequency: 2.0
- static_map: false
- rolling_window: true
- width: 10.0
- height: 10.0
- resolution: 0.025
- origin_x: 0.0
- origin_y: 0.0
+footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.325, -0.325]]
+shutdown_costmaps: true
+TrajectoryPlannerROS:
transform_tolerance: 0.2
costmap_visualization_rate: 0.0
world_model: costmap
Added: pkg/trunk/demos/2dnav_pr2/move_base_local/costmap_params.yaml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base_local/costmap_params.yaml (rev 0)
+++ pkg/trunk/demos/2dnav_pr2/move_base_local/costmap_params.yaml 2009-06-19 01:34:11 UTC (rev 17338)
@@ -0,0 +1,13 @@
+inscribed_radius: 0.325
+publish_voxel_map: true
+global_frame: odom_combined
+robot_base_frame: base_link
+update_frequency: 5.0
+publish_frequency: 2.0
+static_map: false
+rolling_window: true
+width: 10.0
+height: 10.0
+resolution: 0.025
+origin_x: 0.0
+origin_y: 0.0
Modified: pkg/trunk/demos/2dnav_pr2/move_base_local/move_base_local.xml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base_local/move_base_local.xml 2009-06-19 01:31:33 UTC (rev 17337)
+++ pkg/tr...
[truncated message content] |
|
From: <is...@us...> - 2009-06-19 19:22:52
|
Revision: 17367
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17367&view=rev
Author: isucan
Date: 2009-06-19 19:22:50 +0000 (Fri, 19 Jun 2009)
Log Message:
-----------
broadcasting single line pointclouds
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch
pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h
pkg/trunk/util/new_robot_self_filter/self_filter.launch
pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
pkg/trunk/util/new_robot_self_filter/src/self_mask.cpp
Modified: pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch 2009-06-19 18:56:21 UTC (rev 17366)
+++ pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch 2009-06-19 19:22:50 UTC (rev 17367)
@@ -1,6 +1,7 @@
<launch>
<include file="$(find pr2_defs)/pr2_planning_environment.launch" />
+ <include file="$(find pr2_laser_tilt)/laser_tilt_controller.launch" />
<node pkg="point_cloud_assembler" type="laser_scan_assembler_srv" output="screen" name="laser_scan_assembler">
<remap from="scan_in" to="tilt_scan"/>
@@ -18,10 +19,26 @@
<remap from="full_cloud" to="snapshot_cloud" />
</node>
- <include file="$(find pr2_laser_tilt)/laser_tilt_controller.launch" />
+ <node pkg="new_robot_self_filter" type="self_filter" respawn="true" output="screen">
+ <remap from="robot_description" to="robotdesc/pr2" />
+ <remap from="cloud_in" to="snapshot_cloud" />
+ <remap from="cloud_out" to="snapshot_cloud_filtered" />
+ </node>
- <node pkg="new_robot_self_filter" type="self_filter" args="robot_description:=robotdesc/pr2 full_cloud:=snapshot_cloud" respawn="true" output="screen" />
+ <node pkg="laser_scan" type="scan_to_cloud" output="screen">
+ <param name="scan_topic" type="string" value="tilt_scan" />
+ <param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
+ <param name="high_fidelity" type="bool" value="true" />
+ <param name="cloud_topic" type="string" value="tilt_scan_cloud" />
+ </node>
+
+ <node pkg="new_robot_self_filter" type="self_filter" respawn="true" output="screen">
+ <remap from="robot_description" to="robotdesc/pr2" />
+ <remap from="cloud_in" to="tilt_scan_cloud" />
+ <remap from="cloud_out" to="tilt_scan_cloud_filtered" />
+ </node>
+
<include file="$(find collision_map)/collision_map.launch" />
-
+
</launch>
Modified: pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h
===================================================================
--- pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h 2009-06-19 18:56:21 UTC (rev 17366)
+++ pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h 2009-06-19 19:22:50 UTC (rev 17367)
@@ -68,7 +68,7 @@
ROS_INFO("Inverting filter output");
bool accurate;
- nh_.param("~accurate_timing", accurate, true);
+ nh_.param("~accurate_timing", accurate, false);
if (accurate)
ROS_INFO("Using accurate timing");
Modified: pkg/trunk/util/new_robot_self_filter/self_filter.launch
===================================================================
--- pkg/trunk/util/new_robot_self_filter/self_filter.launch 2009-06-19 18:56:21 UTC (rev 17366)
+++ pkg/trunk/util/new_robot_self_filter/self_filter.launch 2009-06-19 19:22:50 UTC (rev 17367)
@@ -1,4 +1,8 @@
<launch>
<include file="$(find pr2_defs)/pr2_planning_environment.launch" />
- <node pkg="new_robot_self_filter" type="self_filter" args="robot_description:=robotdesc/pr2 full_cloud:=snapshot_cloud" respawn="true" output="screen" />
+ <node pkg="new_robot_self_filter" type="self_filter" respawn="true" output="screen">
+ <remap from="robot_description" to="robotdesc/pr2" />
+ <remap from="cloud_in" to="tilt_scan_cloud" />
+ <remap from="cloud_out" to="tilt_scan_cloud_filtered" />
+ </node>
</launch>
Modified: pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp 2009-06-19 18:56:21 UTC (rev 17366)
+++ pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp 2009-06-19 19:22:50 UTC (rev 17367)
@@ -41,13 +41,15 @@
{
public:
- SelfFilter(void) : tf_(ros::Duration(10.0))
+ SelfFilter(void)
{
sf_.configure();
- pointCloudSubscriber_ = nh_.subscribe("full_cloud", 1, &SelfFilter::cloudCallback, this);
- pointCloudPublisher_ = nh_.advertise<robot_msgs::PointCloud>("full_cloud_filtered", 1);
+ pointCloudSubscriber_ = nh_.subscribe("cloud_in", 1, &SelfFilter::cloudCallback, this);
+ pointCloudPublisher_ = nh_.advertise<robot_msgs::PointCloud>("cloud_out", 1);
}
+private:
+
void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
{
robot_msgs::PointCloud out;
@@ -58,9 +60,6 @@
pointCloudPublisher_.publish(out);
}
-private:
-
- tf::TransformListener tf_;
filters::SelfFilter<robot_msgs::PointCloud> sf_;
ros::Publisher pointCloudPublisher_;
ros::Subscriber pointCloudSubscriber_;
@@ -70,13 +69,10 @@
int main(int argc, char **argv)
{
- ros::init(argc, argv, "self_filter");
+ ros::init(argc, argv, "self_filter", ros::init_options::AnonymousName);
SelfFilter s;
ros::spin();
return 0;
}
-
-
-
Modified: pkg/trunk/util/new_robot_self_filter/src/self_mask.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/src/self_mask.cpp 2009-06-19 18:56:21 UTC (rev 17366)
+++ pkg/trunk/util/new_robot_self_filter/src/self_mask.cpp 2009-06-19 19:22:50 UTC (rev 17367)
@@ -139,7 +139,7 @@
float maxT = *std::max_element(times.vals.begin(), times.vals.end());
if (maxT <= FLT_MIN)
{
- ROS_WARN("'stamp' channel contains invalid data");
+ ROS_WARN("'stamps' channel contains invalid data");
maskSimple(data_in, mask);
return;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-06-19 22:27:31
|
Revision: 17380
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17380&view=rev
Author: isucan
Date: 2009-06-19 22:27:26 +0000 (Fri, 19 Jun 2009)
Log Message:
-----------
changed pointcloud processing to remove self filter on a line-by-line basis
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch
pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
Modified: pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch 2009-06-19 22:24:01 UTC (rev 17379)
+++ pkg/trunk/demos/tabletop_manipulation/launch/laser-perception.launch 2009-06-19 22:27:26 UTC (rev 17380)
@@ -3,29 +3,6 @@
<include file="$(find pr2_defs)/pr2_planning_environment.launch" />
<include file="$(find pr2_laser_tilt)/laser_tilt_controller.launch" />
- <node pkg="point_cloud_assembler" type="laser_scan_assembler_srv" output="screen" name="laser_scan_assembler">
- <remap from="scan_in" to="tilt_scan"/>
- <param name="tf_cache_time_secs" type="double" value="10.0" />
- <param name="tf_tolerance_secs" type="double" value="0.0" />
- <param name="max_scans" type="int" value="400" />
- <param name="ignore_laser_skew" type="bool" value="true" />
- <param name="fixed_frame" type="string" value="torso_lift_link" />
- <param name="downsample_factor" type="int" value="2" />
- </node>
-
- <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
- <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 pkg="new_robot_self_filter" type="self_filter" respawn="true" output="screen">
- <remap from="robot_description" to="robotdesc/pr2" />
- <remap from="cloud_in" to="snapshot_cloud" />
- <remap from="cloud_out" to="snapshot_cloud_filtered" />
- </node>
-
-
<node pkg="laser_scan" type="scan_to_cloud" output="screen">
<param name="scan_topic" type="string" value="tilt_scan" />
<param name="scan_filters" textfile="$(find laser_scan)/default_scan_shadows.xml" />
@@ -39,6 +16,22 @@
<remap from="cloud_out" to="tilt_scan_cloud_filtered" />
</node>
+ <node pkg="point_cloud_assembler" type="point_cloud_assembler_srv" output="screen" name="point_cloud_assembler">
+ <remap from="scan_in" to="tilt_scan_cloud_filtered"/>
+ <param name="tf_cache_time_secs" type="double" value="10.0" />
+ <param name="tf_tolerance_secs" type="double" value="0.0" />
+ <param name="max_scans" type="int" value="400" />
+ <param name="ignore_laser_skew" type="bool" value="true" />
+ <param name="fixed_frame" type="string" value="torso_lift_link" />
+ <param name="downsample_factor" type="int" value="2" />
+ </node>
+
+ <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
+ <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
+ <remap from="build_cloud" to="point_cloud_assembler/build_cloud" />
+ <remap from="full_cloud" to="snapshot_cloud" />
+ </node>
+
<include file="$(find collision_map)/collision_map.launch" />
</launch>
Modified: pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp 2009-06-19 22:24:01 UTC (rev 17379)
+++ pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp 2009-06-19 22:27:26 UTC (rev 17380)
@@ -56,7 +56,7 @@
ros::WallTime tm = ros::WallTime::now();
sf_.update(*cloud, out);
double sec = (ros::WallTime::now() - tm).toSec();
- ROS_INFO("Self filter: reduced %d points to %d points in %f seconds", (int)cloud->pts.size(), (int)out.pts.size(), sec);
+ ROS_DEBUG("Self filter: reduced %d points to %d points in %f seconds", (int)cloud->pts.size(), (int)out.pts.size(), sec);
pointCloudPublisher_.publish(out);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-06-20 04:07:01
|
Revision: 17407
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17407&view=rev
Author: isucan
Date: 2009-06-20 04:06:59 +0000 (Sat, 20 Jun 2009)
Log Message:
-----------
utility functions
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h 2009-06-20 04:03:14 UTC (rev 17406)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h 2009-06-20 04:06:59 UTC (rev 17407)
@@ -63,6 +63,13 @@
bool operator==(const StateParams &rhs) const;
+ /** \brief Construct a default state: each value at 0.0, if
+ within bounds. Otherwise, select middle point. */
+ void defaultState(void);
+
+ /** \brief Construct a random state */
+ void randomState(void);
+
/** \brief Mark all values as unseen */
void reset(void);
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-06-20 04:03:14 UTC (rev 17406)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-06-20 04:06:59 UTC (rev 17407)
@@ -64,15 +64,9 @@
void planning_models::KinematicModel::defaultState(void)
{
- /* The default state of the robot. Place each value at 0.0, if
- within bounds. Otherwise, select middle point. */
- double params[m_mi.stateDimension];
- for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
- if (m_mi.stateBounds[2 * i] <= 0.0 && m_mi.stateBounds[2 * i + 1] >= 0.0)
- params[i] = 0.0;
- else
- params[i] = (m_mi.stateBounds[2 * i] + m_mi.stateBounds[2 * i + 1]) / 2.0;
- computeTransforms(params);
+ StateParams sp(this);
+ sp.defaultState();
+ computeTransforms(sp.getParams());
}
void planning_models::KinematicModel::computeTransforms(const double *params)
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp 2009-06-20 04:03:14 UTC (rev 17406)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp 2009-06-20 04:06:59 UTC (rev 17407)
@@ -39,6 +39,7 @@
#include <algorithm>
#include <sstream>
#include <cmath>
+#include <cstdlib>
planning_models::StateParams::StateParams(KinematicModel *model) : m_owner(model), m_mi(model->getModelInfo()), m_params(NULL)
{
@@ -91,6 +92,27 @@
m_seen = sp.m_seen;
}
+void planning_models::StateParams::defaultState(void)
+{
+ for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+ {
+ if (m_mi.stateBounds[2 * i] <= 0.0 && m_mi.stateBounds[2 * i + 1] >= 0.0)
+ m_params[i] = 0.0;
+ else
+ m_params[i] = (m_mi.stateBounds[2 * i] + m_mi.stateBounds[2 * i + 1]) / 2.0;
+ m_seen[i] = true;
+ }
+}
+
+void planning_models::StateParams::randomState(void)
+{
+ for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+ {
+ m_params[i] = (m_mi.stateBounds[2 * i + 1] - m_mi.stateBounds[2 * i]) * ((double)rand() / (RAND_MAX + 1.0)) + m_mi.stateBounds[2 * i];
+ m_seen[i] = true;
+ }
+}
+
void planning_models::StateParams::reset(void)
{
for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml 2009-06-20 04:03:14 UTC (rev 17406)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml 2009-06-20 04:06:59 UTC (rev 17407)
@@ -5,6 +5,7 @@
base
left_arm
right_arm
+ arms
## the definition of each group
groups:
@@ -43,6 +44,29 @@
planner_configs:
RRTConfig2 SBLConfig2 KPIECEConfig2r
+ arms:
+ links:
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ l_upper_arm_roll_link
+ l_upper_arm_link
+ l_elbow_flex_link
+ l_forearm_roll_link
+ l_forearm_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ r_shoulder_pan_link
+ r_shoulder_lift_link
+ r_upper_arm_roll_link
+ r_upper_arm_link
+ r_elbow_flex_link
+ r_forearm_roll_link
+ r_forearm_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ planner_configs:
+ RRTConfig2 SBLConfig2
+
## the planner configurations; each config must have a type, which specifies
## the planner to be used; other parameters can be specified as well, depending
## on the planner
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-06-21 23:28:45
|
Revision: 17434
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17434&view=rev
Author: isucan
Date: 2009-06-21 23:28:43 +0000 (Sun, 21 Jun 2009)
Log Message:
-----------
enabled automatic planner selection
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModel.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_planner/RKPPlannerSetup.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPModel.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPESTSetup.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPIKKPIECESetup.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPIKSBLSetup.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPKPIECESetup.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPLazyRRTSetup.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPPlannerSetup.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPRRTSetup.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPSBLSetup.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml
Added Paths:
-----------
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_planner/RKPLBKPIECESetup.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPLBKPIECESetup.cpp
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-21 23:28:43 UTC (rev 17434)
@@ -63,7 +63,7 @@
{
node_handle_.param<std::string>("~arm", arm_, std::string());
node_handle_.param<bool>("~perform_ik", perform_ik_, true);
-
+
// monitor robot
collisionModels_ = new planning_environment::CollisionModels("robot_description");
planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_);
@@ -112,7 +112,6 @@
req.params.model_id = arm_; // the model to plan for (should be defined in planning.yaml)
- req.params.planner_id = "KPIECE"; // this is optional; the planning node should be able to pick a planner
req.params.distance_metric = "L2Square"; // the metric to be used in the robot's state space
// this volume is only needed if planar or floating joints move in the space
Modified: pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/motion_planning/ompl_planning/CMakeLists.txt 2009-06-21 23:28:43 UTC (rev 17434)
@@ -13,6 +13,7 @@
src/helpers/ompl_planner/RKPRRTSetup.cpp
src/helpers/ompl_planner/RKPIKKPIECESetup.cpp
src/helpers/ompl_planner/RKPKPIECESetup.cpp
+ src/helpers/ompl_planner/RKPLBKPIECESetup.cpp
src/helpers/ompl_planner/RKPSBLSetup.cpp
src/helpers/ompl_extensions/RKPGoalDefinitions.cpp
src/helpers/ompl_extensions/RKPSpaceInformation.cpp
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModel.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModel.h 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModel.h 2009-06-21 23:28:43 UTC (rev 17434)
@@ -45,6 +45,7 @@
#include "kinematic_planning/ompl_planner/RKPESTSetup.h"
#include "kinematic_planning/ompl_planner/RKPIKSBLSetup.h"
#include "kinematic_planning/ompl_planner/RKPKPIECESetup.h"
+#include "kinematic_planning/ompl_planner/RKPLBKPIECESetup.h"
#include "kinematic_planning/ompl_planner/RKPIKKPIECESetup.h"
#include <boost/shared_ptr.hpp>
@@ -81,6 +82,7 @@
void addSBL(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
void addIKSBL(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
void addKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
+ void addLBKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
void addIKKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
};
Added: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_planner/RKPLBKPIECESetup.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_planner/RKPLBKPIECESetup.h (rev 0)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_planner/RKPLBKPIECESetup.h 2009-06-21 23:28:43 UTC (rev 17434)
@@ -0,0 +1,58 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#ifndef KINEMATIC_PLANNING_OMPL_PLANNER_RKP_LBKPIECE_SETUP_
+#define KINEMATIC_PLANNING_OMPL_PLANNER_RKP_LBKPIECE_SETUP_
+
+#include "kinematic_planning/ompl_planner/RKPPlannerSetup.h"
+#include <ompl/extension/samplingbased/kinematic/extension/kpiece/LBKPIECE1.h>
+
+namespace kinematic_planning
+{
+
+ class RKPLBKPIECESetup : public RKPPlannerSetup
+ {
+ public:
+
+ RKPLBKPIECESetup(RKPModelBase *m);
+ virtual ~RKPLBKPIECESetup(void);
+ virtual bool setup(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options);
+ };
+
+} // kinematic_planning
+
+#endif
+
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_planner/RKPPlannerSetup.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_planner/RKPPlannerSetup.h 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_planner/RKPPlannerSetup.h 2009-06-21 23:28:43 UTC (rev 17434)
@@ -77,13 +77,14 @@
RKPModelBase *model;
- std::string name;
- ompl::sb::Planner *mp;
- ompl::sb::GAIK *gaik;
- ompl::sb::SpaceInformationKinematic *si;
- ompl::base::StateValidityChecker *svc;
- std::map<std::string, ompl::base::StateDistanceEvaluator*> sde;
- ompl::sb::PathSmootherKinematic *smoother;
+ std::string name; // name of planner
+ ompl::sb::Planner *mp; // pointer to OMPL instance of planner
+ int priority; // priority of this planner when automatically selecting planners
+ ompl::sb::GAIK *gaik; // tool for performing general IK
+ ompl::sb::SpaceInformationKinematic *si; // space information for the planner
+ ompl::base::StateValidityChecker *svc; // the state validation routine
+ std::map<std::string, ompl::base::StateDistanceEvaluator*> sde; // list of available distance evaluators
+ ompl::sb::PathSmootherKinematic *smoother; // pointer to an OMPL path smoother
};
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPModel.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPModel.cpp 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPModel.cpp 2009-06-21 23:28:43 UTC (rev 17434)
@@ -58,13 +58,16 @@
if (type == "KPIECE")
addKPIECE(cfgs[i]);
else
- if (type == "IKSBL")
- addIKSBL(cfgs[i]);
+ if (type == "LBKPIECE")
+ addLBKPIECE(cfgs[i]);
else
- if (type == "IKKPIECE")
- addIKKPIECE(cfgs[i]);
+ if (type == "IKSBL")
+ addIKSBL(cfgs[i]);
else
- ROS_WARN("Unknown planner type: %s", type.c_str());
+ if (type == "IKKPIECE")
+ addIKKPIECE(cfgs[i]);
+ else
+ ROS_WARN("Unknown planner type: %s", type.c_str());
}
}
@@ -122,7 +125,16 @@
delete kpiece;
}
+void kinematic_planning::RKPModel::addLBKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
+{
+ RKPPlannerSetup *kpiece = new RKPLBKPIECESetup(dynamic_cast<RKPModelBase*>(this));
+ if (kpiece->setup(options))
+ planners[kpiece->name] = kpiece;
+ else
+ delete kpiece;
+}
+
void kinematic_planning::RKPModel::addIKKPIECE(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
{
RKPPlannerSetup *kpiece = new RKPIKKPIECESetup(dynamic_cast<RKPModelBase*>(this));
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp 2009-06-21 23:28:43 UTC (rev 17434)
@@ -54,11 +54,17 @@
/* if the user did not specify a planner, use the first available one */
if (req.params.planner_id.empty())
- {
- if (!m->planners.empty())
- req.params.planner_id = m->planners.begin()->first;
- }
-
+ for (std::map<std::string, RKPPlannerSetup*>::const_iterator it = m->planners.begin() ; it != m->planners.end() ; ++it)
+ if ((req.goal_constraints.pose_constraint.empty() && (it->second->mp->getType() & ompl::sb::PLAN_TO_GOAL_STATE) != 0) ||
+ (!req.goal_constraints.pose_constraint.empty() && (it->second->mp->getType() & ompl::sb::PLAN_TO_GOAL_REGION) != 0))
+ {
+ if (req.params.planner_id.empty())
+ req.params.planner_id = it->first;
+ else
+ if (m->planners[req.params.planner_id]->priority < it->second->priority)
+ req.params.planner_id = it->first;
+ }
+
/* check if desired planner exists */
std::map<std::string, RKPPlannerSetup*>::iterator plannerIt = m->planners.find(req.params.planner_id);
if (plannerIt == m->planners.end())
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPESTSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPESTSetup.cpp 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPESTSetup.cpp 2009-06-21 23:28:43 UTC (rev 17434)
@@ -40,6 +40,7 @@
kinematic_planning::RKPESTSetup::RKPESTSetup(RKPModelBase *m) : RKPPlannerSetup(m)
{
name = "EST";
+ priority = 1;
}
kinematic_planning::RKPESTSetup::~RKPESTSetup(void)
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPIKKPIECESetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPIKKPIECESetup.cpp 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPIKKPIECESetup.cpp 2009-06-21 23:28:43 UTC (rev 17434)
@@ -38,7 +38,8 @@
kinematic_planning::RKPIKKPIECESetup::RKPIKKPIECESetup(RKPModelBase *m) : RKPPlannerSetup(m)
{
- name = "IKKPIECE";
+ name = "IKKPIECE";
+ priority = 5;
}
kinematic_planning::RKPIKKPIECESetup::~RKPIKKPIECESetup(void)
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPIKSBLSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPIKSBLSetup.cpp 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPIKSBLSetup.cpp 2009-06-21 23:28:43 UTC (rev 17434)
@@ -38,7 +38,8 @@
kinematic_planning::RKPIKSBLSetup::RKPIKSBLSetup(RKPModelBase *m) : RKPPlannerSetup(m)
{
- name = "IKSBL";
+ name = "IKSBL";
+ priority = 4;
}
kinematic_planning::RKPIKSBLSetup::~RKPIKSBLSetup(void)
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPKPIECESetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPKPIECESetup.cpp 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPKPIECESetup.cpp 2009-06-21 23:28:43 UTC (rev 17434)
@@ -39,6 +39,7 @@
kinematic_planning::RKPKPIECESetup::RKPKPIECESetup(RKPModelBase *m) : RKPPlannerSetup(m)
{
name = "KPIECE";
+ priority = 3;
}
kinematic_planning::RKPKPIECESetup::~RKPKPIECESetup(void)
Added: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPLBKPIECESetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPLBKPIECESetup.cpp (rev 0)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPLBKPIECESetup.cpp 2009-06-21 23:28:43 UTC (rev 17434)
@@ -0,0 +1,80 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#include "kinematic_planning/ompl_planner/RKPLBKPIECESetup.h"
+
+kinematic_planning::RKPLBKPIECESetup::RKPLBKPIECESetup(RKPModelBase *m) : RKPPlannerSetup(m)
+{
+ name = "LBKPIECE";
+ priority = 11;
+}
+
+kinematic_planning::RKPLBKPIECESetup::~RKPLBKPIECESetup(void)
+{
+ if (dynamic_cast<ompl::sb::LBKPIECE1*>(mp))
+ {
+ ompl::base::ProjectionEvaluator *pe = dynamic_cast<ompl::sb::LBKPIECE1*>(mp)->getProjectionEvaluator();
+ if (pe)
+ delete pe;
+ }
+}
+
+bool kinematic_planning::RKPLBKPIECESetup::setup(boost::shared_ptr<planning_environment::RobotModels::PlannerConfig> &options)
+{
+ preSetup(options);
+
+ ompl::sb::LBKPIECE1 *kpiece = new ompl::sb::LBKPIECE1(si);
+ mp = kpiece;
+
+ if (options->hasParam("range"))
+ {
+ kpiece->setRange(options->getParamDouble("range", kpiece->getRange()));
+ ROS_DEBUG("Range is set to %g", kpiece->getRange());
+ }
+
+ kpiece->setProjectionEvaluator(getProjectionEvaluator(options));
+
+ if (kpiece->getProjectionEvaluator() == NULL)
+ {
+ ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
+ return false;
+ }
+ else
+ {
+ postSetup(options);
+ return true;
+ }
+}
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPLazyRRTSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPLazyRRTSetup.cpp 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPLazyRRTSetup.cpp 2009-06-21 23:28:43 UTC (rev 17434)
@@ -40,6 +40,7 @@
kinematic_planning::RKPLazyRRTSetup::RKPLazyRRTSetup(RKPModelBase *m) : RKPPlannerSetup(m)
{
name = "LazyRRT";
+ priority = 2;
}
kinematic_planning::RKPLazyRRTSetup::~RKPLazyRRTSetup(void)
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPPlannerSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPPlannerSetup.cpp 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPPlannerSetup.cpp 2009-06-21 23:28:43 UTC (rev 17434)
@@ -47,6 +47,7 @@
si = NULL;
svc = NULL;
smoother = NULL;
+ priority = 0;
}
kinematic_planning::RKPPlannerSetup::~RKPPlannerSetup(void)
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPRRTSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPRRTSetup.cpp 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPRRTSetup.cpp 2009-06-21 23:28:43 UTC (rev 17434)
@@ -39,6 +39,7 @@
kinematic_planning::RKPRRTSetup::RKPRRTSetup(RKPModelBase *m) : RKPPlannerSetup(m)
{
name = "RRT";
+ priority = 2;
}
kinematic_planning::RKPRRTSetup::~RKPRRTSetup(void)
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPSBLSetup.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPSBLSetup.cpp 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_planner/RKPSBLSetup.cpp 2009-06-21 23:28:43 UTC (rev 17434)
@@ -39,6 +39,7 @@
kinematic_planning::RKPSBLSetup::RKPSBLSetup(RKPModelBase *m) : RKPPlannerSetup(m)
{
name = "SBL";
+ priority = 10;
}
kinematic_planning::RKPSBLSetup::~RKPSBLSetup(void)
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml 2009-06-21 22:41:41 UTC (rev 17433)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml 2009-06-21 23:28:43 UTC (rev 17434)
@@ -28,7 +28,7 @@
l_wrist_flex_link
l_wrist_roll_link
planner_configs:
- RRTConfig2 SBLConfig2 KPIECEConfig2l
+ RRTConfig2 SBLConfig2 LBKPIECEConfig1 KPIECEConfig2l
right_arm:
links:
@@ -42,7 +42,7 @@
r_wrist_flex_link
r_wrist_roll_link
planner_configs:
- RRTConfig2 SBLConfig2 KPIECEConfig2r
+ RRTConfig2 SBLConfig2 LBKPIECEConfig1 KPIECEConfig2r
arms:
links:
@@ -111,4 +111,8 @@
type: KPIECE
projection: link r_wrist_roll_link
celldim: 0.1 0.1 0.1
-
\ No newline at end of file
+
+ LBKPIECEConfig1:
+ type: LBKPIECE
+ projection: 5 6
+ celldim: 0.1 0.1
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-06-22 19:05:41
|
Revision: 17448
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17448&view=rev
Author: isucan
Date: 2009-06-22 19:05:40 +0000 (Mon, 22 Jun 2009)
Log Message:
-----------
speedup for self collision checking
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_both_arms.yaml
pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_left_arm.yaml
pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_right_arm.yaml
pkg/trunk/world_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/src/environment.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp
Modified: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-22 19:05:40 UTC (rev 17448)
@@ -128,7 +128,7 @@
}
}
else
- ROS_ERROR("Robot model '%s' not found!", description_.c_str());
+ ROS_ERROR("Robot model '%s' not found! Did you remap 'robot_description'?", description_.c_str());
}
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_both_arms.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_both_arms.yaml 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_both_arms.yaml 2009-06-22 19:05:40 UTC (rev 17448)
@@ -7,6 +7,8 @@
torso_lift_link
head_pan_link
head_tilt_link
+ laser_tilt_mount_link
+ base_laser
r_shoulder_pan_link
r_shoulder_lift_link
r_upper_arm_roll_link
@@ -42,12 +44,12 @@
## -- for right arm; self-collision if any link in 'a' collides with some link in 'b'
scg_r:
a: r_forearm_link r_gripper_palm_link r_gripper_l_finger_link r_gripper_r_finger_link r_gripper_l_finger_tip_link r_gripper_r_finger_tip_link
- b: base_link torso_lift_link laser_tilt_mount_link head_tilt_link
+ b: base_link base_laser torso_lift_link laser_tilt_mount_link head_tilt_link
## -- for left arm; self-collision if any link in 'a' collides with some link in 'b'
scg_l:
a: l_forearm_link l_gripper_palm_link l_gripper_l_finger_link l_gripper_r_finger_link l_gripper_l_finger_tip_link l_gripper_r_finger_tip_link
- b: base_link torso_lift_link laser_tilt_mount_link head_tilt_link
+ b: base_link base_laser torso_lift_link laser_tilt_mount_link head_tilt_link
## for arms with each other; self-collision if any link in 'a' collides with some link in 'b'
scg_lr:
@@ -84,6 +86,7 @@
r_shoulder_pan_link
r_shoulder_lift_link
torso_lift_link
+ base_laser
base_link
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_left_arm.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_left_arm.yaml 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_left_arm.yaml 2009-06-22 19:05:40 UTC (rev 17448)
@@ -6,7 +6,9 @@
base_link
torso_lift_link
head_pan_link
- head_tilt_link
+ head_tilt_link
+ laser_tilt_mount_link
+ base_laser
l_shoulder_pan_link
l_shoulder_lift_link
l_upper_arm_roll_link
@@ -28,7 +30,7 @@
## -- for left arm; self-collision if any link in 'a' collides with some link in 'b'
scg_l:
a: l_forearm_link l_gripper_palm_link l_gripper_l_finger_link l_gripper_r_finger_link l_gripper_l_finger_tip_link l_gripper_r_finger_tip_link
- b: base_link torso_lift_link laser_tilt_mount_link head_tilt_link
+ b: base_link base_laser torso_lift_link laser_tilt_mount_link head_tilt_link
## list of links that the robot can see with its sensors (used to remove
## parts of the robot from scanned data)
@@ -47,6 +49,7 @@
l_shoulder_pan_link
l_shoulder_lift_link
torso_lift_link
+ base_laser
base_link
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_right_arm.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_right_arm.yaml 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_right_arm.yaml 2009-06-22 19:05:40 UTC (rev 17448)
@@ -7,6 +7,8 @@
torso_lift_link
head_pan_link
head_tilt_link
+ laser_tilt_mount_link
+ base_laser
r_shoulder_pan_link
r_shoulder_lift_link
r_upper_arm_roll_link
@@ -28,7 +30,7 @@
## -- for right arm; self-collision if any link in 'a' collides with some link in 'b'
scg_r:
a: r_forearm_link r_gripper_palm_link r_gripper_l_finger_link r_gripper_r_finger_link r_gripper_l_finger_tip_link r_gripper_r_finger_tip_link
- b: base_link torso_lift_link laser_tilt_mount_link head_tilt_link
+ b: base_link base_laser torso_lift_link laser_tilt_mount_link head_tilt_link
## list of links that the robot can see with its sensors (used to remove
## parts of the robot from scanned data)
@@ -47,6 +49,7 @@
r_shoulder_pan_link
r_shoulder_lift_link
torso_lift_link
+ base_laser
base_link
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-06-22 19:05:40 UTC (rev 17448)
@@ -93,7 +93,7 @@
bool getSelfCollision(void) const;
/** Add a group of links to be checked for self collision */
- virtual void addSelfCollisionGroup(std::vector<std::string> &links) = 0;
+ virtual void addSelfCollisionGroup(std::vector<std::string> &links);
/** Enable/Disable collision checking for specific links. Return the previous value of the state (1 or 0) if succesful; -1 otherwise */
virtual int setCollisionCheck(const std::string &link, bool state) = 0;
@@ -123,7 +123,10 @@
/** Check if a model is in collision. Contacts are not computed */
virtual bool isCollision(void) = 0;
-
+
+ /** Check for self collision. Contacts are not computed */
+ virtual bool isSelfCollision(void) = 0;
+
/** Get the list of contacts (collisions) */
virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1) = 0;
@@ -161,6 +164,10 @@
protected:
boost::mutex m_lock;
+ std::vector<std::string> m_collisionLinks;
+ std::map<std::string, unsigned int> m_collisionLinkIndex;
+ std::vector< std::vector<bool> > m_selfCollisionTest;
+
bool m_selfCollision;
bool m_verbose;
planning_models::msg::Interface m_msg;
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-06-22 19:05:40 UTC (rev 17448)
@@ -42,10 +42,13 @@
namespace collision_space
{
-
+
/** A class describing an environment for a kinematic robot using ODE */
class EnvironmentModelODE : public EnvironmentModel
- {
+ {
+
+ friend void nearCallbackFn(void *data, dGeomID o1, dGeomID o2);
+
public:
EnvironmentModelODE(void) : EnvironmentModel()
@@ -78,7 +81,10 @@
/** Check if a model is in collision */
virtual bool isCollision(void);
-
+
+ /** Check if a model is in self collision */
+ virtual bool isSelfCollision(void);
+
/** Remove all obstacles from collision model */
virtual void clearObstacles(void);
@@ -220,8 +226,9 @@
std::vector<dGeomID> geom;
bool enabled;
planning_models::KinematicModel::Link *link;
+ unsigned int index;
};
-
+
struct ModelInfo
{
std::vector< kGeom* > linkGeom;
Modified: pkg/trunk/world_models/collision_space/src/environment.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environment.cpp 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/world_models/collision_space/src/environment.cpp 2009-06-22 19:05:40 UTC (rev 17448)
@@ -44,8 +44,37 @@
void collision_space::EnvironmentModel::addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
m_robotModel = model;
+ m_collisionLinks = links;
+ m_selfCollisionTest.resize(links.size());
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ {
+ m_selfCollisionTest[i].resize(links.size(), false);
+ m_collisionLinkIndex[links[i]] = i;
+ }
}
+void collision_space::EnvironmentModel::addSelfCollisionGroup(std::vector<std::string> &links)
+{
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ {
+ if (m_collisionLinkIndex.find(links[i]) == m_collisionLinkIndex.end())
+ {
+ m_msg.error("Unknown link '%s'", links[i].c_str());
+ continue;
+ }
+ for (unsigned int j = i + 1 ; j < links.size() ; ++j)
+ {
+ if (m_collisionLinkIndex.find(links[j]) == m_collisionLinkIndex.end())
+ {
+ m_msg.error("Unknown link '%s'", links[j].c_str());
+ continue;
+ }
+ m_selfCollisionTest[m_collisionLinkIndex[links[i]]][m_collisionLinkIndex[links[j]]] = true;
+ m_selfCollisionTest[m_collisionLinkIndex[links[j]]][m_collisionLinkIndex[links[i]]] = true;
+ }
+ }
+}
+
void collision_space::EnvironmentModel::lock(void)
{
m_lock.lock();
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-06-22 19:05:40 UTC (rev 17448)
@@ -56,41 +56,35 @@
{
collision_space::EnvironmentModel::addRobotModel(model, links, scale, padding);
- m_modelGeom.space = dHashSpaceCreate(0);
+ m_modelGeom.space = dSweepAndPruneSpaceCreate(0, dSAP_AXES_XZY);
m_modelGeom.scale = scale;
m_modelGeom.padding = padding;
- std::map<std::string, bool> exists;
for (unsigned int i = 0 ; i < links.size() ; ++i)
- exists[links[i]] = true;
-
- for (unsigned int j = 0 ; j < m_robotModel->getRobotCount() ; ++j)
{
- planning_models::KinematicModel::Robot *robot = m_robotModel->getRobot(j);
- for (unsigned int i = 0 ; i < robot->links.size() ; ++i)
+ /* skip this link if we have no geometry or if the link
+ name is not specified as enabled for collision
+ checking */
+ planning_models::KinematicModel::Link *link = m_robotModel->getLink(links[i]);
+ if (!link || !link->shape)
+ continue;
+
+ kGeom *kg = new kGeom();
+ kg->link = link;
+ kg->enabled = true;
+ kg->index = i;
+ dGeomID g = createODEGeom(m_modelGeom.space, link->shape, scale, padding);
+ assert(g);
+ dGeomSetData(g, reinterpret_cast<void*>(kg));
+ kg->geom.push_back(g);
+ for (unsigned int k = 0 ; k < kg->link->attachedBodies.size() ; ++k)
{
- /* skip this link if we have no geometry or if the link
- name is not specified as enabled for collision
- checking */
- if (!robot->links[i]->shape)
- continue;
- if (exists.find(robot->links[i]->name) == exists.end())
- continue;
-
- kGeom *kg = new kGeom();
- kg->link = robot->links[i];
- kg->enabled = true;
- dGeomID g = createODEGeom(m_modelGeom.space, robot->links[i]->shape, scale, padding);
- assert(g);
- kg->geom.push_back(g);
- for (unsigned int k = 0 ; k < kg->link->attachedBodies.size() ; ++k)
- {
- dGeomID ga = createODEGeom(m_modelGeom.space, kg->link->attachedBodies[k]->shape, scale, padding);
- assert(ga);
- kg->geom.push_back(ga);
- }
- m_modelGeom.linkGeom.push_back(kg);
+ dGeomID ga = createODEGeom(m_modelGeom.space, kg->link->attachedBodies[k]->shape, scale, padding);
+ assert(ga);
+ dGeomSetData(ga, NULL);
+ kg->geom.push_back(ga);
}
+ m_modelGeom.linkGeom.push_back(kg);
}
}
@@ -150,6 +144,7 @@
{
dGeomID ga = createODEGeom(m_modelGeom.space, kg->link->attachedBodies[k]->shape, m_modelGeom.scale, m_modelGeom.padding);
assert(ga);
+ dGeomSetData(ga, NULL);
kg->geom.push_back(ga);
}
}
@@ -299,81 +294,96 @@
return m_modelGeom.space;
}
-struct CollisionData
+namespace collision_space
{
- CollisionData(void)
+
+ struct CollisionData
{
- done = false;
- collides = false;
- max_contacts = 0;
- contacts = NULL;
- link1 = link2 = NULL;
- }
+ CollisionData(void)
+ {
+ done = false;
+ collides = false;
+ max_contacts = 0;
+ contacts = NULL;
+ selfCollisionTest = NULL;
+ link1 = link2 = NULL;
+ }
+
+ bool done;
+
+ bool collides;
+ unsigned int max_contacts;
+ std::vector<EnvironmentModelODE::Contact> *contacts;
+ std::vector< std::vector<bool> > *selfCollisionTest;
+
+ planning_models::KinematicModel::Link *link1;
+ planning_models::KinematicModel::Link *link2;
+ };
- bool done;
-
- bool collides;
- unsigned int max_contacts;
- std::vector<collision_space::EnvironmentModelODE::Contact> *contacts;
-
- planning_models::KinematicModel::Link *link1;
- planning_models::KinematicModel::Link *link2;
-};
-
-static void nearCallbackFn(void *data, dGeomID o1, dGeomID o2)
-{
- CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
-
- if (cdata->done)
- return;
-
- if (cdata->contacts)
+ void nearCallbackFn(void *data, dGeomID o1, dGeomID o2)
{
- static const int MAX_CONTACTS = 3;
- dContact contact[MAX_CONTACTS];
- int numc = dCollide (o1, o2, MAX_CONTACTS,
- &contact[0].geom, sizeof(dContact));
- if (numc)
+ CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
+
+ if (cdata->done)
+ return;
+
+ if (cdata->selfCollisionTest)
{
- cdata->collides = true;
- for (int i = 0 ; i < numc ; ++i)
+ EnvironmentModelODE::kGeom* kg1 = reinterpret_cast<EnvironmentModelODE::kGeom*>(dGeomGetData(o1));
+ EnvironmentModelODE::kGeom* kg2 = reinterpret_cast<EnvironmentModelODE::kGeom*>(dGeomGetData(o2));
+ if (kg1 && kg2)
+ if (cdata->selfCollisionTest->at(kg1->index)[kg2->index] == false)
+ return;
+ }
+
+ if (cdata->contacts)
+ {
+ static const int MAX_CONTACTS = 3;
+ dContact contact[MAX_CONTACTS];
+ int numc = dCollide (o1, o2, MAX_CONTACTS,
+ &contact[0].geom, sizeof(dContact));
+ if (numc)
{
- if (cdata->contacts->size() < cdata->max_contacts)
+ cdata->collides = true;
+ for (int i = 0 ; i < numc ; ++i)
{
- collision_space::EnvironmentModelODE::Contact add;
-
- add.pos.setX(contact[i].geom.pos[0]);
- add.pos.setY(contact[i].geom.pos[1]);
- add.pos.setZ(contact[i].geom.pos[2]);
-
- add.normal.setX(contact[i].geom.normal[0]);
- add.normal.setY(contact[i].geom.normal[1]);
- add.normal.setZ(contact[i].geom.normal[2]);
-
- add.depth = contact[i].geom.depth;
-
- add.link1 = cdata->link1;
- add.link2 = cdata->link2;
-
- cdata->contacts->push_back(add);
+ if (cdata->contacts->size() < cdata->max_contacts)
+ {
+ collision_space::EnvironmentModelODE::Contact add;
+
+ add.pos.setX(contact[i].geom.pos[0]);
+ add.pos.setY(contact[i].geom.pos[1]);
+ add.pos.setZ(contact[i].geom.pos[2]);
+
+ add.normal.setX(contact[i].geom.normal[0]);
+ add.normal.setY(contact[i].geom.normal[1]);
+ add.normal.setZ(contact[i].geom.normal[2]);
+
+ add.depth = contact[i].geom.depth;
+
+ add.link1 = cdata->link1;
+ add.link2 = cdata->link2;
+
+ cdata->contacts->push_back(add);
+ }
+ else
+ break;
}
- else
- break;
}
+ if (cdata->contacts->size() >= cdata->max_contacts)
+ cdata->done = true;
}
- if (cdata->contacts->size() >= cdata->max_contacts)
- cdata->done = true;
- }
- else
- {
- static const int MAX_CONTACTS = 1;
- dContact contact[MAX_CONTACTS];
- int numc = dCollide (o1, o2, MAX_CONTACTS,
- &contact[0].geom, sizeof(dContact));
- if (numc)
+ else
{
- cdata->collides = true;
- cdata->done = true;
+ static const int MAX_CONTACTS = 1;
+ dContact contact[MAX_CONTACTS];
+ int numc = dCollide (o1, o2, MAX_CONTACTS,
+ &contact[0].geom, sizeof(dContact));
+ if (numc)
+ {
+ cdata->collides = true;
+ cdata->done = true;
+ }
}
}
}
@@ -381,6 +391,7 @@
bool collision_space::EnvironmentModelODE::getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count)
{
CollisionData cdata;
+ cdata.selfCollisionTest = &m_selfCollisionTest;
cdata.contacts = &contacts;
cdata.max_contacts = max_count;
contacts.clear();
@@ -391,58 +402,24 @@
bool collision_space::EnvironmentModelODE::isCollision(void)
{
CollisionData cdata;
+ cdata.selfCollisionTest = &m_selfCollisionTest;
testCollision(reinterpret_cast<void*>(&cdata));
return cdata.collides;
}
+bool collision_space::EnvironmentModelODE::isSelfCollision(void)
+{
+ CollisionData cdata;
+ cdata.selfCollisionTest = &m_selfCollisionTest;
+ testSelfCollision(reinterpret_cast<void*>(&cdata));
+ return cdata.collides;
+}
+
void collision_space::EnvironmentModelODE::testSelfCollision(void *data)
{
- CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
- for (int i = m_modelGeom.selfCollision.size() - 1 ; i >= 0 && !cdata->done ; --i)
- {
- const std::vector<unsigned int> &vec = m_modelGeom.selfCollision[i];
- unsigned int n = vec.size();
-
- for (unsigned int j = 0 ; j < n && !cdata->done ; ++j)
- {
- cdata->link1 = m_modelGeom.linkGeom[vec[j]]->link;
- const unsigned int njg = m_modelGeom.linkGeom[vec[j]]->geom.size();
-
- for (unsigned int k = j + 1 ; k < n && !cdata->done; ++k)
- {
- // dSpaceCollide2 expects AABBs to be computed, so
- // we force that by calling dGeomGetAABB. Since we
- // get the data anyway, we attempt to speed things
- // up using it.
-
- const unsigned int nkg = m_modelGeom.linkGeom[vec[k]]->geom.size();
- cdata->link2 = m_modelGeom.linkGeom[vec[k]]->link;
-
- /* this will account for attached bodies as well */
- for (unsigned int jg = 0 ; jg < njg && !cdata->done; ++jg)
- for (unsigned int kg = 0 ; kg < nkg && !cdata->done; ++kg)
- {
- dGeomID g1 = m_modelGeom.linkGeom[vec[j]]->geom[jg];
- dGeomID g2 = m_modelGeom.linkGeom[vec[k]]->geom[kg];
-
- dReal aabb1[6], aabb2[6];
- dGeomGetAABB(g1, aabb1);
- dGeomGetAABB(g2, aabb2);
-
- if (!(aabb1[2] > aabb2[3] ||
- aabb1[3] < aabb2[2] ||
- aabb1[4] > aabb2[5] ||
- aabb1[5] < aabb2[4]))
- dSpaceCollide2(g1, g2, data, nearCallbackFn);
-
- if (cdata->collides && m_verbose)
- m_msg.inform("Self-collision between '%s' and '%s'\n",
- m_modelGeom.linkGeom[vec[j]]->link->name.c_str(), m_modelGeom.linkGeom[vec[k]]->link->name.c_str());
- }
- }
- }
- }
+ dSpaceCollide(m_modelGeom.space, data, nearCallbackFn);
}
+
void collision_space::EnvironmentModelODE::testStaticBodyCollision(void *data)
{
CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
@@ -549,6 +526,8 @@
void collision_space::EnvironmentModelODE::addSelfCollisionGroup(std::vector<std::string> &links)
{
+ EnvironmentModel::addSelfCollisionGroup(links);
+
unsigned int pos = m_modelGeom.selfCollision.size();
m_modelGeom.selfCollision.resize(pos + 1);
for (unsigned int i = 0 ; i < links.size() ; ++i)
@@ -557,7 +536,6 @@
if (m_modelGeom.linkGeom[j]->link->name == links[i])
m_modelGeom.selfCollision[pos].push_back(j);
}
-
}
int collision_space::EnvironmentModelODE::setCollisionCheck(const std::string &link, bool state)
Modified: pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp
===================================================================
--- pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp 2009-06-22 18:53:38 UTC (rev 17447)
+++ pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp 2009-06-22 19:05:40 UTC (rev 17448)
@@ -103,6 +103,11 @@
for (unsigned int i = 0 ; i < K ; ++i)
em->isCollision();
ROS_INFO("%f collision tests per second (with self collision checking)", (double)K/(ros::WallTime::now() - tm).toSec());
+
+ tm = ros::WallTime::now();
+ for (unsigned int i = 0 ; i < K ; ++i)
+ em->isSelfCollision();
+ ROS_INFO("%f collision tests per second (only self collision checking)", (double)K/(ros::WallTime::now() - tm).toSec());
}
protected:
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tpr...@us...> - 2009-06-23 01:22:34
|
Revision: 17474
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17474&view=rev
Author: tpratkanis
Date: 2009-06-23 00:33:43 +0000 (Tue, 23 Jun 2009)
Log Message:
-----------
Add new handlePreempt method to robot actions. This allows you to create an event handler that triggers when preemption occurs.
Modified Paths:
--------------
pkg/trunk/common/robot_actions/include/robot_actions/action.h
pkg/trunk/highlevel/test_robot_actions/test/utest.cc
Modified: pkg/trunk/common/robot_actions/include/robot_actions/action.h
===================================================================
--- pkg/trunk/common/robot_actions/include/robot_actions/action.h 2009-06-23 00:22:26 UTC (rev 17473)
+++ pkg/trunk/common/robot_actions/include/robot_actions/action.h 2009-06-23 00:33:43 UTC (rev 17474)
@@ -78,6 +78,11 @@
* @param feedback The feedback message. At the end of the function, this will be published.
*/
virtual ResultStatus execute(const Goal& goal, Feedback& feedback) = 0;
+
+ /**
+ * @brief Blocking, user specified code for preemption. By defualt, it does nothing. Please keep in mind that this function will be called on termination.
+ */
+ virtual void handlePreempt() {}
/**
* @brief Called by the user to check if the action has a new goal.
@@ -117,6 +122,7 @@
*/
void preempt() {
_preempt_request = true;
+ handlePreempt();
ROS_DEBUG("[%s]Preempt requested\n", getName().c_str());
while(isActive()) {
ros::Duration d; d.fromSec(0.001);
@@ -190,7 +196,7 @@
}
virtual ~Action(){
- ROS_ASSERT_MSG(_terminated, "Actions must be terminated before they are deleted, or there will be a segfault. In this case, action '%s' terminated was terminated before it was deleted.", _name.c_str());
+ ROS_ASSERT_MSG(_terminated, "Actions must be terminated before they are deleted, or there will be a segfault. In this case, action '%s' was not terminated before it was deleted.", _name.c_str());
if(_action_thread != NULL){
_action_thread->join();
delete _action_thread;
Modified: pkg/trunk/highlevel/test_robot_actions/test/utest.cc
===================================================================
--- pkg/trunk/highlevel/test_robot_actions/test/utest.cc 2009-06-23 00:22:26 UTC (rev 17473)
+++ pkg/trunk/highlevel/test_robot_actions/test/utest.cc 2009-06-23 00:33:43 UTC (rev 17474)
@@ -17,9 +17,10 @@
class MyAction: public robot_actions::Action<Float32, Float32> {
public:
- MyAction(std::string name): robot_actions::Action<Float32, Float32>(name), _use_deactivate_wait(false) {}
+ MyAction(std::string name): robot_actions::Action<Float32, Float32>(name), _use_deactivate_wait(false), _handled_preempt(false) {}
bool _use_deactivate_wait;
+ bool _handled_preempt;
const static int FAIL_IF = 30;
@@ -63,6 +64,11 @@
return waitForDeactivation(feedback);
}
+
+ virtual void handlePreempt() {
+ _handled_preempt = true;
+ }
+
};
/**
@@ -108,6 +114,7 @@
// Activate
a.activate(g);
ASSERT_EQ(c._status.value, foo.ACTIVE);
+ ASSERT_EQ(a._handled_preempt, false);
// Sleep for longer than the goal. Should be finished
ros::Duration d;
@@ -123,6 +130,7 @@
a.preempt();
d.sleep();
ASSERT_EQ(c._status.value == foo.PREEMPTED, true);
+ ASSERT_EQ(a._handled_preempt, true);
ASSERT_EQ(c._value < g.data, true);
// Message adapter connects an action to a ros message context
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-06-23 02:51:37
|
Revision: 17482
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17482&view=rev
Author: isucan
Date: 2009-06-23 02:51:34 +0000 (Tue, 23 Jun 2009)
Log Message:
-----------
separated out the geometric shapes from planning_models and collision_space; added a new collision map node that can deal with occlusions
Modified Paths:
--------------
pkg/trunk/mapping/collision_map/CMakeLists.txt
pkg/trunk/mapping/collision_map/manifest.xml
pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/planning_environment/manifest.xml
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_models/CMakeLists.txt
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/manifest.xml
pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h
pkg/trunk/util/new_robot_self_filter/manifest.xml
pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
pkg/trunk/util/new_robot_self_filter/src/self_mask.cpp
pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp
pkg/trunk/world_models/collision_space/CMakeLists.txt
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/manifest.xml
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
pkg/trunk/world_models/test_collision_space/src/test_cs_point_inclusion.cpp
Added Paths:
-----------
pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
pkg/trunk/util/geometric_shapes/
pkg/trunk/util/geometric_shapes/.build_version
pkg/trunk/util/geometric_shapes/.rosgcov_files
pkg/trunk/util/geometric_shapes/CMakeLists.txt
pkg/trunk/util/geometric_shapes/Makefile
pkg/trunk/util/geometric_shapes/include/
pkg/trunk/util/geometric_shapes/include/geometric_shapes/
pkg/trunk/util/geometric_shapes/include/geometric_shapes/bodies.h
pkg/trunk/util/geometric_shapes/include/geometric_shapes/shapes.h
pkg/trunk/util/geometric_shapes/mainpage.dox
pkg/trunk/util/geometric_shapes/manifest.xml
pkg/trunk/util/geometric_shapes/src/
pkg/trunk/util/geometric_shapes/src/bodies.cpp
pkg/trunk/util/geometric_shapes/src/load_mesh.cpp
pkg/trunk/util/geometric_shapes/test/
pkg/trunk/util/geometric_shapes/test/test_point_inclusion.cpp
Removed Paths:
-------------
pkg/trunk/motion_planning/planning_models/include/planning_models/shapes.h
pkg/trunk/motion_planning/planning_models/src/load_mesh.cpp
pkg/trunk/world_models/collision_space/include/collision_space/bodies.h
pkg/trunk/world_models/collision_space/src/bodies.cpp
pkg/trunk/world_models/collision_space/test/
Modified: pkg/trunk/mapping/collision_map/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-06-23 02:51:34 UTC (rev 17482)
@@ -79,3 +79,5 @@
rospack_add_executable(collision_map_node src/collision_map.cpp)
rospack_add_executable(collision_map_buffer_node src/collision_map_buffer.cpp)
+rospack_add_executable(collision_map_occ_node src/collision_map_occ.cpp)
+rospack_add_openmp_flags(collision_map_occ_node)
Modified: pkg/trunk/mapping/collision_map/manifest.xml
===================================================================
--- pkg/trunk/mapping/collision_map/manifest.xml 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/mapping/collision_map/manifest.xml 2009-06-23 02:51:34 UTC (rev 17482)
@@ -15,4 +15,5 @@
<depend package="robot_srvs" />
<depend package="visualization_msgs" />
<depend package="point_cloud_mapping" />
+ <depend package="new_robot_self_filter" />
</package>
Added: pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp (rev 0)
+++ pkg/trunk/mapping/collision_map/src/collision_map_occ.cpp 2009-06-23 02:51:34 UTC (rev 17482)
@@ -0,0 +1,594 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#include <ros/ros.h>
+#include <robot_msgs/PointCloud.h>
+#include <robot_msgs/CollisionMap.h>
+#include <tf/transform_listener.h>
+#include <tf/message_notifier.h>
+#include <robot_self_filter/self_mask.h>
+#include <boost/thread/mutex.hpp>
+#include <boost/bind.hpp>
+#include <vector>
+#include <algorithm>
+#include <set>
+#include <iterator>
+
+class CollisionMapperOcc
+{
+public:
+
+ CollisionMapperOcc(void) : sf_(tf_),
+ mn_(tf_, boost::bind(&CollisionMapperOcc::cloudCallback, this, _1), "cloud_in", "", 1)
+ {
+ // a frame that does not move with the robot
+ nh_.param<std::string>("~fixed_frame", fixedFrame_, "odom");
+
+ // a frame that moves with the robot
+ nh_.param<std::string>("~robot_frame", robotFrame_, "base_link");
+
+ // bounds of collision map in robot frame
+ nh_.param<double>("~dimension_x", bi_.dimensionX, 1.0);
+ nh_.param<double>("~dimension_y", bi_.dimensionY, 1.5);
+ nh_.param<double>("~dimension_z", bi_.dimensionZ, 2.0);
+
+ // origin of collision map in the robot frame
+ nh_.param<double>("~origin_x", bi_.originX, 1.1);
+ nh_.param<double>("~origin_y", bi_.originY, 0.0);
+ nh_.param<double>("~origin_z", bi_.originZ, 0.0);
+
+ // sensor origin in robot frame
+ nh_.param<double>("~sensor_x", bi_.sensorX, 0.05);
+ nh_.param<double>("~sensor_y", bi_.sensorY, 0.0);
+ nh_.param<double>("~sensor_z", bi_.sensorZ, 1.0);
+
+ // resolution
+ nh_.param<double>("~resolution", bi_.resolution, 0.015);
+
+ ROS_INFO("Maintaining occlusion map in frame '%s', with origin at (%f, %f, %f) and dimension (%f, %f, %f), resolution of %f; sensor is at (%f, %f, %f), fixed fame is '%s'.",
+ robotFrame_.c_str(), bi_.dimensionX, bi_.dimensionY, bi_.dimensionZ, bi_.originX, bi_.originY, bi_.originZ, bi_.resolution, bi_.sensorX, bi_.sensorY, bi_.sensorZ, fixedFrame_.c_str());
+
+ // compute some useful values
+ bi_.sx = (int)(0.5 + (bi_.sensorX - bi_.originX) / bi_.resolution);
+ bi_.sy = (int)(0.5 + (bi_.sensorY - bi_.originY) / bi_.resolution);
+ bi_.sz = (int)(0.5 + (bi_.sensorZ - bi_.originZ) / bi_.resolution);
+
+ bi_.minX = (int)(0.5 + (-bi_.dimensionX - bi_.originX) / bi_.resolution);
+ bi_.maxX = (int)(0.5 + (bi_.dimensionX - bi_.originX) / bi_.resolution);
+
+ bi_.minY = (int)(0.5 + (-bi_.dimensionY - bi_.originY) / bi_.resolution);
+ bi_.maxY = (int)(0.5 + (bi_.dimensionY - bi_.originY) / bi_.resolution);
+
+ bi_.minZ = (int)(0.5 + (-bi_.dimensionZ - bi_.originZ) / bi_.resolution);
+ bi_.maxZ = (int)(0.5 + (bi_.dimensionZ - bi_.originZ) / bi_.resolution);
+
+ bi_.real_minX = -bi_.dimensionX + bi_.originX;
+ bi_.real_maxX = bi_.dimensionX + bi_.originX;
+ bi_.real_minY = -bi_.dimensionY + bi_.originY;
+ bi_.real_maxY = bi_.dimensionY + bi_.originY;
+ bi_.real_minZ = -bi_.dimensionZ + bi_.originZ;
+ bi_.real_maxZ = bi_.dimensionZ + bi_.originZ;
+
+
+ // configure the self mask and the message notifier
+ sf_.configure();
+ std::vector<std::string> frames;
+ sf_.getLinkFrames(frames);
+ if (std::find(frames.begin(), frames.end(), robotFrame_) == frames.end())
+ frames.push_back(robotFrame_);
+ mn_.setTargetFrame(frames);
+
+ // advertise our topic
+ cmapPublisher_ = nh_.advertise<robot_msgs::CollisionMap>("collision_map_occ", 1);
+ }
+
+private:
+
+ struct CollisionPoint
+ {
+ int x, y, z;
+ };
+
+ struct CollisionPointOrder
+ {
+ bool operator()(const CollisionPoint &a, const CollisionPoint &b)
+ {
+ if (a.x < b.x)
+ return true;
+ if (a.x > b.x)
+ return false;
+ if (a.y < b.y)
+ return true;
+ if (a.y > b.y)
+ return false;
+ return a.z < b.z;
+ }
+ };
+
+ struct BoxInfo
+ {
+ double dimensionX, dimensionY, dimensionZ;
+ double originX, originY, originZ;
+ double sensorX, sensorY, sensorZ;
+ double resolution;
+ int sx, sy, sz;
+ int minX, minY, minZ;
+ int maxX, maxY, maxZ;
+ double real_minX, real_minY, real_minZ;
+ double real_maxX, real_maxY, real_maxZ;
+ };
+
+ typedef std::set<CollisionPoint, CollisionPointOrder> CMap;
+
+ void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
+ {
+ std::vector<bool> mask;
+ robot_msgs::PointCloud out;
+ ros::WallTime tm = ros::WallTime::now();
+
+#pragma omp parallel sections
+ {
+
+#pragma omp section
+ {
+ // transform the pointcloud to the robot frame
+ // since we need the points in this frame (around the robot)
+ // to compute the collision map
+ tf_.transformPointCloud(robotFrame_, *cloud, out);
+ }
+
+#pragma omp section
+ {
+ // separate the received points into ones on the robot and ones that are obstacles
+ // the frame of the cloud does not matter here
+ sf_.mask(*cloud, mask);
+ }
+ }
+
+ CMap obstacles;
+ CMap self;
+
+ // compute collision maps for the points on the robot and points that define obstacles
+#pragma omp parallel sections
+ {
+
+#pragma omp section
+ {
+ constructCollisionMap(out, mask, true, obstacles);
+ }
+#pragma omp section
+ {
+ constructCollisionMap(out, mask, false, self);
+ }
+ }
+
+ // try to transform the previous map (if it exists) to the new frame
+ if (!currentMap_.empty())
+ if (!transformMap(currentMap_, header_, out.header))
+ currentMap_.clear();
+ header_ = out.header;
+
+ if (currentMap_.empty())
+ {
+ // if we have no previous information, the map is simply what we see + we assume space hidden by self is obstructed
+ CMap occ_self;
+ findSelfOcclusion(self, occ_self);
+ std::set_union(obstacles.begin(), obstacles.end(),
+ occ_self.begin(), occ_self.end(),
+ std::inserter(currentMap_, currentMap_.begin()),
+ CollisionPointOrder());
+ }
+ else
+ {
+ CMap occ_self;
+ CMap diff;
+
+#pragma omp parallel sections
+ {
+#pragma omp section
+ {
+ // find the set of points under the parts seen by the sensor
+ findSelfOcclusion(self, occ_self);
+ }
+#pragma omp section
+ {
+ // find the new obstacles that could be occluding information
+ std::set_difference(obstacles.begin(), obstacles.end(),
+ currentMap_.begin(), currentMap_.end(),
+ std::inserter(diff, diff.begin()),
+ CollisionPointOrder());
+ }
+ }
+
+ // this is the set of points that could be occluding information
+ CMap occ;
+ std::set_union(diff.begin(), diff.end(),
+ occ_self.begin(), occ_self.end(),
+ std::inserter(occ, occ.begin()),
+ CollisionPointOrder());
+
+ // find the points in the previous map that are now occluded
+ CMap keep;
+ findOcclusionsInMap(currentMap_, occ, keep);
+
+ // the new map is the new set of obstacles + occluded information
+ currentMap_.clear();
+ std::set_union(obstacles.begin(), obstacles.end(),
+ keep.begin(), keep.end(),
+ std::inserter(currentMap_, currentMap_.begin()),
+ CollisionPointOrder());
+ }
+
+ double sec = (ros::WallTime::now() - tm).toSec();
+
+ ROS_INFO("Updated collision map with %d points at %f Hz", currentMap_.size(), 1.0/sec);
+
+ publishCollisionMap(currentMap_);
+ }
+
+ bool transformMap(CMap &map, const roslib::Header &from, const roslib::Header &to)
+ {
+ return true;
+
+ if (tf_.canTransform(to.frame_id, to.stamp, from.frame_id, from.stamp, fixedFrame_))
+ {
+ tf::Stamped<btTransform> transf;
+ tf_.lookupTransform(to.frame_id, to.stamp, from.frame_id, from.stamp, fixedFrame_, transf);
+
+ // copy data to temporary location
+ const int n = map.size();
+ std::vector<CollisionPoint> pts(n);
+ std::copy(map.begin(), map.end(), pts.begin());
+ map.clear();
+
+#pragma omp parallel for
+ for (int i = 0 ; i < n ; ++i)
+ {
+ btVector3 p(pts[i].x * bi_.resolution + bi_.originX, pts[i].y * bi_.resolution + bi_.originY, pts[i].z * bi_.resolution + bi_.originZ);
+ p = transf * p;
+ if (p.x() > bi_.real_minX && p.x() < bi_.real_maxX && p.y() > bi_.real_minY && p.y() < bi_.real_maxY && p.z() > bi_.real_minZ && p.z() < bi_.real_maxZ)
+ {
+ CollisionPoint c;
+ c.x = (int)(0.5 + (p.x() - bi_.originX) / bi_.resolution);
+ c.y = (int)(0.5 + (p.y() - bi_.originY) / bi_.resolution);
+ c.z = (int)(0.5 + (p.z() - bi_.originZ) / bi_.resolution);
+ map.insert(c);
+ }
+ }
+
+ return true;
+ }
+ else
+ {
+ ROS_WARN("Unable to transform previous collision map into new frame");
+ return false;
+ }
+ }
+
+ void findOcclusionsInMap(CMap &previous, const CMap &occ, CMap &keep)
+ {
+ // OpenMP need an int as the lookup variable, but for set,
+ // this is not possible, so we copy to a vector
+ const int n = occ.size();
+ std::vector<CollisionPoint> pts(n);
+ std::copy(occ.begin(), occ.end(), pts.begin());
+
+ // we are doing OpenMP parallelism, but we use a mutex to synchronize
+ boost::mutex lock;
+
+#pragma omp parallel for schedule(dynamic)
+ for (int i = 0 ; i < n ; ++i)
+ // run this function in a visitor pattern (it calls the callback at every cell it finds along the line)
+ bresenham_line_3D(bi_.sx, bi_.sy, bi_.sz, pts[i].x, pts[i].y, pts[i].z,
+ bi_.minX, bi_.minY, bi_.minZ, bi_.maxX, bi_.maxY, bi_.maxZ,
+ boost::bind(&CollisionMapperOcc::findOcclusionsInMapAux, this, &previous, &keep, &lock, _1, _2, _3));
+ }
+
+ bool findOcclusionsInMapAux(CMap *previous, CMap *keep, boost::mutex *lock, int x, int y, int z)
+ {
+ CollisionPoint p;
+ p.x = x;
+ p.y = y;
+ p.z = z;
+
+ // if we are occluding this point, remember it
+ if (previous->find(p) != previous->end())
+ {
+ lock->lock();
+ keep->insert(p);
+ lock->unlock();
+ // we can terminate
+ return true;
+ }
+
+ return true;
+ }
+
+ void findSelfOcclusion(const CMap &self, CMap &occ)
+ {
+ // tell the self filter the frame in which we call getMask()
+ sf_.assumeFrame(header_);
+
+ // we are doing OpenMP parallelism, but we use a mutex to synchronize
+ boost::mutex lock;
+
+ // OpenMP need an int as the lookup variable, but for set,
+ // this is not possible, so we copy to a vector
+ const int n = self.size();
+ std::vector<CollisionPoint> pts(n);
+ std::copy(self.begin(), self.end(), pts.begin());
+
+#pragma omp parallel for schedule(dynamic)
+ for (int i = 0 ; i < n ; ++i)
+ {
+ int state = 0;
+ // run this function in a visitor pattern (it calls the callback at every cell it finds along the line)
+ bresenham_line_3D(bi_.sx, bi_.sy, bi_.sz, pts[i].x, pts[i].y, pts[i].z,
+ bi_.minX, bi_.minY, bi_.minZ, bi_.maxX, bi_.maxY, bi_.maxZ,
+ boost::bind(&CollisionMapperOcc::findSelfOcclusionAux, this, &occ, &state, &lock, _1, _2, _3));
+ }
+ }
+
+ bool findSelfOcclusionAux(CMap *occ, int *state, boost::mutex *lock, int x, int y, int z)
+ {
+ // this is the point in the robotFrame_
+ bool out = sf_.getMask(x * bi_.resolution + bi_.originX, y * bi_.resolution + bi_.originY, z * bi_.resolution + bi_.originZ);
+
+ // if we are already inside the robot, we mark the fact we want to stop when we are outside
+ if (out == false)
+ {
+ *state = 8;
+ return false;
+ }
+ else
+ {
+ // if we are now outside, but have seen the inside, we have a point we need to add to the collision map
+ if (*state == 8)
+ {
+ CollisionPoint c;
+ c.x = x;
+ c.y = y;
+ c.z = z;
+ lock->lock();
+ occ->insert(c);
+ lock->unlock();
+ return true;
+ }
+ else
+ // if we have not seen the inside, but we are outside, it could be we are just above the obstacle,
+ // so we check the next cell along the line as well
+ if (*state == 0)
+ {
+ *state = 1;
+ return false;
+ }
+ else
+ // at this point, the ray is probably barely touching the padding of the arm, so it is safe to ignore
+ return true;
+ }
+ }
+
+ /** Construct an axis-aligned collision map from a point cloud assumed to be in the robot frame */
+ void constructCollisionMap(const robot_msgs::PointCloud &cloud, const std::vector<bool> &mask, bool keep, CMap &map)
+ {
+ const unsigned int n = cloud.pts.size();
+ CollisionPoint c;
+
+ for (unsigned int i = 0 ; i < n ; ++i)
+ if (mask[i] == keep)
+ {
+ const robot_msgs::Point32 &p = cloud.pts[i];
+ if (p.x > bi_.real_minX && p.x < bi_.real_maxX && p.y > bi_.real_minY && p.y < bi_.real_maxY && p.z > bi_.real_minZ && p.z < bi_.real_maxZ)
+ {
+ c.x = (int)(0.5 + (p.x - bi_.originX) / bi_.resolution);
+ c.y = (int)(0.5 + (p.y - bi_.originY) / bi_.resolution);
+ c.z = (int)(0.5 + (p.z - bi_.originZ) / bi_.resolution);
+ map.insert(c);
+ }
+ }
+ }
+
+
+ /* Based on http://www.cit.gu.edu.au/~anthony/info/graphics/bresenham.procs */
+ /* Form the line (x1, y1, z1) -> (x2, y2, z2) and generate points on it starting at (x2, y2, z2)
+ * until it reaches a boundary of the box (minX, minY, minZ) -> (maxX, maxY, maxZ).
+ * If the callback returns true, the segment is stopped */
+ void bresenham_line_3D(int x1, int y1, int z1, int x2, int y2, int z2,
+ int minX, int minY, int minZ, int maxX, int maxY, int maxZ,
+ const boost::function<bool(int, int, int)> &callback) const
+ {
+ int i, dx, dy, dz, l, m, n, x_inc, y_inc, z_inc, err_1, err_2, dx2, dy2, dz2;
+ int pixel[3];
+
+ pixel[0] = x2;
+ pixel[1] = y2;
+ pixel[2] = z2;
+ dx = x2 - x1;
+ dy = y2 - y1;
+ dz = z2 - z1;
+
+ int c = -1;
+ if (dx != 0)
+ {
+ int cx = (maxX - x1) / dx;
+ if (cx <= 0)
+ cx = (minX - x1) / dx;
+ if (cx < c || c < 0)
+ c = cx;
+ }
+
+ if (dy != 0)
+ {
+ int cy = (maxY - y1) / dy;
+ if (cy <= 0)
+ cy = (minY - y1) / dy;
+ if (cy < c || c < 0)
+ c = cy;
+ }
+
+ if (dz != 0)
+ {
+ int cz = (maxZ - z1) / dz;
+ if (cz <= 0)
+ cz = (minZ - z1) / dz;
+ if (cz < c || c < 0)
+ c = cz;
+ }
+
+ if (c > 0)
+ {
+ dx *= c;
+ dy *= c;
+ dz *= c;
+ }
+
+ x_inc = (dx < 0) ? -1 : 1;
+ l = abs(dx);
+ y_inc = (dy < 0) ? -1 : 1;
+ m = abs(dy);
+ z_inc = (dz < 0) ? -1 : 1;
+ n = abs(dz);
+ dx2 = l << 1;
+ dy2 = m << 1;
+ dz2 = n << 1;
+
+ if ((l >= m) && (l >= n)) {
+ err_1 = dy2 - l;
+ err_2 = dz2 - l;
+ for (i = 0; i < l; i++) {
+ if (callback(pixel[0], pixel[1], pixel[2]))
+ return;
+ if (err_1 > 0) {
+ pixel[1] += y_inc;
+ err_1 -= dx2;
+ }
+ if (err_2 > 0) {
+ pixel[2] += z_inc;
+ err_2 -= dx2;
+ }
+ err_1 += dy2;
+ err_2 += dz2;
+ pixel[0] += x_inc;
+ }
+ } else if ((m >= l) && (m >= n)) {
+ err_1 = dx2 - m;
+ err_2 = dz2 - m;
+ for (i = 0; i < m; i++) {
+ if (callback(pixel[0], pixel[1], pixel[2]))
+ return;
+ if (err_1 > 0) {
+ pixel[0] += x_inc;
+ err_1 -= dy2;
+ }
+ if (err_2 > 0) {
+ pixel[2] += z_inc;
+ err_2 -= dy2;
+ }
+ err_1 += dx2;
+ err_2 += dz2;
+ pixel[1] += y_inc;
+ }
+ } else {
+ err_1 = dy2 - n;
+ err_2 = dx2 - n;
+ for (i = 0; i < n; i++) {
+ if (callback(pixel[0], pixel[1], pixel[2]))
+ return;
+ if (err_1 > 0) {
+ pixel[1] += y_inc;
+ err_1 -= dz2;
+ }
+ if (err_2 > 0) {
+ pixel[0] += x_inc;
+ err_2 -= dz2;
+ }
+ err_1 += dy2;
+ err_2 += dx2;
+ pixel[2] += z_inc;
+ }
+ }
+ callback(pixel[0], pixel[1], pixel[2]);
+ }
+
+ void publishCollisionMap(const CMap &map) const
+ {
+ robot_msgs::CollisionMap cmap;
+ cmap.header = header_;
+ const unsigned int ms = map.size();
+
+ for (CMap::const_iterator it = map.begin() ; it != map.end() ; ++it)
+ {
+ const CollisionPoint &cp = *it;
+ robot_msgs::OrientedBoundingBox box;
+ box.extents.x = box.extents.y = box.extents.z = bi_.resolution;
+ box.axis.x = box.axis.y = 0.0; box.axis.z = 1.0;
+ box.angle = 0.0;
+ box.center.x = cp.x * bi_.resolution + bi_.originX;
+ box.center.y = cp.y * bi_.resolution + bi_.originY;
+ box.center.z = cp.z * bi_.resolution + bi_.originZ;
+ cmap.boxes.push_back(box);
+ }
+ cmapPublisher_.publish(cmap);
+
+ ROS_DEBUG("Published collision map with %u boxes", ms);
+ }
+
+ tf::TransformListener tf_;
+ robot_self_filter::SelfMask sf_;
+ tf::MessageNotifier<robot_msgs::PointCloud> mn_;
+ ros::NodeHandle nh_;
+ ros::Publisher cmapPublisher_;
+ roslib::Header header_;
+
+ CMap currentMap_;
+ BoxInfo bi_;
+ std::string fixedFrame_;
+ std::string robotFrame_;
+
+};
+
+int main (int argc, char** argv)
+{
+ ros::init(argc, argv, "collision_map_occ");
+
+ CollisionMapperOcc cm;
+
+ ros::spin();
+
+ return 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-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-06-23 02:51:34 UTC (rev 17482)
@@ -134,7 +134,7 @@
header.frame_id = link->name;
for (unsigned int i = 0 ; i < link->attachedBodies.size() ; ++i)
{
- planning_models::shapes::Box *box = dynamic_cast<planning_models::shapes::Box*>(link->attachedBodies[i]->shape);
+ shapes::Box *box = dynamic_cast<shapes::Box*>(link->attachedBodies[i]->shape);
if (box)
{
btVector3 &v = link->attachedBodies[i]->attachTrans.getOrigin();
Modified: pkg/trunk/motion_planning/planning_environment/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-06-23 02:51:34 UTC (rev 17482)
@@ -17,6 +17,7 @@
<depend package="robot_msgs" />
<depend package="mechanism_msgs" />
<depend package="motion_planning_msgs" />
+ <depend package="geometric_shapes" />
<depend package="planning_models" />
<depend package="collision_space" />
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-23 02:51:34 UTC (rev 17482)
@@ -219,7 +219,7 @@
link->attachedBodies[j]->attachTrans.setOrigin(btVector3(centerP.point.x, centerP.point.y, centerP.point.z));
// this is a HACK! we should have orientation
- planning_models::shapes::Box *box = new planning_models::shapes::Box();
+ shapes::Box *box = new shapes::Box();
box->size[0] = attachedObject->objects[i].max_bound.x - attachedObject->objects[i].min_bound.x;
box->size[1] = attachedObject->objects[i].max_bound.y - attachedObject->objects[i].min_bound.y;
box->size[2] = attachedObject->objects[i].max_bound.z - attachedObject->objects[i].min_bound.z;
Modified: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-06-23 02:51:34 UTC (rev 17482)
@@ -7,7 +7,6 @@
rospack_add_library(planning_models src/kinematic.cpp
src/kinematic_state_params.cpp
- src/load_mesh.cpp
src/output.cpp)
# Unit tests
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06-23 02:51:34 UTC (rev 17482)
@@ -37,7 +37,7 @@
#ifndef PLANNING_MODELS_KINEMATIC_ROBOT_MODEL_
#define PLANNING_MODELS_KINEMATIC_ROBOT_MODEL_
-#include "planning_models/shapes.h"
+#include <geometric_shapes/shapes.h>
#include "planning_models/output.h"
#include <urdf/URDF.h>
Deleted: pkg/trunk/motion_planning/planning_models/include/planning_models/shapes.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/shapes.h 2009-06-23 02:05:11 UTC (rev 17481)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/shapes.h 2009-06-23 02:51:34 UTC (rev 17482)
@@ -1,177 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-/** \author Ioan Sucan */
-
-#ifndef PLANNING_MODELS_SHAPES_
-#define PLANNING_MODELS_SHAPES_
-
-#include <cstdlib>
-
-namespace planning_models
-{
-
- namespace shapes
- {
-
- /** A basic definition of a shape. Shapes to be centered at origin */
- class Shape
- {
- public:
- Shape(void)
- {
- type = UNKNOWN;
- }
-
- virtual ~Shape(void)
- {
- }
-
- enum { UNKNOWN, SPHERE, CYLINDER, BOX, MESH }
- type;
-
- };
-
- /** Definition of a sphere */
- class Sphere : public Shape
- {
- public:
- Sphere(void) : Shape()
- {
- type = SPHERE;
- radius = 0.0;
- }
-
- Sphere(double r) : Shape()
- {
- type = SPHERE;
- radius = r;
- }
-
- double radius;
- };
-
- /** Definition of a cylinder */
- class Cylinder : public Shape
- {
- public:
- Cylinder(void) : Shape()
- {
- type = CYLINDER;
- length = radius = 0.0;
- }
-
- Cylinder(double r, double l) : Shape()
- {
- type = CYLINDER;
- length = l;
- radius = r;
- }
-
- double length, radius;
- };
-
- /** Definition of a box */
- class Box : public Shape
- {
- public:
- Box(void) : Shape()
- {
- type = BOX;
- size[0] = size[1] = size[2] = 0.0;
- }
-
- Box(double x, double y, double z) : Shape()
- {
- type = BOX;
- size[0] = x;
- size[1] = y;
- size[2] = z;
- }
-
- /** x, y, z */
- double size[3];
- };
-
- /** Definition of a mesh */
- class Mesh : public Shape
- {
- public...
[truncated message content] |
|
From: <is...@us...> - 2009-06-23 02:55:19
|
Revision: 17484
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17484&view=rev
Author: isucan
Date: 2009-06-23 02:55:18 +0000 (Tue, 23 Jun 2009)
Log Message:
-----------
renamed new_robot_self_filter to robot_self_filter
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/manifest.xml
pkg/trunk/mapping/collision_map/manifest.xml
pkg/trunk/util/robot_self_filter/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/util/robot_self_filter/
Removed Paths:
-------------
pkg/trunk/util/new_robot_self_filter/
Modified: pkg/trunk/demos/tabletop_manipulation/manifest.xml
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/manifest.xml 2009-06-23 02:51:53 UTC (rev 17483)
+++ pkg/trunk/demos/tabletop_manipulation/manifest.xml 2009-06-23 02:55:18 UTC (rev 17484)
@@ -7,7 +7,7 @@
<depend package="2dnav_pr2"/>
<depend package="pr2_gazebo"/>
<depend package="laser_scan"/>
- <depend package="new_robot_self_filter"/>
+ <depend package="robot_self_filter"/>
<depend package="point_cloud_assembler"/>
<depend package="table_object_detector"/>
<depend package="collision_map"/>
Modified: pkg/trunk/mapping/collision_map/manifest.xml
===================================================================
--- pkg/trunk/mapping/collision_map/manifest.xml 2009-06-23 02:51:53 UTC (rev 17483)
+++ pkg/trunk/mapping/collision_map/manifest.xml 2009-06-23 02:55:18 UTC (rev 17484)
@@ -15,5 +15,5 @@
<depend package="robot_srvs" />
<depend package="visualization_msgs" />
<depend package="point_cloud_mapping" />
- <depend package="new_robot_self_filter" />
+ <depend package="robot_self_filter" />
</package>
Property changes on: pkg/trunk/util/robot_self_filter
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/util/new_robot_self_filter:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Modified: pkg/trunk/util/robot_self_filter/CMakeLists.txt
===================================================================
--- pkg/trunk/util/new_robot_self_filter/CMakeLists.txt 2009-06-23 02:51:53 UTC (rev 17483)
+++ pkg/trunk/util/robot_self_filter/CMakeLists.txt 2009-06-23 02:55:18 UTC (rev 17484)
@@ -10,7 +10,7 @@
set(ROS_BUILD_TYPE Release)
-rospack(new_robot_self_filter)
+rospack(robot_self_filter)
rospack_add_library(new_robot_self_filter src/self_mask.cpp)
rospack_add_openmp_flags(new_robot_self_filter)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-06-23 03:34:52
|
Revision: 17485
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17485&view=rev
Author: vijaypradeep
Date: 2009-06-23 03:34:46 +0000 (Tue, 23 Jun 2009)
Log Message:
-----------
Changing namespacing for stereodcam, stereoproc, and associated launch files
Modified Paths:
--------------
pkg/trunk/calibration/kinematic_calibration/demo/videre.launch
pkg/trunk/drivers/cam/dcam/src/nodes/stereodcam.cpp
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.machine
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.machine
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.machine
pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp
Added Paths:
-----------
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/stereo.launch
pkg/trunk/vision/stereo_image_proc/stereoproc.launch
Removed Paths:
-------------
pkg/trunk/vision/stereo_image_proc/stereo.launch
Modified: pkg/trunk/calibration/kinematic_calibration/demo/videre.launch
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/demo/videre.launch 2009-06-23 02:55:18 UTC (rev 17484)
+++ pkg/trunk/calibration/kinematic_calibration/demo/videre.launch 2009-06-23 03:34:46 UTC (rev 17485)
@@ -4,7 +4,7 @@
-->
<param name="videre_mode" type="str" value="rectified"/>
<!-- params for the stereo_image_proc node -->
- <param name="exposure" type="int" value="0"/>
+ <param name="exposure" type="int" value="20"/>
<param name="exposure_auto" type="bool" value="false"/>
<!-- <param name="brightness" type="int" value="5"/ -->
Modified: pkg/trunk/drivers/cam/dcam/src/nodes/stereodcam.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam/src/nodes/stereodcam.cpp 2009-06-23 02:55:18 UTC (rev 17484)
+++ pkg/trunk/drivers/cam/dcam/src/nodes/stereodcam.cpp 2009-06-23 03:34:46 UTC (rev 17485)
@@ -163,8 +163,6 @@
std_msgs::Empty check_params_msg_;
- std::string stereo_name_;
-
public:
dcam::StereoDcam* stcam_;
@@ -184,30 +182,28 @@
diagnostic_.add( timestamp_diag_ );
diagnostic_.addUpdater( &StereoDcamNode::freqStatus );
- stereo_name_ = mapName("stereo") + std::string("/");
-
// If there is a camera...
if (num_cams > 0)
{
// Check our gui parameter, or else use first camera
uint64_t guid;
- if (hasParam(stereo_name_+ std::string("guid")))
+ if (hasParam("~guid"))
{
string guid_str;
- getParam(stereo_name_ + std::string("guid"), guid_str);
+ getParam("~guid", guid_str);
guid = strtoll(guid_str.c_str(), NULL, 16);
} else {
guid = dcam::getGuid(0);
}
- param(stereo_name_ + std::string("frame_id"), frame_id_, string("stereo"));
+ param("~frame_id", frame_id_, string("stereo"));
// Get the ISO speed parameter if available
string str_speed;
dc1394speed_t speed;
- param(stereo_name_ + std::string("speed"), str_speed, string("S400"));
+ param("~speed", str_speed, string("S400"));
if (str_speed == string("S100"))
speed = DC1394_ISO_SPEED_100;
else if (str_speed == string("S200"))
@@ -218,7 +214,7 @@
// Get the FPS parameter if available;
double dbl_fps;
dc1394framerate_t fps;
- param(stereo_name_ + std::string("fps"), dbl_fps, 30.0);
+ param("~fps", dbl_fps, 30.0);
desired_freq_ = dbl_fps;
if (dbl_fps >= 240.0)
fps = DC1394_FRAMERATE_240;
@@ -243,7 +239,7 @@
// Get the videre processing mode if available:
string str_videre_mode;
videre_proc_mode_t videre_mode = PROC_MODE_NONE;
- param(stereo_name_ + std::string("videre_mode"), str_videre_mode, string("none"));
+ param("~videre_mode", str_videre_mode, string("none"));
if (str_videre_mode == string("rectified"))
videre_mode = PROC_MODE_RECTIFIED;
else if (str_videre_mode == string("disparity"))
@@ -259,14 +255,14 @@
// Fetch the camera string and send it to the parameter server if people want it (they shouldn't)
std::string params(stcam_->getParameters());
- setParam(stereo_name_ + std::string("params"), params);
+ setParam("~params", params);
- setParam(stereo_name_ + std::string("exposure_max"), (int)stcam_->expMax);
- setParam(stereo_name_ + std::string("exposure_min"), (int)stcam_->expMin);
- setParam(stereo_name_ + std::string("gain_max"), (int)stcam_->gainMax);
- setParam(stereo_name_ + std::string("gain_min"), (int)stcam_->gainMin);
- setParam(stereo_name_ + std::string("brightness_max"), (int)stcam_->brightMax);
- setParam(stereo_name_ + std::string("brightness_min"), (int)stcam_->brightMin);
+ setParam("~exposure_max", (int)stcam_->expMax);
+ setParam("~exposure_min", (int)stcam_->expMin);
+ setParam("~gain_max", (int)stcam_->gainMax);
+ setParam("~gain_min", (int)stcam_->gainMin);
+ setParam("~brightness_max", (int)stcam_->brightMax);
+ setParam("~brightness_min", (int)stcam_->brightMin);
// Configure camera
stcam_->setFormat(mode, fps, speed);
@@ -277,9 +273,9 @@
stcam_->setSpeckleDiff(10);
stcam_->setCompanding(true);
- advertise<image_msgs::RawStereo>(stereo_name_ + std::string("raw_stereo"), 1);
+ advertise<image_msgs::RawStereo>("raw_stereo", 1);
- subscribe(stereo_name_ + std::string("check_params"), check_params_msg_, &StereoDcamNode::checkParams, 1);
+ subscribe("~check_params", check_params_msg_, &StereoDcamNode::checkParams, 1);
// Start the camera
stcam_->start();
@@ -302,32 +298,32 @@
void checkAndSetAll()
{
- checkAndSetIntBool("exposure", boost::bind(&dcam::Dcam::setExposure, stcam_, _1, _2));
- checkAndSetIntBool("gain", boost::bind(&dcam::Dcam::setGain, stcam_, _1, _2));
- checkAndSetIntBool("brightness", boost::bind(&dcam::Dcam::setBrightness, stcam_, _1, _2));
- checkAndSetBool("companding", boost::bind(&dcam::Dcam::setCompanding, stcam_, _1));
- checkAndSetBool("hdr", boost::bind(&dcam::Dcam::setHDR, stcam_, _1));
- checkAndSetBool("unique_check", boost::bind(&dcam::StereoDcam::setUniqueCheck, stcam_, _1));
- checkAndSetInt("texture_thresh", boost::bind(&dcam::StereoDcam::setTextureThresh, stcam_, _1));
- checkAndSetInt("unique_thresh", boost::bind(&dcam::StereoDcam::setUniqueThresh, stcam_, _1));
- checkAndSetInt("smoothness_thresh", boost::bind(&dcam::StereoDcam::setSmoothnessThresh, stcam_, _1));
- checkAndSetInt("horopter", boost::bind(&dcam::StereoDcam::setHoropter, stcam_, _1));
- checkAndSetInt("speckle_size", boost::bind(&dcam::StereoDcam::setSpeckleSize, stcam_, _1));
- checkAndSetInt("speckle_diff", boost::bind(&dcam::StereoDcam::setSpeckleDiff, stcam_, _1));
- checkAndSetInt("corr_size", boost::bind(&dcam::StereoDcam::setCorrsize, stcam_, _1));
- checkAndSetInt("num_disp", boost::bind(&dcam::StereoDcam::setNumDisp, stcam_, _1));
+ checkAndSetIntBool("~exposure", boost::bind(&dcam::Dcam::setExposure, stcam_, _1, _2));
+ checkAndSetIntBool("~gain", boost::bind(&dcam::Dcam::setGain, stcam_, _1, _2));
+ checkAndSetIntBool("~brightness", boost::bind(&dcam::Dcam::setBrightness, stcam_, _1, _2));
+ checkAndSetBool("~companding", boost::bind(&dcam::Dcam::setCompanding, stcam_, _1));
+ checkAndSetBool("~hdr", boost::bind(&dcam::Dcam::setHDR, stcam_, _1));
+ checkAndSetBool("~unique_check", boost::bind(&dcam::StereoDcam::setUniqueCheck, stcam_, _1));
+ checkAndSetInt("~texture_thresh", boost::bind(&dcam::StereoDcam::setTextureThresh, stcam_, _1));
+ checkAndSetInt("~unique_thresh", boost::bind(&dcam::StereoDcam::setUniqueThresh, stcam_, _1));
+ checkAndSetInt("~smoothness_thresh", boost::bind(&dcam::StereoDcam::setSmoothnessThresh, stcam_, _1));
+ checkAndSetInt("~horopter", boost::bind(&dcam::StereoDcam::setHoropter, stcam_, _1));
+ checkAndSetInt("~speckle_size", boost::bind(&dcam::StereoDcam::setSpeckleSize, stcam_, _1));
+ checkAndSetInt("~speckle_diff", boost::bind(&dcam::StereoDcam::setSpeckleDiff, stcam_, _1));
+ checkAndSetInt("~corr_size", boost::bind(&dcam::StereoDcam::setCorrsize, stcam_, _1));
+ checkAndSetInt("~num_disp", boost::bind(&dcam::StereoDcam::setNumDisp, stcam_, _1));
}
void checkAndSetIntBool(std::string param_name, boost::function<void(int, bool)> setfunc)
{
- if (hasParam(stereo_name_ + param_name) || hasParam(stereo_name_ + param_name + std::string("_auto")))
+ if (hasParam(param_name) || hasParam(param_name + std::string("_auto")))
{
int val = 0;
bool isauto = false;
- param( stereo_name_ + param_name, val, 0);
- param( stereo_name_ + param_name + std::string("_auto"), isauto, false);
+ param( param_name, val, 0);
+ param( param_name + std::string("_auto"), isauto, false);
int testval = (val * (!isauto));
@@ -343,11 +339,11 @@
void checkAndSetBool(std::string param_name, boost::function<bool(bool)> setfunc)
{
- if (hasParam(stereo_name_ + param_name))
+ if (hasParam(param_name))
{
bool on = false;
- param(stereo_name_ + param_name, on, false);
+ param(param_name, on, false);
std::map<std::string, int>::iterator cacheval = paramcache_.find(param_name);
@@ -362,12 +358,12 @@
void checkAndSetInt(std::string param_name, boost::function<bool(int)> setfunc)
{
- if (hasParam(stereo_name_ + param_name))
+ if (hasParam(param_name))
{
int val = 0;
- param(stereo_name_ + param_name, val, 0);
+ param( param_name, val, 0);
std::map<std::string, int>::iterator cacheval = paramcache_.find(param_name);
@@ -409,7 +405,7 @@
// Doing this as a stopgap
// measure.
timestamp_diag_.tick(raw_stereo_.header.stamp);
- publish(stereo_name_ + std::string("raw_stereo"), raw_stereo_);
+ publish("raw_stereo", raw_stereo_);
count_++;
return true;
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch 2009-06-23 02:55:18 UTC (rev 17484)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch 2009-06-23 03:34:46 UTC (rev 17485)
@@ -47,16 +47,8 @@
<param name="imu/autocalibrate" type="bool" value="true" />
<node machine="four" pkg="imu_node" type="imu_node" output="screen"/>
-<!-- Stereo cam
- <node machine="four" pkg="dcam" type="stereodcam" name="stereodcam" args="">
- <param name="videre_mode" type="string" value="disparity_raw" />
- <param name="fps" type="double" value="15.0"/>
- <param name="do_colorize" type="bool" value="False"/>
- <param name="do_rectify" type="bool" value="False"/>
- <param name="do_stereo" type="bool" value="False"/>
- <param name="frame_id" type="string" value="stereo_optical_frame"/>
-</node>
--->
+<!-- Videre Stereo cam -->
+ <include file="$(find pr2_alpha)/stereo.launch" />
<!-- Sound -->
<node pkg="sound_play" type="soundplay_node.py" machine="three" />
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.machine
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.machine 2009-06-23 02:55:18 UTC (rev 17484)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.machine 2009-06-23 03:34:46 UTC (rev 17485)
@@ -2,7 +2,8 @@
<machine name="realtime_root" user="root" address="pre1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="never"/>
<machine name="realtime" address="pre1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
- <machine name="two" address="pre2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="true"/>
- <machine name="three" address="pre3" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
- <machine name="four" address="pre4" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
+ <machine name="two" address="pre2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="true"/>
+ <machine name="three" address="pre3" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
+ <machine name="four" address="pre4" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
+ <machine name="stereo" address="pre4" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
</launch>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-06-23 02:55:18 UTC (rev 17484)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-06-23 03:34:46 UTC (rev 17485)
@@ -47,22 +47,8 @@
<param name="imu/autocalibrate" type="bool" value="true" />
<node machine="four" pkg="imu_node" type="imu_node" output="screen"/>
-<!-- Stereo cam -->
- <group ns="stereo">
- <param name="videre_mode" type="str" value="none"/>
- <param name="do_colorize" type="bool" value="True"/>
- <param name="do_rectify" type="bool" value="True"/>
- <param name="do_stereo" type="bool" value="True"/>
- <param name="do_calc_points" type="bool" value="True"/>
- <param name="do_keep_coords" type="bool" value="True"/>
- <param name="fps" type="double" value="15.0"/>
- <param name="frame_id" type="string" value="stereo_optical_frame"/>
- <param name="exposure_auto" type="bool" value="true"/>
- <param name="brightness_auto" type="bool" value="true"/>
- <param name="gain_auto" type="bool" value="true"/>
- <param name="num_disp" value="128"/>
- </group>
- <node machine="four" pkg="dcam" type="stereodcam" respawn="false"/>
+ <!-- Videre Stereo cam -->
+ <include file="$(find pr2_alpha)/stereo.launch" />
<!-- Prosilica camera setup -->
<group ns="prosilica">
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.machine
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.machine 2009-06-23 02:55:18 UTC (rev 17484)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.machine 2009-06-23 03:34:46 UTC (rev 17485)
@@ -5,4 +5,7 @@
<machine name="two" address="prf2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="true" />
<machine name="three" address="prf3" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
<machine name="four" address="prf4" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
+
+ <!-- Defines the machine that the Videre Stereocam is attached to -->
+ <machine name="stereo" address="prf4" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
</launch>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch 2009-06-23 02:55:18 UTC (rev 17484)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch 2009-06-23 03:34:46 UTC (rev 17485)
@@ -76,17 +76,8 @@
<node pkg="runtime_monitor" type="disk_usage.py" args="$(optenv HOME /home)" machine="three"/>
<node pkg="runtime_monitor" type="disk_usage.py" args="$(optenv HOME /home)" machine="four"/>
-<!-- Stereo cam -->
- <group ns="stereo">
- <param name="videre_mode" type="str" value="none"/>
- <param name="exposure_auto" type="bool" value="True" />
- <param name="gain_auto" type="bool" value="True" />
- <param name="brightness_auto" type="bool" value="True" />
- <param name="fps" type="double" value="15.0"/>
- <param name="frame_id" type="string" value="stereo_optical_frame"/>
- <param name="num_disp" value="128"/>
- </group>
- <node machine="four" pkg="dcam" type="stereodcam" respawn="false"/>
+ <!-- Videre Stereo cam -->
+ <include file="$(find pr2_alpha)/stereo.launch" />
<!-- Plug to chessboard offsets -->
<group ns="plug_detector">
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.machine
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.machine 2009-06-23 02:55:18 UTC (rev 17484)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.machine 2009-06-23 03:34:46 UTC (rev 17485)
@@ -2,7 +2,8 @@
<machine name="realtime_root" user="root" address="prg1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="never" />
<machine name="realtime" address="prg1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
- <machine name="two" address="prg2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="true"/>
- <machine name="three" address="prg3" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
- <machine name="four" address="prg4" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
+ <machine name="two" address="prg2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="true"/>
+ <machine name="three" address="prg3" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
+ <machine name="four" address="prg4" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
+ <machine name="stereo" address="prg4" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
</launch>
Added: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/stereo.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/stereo.launch (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/stereo.launch 2009-06-23 03:34:46 UTC (rev 17485)
@@ -0,0 +1,35 @@
+<launch>
+ <!-- Brings up the stereocam on the PR2. Since the machine
+ tag is definied, this file needs to be included as part
+ of a launch file that defines the machine (ie. prx.launch) -->
+
+ <group ns="stereo">
+ <!-- Start the 1394 videre camera driver -->
+ <node machine="stereo" pkg="dcam" type="stereodcam" respawn="false" >
+ <param name="videre_mode" type="str" value="none"/>
+ <param name="fps" type="double" value="4.0"/>
+ <param name="frame_id" type="string" value="stereo_optical_frame"/>
+ <param name="exposure_auto" type="bool" value="true"/>
+ <param name="brightness_auto" type="bool" value="true"/>
+ <param name="gain_auto" type="bool" value="true"/>
+ </node>
+
+ <!-- Start the stereo processing node -->
+ <!-- This can be run on any machine, but running
+ it on the same machine as stereodcam reduces network latency -->
+
+ <!-- Probably don't want to autostart stereoproc right now, since it's not a hardware driver -->
+
+ <!--
+ <node machine="stereo" pkg="stereo_image_proc" type="stereoproc"
+ respawn="false" output="screen" name="stereoproc">
+ <param name="do_colorize" type="bool" value="True"/>
+ <param name="do_rectify" type="bool" value="true"/>
+ <param name="do_stereo" type="bool" value="True"/>
+ <param name="do_calc_points" type="bool" value="True"/>
+ <param name="do_keep_coords" type="bool" value="True"/>
+ </node>
+ -->
+ </group>
+
+</launch>
Modified: pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp
===================================================================
--- pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp 2009-06-23 02:55:18 UTC (rev 17484)
+++ pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp 2009-06-23 03:34:46 UTC (rev 17485)
@@ -75,28 +75,23 @@
string frame_id_;
- std::string stereo_name_;
-
public:
cam::StereoData* stdata_;
StereoProc() : ros::Node("stereoproc"), diagnostic_(this), count_(0), stdata_(NULL)
{
-
- stereo_name_ = mapName("stereo") + std::string("/");
-
diagnostic_.addUpdater( &StereoProc::freqStatus );
- param(stereo_name_ + std::string("do_colorize"), do_colorize_, false);
+ param("~do_colorize", do_colorize_, false);
- param(stereo_name_ + std::string("do_rectify"), do_rectify_, false);
+ param("~do_rectify", do_rectify_, false);
- param(stereo_name_ + std::string("do_stereo"), do_stereo_, false);
+ param("~do_stereo", do_stereo_, false);
- param(stereo_name_ + std::string("do_calc_points"), do_calc_points_, false);
+ param("~do_calc_points", do_calc_points_, false);
- param(stereo_name_ + std::string("do_keep_coords"), do_keep_coords_, false);
+ param("~do_keep_coords", do_keep_coords_, false);
if (do_keep_coords_) {
ROS_INFO("I'm keeping the image coordonate in the point cloud\n");
@@ -115,38 +110,38 @@
// stdata_->setSpeckleDiff(10);
int unique_thresh;
- param(stereo_name_ + "unique_thresh", unique_thresh, 36);
+ param("~unique_thresh", unique_thresh, 36);
stdata_->setTextureThresh(unique_thresh);
int texture_thresh;
- param(stereo_name_ + "texture_thresh", texture_thresh, 30);
+ param("~texture_thresh", texture_thresh, 30);
stdata_->setUniqueThresh(texture_thresh);
int speckle_size;
- param(stereo_name_ + "speckle_size", speckle_size, 100);
+ param("~speckle_size", speckle_size, 100);
stdata_->setSpeckleRegionSize(speckle_size);
int speckle_diff;
- param(stereo_name_ + "speckle_diff", speckle_diff, 10);
+ param("~speckle_diff", speckle_diff, 10);
stdata_->setSpeckleDiff(speckle_diff);
int smoothness_thresh;
- if (getParam(stereo_name_ + "smoothness_thresh", smoothness_thresh))
+ if (getParam("~smoothness_thresh", smoothness_thresh))
stdata_->setSmoothnessThresh(smoothness_thresh);
int horopter;
- if (getParam(stereo_name_ + "horopter", horopter))
+ if (getParam("~horopter", horopter))
stdata_->setHoropter(horopter);
int corr_size;
- if (getParam(stereo_name_ + "corr_size", corr_size))
+ if (getParam("~corr_size", corr_size))
stdata_->setCorrSize(corr_size);
int num_disp;
- if (getParam(stereo_name_ + "num_disp", num_disp))
+ if (getParam("~num_disp", num_disp))
stdata_->setNumDisp(num_disp);
- subscribe(stereo_name_ + std::string("raw_stereo"), raw_stereo_, &StereoProc::rawCb, 1);
+ subscribe("raw_stereo", raw_stereo_, &StereoProc::rawCb, 1);
}
~StereoProc()
@@ -190,8 +185,8 @@
void publishCam()
{
- publishImages(stereo_name_ + std::string("left/"), stdata_->imLeft);
- publishImages(stereo_name_ + std::string("right/"), stdata_->imRight);
+ publishImages("left/", stdata_->imLeft);
+ publishImages("right/", stdata_->imRight);
if (stdata_->hasDisparity)
{
@@ -217,7 +212,7 @@
disparity_info_.speckle_region_size = stdata_->speckleRegionSize;
disparity_info_.unique_check = stdata_->unique_check;
- publish(stereo_name_ + std::string("disparity_info"), disparity_info_);
+ publish("disparity_info", disparity_info_);
fillImage(img_, "disparity",
stdata_->imHeight, stdata_->imWidth, 1,
@@ -226,7 +221,7 @@
img_.header.stamp = raw_stereo_.header.stamp;
img_.header.frame_id = raw_stereo_.header.frame_id;
- publish(stereo_name_ + std::string("disparity"), img_);
+ publish("disparity", img_);
}
if (do_calc_points_)
@@ -266,7 +261,7 @@
}
}
- publish(stereo_name_ + std::string("cloud"), cloud_);
+ publish("cloud", cloud_);
}
stereo_info_.header.stamp = raw_stereo_.header.stamp;
@@ -278,7 +273,7 @@
memcpy((char*)(&stereo_info_.Om[0]), (char*)(stdata_->Om), 3*sizeof(double));
memcpy((char*)(&stereo_info_.RP[0]), (char*)(stdata_->RP), 16*sizeof(double));
- publish(stereo_name_ + std::string("stereo_info"), stereo_info_);
+ publish("stereo_info", stereo_info_);
}
void publishImages(std::string base_name, cam::ImageData* img_data)
@@ -391,19 +386,19 @@
void advertiseCam()
{
- advertise<image_msgs::StereoInfo>(stereo_name_ + std::string("stereo_info"), 1);
+ advertise<image_msgs::StereoInfo>("stereo_info", 1);
- advertiseImages(stereo_name_ + std::string("left/"), stdata_->imLeft);
- advertiseImages(stereo_name_ + std::string("right/"), stdata_->imRight);
+ advertiseImages("left/", stdata_->imLeft);
+ advertiseImages("right/", stdata_->imRight);
if (stdata_->hasDisparity)
{
- advertise<image_msgs::DisparityInfo>(stereo_name_ + std::string("disparity_info"), 1);
- advertise<image_msgs::Image>(stereo_name_ + std::string("disparity"), 1);
+ advertise<image_msgs::DisparityInfo>("disparity_info", 1);
+ advertise<image_msgs::Image>("disparity", 1);
}
if (do_calc_points_)
- advertise<robot_msgs::PointCloud>(stereo_name_ + std::string("cloud"),1);
+ advertise<robot_msgs::PointCloud>("cloud",1);
}
Deleted: pkg/trunk/vision/stereo_image_proc/stereo.launch
===================================================================
--- pkg/trunk/vision/stereo_image_proc/stereo.launch 2009-06-23 02:55:18 UTC (rev 17484)
+++ pkg/trunk/vision/stereo_image_proc/stereo.launch 2009-06-23 03:34:46 UTC (rev 17485)
@@ -1,40 +0,0 @@
-<launch>
- <!-- videre_mode should be one of the following:
- disparity: Disparity and rectification done on chip.
- Provides: Disparity and left mono image
- disparity_raw: Disparity done on chip (debayer and rectification in software).
- Provides: disparity and left color image.
- none: No stereo on chip (all processing done in software).
- Provides: all 3 images available
- rectified: Rectification on chip
- Provides: all 3 images available but no color
- -->
- <param name="stereo/videre_mode" type="str" value="none"/>
- <param name="stereo/do_colorize" type="bool" value="True"/>
- <param name="stereo/do_rectify" type="bool" value="True"/>
- <param name="stereo/do_stereo" type="bool" value="True"/>
- <param name="stereo/do_calc_points" type="bool" value="True"/>
- <param name="stereo/do_keep_coords" type="bool" value="True"/>
-
- <!-- stereo processing parameters
- texture_thresh: Threshold for removing bad disparities based on texture
- Default value: 30
- unique_thresh: Threshold for removing bad disparities based on ambiguity
- Default value: 36
- speckle_diff: Threshold for region-growing (1/16 pixel disparity)
- Default value: 10
- speckle_size: Threshold for disparity region size (pixels)
- Default value: 100
- horopter: X offset for close-in stereo (pixels)
- Default value: 0
- corr_size: Correlation window size (pixels)
- Default value: 15
- num_disp: Number of disparities (pixels)
- Default value: 64
- -->
-
- <param name="stereo/num_disp" type="int" value="64"/>
-
- <node pkg="stereo_image_proc" type="stereoproc" respawn="false" output="screen"/>
-</launch>
-
Copied: pkg/trunk/vision/stereo_image_proc/stereoproc.launch (from rev 17442, pkg/trunk/vision/stereo_image_proc/stereo.launch)
===================================================================
--- pkg/trunk/vision/stereo_image_proc/stereoproc.launch (rev 0)
+++ pkg/trunk/vision/stereo_image_proc/stereoproc.launch 2009-06-23 03:34:46 UTC (rev 17485)
@@ -0,0 +1,40 @@
+<launch>
+ <!-- videre_mode should be one of the following:
+ disparity: Disparity and rectification done on chip.
+ Provides: Disparity and left mono image
+ disparity_raw: Disparity done on chip (debayer and rectification in software).
+ Provides: disparity and left color image.
+ none: No stereo on chip (all processing done in software).
+ Provides: all 3 images available
+ rectified: Rectification on chip
+ Provides: all 3 images available but no color
+ -->
+ <!-- stereo processing parameters
+ texture_thresh: Threshold for removing bad disparities based on texture
+ Default value: 30
+ unique_thresh: Threshold for removing bad disparities based on ambiguity
+ Default value: 36
+ speckle_diff: Threshold for region-growing (1/16 pixel disparity)
+ Default value: 10
+ speckle_size: Threshold for disparity region size (pixels)
+ Default value: 100
+ horopter: X offset for close-in stereo (pixels)
+ Default value: 0
+ corr_size: Correlation window size (pixels)
+ Default value: 15
+ num_disp: Numb...
[truncated message content] |
Thanks for helping keep SourceForge clean.
X