Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21754] pkg/trunk
|
From: <vij...@us...> - 2009-08-13 01:57:47
|
Revision: 21754
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21754&view=rev
Author: vijaypradeep
Date: 2009-08-13 01:57:33 +0000 (Thu, 13 Aug 2009)
Log Message:
-----------
Porting move_arm to the new actionlib interface. Code compiles, but not yet tested
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/CMakeLists.txt
pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
pkg/trunk/highlevel/move_arm/manifest.xml
pkg/trunk/highlevel/move_arm/msg/MoveArmGoal.msg
pkg/trunk/highlevel/move_arm/src/actuate_gripper_action.cpp
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server.h
pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server_imp.h
Added Paths:
-----------
pkg/trunk/highlevel/move_arm/action/
pkg/trunk/highlevel/move_arm/action/ActuateGripper.action
pkg/trunk/highlevel/move_arm/action/MoveArm.action
pkg/trunk/highlevel/move_arm/include/
pkg/trunk/highlevel/move_arm/include/move_arm/
pkg/trunk/highlevel/move_arm/include/move_arm/move_arm_core.h
pkg/trunk/highlevel/move_arm/msg/ActuateGripperAction.msg
pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionFeedback.msg
pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionGoal.msg
pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionResult.msg
pkg/trunk/highlevel/move_arm/msg/ActuateGripperFeedback.msg
pkg/trunk/highlevel/move_arm/msg/ActuateGripperGoal.msg
pkg/trunk/highlevel/move_arm/msg/ActuateGripperResult.msg
pkg/trunk/highlevel/move_arm/msg/MoveArmAction.msg
pkg/trunk/highlevel/move_arm/msg/MoveArmActionFeedback.msg
pkg/trunk/highlevel/move_arm/msg/MoveArmActionGoal.msg
pkg/trunk/highlevel/move_arm/msg/MoveArmActionResult.msg
pkg/trunk/highlevel/move_arm/msg/MoveArmFeedback.msg
pkg/trunk/highlevel/move_arm/msg/MoveArmResult.msg
Removed Paths:
-------------
pkg/trunk/highlevel/move_arm/msg/MoveArmState.msg
Modified: pkg/trunk/highlevel/move_arm/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/move_arm/CMakeLists.txt 2009-08-13 01:35:58 UTC (rev 21753)
+++ pkg/trunk/highlevel/move_arm/CMakeLists.txt 2009-08-13 01:57:33 UTC (rev 21754)
@@ -22,8 +22,8 @@
# simple tests
-rospack_add_executable(oscillate_move test/oscillate_move.cpp)
-rospack_link_boost(oscillate_move thread)
+# rospack_add_executable(oscillate_move test/oscillate_move.cpp)
+# rospack_link_boost(oscillate_move thread)
# actuate gripper action test
-rospack_add_executable(actuate_gripper_action_test test/test_actuate_gripper.cpp)
+#rospack_add_executable(actuate_gripper_action_test test/test_actuate_gripper.cpp)
Added: pkg/trunk/highlevel/move_arm/action/ActuateGripper.action
===================================================================
--- pkg/trunk/highlevel/move_arm/action/ActuateGripper.action (rev 0)
+++ pkg/trunk/highlevel/move_arm/action/ActuateGripper.action 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1,3 @@
+float64 data
+---
+---
Copied: pkg/trunk/highlevel/move_arm/action/MoveArm.action (from rev 21753, pkg/trunk/highlevel/move_arm/msg/MoveArmGoal.msg)
===================================================================
--- pkg/trunk/highlevel/move_arm/action/MoveArm.action (rev 0)
+++ pkg/trunk/highlevel/move_arm/action/MoveArm.action 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1,18 @@
+
+# The goal state for the model to plan for. The state is not necessarily explicit.
+# The goal is considered achieved if all the constraints are satisfied.
+# An explicit state can be specified by setting joint constraints to a specific value.
+motion_planning_msgs/KinematicConstraints goal_constraints
+
+# The set of constraints that need to be satisfied by the entire solution path.
+motion_planning_msgs/KinematicConstraints path_constraints
+
+# The set of allowed contacts
+motion_planning_msgs/AcceptableContact[] contacts
+---
+---
+int32 mode
+int32 PLANNING=1
+int32 MOVING=2
+
+duration time_to_completion
Added: pkg/trunk/highlevel/move_arm/include/move_arm/move_arm_core.h
===================================================================
--- pkg/trunk/highlevel/move_arm/include/move_arm/move_arm_core.h (rev 0)
+++ pkg/trunk/highlevel/move_arm/include/move_arm/move_arm_core.h 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1,207 @@
+/*********************************************************************
+ *
+ * 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.
+
+ *
+ * Authors: Sachin Chitta, Ioan Sucan
+ *********************************************************************/
+
+#ifndef MOVE_ARM_MOVE_ARM_CORE_H_
+#define MOVE_ARM_MOVE_ARM_CORE_H_
+
+#include <ros/ros.h>
+
+#include <pr2_mechanism_controllers/TrajectoryStart.h>
+#include <pr2_mechanism_controllers/TrajectoryQuery.h>
+#include <pr2_mechanism_controllers/TrajectoryCancel.h>
+
+#include <planning_environment/monitors/planning_monitor.h>
+
+namespace move_arm
+{
+
+/// the string used internally to access control starting service; this should be remaped in the launch file
+static const std::string CONTROL_START_NAME = "controller_start";
+
+/// the string used internally to access control querying service; this should be remaped in the launch file
+static const std::string CONTROL_QUERY_NAME = "controller_query";
+
+/// the string used internally to access control canceling service; this should be remaped in the launch file
+static const std::string CONTROL_CANCEL_NAME = "controller_cancel";
+
+/// the string used internally to access the long range motion planning service; this should be remaped in the launch file
+static const std::string LR_MOTION_PLAN_NAME = "get_motion_plan_lr";
+
+/// the string used internally to access the short range motion planning service; this should be remaped in the launch file
+static const std::string SR_MOTION_PLAN_NAME = "get_motion_plan_sr";
+
+/// the string used internally to access valid state searching service; this should be remaped in the launch file
+static const std::string SEARCH_VALID_STATE_NAME = "get_valid_state";
+
+/// the string used internally to access inverse kinematics service; this should be remaped in the launch file
+static const std::string ARM_IK_NAME = "arm_ik";
+
+
+/** \brief Configuration of actions that need to actuate parts of the robot */
+class MoveArmCore
+{
+ friend class MoveArm;
+
+public:
+
+ MoveArmCore(void)
+ {
+ collisionModels_ = NULL;
+ planningMonitor_ = NULL;
+ }
+
+ virtual ~MoveArmCore(void)
+ {
+ if (planningMonitor_)
+ delete planningMonitor_;
+ if (collisionModels_)
+ delete collisionModels_;
+ }
+
+
+ bool configure(void)
+ {
+ nodeHandle_.param<std::string>("~group", group_, std::string());
+
+ if (group_.empty())
+ {
+ ROS_ERROR("No '~group' parameter specified. Without the name of the group of joints to plan for, action cannot start");
+ return false;
+ }
+
+ // monitor robot
+ collisionModels_ = new planning_environment::CollisionModels("robot_description");
+ planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, &tf_);
+
+ if (!collisionModels_->loadedModels())
+ return false;
+
+ nodeHandle_.param<bool>("~perform_ik", perform_ik_, true);
+
+ if (collisionModels_->getKinematicModel()->getGroupID(group_) < 0)
+ {
+ ROS_ERROR("Group '%s' is not known", group_.c_str());
+ return false;
+ }
+ else
+ ROS_INFO("Configuring action core for '%s' (IK is %senabled)", group_.c_str(), perform_ik_ ? "" : "not ");
+
+ planningMonitor_->waitForState();
+ planningMonitor_->waitForMap();
+
+ if (!getControlJointNames(groupJointNames_))
+ return false;
+
+ nodeHandle_.param<bool>("~show_collisions", show_collisions_, false);
+
+ if (show_collisions_)
+ ROS_INFO("Found collisions will be displayed as visualization markers");
+
+ return true;
+ }
+
+protected:
+
+ bool getControlJointNames(std::vector<std::string> &joint_names)
+ {
+ ros::ServiceClient client_query = nodeHandle_.serviceClient<pr2_mechanism_controllers::TrajectoryQuery>(CONTROL_QUERY_NAME);
+ pr2_mechanism_controllers::TrajectoryQuery::Request req_query;
+ pr2_mechanism_controllers::TrajectoryQuery::Response res_query;
+ req_query.trajectoryid = pr2_mechanism_controllers::TrajectoryQuery::Request::Query_Joint_Names;
+
+ bool result = client_query.call(req_query, res_query);
+
+ if (!result)
+ {
+ ROS_INFO("Querying controller for joint names ...");
+ ros::Duration(5.0).sleep();
+ result = client_query.call(req_query, res_query);
+ if (result)
+ ROS_INFO("Joint names received");
+ }
+
+ if (!result)
+ {
+ ROS_ERROR("Unable to retrieve controller joint names from control query service");
+ return false;
+ }
+
+ joint_names = res_query.jointnames;
+
+ // make sure we have the right joint names
+ for(unsigned int i = 0; i < joint_names.size() ; ++i)
+ {
+ if (planning_models::KinematicModel::Joint *j = planningMonitor_->getKinematicModel()->getJoint(joint_names[i]))
+ {
+ ROS_DEBUG("Using joing '%s' with %u parameters", j->name.c_str(), j->usedParams);
+ if (planningMonitor_->getKinematicModel()->getJointIndexInGroup(j->name, group_) < 0)
+ return false;
+ }
+ else
+ {
+ ROS_ERROR("Joint '%s' is not known", joint_names[i].c_str());
+ return false;
+ }
+ }
+
+ std::vector<std::string> groupNames;
+ planningMonitor_->getKinematicModel()->getJointsInGroup(groupNames, group_);
+ if (groupNames.size() != joint_names.size())
+ {
+ ROS_ERROR("The group '%s' does not have the same number of joints as the controller can handle", group_.c_str());
+ return false;
+ }
+
+ return true;
+ }
+
+ ros::NodeHandle nodeHandle_;
+ tf::TransformListener tf_;
+ planning_environment::CollisionModels *collisionModels_;
+ planning_environment::PlanningMonitor *planningMonitor_;
+
+ std::string group_;
+ std::vector<std::string> groupJointNames_;
+ bool perform_ik_;
+ bool show_collisions_;
+
+};
+
+}
+
+#endif
Modified: pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-08-13 01:35:58 UTC (rev 21753)
+++ pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-08-13 01:57:33 UTC (rev 21754)
@@ -23,7 +23,7 @@
<param name="refresh_interval_kinematic_state" type="double" value="1.0" />
<param name="bounding_planes" type="string" value="0 0 1 -0.01" />
</node>
-
+<!--
<include file="$(find move_arm)/launch/gripper_rarm.launch" />
-
+ -->
</launch>
Modified: pkg/trunk/highlevel/move_arm/manifest.xml
===================================================================
--- pkg/trunk/highlevel/move_arm/manifest.xml 2009-08-13 01:35:58 UTC (rev 21753)
+++ pkg/trunk/highlevel/move_arm/manifest.xml 2009-08-13 01:57:33 UTC (rev 21754)
@@ -20,8 +20,7 @@
<depend package="geometric_shapes" />
<depend package="planning_models" />
<depend package="planning_environment" />
- <depend package="robot_actions"/>
- <depend package="pr2_robot_actions"/>
+ <depend package="actionlib"/>
<depend package="pr2_mechanism_controllers"/>
<export>
Added: pkg/trunk/highlevel/move_arm/msg/ActuateGripperAction.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/ActuateGripperAction.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/ActuateGripperAction.msg 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1,4 @@
+
+ActuateGripperActionGoal action_goal
+ActuateGripperActionResult action_result
+ActuateGripperActionFeedback action_feedback
Added: pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionFeedback.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionFeedback.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionFeedback.msg 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib/GoalStatus status
+ActuateGripperFeedback feedback
Added: pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionGoal.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionGoal.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionGoal.msg 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib/GoalID goal_id
+ActuateGripperGoal goal
Added: pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionResult.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionResult.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/ActuateGripperActionResult.msg 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib/GoalStatus status
+ActuateGripperResult result
Added: pkg/trunk/highlevel/move_arm/msg/ActuateGripperFeedback.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/ActuateGripperFeedback.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/ActuateGripperFeedback.msg 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1,2 @@
+float64 crap
+
Added: pkg/trunk/highlevel/move_arm/msg/ActuateGripperGoal.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/ActuateGripperGoal.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/ActuateGripperGoal.msg 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1 @@
+float64 data
Added: pkg/trunk/highlevel/move_arm/msg/ActuateGripperResult.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/ActuateGripperResult.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/ActuateGripperResult.msg 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1 @@
+float64 crap
Added: pkg/trunk/highlevel/move_arm/msg/MoveArmAction.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/MoveArmAction.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/MoveArmAction.msg 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1,4 @@
+
+MoveArmActionGoal action_goal
+MoveArmActionResult action_result
+MoveArmActionFeedback action_feedback
Added: pkg/trunk/highlevel/move_arm/msg/MoveArmActionFeedback.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/MoveArmActionFeedback.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/MoveArmActionFeedback.msg 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib/GoalStatus status
+MoveArmFeedback feedback
Added: pkg/trunk/highlevel/move_arm/msg/MoveArmActionGoal.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/MoveArmActionGoal.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/MoveArmActionGoal.msg 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib/GoalID goal_id
+MoveArmGoal goal
Added: pkg/trunk/highlevel/move_arm/msg/MoveArmActionResult.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/MoveArmActionResult.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/MoveArmActionResult.msg 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1,4 @@
+
+Header header
+actionlib/GoalStatus status
+MoveArmResult result
Added: pkg/trunk/highlevel/move_arm/msg/MoveArmFeedback.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/MoveArmFeedback.msg (rev 0)
+++ pkg/trunk/highlevel/move_arm/msg/MoveArmFeedback.msg 2009-08-13 01:57:33 UTC (rev 21754)
@@ -0,0 +1,6 @@
+int32 mode
+int32 PLANNING=1
+int32 MOVING=2
+
+duration time_to_completion
+
Modified: pkg/trunk/highlevel/move_arm/msg/MoveArmGoal.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/MoveArmGoal.msg 2009-08-13 01:35:58 UTC (rev 21753)
+++ pkg/trunk/highlevel/move_arm/msg/MoveArmGoal.msg 2009-08-13 01:57:33 UTC (rev 21754)
@@ -1,3 +1,4 @@
+
# The goal state for the model to plan for. The state is not necessarily explicit.
# The goal is considered achieved if all the constraints are satisfied.
# An explicit state can be specified by setting joint constraints to a specific value.
Deleted: pkg/trunk/highlevel/move_arm/msg/MoveArmState.msg
===================================================================
--- pkg/trunk/highlevel/move_arm/msg/MoveArmState.msg 2009-08-13 01:35:58 UTC (rev 21753)
+++ pkg/trunk/highlevel/move_arm/msg/MoveArmState.msg 2009-08-13 01:57:33 UTC (rev 21754)
@@ -1,9 +0,0 @@
-# Status
-robot_actions/ActionStatus status
-
-MoveArmGoal goal
-
-int32 PLANNING=1 # when arm is stopped and planning is being performed
-int32 MOVING=2 # when we are executing a plan
-
-int32 feedback
Modified: pkg/trunk/highlevel/move_arm/src/actuate_gripper_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/actuate_gripper_action.cpp 2009-08-13 01:35:58 UTC (rev 21753)
+++ pkg/trunk/highlevel/move_arm/src/actuate_gripper_action.cpp 2009-08-13 01:57:33 UTC (rev 21754)
@@ -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:
- *
+ *
* * 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
@@ -17,7 +17,7 @@
* * 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
@@ -34,81 +34,74 @@
/* Author: Sachin Chitta */
-#include "pr2_robot_actions/ActuateGripperState.h"
-#include "std_msgs/Float64.h"
-
-/** Actions and messages */
-#include <robot_actions/action.h>
-#include <robot_actions/action_runner.h>
-
#include <ros/ros.h>
-class ActuateGripperAction : public robot_actions::Action<std_msgs::Float64, std_msgs::Float64>
+#include <actionlib/server/single_goal_action_server.h>
+#include <move_arm/ActuateGripperAction.h>
+#include <std_msgs/Float64.h>
+
+class ActuateGripperAction
{
-public:
-
- ActuateGripperAction(const std::string &arm) : robot_actions::Action<std_msgs::Float64, std_msgs::Float64>("actuate_gripper_" + arm)
+public:
+
+ ActuateGripperAction(const std::string &arm) : as_(nh_, "actuate_gripper_" + arm, boost::bind(&ActuateGripperAction::execute, this, _1))
+ {
+ pub_ = nh_.advertise<std_msgs::Float64>("gripper_command", 10);
+ }
+
+ ~ActuateGripperAction(void)
+ {
+ }
+
+ void execute(const move_arm::ActuateGripperGoalConstPtr &goal)
+ {
+ std_msgs::Float64 gripper_msg;
+ gripper_msg.data = goal->data;
+ pub_.publish(gripper_msg);
+
+ ros::Rate r(10.0);
+ ros::Time start = ros::Time::now();
+ bool result = true;
+ while(ros::Time::now()-start < ros::Duration(4.0))
{
- pub_ = nh_.advertise<std_msgs::Float64>("gripper_command", 10);
+ if (as_.isPreemptRequested())
+ {
+ gripper_msg.data = 0.0;
+ pub_.publish(gripper_msg);
+ ROS_INFO("ActuateGripperAction: preempted");
+ as_.setPreempted();
+ result = false;
+ break;
+ }
+ r.sleep();
}
-
- ~ActuateGripperAction(void)
- {
- }
-
- robot_actions::ResultStatus execute(const std_msgs::Float64& goal, std_msgs::Float64& feedback)
- {
- ROS_INFO("ActuateGripperAction: execute");
-
- // set default feedback
- feedback.data = goal.data;
-
- std_msgs::Float64 gripper_msg;
- gripper_msg.data = goal.data;
- pub_.publish(gripper_msg);
-
- ros::Rate r(10.0);
- ros::Time start = ros::Time::now();
-
- while(ros::Time::now()-start < ros::Duration(4.0))
- {
- if (isPreemptRequested()) {
- gripper_msg.data = 0.0;
- pub_.publish(gripper_msg);
- ROS_INFO("ActuateGripperAction: preempted");
- return robot_actions::PREEMPTED;
- }
- r.sleep();
- }
- ROS_INFO("ActuateGripperAction: Done");
- return robot_actions::SUCCESS;
- }
+ if (result)
+ as_.setSucceeded();
+ }
protected:
-
- ros::Publisher pub_;
- ros::NodeHandle nh_;
-
+
+ ros::NodeHandle nh_;
+ actionlib::SingleGoalActionServer<move_arm::ActuateGripperAction> as_;
+ ros::Publisher pub_;
+
};
int main(int argc, char** argv)
{
- ros::init(argc, argv, "actuate_gripper", ros::init_options::AnonymousName);
+ ros::init(argc, argv, "actuate_gripper");
- ros::NodeHandle node;
- std::string arm_name;
- node.param<std::string>("~arm", arm_name, std::string());
-
- if (arm_name.empty())
- ROS_ERROR("No '~arm' parameter specified");
- else
- {
- ActuateGripperAction actuate_gripper(arm_name);
- robot_actions::ActionRunner runner(20.0);
- runner.connect<std_msgs::Float64, pr2_robot_actions::ActuateGripperState, std_msgs::Float64>(actuate_gripper);
- runner.run();
- ros::spin();
- }
-
- return 0;
+ ros::NodeHandle node;
+ std::string arm_name;
+ node.param<std::string>("~arm", arm_name, std::string());
+
+ if (arm_name.empty())
+ ROS_ERROR("No '~arm' parameter specified");
+ else
+ {
+ ActuateGripperAction actuate_gripper(arm_name);
+ ros::spin();
+ }
+
+ return 0;
}
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-13 01:35:58 UTC (rev 21753)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-08-13 01:57:33 UTC (rev 21754)
@@ -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:
- *
+ *
* * 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
@@ -17,7 +17,7 @@
* * 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
@@ -37,14 +37,13 @@
/* \author: Ioan Sucan */
#include <ros/ros.h>
-#include <robot_actions/action_client.h>
+#include <actionlib/client/simple_action_client.h>
#include <planning_environment/monitors/kinematic_model_state_monitor.h>
#include <motion_planning_msgs/KinematicPath.h>
#include <manipulation_srvs/IKService.h>
-#include <move_arm/MoveArmGoal.h>
-#include <move_arm/MoveArmState.h>
-#include <pr2_robot_actions/ActuateGripperState.h>
+#include <move_arm/MoveArmAction.h>
+#include <move_arm/ActuateGripperAction.h>
#include <std_msgs/Float64.h>
#include <boost/thread/thread.hpp>
@@ -85,7 +84,7 @@
void printJoints(const planning_environment::KinematicModelStateMonitor &km, const std::vector<std::string> &names)
{
const planning_models::KinematicModel::ModelInfo &mi = km.getKinematicModel()->getModelInfo();
-
+
for (unsigned int i = 0 ; i < names.size(); ++i)
{
int idx = km.getKinematicModel()->getJointIndex(names[i]);
@@ -101,7 +100,7 @@
}
void goalToState(const move_arm::MoveArmGoal &goal, planning_models::StateParams &sp)
-{
+{
for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size() ; ++i)
{
sp.setParamsJoint(&goal.goal_constraints.joint_constraint[i].value[0],
@@ -149,7 +148,7 @@
goal.contacts[0].links.push_back("r_gripper_l_finger_link");
goal.contacts[0].links.push_back("r_gripper_r_finger_link");
goal.contacts[0].links.push_back("r_gripper_l_finger_tip_link");
- goal.contacts[0].links.push_back("r_gripper_r_finger_tip_link");
+ goal.contacts[0].links.push_back("r_gripper_r_finger_tip_link");
goal.contacts[0].links.push_back("r_gripper_palm_link");
goal.contacts[0].depth = 0.04;
goal.contacts[0].bound.type = mapping_msgs::Object::SPHERE;
@@ -158,9 +157,9 @@
goal.contacts[0].pose.header.stamp = ros::Time::now();
goal.contacts[0].pose.header.frame_id = "/base_link";
goal.contacts[0].pose.pose.position.x = 1;
- goal.contacts[0].pose.pose.position.y = 0;
- goal.contacts[0].pose.pose.position.z = 0.5;
-
+ goal.contacts[0].pose.pose.position.y = 0;
+ goal.contacts[0].pose.pose.position.z = 0.5;
+
goal.contacts[0].pose.pose.orientation.x = 0;
goal.contacts[0].pose.pose.orientation.y = 0;
goal.contacts[0].pose.pose.orientation.z = 0;
@@ -170,33 +169,33 @@
void setupGoalEEf(const std::string &link, const std::vector<double> &pz, move_arm::MoveArmGoal &goal)
{
goal.goal_constraints.pose_constraint.resize(1);
- goal.goal_constraints.pose_constraint[0].type = motion_planning_msgs::PoseConstraint::POSITION_X + motion_planning_msgs::PoseConstraint::POSITION_Y + motion_planning_msgs::PoseConstraint::POSITION_Z +
+ goal.goal_constraints.pose_constraint[0].type = motion_planning_msgs::PoseConstraint::POSITION_X + motion_planning_msgs::PoseConstraint::POSITION_Y + motion_planning_msgs::PoseConstraint::POSITION_Z +
+ motion_planning_msgs::PoseConstraint::ORIENTATION_R + motion_planning_msgs::PoseConstraint::ORIENTATION_P + motion_planning_msgs::PoseConstraint::ORIENTATION_Y;
goal.goal_constraints.pose_constraint[0].link_name = link;
goal.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
goal.goal_constraints.pose_constraint[0].pose.header.frame_id = "/base_link";
goal.goal_constraints.pose_constraint[0].pose.pose.position.x = pz[0];
- goal.goal_constraints.pose_constraint[0].pose.pose.position.y = pz[1];
- goal.goal_constraints.pose_constraint[0].pose.pose.position.z = pz[2];
-
+ goal.goal_constraints.pose_constraint[0].pose.pose.position.y = pz[1];
+ goal.goal_constraints.pose_constraint[0].pose.pose.position.z = pz[2];
+
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.x = pz[3];
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.y = pz[4];
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.z = pz[5];
goal.goal_constraints.pose_constraint[0].pose.pose.orientation.w = pz[6];
-
+
goal.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.005;
goal.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.005;
goal.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.01;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.005;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.005;
goal.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.005;
-
+
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.005;
- goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.005;
+ goal.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.005;
goal.goal_constraints.pose_constraint[0].orientation_importance = 0.2;
@@ -204,7 +203,7 @@
goal.contacts[0].links.push_back("r_gripper_l_finger_link");
goal.contacts[0].links.push_back("r_gripper_r_finger_link");
goal.contacts[0].links.push_back("r_gripper_l_finger_tip_link");
- goal.contacts[0].links.push_back("r_gripper_r_finger_tip_link");
+ goal.contacts[0].links.push_back("r_gripper_r_finger_tip_link");
goal.contacts[0].links.push_back("r_gripper_palm_link");
goal.contacts[0].depth = 0.4;
goal.contacts[0].bound.type = mapping_msgs::Object::SPHERE;
@@ -219,7 +218,7 @@
sp.enforceBounds();
for (unsigned int i = 0 ; i < names.size() ; ++i)
{
- goal.goal_constraints.joint_constraint[i].value[0] =
+ goal.goal_constraints.joint_constraint[i].value[0] =
sp.getParamsJoint(goal.goal_constraints.joint_constraint[i].joint_name)[0];
}
}
@@ -229,34 +228,34 @@
std::vector<std::string> names;
for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size(); ++i)
{
- std::cout << " " << goal.goal_constraints.joint_constraint[i].joint_name << " = "
+ std::cout << " " << goal.goal_constraints.joint_constraint[i].joint_name << " = "
<< goal.goal_constraints.joint_constraint[i].value[0] - km.getRobotState()->getParamsJoint(goal.goal_constraints.joint_constraint[i].joint_name)[0]
<< std::endl;
- names.push_back(goal.goal_constraints.joint_constraint[i].joint_name);
+ names.push_back(goal.goal_constraints.joint_constraint[i].joint_name);
}
btTransform pose1 = effPosition(km, goal);
move_arm::MoveArmGoal temp;
setConfig(km.getRobotState(), names, temp);
btTransform pose2 = effPosition(km, temp);
- std::cout << std::endl;
+ std::cout << std::endl;
double dist = pose1.getOrigin().distance(pose2.getOrigin());
std::cout << " -position distance: " << dist << std::endl;
double angle = pose1.getRotation().angle(pose2.getRotation());
std::cout << " -rotation distance: " << angle << std::endl;
}
-
+
void viewState(ros::Publisher &view, const planning_environment::KinematicModelStateMonitor &km, const planning_models::StateParams &st)
{
- motion_planning_msgs::KinematicPath kp;
-
+ motion_planning_msgs::KinematicPath kp;
+
kp.header.frame_id = km.getFrameId();
kp.header.stamp = km.lastJointStateUpdate();
-
+
// fill in start state with current one
std::vector<planning_models::KinematicModel::Joint*> joints;
km.getKinematicModel()->getJoints(joints);
-
+
kp.start_state.resize(joints.size());
for (unsigned int i = 0 ; i < joints.size() ; ++i)
{
@@ -280,53 +279,53 @@
int main(int argc, char **argv)
{
- ros::init(argc, argv, "cmd_line_move_arm", ros::init_options::NoSigintHandler | ros::init_options::AnonymousName);
-
- std::string arm = "r";
- if (argc >= 2)
- if (argv[1][0] == 'l')
- arm = "l";
-
- ros::NodeHandle nh;
- robot_actions::ActionClient<move_arm::MoveArmGoal, move_arm::MoveArmState, int32_t> move_arm(arm == "r" ? "move_right_arm" : "move_left_arm");
- robot_actions::ActionClient<std_msgs::Float64, pr2_robot_actions::ActuateGripperState, std_msgs::Float64> gripper(arm == "r" ? "actuate_gripper_right_arm" : "actuate_gripper_left_arm");
- ros::Publisher view = nh.advertise<motion_planning_msgs::KinematicPath>("executing_kinematic_path", 1);
-
- int32_t feedback;
- std::map<std::string, move_arm::MoveArmGoal> goals;
-
- std::vector<std::string> names(7);
- names[0] = arm + "_shoulder_pan_joint";
- names[1] = arm + "_shoulder_lift_joint";
- names[2] = arm + "_upper_arm_roll_joint";
- names[3] = arm + "_elbow_flex_joint";
- names[4] = arm + "_forearm_roll_joint";
- names[5] = arm + "_wrist_flex_joint";
- names[6] = arm + "_wrist_roll_joint";
-
+ ros::init(argc, argv, "cmd_line_move_arm", ros::init_options::NoSigintHandler | ros::init_options::AnonymousName);
- planning_environment::RobotModels rm("robot_description");
- if (!rm.loadedModels())
- return 0;
+ std::string arm = "r";
+ if (argc >= 2)
+ if (argv[1][0] == 'l')
+ arm = "l";
- boost::thread th(&spinThread);
- tf::TransformListener tf;
- planning_environment::KinematicModelStateMonitor km(&rm, &tf);
- km.waitForState();
-
- std::cout << std::endl << std::endl << "Using joints:" << std::endl;
- printJoints(km, names);
- std::cout << std::endl;
- double allowed_time = 10.0;
+ ros::NodeHandle nh;
- while (nh.ok() && std::cin.good() && !std::cin.eof())
- {
- std::cout << "command> ";
- std::string cmd;
- std::getline(std::cin, cmd);
- boost::trim(cmd);
- ros::spinOnce();
-
+ actionlib::SimpleActionClient<move_arm::MoveArmAction> move_arm(nh, arm == "r" ? "move_right_arm" : "move_left_arm");
+ actionlib::SimpleActionClient<move_arm::ActuateGripperAction> gripper(nh, arm == "r" ? "actuate_gripper_right_arm" : "actuate_gripper_left_arm");
+ ros::Publisher view = nh.advertise<motion_planning_msgs::KinematicPath>("executing_kinematic_path", 1);
+
+ std::map<std::string, move_arm::MoveArmGoal> goals;
+
+ std::vector<std::string> names(7);
+ names[0] = arm + "_shoulder_pan_joint";
+ names[1] = arm + "_shoulder_lift_joint";
+ names[2] = arm + "_upper_arm_roll_joint";
+ names[3] = arm + "_elbow_flex_joint";
+ names[4] = arm + "_forearm_roll_joint";
+ names[5] = arm + "_wrist_flex_joint";
+ names[6] = arm + "_wrist_roll_joint";
+
+
+ planning_environment::RobotModels rm("robot_description");
+ if (!rm.loadedModels())
+ return 0;
+
+ boost::thread th(&spinThread);
+ tf::TransformListener tf;
+ planning_environment::KinematicModelStateMonitor km(&rm, &tf);
+ km.waitForState();
+
+ std::cout << std::endl << std::endl << "Using joints:" << std::endl;
+ printJoints(km, names);
+ std::cout << std::endl;
+ double allowed_time = 10.0;
+
+ while (nh.ok() && std::cin.good() && !std::cin.eof())
+ {
+ std::cout << "command> ";
+ std::string cmd;
+ std::getline(std::cin, cmd);
+ boost::trim(cmd);
+ ros::spinOnce();
+
if (!nh.ok())
break;
@@ -382,12 +381,12 @@
if (goals.find(config) == goals.end())
std::cout << "Configuration '" << config << "' not found" << std::endl;
else
- goals.erase(config);
+ goals.erase(config);
}
else
if (cmd == "view")
{
- planning_models::StateParams st(*km.getRobotState());
+ planning_models::StateParams st(*km.getRobotState());
viewState(view, km, st);
}
else
@@ -399,7 +398,7 @@
std::cout << "Configuration '" << config << "' not found" << std::endl;
else
{
- planning_models::StateParams st(*km.getRobotState());
+ planning_models::StateParams st(*km.getRobotState());
goalToState(goals[config], st);
viewState(view, km, st);
}
@@ -451,17 +450,17 @@
{
std::stringstream ss(cmd.substr(3));
std::string config;
-
+
if (ss.good() && !ss.eof())
ss >> config;
-
+
if (config.empty())
std::cerr << "Configuration name required. See 'help'" << std::endl;
else
{
double x, y, z;
bool err = true;
-
+
if (ss.good() && !ss.eof())
{
ss >> x;
@@ -473,7 +472,7 @@
ss >> z;
err = false;
std::cout << "Performing IK to " << x << ", " << y << ", " << z << ", 0, 0, 0, 1..." << std::endl;
-
+
ros::ServiceClient client = nh.serviceClient<manipulation_srvs::IKService>("arm_ik");
manipulation_srvs::IKService::Request request;
manipulation_srvs::IKService::Response response;
@@ -487,10 +486,10 @@
request.data.pose_stamped.pose.orientation.z = 0;
request.data.pose_stamped.pose.orientation.w = 1;
request.data.joint_names = names;
-
+
planning_models::StateParams rs(*km.getRobotState());
rs.randomState();
-
+
for(unsigned int i = 0; i < names.size() ; ++i)
{
const double *params = rs.getParamsJoint(names[i]);
@@ -499,9 +498,9 @@
request.data.positions.push_back(params[j]);
}
if (client.call(request, response))
- {
+ {
planning_models::StateParams sp(*km.getRobotState());
- unsigned int n = 0;
+ unsigned int n = 0;
for (unsigned int i = 0 ; i < names.size() ; ++i)
{
unsigned int u = km.getKinematicModel()->getJoint(names[i])->usedParams;
@@ -511,7 +510,7 @@
sp.setParamsJoint(params, names[i]);
std::cout << names[i] << " = " << params[0] << std::endl;
}
- n += u;
+ n += u;
}
setConfig(&sp, names, goals[config]);
std::cout << "Success!" << std::endl;
@@ -526,63 +525,77 @@
}
}
else
- if (cmd.length() > 3 && cmd.substr(0, 3) == "go ")
- {
- std::string config = cmd.substr(3);
- boost::trim(config);
- if (goals.find(config) == goals.end())
- {
- std::stringstream ss(config);
- std::vector<double> nrs;
- while (ss.good() && !ss.eof())
- {
- double value;
- ss >> value;
- nrs.push_back(value);
- }
+ if (cmd.length() > 3 && cmd.substr(0, 3) == "go ")
+ {
+ std::string config = cmd.substr(3);
+ boost::trim(config);
+ if (goals.find(config) == goals.end())
+ {
+ std::stringstream ss(config);
+ std::vector<double> nrs;
+ while (ss.good() && !ss.eof())
+ {
+ double value;
+ ss >> value;
+ nrs.push_back(value);
+ }
- bool err = true;
- if (nrs.size() == 3)
- {
- nrs.push_back(0);
- nrs.push_back(0);
- nrs.push_back(0);
- nrs.push_back(1);
- }
+ bool err = true;
+ if (nrs.size() == 3)
+ {
+ nrs.push_back(0);
+ nrs.push_back(0);
+ nrs.push_back(0);
+ nrs.push_back(1);
+ }
- if (nrs.size() == 7)
- {
- err = false;
+ if (nrs.size() == 7)
+ {
+ err = false;
- std::string link = km.getKinematicModel()->getJoint(names.back())->after->name;
- std::cout << "Moving " << link << " to " << nrs[0] << ", " << nrs[1] << ", " << nrs[2] << ", " <<
- nrs[3] << ", " << nrs[4] << ", " << nrs[5] << ", " << nrs[6] << "..." << std::endl;
- move_arm::MoveArmGoal g;
- setupGoalEEf(link, nrs, g);
- if (move_arm.execute(g, feedback, ros::Duration(allowed_time)) != robot_actions::SUCCESS)
- std::cerr << "Failed achieving goal" << std::endl;
- else
- std::cout << "Success!" << std::endl;
-
+ std::string link = km.getKinematicModel()->getJoint(names.back())->after->name;
+ std::cout << "Moving " << link << " to " << nrs[0] << ", " << nrs[1] << ", " << nrs[2] << ", " <<
+ nrs[3] << ", " << nrs[4] << ", " << nrs[5] << ", " << nrs[6] << "..." << std::endl;
+ move_arm::MoveArmGoal g;
+ setupGoalEEf(link, nrs, g);
-
- }
-
-
- if (err)
- std::cout << "Configuration '" << config << "' not found" << std::endl;
- }
-
- else
- {
- std::cout << "Moving to " << config << "..." << std::endl;
- std::cout << " (" << goals[config].contacts.size() << " allowed contacts)" << std::endl;
- if (move_arm.execute(goals[config], feedback, ros::Duration(allowed_time)) != robot_actions::SUCCESS)
- std::cerr << "Failed achieving goal" << std::endl;
- else
- std::cout << "Success!" << std::endl;
- }
- }
+
+ bool finished_within_time;
+ move_arm.sendGoal(g);
+ finished_within_time = move_arm.waitForGoalToFinish(ros::Duration(allowed_time));
+
+ if (!finished_within_time)
+ {
+ move_arm.cancelGoal();
+ std::cerr << "Timed out achieving goal" << std::endl;
+ }
+ else
+ std::cout << "Final state is " << move_arm.getTerminalState().toString() << std::endl;
+
+ }
+
+
+ if (err)
+ std::cout << "Configuration '" << config << "' not found" << std::endl;
+ }
+
+ else
+ {
+ std::cout << "Moving to " << config << "..." << std::endl;
+
+ bool finished_within_time;
+ move_arm.sendGoal(goals[config]);
+ finished_within_time = move_arm.waitForGoalToFinish(ros::Duration(allowed_time));
+
+ if (!finished_within_time)
+ {
+ move_arm.cancelGoal();
+ std::cerr << "Timed out achieving goal" << std::endl;
+ }
+ else
+ std::cout << "Final state is " << move_arm.getTerminalState().toString() << std::endl;
+ }
+ }
else
if (cmd.length() > 5 && cmd.find_first_of("[") != std::string::npos)
{
@@ -636,18 +649,23 @@
else
if (cmd.length() > 5 && cmd.substr(0, 5) == "grip ")
{
- std::stringstream ss(cmd.substr(5));
- if (ss.good() && !ss.eof())
+ std::stringstream ss(cmd.substr(5));
+ if (ss.good() && !ss.eof())
+ {
+ move_arm::ActuateGripperGoal g;
+ ss >> g.data;
+
+ gripper.sendGoal(g);
+ bool finished_before_timeout = gripper.waitForGoalToFinish(ros::Duration(allowed_time));
+ if (finished_before_timeout)
+ std::cout << "Final state is " << gripper.getTerminalState().toString() << std::endl;
+ else
{
- std_msgs::Float64 g, fb;
- ss >> g.data;
- if (gripper.execute(g, fb, ros::Duration(allowed_time)) != robot_actions::SUCCESS)
- std::cerr << "Failed achieving goal" << std::endl;
- else
- std::cout << "Success!" << std::endl;
+ gripper.cancelGoal();
+ std::cerr << "Failed achieving goal" << std::endl;
}
- else
- std::cerr << "A floating point value expected but '" << cmd.substr(5) << "' was given" << std::endl;
+ }
+ std::cerr << "A floating point value expected but '" << cmd.substr(5) << "' was given" << std::endl;
}
else
if (goals.find(cmd) != goals.end())
@@ -656,10 +674,10 @@
std::cout << std::endl;
btTransform p = effPosition(km, goals[cmd]);
printPose(p);
- }
+ }
else
std::cerr << "Unknown command. Try 'help'" << std::endl;
}
-
+
return 0;
}
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-13 01:35:58 UTC (rev 21753)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-08-13 01:57:33 UTC (rev 21754)
@@ -1,1092 +1,939 @@
/*********************************************************************
-*
-* 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.
+ *
+ * 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.
-*
-* Authors: Sachin Chitta, Ioan Sucan
-*********************************************************************/
+ *
+ * Authors: Sachin Chitta, Ioan Sucan
+ *********************************************************************/
-#include <ros/ros.h>
+#include "move_arm/move_arm_core.h"
+#include <actionlib/server/single_goal_action_server.h>
+#include <move_arm/MoveArmAction.h>
-#include <robot_actions/action.h>
-#include <robot_actions/action_runner.h>
-#include <move_arm/MoveArmState.h>
-#include <move_arm/MoveArmGoal.h>
-
#include <manipulation_msgs/JointTraj.h>
#include <manipulation_srvs/IKService.h>
-#include <pr2_mechanism_controllers/TrajectoryStart.h>
-#include <pr2_mechanism_controllers/TrajectoryQuery.h>
-#include <pr2_mechanism_controllers/TrajectoryCancel.h>
-
#include <motion_planning_msgs/GetMotionPlan.h>
#include <motion_planning_msgs/ConvertToJointConstraint.h>
#include <visualization_msgs/Marker.h>
-#include <planning_environment/monitors/planning_monitor.h>
#include <planning_environment/util/construct_object.h>
#include <geometric_shapes/bodies.h>
#include <algorithm>
#include <cstdlib>
-using namespace robot_actions;
+namespace move_arm
+{
-/// the string used internally to access control starting service; this should be remaped in the launch file
-static const std::string CONTROL_START_NAME = "controller_start";
-/// the string used internally to access control querying service; this should be remaped in the launch file
-static const std::string CONTROL_QUERY_NAME = "controller_query";
-/// the string used internally to access control canceling service; this should be remaped in the launch file
-static const std::string CONTROL_CANCEL_NAME = "controller_cancel";
+class MoveArm
+{
+public:
-/// the string used internally to access the long range motion planning service; this should be remaped in the launch file
-static const std::string LR_MOTION_PLAN_NAME = "get_motion_plan_lr";
+ MoveArm(MoveArmCore &core) : core_(core), as_(nh_, "move_" + core.group_, boost::bind(&MoveArm::execute, this, _1))
+ {
-/// the string used internally to access the short range motion planning service; this should be remaped in the launch file
-static const std::string SR_MOTION_PLAN_NAME = "get_motion_plan_sr";
+ if (core_.show_collisions_)
+ visMarkerPublisher_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 128);
-/// the string used internally to access valid state searching service; this should be remaped in the launch file
-static const std::string SEARCH_VALID_STATE_NAME = "get_valid_state";
+ // advertise the topic for displaying kinematic plans
+ displayPathPublisher_ = nh_.advertise<motion_planning_msgs::KinematicPath>("executing_kinematic_path", 10);
-/// the string used internally to access inverse kinematics service; this should be remaped in the launch file
-static const std::string ARM_IK_NAME = "arm_ik";
+ planningMonitor_ = core_.planningMonitor_;
+ tf_ = &core_.tf_;
+ planningMonitor_->getEnvironmentModel()->setVerbose(false);
+ }
-/** \brief Configuration of actions that need to actuate parts of the robot */
-class MoveBodyCore
-{
- friend class MoveArm;
-
-public:
-
- MoveBodyCore(void)
- {
- collisionModels_ = NULL;
- planningMonitor_ = NULL;
+ virtual ~MoveArm()
+ {
+ }
+
+private:
+
+ // construct a list of states with cost
+ struct CostState
+ {
+ planning_models::StateParams *state;
+ double cost;
+ unsigned int index;
+ };
+
+ struct CostStateOrder
+ {
+ bool operator()(const CostState& a, const CostState& b) const
+ {
+ return a.cost < b.cost;
}
-
- virtual ~MoveBodyCore(void)
+ };
+
+ struct CollisionCost
+ {
+ CollisionCost(void)
{
- if (planningMonitor_)
- delete planningMonitor_;
- if (collisionModels_)
- delete collisionModels_;
+ cost = 0.0;
+ sum = 0.0;
}
-
-
- bool configure(void)
+
+ double cost;
+ double sum;
+ };
+
+
+ /** \brief The ccost and display arguments should be bound by the caller. This is a callback function that gets called by the planning
+ * environment when a collision is found */
+ void contactFound(CollisionCost *ccost, bool display, collision_space::EnvironmentModel::Contact &contact)
+ {
+ double cdepth = fabs(contact.depth);
+
+ if (ccost->cost < cdepth)
+ ccost->cost = cdepth;
+ ccost->sum += cdepth;
+
+ if (display)
{
- nodeHandle_.param<std::string>("~group", group_, std::string());
-
- if (group_.empty())
- {
- ROS_ERROR("No '~group' parameter specified. Without the name of the group of joints to plan for, action cannot start");
- return false;
- }
+ static int count = 0;
+ visualization_msgs::Marker mk;
+ mk.header.stamp = planningMonitor_->lastMapUpdate();
+ mk.header.frame_id = planningMonitor_->getFrameId();
+ mk.ns = ros::this_node::getName();
+ mk.id = count++;
+ mk.type = visualization_msgs::Marker::SPHERE;
+ mk.action = visualization_msgs::Marker::ADD;
+ mk.pose.position.x = contact.pos.x();
+ mk.pose.position.y = contact.pos.y();
+ mk.pose.position.z = contact.pos.z();
+ mk.pose.orientation.w = 1.0;
- // monitor robot
- collisionModels_ = new planning_environment::CollisionModels("robot_description");
- planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, &tf_);
-
- if (!collisionModels_->loadedModels())
- return false;
-
- nodeHandle_.param<bool>("~perform_ik", perform_ik_, true);
-
- if (collisionModels_->getKinematicModel()->getGroupID(group_) < 0)
- {
- ROS_ERROR("Group '%s' is not known", group_.c_str());
- return false;
- }
- else
- ROS_INFO("Configuring action core for '%s' (IK is %senabled)", group_.c_str(), perform_ik_ ? "" : "not ");
-
- planningMonitor_->waitForState();
- planningMonitor_->waitForMap();
-
- if (!getControlJointNames(groupJointNames_))
- return false;
-
- nodeHandle_.param<bool>("~show_collisions", show_collisions_, false);
- nodeHandle_.param<bool>("~unsafe_paths", unsafe_paths_, false);
+ mk.scale.x = mk.scale.y = mk.scale.z = 0.03;
- if (show_collisions_)
- ROS_INFO("Found collisions will be displayed as visualization markers");
-
- if (unsafe_paths_)
- ROS_WARN("Paths will NOT be monitored for collision once they have been sent to the controller");
-
- return true;
+ mk.color.a = 0.6;
+ mk.color.r = 1.0;
+ mk.color.g = 0.04;
+ mk.color.b = 0.04;
+
+ mk.lifetime = ros::Duration(30.0);
+
+ visMarkerPublisher_.publish(mk);
}
-
-protected:
+ }
- bool getControlJointNames(std::vector<std::string> &joint_names)
+ /** \brief Evaluate the cost of a state, in terms of collisions */
+ double computeStateCollisionCost(const planning_models::StateParams *sp)
+ {
+ CollisionCost ccost;
+
+ std::vector<collision_space::EnvironmentModel::AllowedContact> ac = planningMonitor_->getAllowedContacts();
+ planningMonitor_->clearAllowedContacts();
+ planningMonitor_->setOnCollisionContactCallback(boost::bind(&MoveArm::contactFound, this, &ccost, false, _1), 0);
+
+ // check for collision, getting all contacts
+ planningMonitor_->isStateValid(sp, planning_environment::PlanningMonitor::COLLISION_TEST);
+
+ planningMonitor_->setOnCollisionContactCallback(NULL);
+ planningMonitor_->setAllowedContacts(ac);
+
+ return ccost.sum;
+ }
+
+
+ /** \brief If we have a complex goal for which we have not yet found a valid goal state, we use this function*/
+ bool solveGoalComplex(std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ motion_planning_msgs::GetMotionPlan::Request &req)
+ {
+ ROS_DEBUG("Acting on goal with unknown valid goal state ...");
+
+
+ // we make a request to a service that attempts to find a valid state close to the goal
+ motion_planning_msgs::ConvertToJointConstraint::Request c_req;
+ c_req.params = req.params;
+ c_req.start_state = req.start_state;
+ c_req.constraints = req.goal_constraints;
+ c_req.names = core_.groupJointNames_;
+ c_req.states.resize(states.size());
+ c_req.allowed_time = 1.0;
+
+ // if we have hints about where the goal might be, we set them here
+ for (unsigned int i = 0 ; i < states.size() ; ++i)
+ states[i]->copyParamsJoints(c_req.states[i].vals, core_.groupJointNames_);
+
+ motion_planning_msgs::ConvertToJointConstraint::Response c_res;
+ ros::ServiceClient s_client = nh_.serviceClient<motion_planning_msgs::ConvertToJointConstraint>(SEARCH_VALID_STATE_NAME);
+ if (s_client.call(c_req, c_res))
{
- ros::ServiceClient client_query = nodeHandle_.serviceClient<pr2_mechanism_controllers::TrajectoryQuery>(CONTROL_QUERY_NAME);
- pr2_mechanism_controllers::TrajectoryQuery::Request req_query;
- pr2_mechanism_controllers::TrajectoryQuery::Response res_query;
- req_query.trajectoryid = pr2_mechanism_controllers::TrajectoryQuery::Request::Query_Joint_Names;
-
- bool result = client_query.call(req_query, res_query);
-
- if (!result)
- {
- ROS_INFO("Querying controller for joint names ...");
- ros::Duration(5.0).sleep();
- result = client_query.call(req_query, res_query);
- if (result)
- ROS_INFO("Joint names received");
- }
-
- if (!result)
- {
- ROS_ERROR("Unable to retrieve controller joint names from control query service");
- return false;
- }
-
- joint_names = res_query.jointnames;
-
- // make sure we have the right joint names
- for(unsigned int i = 0; i < joint_names.size() ; ++i)
- {
- if (planning_models::KinematicModel::Joint *j = planningMonitor_->getKinematicModel()->getJoint(joint_names[i]))
- {
- ROS_DEBUG("Using joing '%s' with %u parameters", j->name.c_str(), j->usedParams);
- if (planningMonitor_->getKinematicModel()->getJointIndexInGroup(j->name, group_) < 0)
- return false;
- }
- else
- {
- ROS_ERROR("Joint '%s' is not known", joint_names[i].c_str());
- return false;
- }
- }
-
- std::vector<std::string> groupNames;
- planningMonitor_->getKinematicModel()->getJointsInGroup(groupNames, group_);
- if (groupNames.size() != joint_names.size())
- {
- ROS_ERROR("The group '%s' does not have the same number of joints as the controller can handle", group_.c_str());
- return false;
- }
-
- return true;
- }
-
- ros::NodeHandle nodeHandle_;
- tf::TransformListener tf_;
- planning_environment::CollisionModels *collisionModels_;
- planning_environment::PlanningMonitor *planningMonitor_;
-
- std::string group_;
- std::vector<std::string> groupJointNames_;
- bool perform_ik_;
- bool unsafe_paths_;
- bool show_collisions_;
+ // if we found a valid state
+ if (!c_res.joint_constraint.empty())
+ {
-};
+ // construct a state representation from our goal joint
+ boost::shared_ptr<planning_models::StateParams> sp(new planning_models::StateParams(*planningMonitor_->getRobotState()));
-
-class MoveArm : public robot_actions::Action<move_arm::MoveArmGoal, int32_t>
-{
-public:
-
- MoveArm(MoveBodyCore &core) : Action<move_arm::MoveArmGoal, int32_t>("move_" + core.group_), core_(core)
- {
- if (core_.show_collisions_)
- visMarkerPublisher_ = core_.nodeHandle_.advertise<visualization_msgs::Marker>("visualization_marker", 128);
+ for (unsigned int i = 0 ; i < c_res.joint_constraint.size() ; ++i)
+ {
+ const motion_planning_msgs::JointConstra...
[truncated message content] |
