Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17206] pkg/trunk
|
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.
|
