From ca06dd7d9705b9b15ba1762adaa85bdeac3be33e Mon Sep 17 00:00:00 2001 From: "Isaac I.Y. Saito" <130s@2000.jukuin.keio.ac.jp> Date: Sun, 31 Jan 2016 10:57:27 -0800 Subject: [PATCH] Add point head node and method (dependent on PR2 PointHead action). --- hironx_head_action/CMakeLists.txt | 39 ++ hironx_head_action/launch/head_action.launch | 14 + hironx_head_action/package.xml | 43 ++ hironx_head_action/src/point_frame.cpp | 560 ++++++++++++++++++ hironx_head_action/test/test-head-action.test | 12 + hironx_head_action/test/test_head_action.py | 46 ++ .../launch/hironx_ros_bridge.launch | 4 + .../src/hironx_ros_bridge/ros_client.py | 51 +- 8 files changed, 768 insertions(+), 1 deletion(-) create mode 100644 hironx_head_action/CMakeLists.txt create mode 100644 hironx_head_action/launch/head_action.launch create mode 100644 hironx_head_action/package.xml create mode 100644 hironx_head_action/src/point_frame.cpp create mode 100644 hironx_head_action/test/test-head-action.test create mode 100755 hironx_head_action/test/test_head_action.py diff --git a/hironx_head_action/CMakeLists.txt b/hironx_head_action/CMakeLists.txt new file mode 100644 index 00000000..0ad42b0b --- /dev/null +++ b/hironx_head_action/CMakeLists.txt @@ -0,0 +1,39 @@ +# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html +cmake_minimum_required(VERSION 2.8.3) +project(hironx_head_action) + +# Load catkin and all dependencies required for this package +find_package(catkin REQUIRED COMPONENTS + actionlib + geometry_msgs + kdl_parser + message_filters + pr2_controllers_msgs + roscpp + sensor_msgs + trajectory_msgs + tf + tf_conversions) + +find_package(orocos_kdl REQUIRED) + +find_package(Boost REQUIRED COMPONENTS thread) +include_directories(${Boost_INCLUDE_DIRS} + ${orocos_kdl_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) + +catkin_package() + +add_executable(head_action src/point_frame.cpp) +target_link_libraries(head_action ${Boost_LIBRARIES} + ${orocos_kdl_LIBRARIES} ${catkin_LIBRARIES}) +add_dependencies(head_action ${catkin_EXPORTED_TARGETS}) + +install(TARGETS head_action + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(DIRECTORY test launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) + +# this test depends on https://github.com/start-jsk/rtmros_common/pull/966 +add_rostest(test/test-head-action.test) diff --git a/hironx_head_action/launch/head_action.launch b/hironx_head_action/launch/head_action.launch new file mode 100644 index 00000000..43de1077 --- /dev/null +++ b/hironx_head_action/launch/head_action.launch @@ -0,0 +1,14 @@ + + + + + + + + success_angle_threshold: 0.01 + + + + + diff --git a/hironx_head_action/package.xml b/hironx_head_action/package.xml new file mode 100644 index 00000000..6a779c9c --- /dev/null +++ b/hironx_head_action/package.xml @@ -0,0 +1,43 @@ + + hironx_head_action + 1.1.5 + The head action is a node that provides an action interface for + pointing the head of the robot. It passes trajectory goals to the + controller, and reports success when they have finished executing. + + Originally developed at pr2_head_action + + TORK + BSD + http://wiki.ros.org/hironx_head_action + + + Stuart Glaser + Isaac I. Y. Saito + + catkin + + actionlib + geometry_msgs + kdl_parser + message_filters + orocos_kdl + pr2_controllers_msgs + roscpp + sensor_msgs + tf + tf_conversions + trajectory_msgs + + actionlib + geometry_msgs + kdl_parser + message_filters + orocos_kdl + pr2_controllers_msgs + roscpp + sensor_msgs + tf + tf_conversions + trajectory_msgs + diff --git a/hironx_head_action/src/point_frame.cpp b/hironx_head_action/src/point_frame.cpp new file mode 100644 index 00000000..d2b6a28a --- /dev/null +++ b/hironx_head_action/src/point_frame.cpp @@ -0,0 +1,560 @@ +/********************************************************************* + * 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 + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include "kdl/chainfksolverpos_recursive.hpp" +#include "kdl_parser/kdl_parser.hpp" + +#include "tf_conversions/tf_kdl.h" +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include + + +void printVector3(const char * label, tf::Vector3 v) +{ + printf("%s % 7.3f % 7.3f % 7.3f\n", label, v.x(), v.y(), v.z() ); + +} + +class ControlHead +{ +private: + typedef actionlib::ActionServer PHAS; + typedef PHAS::GoalHandle GoalHandle; + + std::string node_name_; + std::string action_name_; + std::string root_; + std::string tip_; + std::string pan_link_; + std::string tilt_link_; + std::string default_pointing_frame_; + std::string pointing_frame_; + tf::Vector3 pointing_axis_; + std::vector< std::string> joint_names_; + + ros::NodeHandle nh_, pnh_; + ros::Publisher pub_controller_command_; + ros::Subscriber sub_controller_state_; + ros::Subscriber command_sub_; + ros::Timer watchdog_timer_; + + PHAS action_server_; + bool has_active_goal_; + GoalHandle active_goal_; + tf::Stamped target_in_pan_; + std::string pan_parent_; + double success_angle_threshold_; + + geometry_msgs::PointStamped target_; + KDL::Tree tree_; + KDL::Chain chain_; + tf::Point target_in_root_; + + boost::scoped_ptr pose_solver_; + boost::scoped_ptr jac_solver_; + + tf::TransformListener tfl_; + urdf::Model urdf_model_; + +public: + + ControlHead(const ros::NodeHandle &node) : + node_name_(ros::this_node::getName()) + , action_name_("point_head_action") + , nh_(node) + , pnh_("~") + , action_server_(nh_, action_name_.c_str(), + boost::bind(&ControlHead::goalCB, this, _1), + boost::bind(&ControlHead::cancelCB, this, _1), false) + , has_active_goal_(false) + { + pnh_.param("pan_link", pan_link_, std::string("HEAD_JOINT0_Link")); + pnh_.param("tilt_link", tilt_link_, std::string("HEAD_JOINT1_Link")); + pnh_.param("default_pointing_frame", default_pointing_frame_, std::string("HEAD_TARGET_Link")); + pnh_.param("success_angle_threshold", success_angle_threshold_, 0.1); + + if(pan_link_[0] == '/') pan_link_.erase(0, 1); + if(default_pointing_frame_[0] == '/') default_pointing_frame_.erase(0, 1); + + // Connects to the controller + pub_controller_command_ = + nh_.advertise("command", 2); + sub_controller_state_ = + nh_.subscribe("state", 1, &ControlHead::controllerStateCB, this); + + // Should only ever happen on first call... move to constructor? + if(tree_.getNrOfJoints() == 0) + { + std::string robot_desc_string; + nh_.param("/robot_description", robot_desc_string, std::string()); + ROS_DEBUG("Reading tree from robot_description..."); + if (!kdl_parser::treeFromString(robot_desc_string, tree_)){ + ROS_ERROR("Failed to construct kdl tree"); + exit(-1); + } + if (!urdf_model_.initString(robot_desc_string)){ + ROS_ERROR("Failed to parse urdf string for urdf::Model."); + exit(-2); + } + } +#if 0 + if( tree_.getSegment(default_pointing_frame_) == tree_.getSegments().end() ) { + KDL::Segment pointing_frame_segment(default_pointing_frame_, KDL::Joint(KDL::Joint::None), KDL::Frame(KDL::Vector(0.3, 0, 0.1))); + tree_.addSegment(pointing_frame_segment, tilt_link_); + } +#endif + + ROS_DEBUG("Tree has %d joints and %d segments.", tree_.getNrOfJoints(), tree_.getNrOfSegments()); + + action_server_.start(); + + //watchdog_timer_ = nh_.createTimer(ros::Duration(1.0), &ControlHead::watchdog, this); + } + + void goalCB(GoalHandle gh) + { + // Before we do anything, we need to know that name of the pan_link's parent, which we will treat as the root. + if (root_.empty()) + { + for (int i = 0; i < 10; ++i) + { + try { + tfl_.getParent(pan_link_, ros::Time(), root_); + break; + } + catch (const tf::TransformException &ex) {} + ros::Duration(0.5).sleep(); + } + if (root_.empty()) + { + ROS_ERROR("Could not get parent of %s in the TF tree", pan_link_.c_str()); + gh.setRejected(); + return; + } + } + if(root_[0] == '/') root_.erase(0, 1); + + ROS_DEBUG("Got point head goal!"); + + // Process pointing frame and axis + const geometry_msgs::PointStamped &target = gh.getGoal()->target; + pointing_frame_ = gh.getGoal()->pointing_frame; + std::cerr << pointing_frame_ << std::endl; + if(pointing_frame_.length() == 0) + { + ROS_WARN("Pointing frame not specified, using %s [1, 0, 0] by default.", default_pointing_frame_.c_str()); + pointing_frame_ = default_pointing_frame_; + pointing_axis_ = tf::Vector3(1.0, 0.0, 0.0); + } + else + { + if(pointing_frame_[0] == '/') pointing_frame_.erase(0, 1); + pointing_axis_ = tf::Vector3(1.0, 0.0, 0.0); + bool ret1 = false; + try + { + std::string error_msg; + ret1 = tfl_.waitForTransform(pan_link_, pointing_frame_, target.header.stamp, + ros::Duration(5.0), ros::Duration(0.01), &error_msg); + + tf::vector3MsgToTF(gh.getGoal()->pointing_axis, pointing_axis_); + if(pointing_axis_.length() < 0.1) + { + size_t found = pointing_frame_.find("optical_frame"); + if (found != std::string::npos) + { + ROS_WARN("Pointing axis appears to be zero-length. Using [0, 0, 1] as default for an optical frame."); + pointing_axis_ = tf::Vector3(0, 0, 1); + } + else + { + ROS_WARN("Pointing axis appears to be zero-length. Using [1, 0, 0] as default for a non-optical frame."); + pointing_axis_ = tf::Vector3(1, 0, 0); + } + } + else + { + pointing_axis_.normalize(); + } + } + catch(const tf::TransformException &ex) + { + ROS_ERROR("Transform failure (%d): %s", ret1, ex.what()); + gh.setRejected(); + return; + } + } + + //Put the target point in the root frame (usually torso_lift_link). + bool ret1 = false; + try + { + std::string error_msg; + ret1 = tfl_.waitForTransform(root_.c_str(), target.header.frame_id, target.header.stamp, + ros::Duration(5.0), ros::Duration(0.01), &error_msg); + + geometry_msgs::PointStamped target_in_root_msg; + tfl_.transformPoint(root_.c_str(), target, target_in_root_msg ); + tf::pointMsgToTF(target_in_root_msg.point, target_in_root_); + } + catch(const tf::TransformException &ex) + { + ROS_ERROR("Transform failure (%d): %s", ret1, ex.what()); + gh.setRejected(); + return; + } + + if( tip_.compare(pointing_frame_) != 0 ) + { + bool success = tree_.getChain(root_.c_str(), pointing_frame_.c_str(), chain_); + if( !success ) + { + ROS_ERROR("Couldn't create chain from %s to %s.", root_.c_str(), pointing_frame_.c_str()); + gh.setRejected(); + return; + } + tip_ = pointing_frame_; + + pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_)); + jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_)); + joint_names_.resize(chain_.getNrOfJoints()); + } + ROS_INFO("create chain from %s to %s.", root_.c_str(), pointing_frame_.c_str()); + ROS_INFO("create tip %s.", tip_.c_str()); + + unsigned int joints = chain_.getNrOfJoints(); + ROS_INFO("create joints %d", joints); + + int segments = chain_.getNrOfSegments(); + ROS_INFO("Parsed urdf from %s to %s and found %d joints and %d segments.", root_.c_str(), pointing_frame_.c_str(), joints, segments); + for(int i = 0; i < segments; i++) + { + KDL::Segment segment = chain_.getSegment(i); + ROS_INFO("Segment %d, %s: joint %s type %s", + i, segment.getName().c_str(), segment.getJoint().getName().c_str(), segment.getJoint().getTypeName().c_str() ); + joint_names_[i] = segment.getJoint().getName(); + } + + KDL::JntArray jnt_pos(joints), jnt_eff(joints); + KDL::Jacobian jacobian(joints); +#if 0 + pr2_controllers_msgs::QueryTrajectoryState traj_state; + traj_state.request.time = ros::Time::now() + ros::Duration(0.01); + if (!cli_query_traj_.call(traj_state)) + { + ROS_ERROR("Service call to query controller trajectory failed."); + gh.setRejected(); + return; + } + if(traj_state.response.name.size() != joints) + { + ROS_ERROR("Number of joints mismatch: urdf chain vs. trajectory controller state."); + gh.setRejected(); + return; + } +#endif + std::vector limits_(joints); + + // Get initial joint positions and joint limits. + for(unsigned int i = 0; i < joints; i++) + { + //joint_names_[i] = traj_state.response.name[i]; + limits_[i] = *(urdf_model_.joints_[joint_names_[i].c_str()]->limits); + //ROS_DEBUG("Joint %d %s: %f, limits: %f %f", i, traj_state.response.name[i].c_str(), traj_state.response.position[i], limits_[i].lower, limits_[i].upper); + //jnt_pos(i) = traj_state.response.position[i]; + jnt_pos(i) = 0; + } + + int count = 0; + int limit_flips = 0; + float correction_angle = 2*M_PI; + float correction_delta = 2*M_PI; + while( ros::ok() && fabs(correction_delta) > 0.001) + { + //get the pose and jacobian for the current joint positions + KDL::Frame pose; + pose_solver_->JntToCart(jnt_pos, pose); + jac_solver_->JntToJac(jnt_pos, jacobian); + + tf::Transform frame_in_root; + tf::poseKDLToTF(pose, frame_in_root); + + tf::Vector3 axis_in_frame = pointing_axis_.normalized(); + tf::Vector3 target_from_frame = (target_in_root_ - frame_in_root.getOrigin()).normalized(); + tf::Vector3 current_in_frame = frame_in_root.getBasis().inverse()*target_from_frame; + float prev_correction = correction_angle; + correction_angle = current_in_frame.angle(axis_in_frame); + correction_delta = correction_angle - prev_correction; + ROS_INFO("At step %d, joint poses are %.4f and %.4f, angle error is %f, angle delta is %f !", count, jnt_pos(0), jnt_pos(1), correction_angle, correction_delta); + if(correction_angle < 0.5*success_angle_threshold_) break; + tf::Vector3 correction_axis = frame_in_root.getBasis()*(axis_in_frame.cross(current_in_frame).normalized()); + //printVector3("correction_axis in root:", correction_axis); + tf::Transform correction_tf(tf::Quaternion(correction_axis, 0.5*correction_angle), tf::Vector3(0,0,0)); + KDL::Frame correction_kdl; + tf::transformTFToKDL(correction_tf, correction_kdl); + + // We apply a "wrench" proportional to the desired correction + KDL::Frame identity_kdl; + KDL::Twist twist = diff(correction_kdl, identity_kdl); + KDL::Wrench wrench_desi; + for (unsigned int i=0; i<6; i++) + wrench_desi(i) = -1.0*twist(i); + + // Converts the "wrench" into "joint corrections" with a jacbobian-transpose + for (unsigned int i = 0; i < joints; i++) + { + jnt_eff(i) = 0; + for (unsigned int j=0; j<6; j++) + jnt_eff(i) += (jacobian(j,i) * wrench_desi(j)); + jnt_pos(i) += jnt_eff(i); + } + + // account for pan_link joint limit in back. + if(jnt_pos(0) < limits_[0].lower && limit_flips++ == 0){ jnt_pos(0) += 1.5*M_PI; } + if(jnt_pos(0) > limits_[0].upper && limit_flips++ == 0){ jnt_pos(0) -= 1.5*M_PI; } + + jnt_pos(1) = std::max(limits_[1].lower, jnt_pos(1)); + jnt_pos(1) = std::min(limits_[1].upper, jnt_pos(1)); + + count++; + + if(limit_flips > 1){ + ROS_ERROR("Goal is out of joint limits, trying to point there anyway... \n"); + break; + } + if(count > 100){ + ROS_ERROR("Could not reach the goal, trying to point there anyway... \n"); + break; + } + } + ROS_DEBUG("Iterative solver took %d steps", count); + ROS_INFO("Iterative solver took %d steps", count); + + std::vector q_goal(joints); + + for(unsigned int i = 0; i < joints; i++) + { + jnt_pos(i) = std::max(limits_[i].lower, jnt_pos(i)); + jnt_pos(i) = std::min(limits_[i].upper, jnt_pos(i)); + q_goal[i] = jnt_pos(i); + ROS_DEBUG("Joint %d %s: %f", i, joint_names_[i].c_str(), jnt_pos(i)); + } + + if (has_active_goal_) + { + active_goal_.setCanceled(); + has_active_goal_ = false; + } + + gh.setAccepted(); + active_goal_ = gh; + has_active_goal_ = true; + + + // Computes the duration of the movement. + ros::Duration min_duration(0.01); + + if (gh.getGoal()->min_duration > min_duration) + min_duration = gh.getGoal()->min_duration; + + // Determines if we need to increase the duration of the movement in order to enforce a maximum velocity. + if (gh.getGoal()->max_velocity > 0) + { + // Very approximate velocity limiting. + // double dist = sqrt(pow(q_goal[0] - traj_state.response.position[0], 2) + + // pow(q_goal[1] - traj_state.response.position[1], 2)); + double dist = sqrt(pow(q_goal[0] - 0, 2) + + pow(q_goal[1] - 0, 2)); + ros::Duration limit_from_velocity(dist / gh.getGoal()->max_velocity); + if (limit_from_velocity > min_duration) + min_duration = limit_from_velocity; + } + + + // Computes the command to send to the trajectory controller. + trajectory_msgs::JointTrajectory traj; + //traj.header.stamp = traj_state.request.time; + traj.header.stamp = ros::Time::now(); + + traj.joint_names.push_back(joint_names_[0]); + traj.joint_names.push_back(joint_names_[1]); + + traj.points.resize(1); + //traj.points.resize(2); + //traj.points[0].positions = traj_state.response.position; + //traj.points[0].velocities = traj_state.response.velocity; + //traj.points[0].time_from_start = ros::Duration(0.0); + traj.points[0].positions = q_goal; + traj.points[0].velocities.push_back(0); + traj.points[0].velocities.push_back(0); + traj.points[0].time_from_start = ros::Duration(min_duration); + + pub_controller_command_.publish(traj); + } + + + void watchdog(const ros::TimerEvent &e) + { + ros::Time now = ros::Time::now(); + + // Aborts the active goal if the controller does not appear to be active. + if (has_active_goal_) + { + bool should_abort = false; + if (!last_controller_state_) + { + should_abort = true; + ROS_WARN("Aborting goal because we have never heard a controller state message."); + } + else if ((now - last_controller_state_->header.stamp) > ros::Duration(5.0)) + { + should_abort = true; + ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds", + (now - last_controller_state_->header.stamp).toSec()); + } + + if (should_abort) + { + // Stops the controller. + trajectory_msgs::JointTrajectory empty; + empty.joint_names = joint_names_; + pub_controller_command_.publish(empty); + + // Marks the current goal as aborted. + active_goal_.setAborted(); + has_active_goal_ = false; + } + } + } + + + void cancelCB(GoalHandle gh) + { + if (active_goal_ == gh) + { + // Stops the controller. + trajectory_msgs::JointTrajectory empty; + empty.joint_names = joint_names_; + pub_controller_command_.publish(empty); + + // Marks the current goal as canceled. + active_goal_.setCanceled(); + has_active_goal_ = false; + } + } + + pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr last_controller_state_; + void controllerStateCB(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr &msg) + { + last_controller_state_ = msg; + ros::Time now = ros::Time::now(); + + if (!has_active_goal_) + return; + + //! \todo Support frames that are not the pan link itself + try + { + KDL::JntArray jnt_pos(msg->joint_names.size()); + for(unsigned int i = 0; i < msg->joint_names.size(); i++) + jnt_pos(i) = msg->actual.positions[i]; + + KDL::Frame pose; + pose_solver_->JntToCart(jnt_pos, pose); + + tf::Transform frame_in_root; + tf::poseKDLToTF(pose, frame_in_root); + + tf::Vector3 axis_in_frame = pointing_axis_.normalized(); + tf::Vector3 target_from_frame = target_in_root_ - frame_in_root.getOrigin(); + + target_from_frame.normalize(); + tf::Vector3 current_in_frame = frame_in_root.getBasis().inverse()*target_from_frame; + + pr2_controllers_msgs::PointHeadFeedback feedback; + feedback.pointing_angle_error = current_in_frame.angle(axis_in_frame); + active_goal_.publishFeedback(feedback); + + if (feedback.pointing_angle_error < success_angle_threshold_) + { + active_goal_.setSucceeded(); + has_active_goal_ = false; + } + } + catch(const tf::TransformException &ex) + { + ROS_ERROR("Could not transform: %s", ex.what()); + } + } + + +}; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "point_head_action"); + ros::NodeHandle node; + ControlHead ch(node); + ros::spin(); + return 0; +} + diff --git a/hironx_head_action/test/test-head-action.test b/hironx_head_action/test/test-head-action.test new file mode 100644 index 00000000..30401164 --- /dev/null +++ b/hironx_head_action/test/test-head-action.test @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/hironx_head_action/test/test_head_action.py b/hironx_head_action/test/test_head_action.py new file mode 100755 index 00000000..97723117 --- /dev/null +++ b/hironx_head_action/test/test_head_action.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python + +import rospy, actionlib +from pr2_controllers_msgs.msg import PointHeadAction, PointHeadGoal +from geometry_msgs.msg import PointStamped + +import unittest + +class TestHeadAction(unittest.TestCase): + + client = None + + @classmethod + def setUpClass(self): + rospy.init_node('head_action_client') + self.client = actionlib.SimpleActionClient('head_controller/point_head_action', PointHeadAction) + rospy.loginfo("wait_for_server") + self.client.wait_for_server() + + def test_head_goal(self): + goal = PointHeadGoal() + point = PointStamped() + point.header.frame_id = '/WAIST' + point.point.x = 2 + point.point.y = 0 + point.point.z = 1 + goal.target = point + goal.pointing_frame = "HEAD_JOINT1_Link" + goal.pointing_axis.x = 1; + goal.pointing_axis.y = 0; + goal.pointing_axis.z = 0; + goal.min_duration = rospy.Duration(0.5) + goal.max_velocity = 1.0 + rospy.loginfo("send_goal") + self.client.send_goal(goal) + rospy.loginfo("wait_for_result") + self.client.wait_for_result() + self.assertTrue(self.client.get_result()) + +# for debug +# $ python -m unittest test_head_action.TestHeadAction.test_head_goal +# +if __name__ == '__main__': + import rostest + rostest.rosrun('hironx_head_action', 'test_head_action', TestHeadAction) + diff --git a/hironx_ros_bridge/launch/hironx_ros_bridge.launch b/hironx_ros_bridge/launch/hironx_ros_bridge.launch index 2f3ea32a..8a1ef571 100644 --- a/hironx_ros_bridge/launch/hironx_ros_bridge.launch +++ b/hironx_ros_bridge/launch/hironx_ros_bridge.launch @@ -61,4 +61,8 @@ + + + + diff --git a/hironx_ros_bridge/src/hironx_ros_bridge/ros_client.py b/hironx_ros_bridge/src/hironx_ros_bridge/ros_client.py index b35005ad..b5fe756e 100644 --- a/hironx_ros_bridge/src/hironx_ros_bridge/ros_client.py +++ b/hironx_ros_bridge/src/hironx_ros_bridge/ros_client.py @@ -43,7 +43,9 @@ from rospy import ROSInitException from control_msgs.msg import FollowJointTrajectoryAction from control_msgs.msg import FollowJointTrajectoryGoal -from geometry_msgs.msg import Pose +from geometry_msgs.msg import PointStamped, Pose +#from control_msgs.msg import PointHeadAction, PointHeadGoal # Use pr2_controllers_msgs instead, since we're using PR2 control manager +from pr2_controllers_msgs.msg import PointHeadAction, PointHeadGoal from trajectory_msgs.msg import JointTrajectoryPoint from tf.transformations import quaternion_from_euler, euler_from_quaternion, compose_matrix, translation_from_matrix, euler_from_matrix import numpy @@ -99,6 +101,7 @@ def __init__(self, jointgroups=None): # See the doc in the method for the reason why this line is still kept # even after this class has shifted MoveIt! intensive. + rospy.loginfo('Initializing action clients') self._init_action_clients() if not rospy.has_param('robot_description_semantic'): @@ -157,6 +160,9 @@ def _init_action_clients(self): '/head_controller/follow_joint_trajectory_action', FollowJointTrajectoryAction) self._aclient_torso = actionlib.SimpleActionClient( '/torso_controller/follow_joint_trajectory_action', FollowJointTrajectoryAction) + # For head move http://wiki.ros.org/pr2_controllers/Tutorials/Moving%20the%20Head + self._aclient_point_head = actionlib.SimpleActionClient( + '/head_controller/point_head_action', PointHeadAction) self._aclient_larm.wait_for_server() self._goal_larm = FollowJointTrajectoryGoal() @@ -192,6 +198,14 @@ def _init_action_clients(self): self._goal_larm.trajectory.joint_names, self._goal_rarm.trajectory.joint_names)) + # we are pointing the high-def camera frame + # pointing_axis defaults to X-axis) + self._goal_head_point = PointHeadGoal() + self._goal_head_point.pointing_frame = "HEAD_JOINT1_Link" # TODO: Not sure if HEAD_JOINT1_LINK is the most optimized link for HiroNXO for this purpose. + self._goal_head_point.pointing_axis.x = 1 + self._goal_head_point.pointing_axis.y = 0 + self._goal_head_point.pointing_axis.z = 0 + def go_init(self, init_pose_type=0, task_duration=7.0): ''' Change the posture of the entire robot to the pre-defined init pose. @@ -383,3 +397,38 @@ def set_pose(self, joint_group, position, rpy=None, task_duration=7.0, (movegr.go(do_wait) or movegr.go(do_wait) or rospy.logerr('MoveGroup.go fails; jointgr={}'.format(joint_group))) + + def look_at(self, target_frameid, target_x, target_y, target_z, + min_duration=0.5, max_velocity=1.0): + ''' + Python version of "Moving the Head" from PR2 tutorial -- + http://wiki.ros.org/pr2_controllers/Tutorials/Moving%20the%20Head + @param min_duration float: sec. Operation takes no shorter than this + amount of time. + @param max_velocity float: rad/s. Operation goes no faster than this. + ''' + goal = self._goal_head_point + + # the target point, expressed in the requested frame + target_point = PointStamped() + target_point.header.frame_id = target_frameid + target_point.point.x = target_x + target_point.point.y = target_y + target_point.point.z = target_z + + goal.target = target_point + + # take at least 0.5 seconds to get there + goal.min_duration = rospy.Duration(min_duration) + + # and go no faster than this rad/s + goal.max_velocity = max_velocity; + + # send the goal + self._aclient_point_head.send_goal(goal) + # TODO: Is method name correct? Does action client in python exist? + rospy.loginfo('goal sent via aclient_point_head') + + # wait for it to get there (abort after 2 secs to prevent getting stuck) + self._aclient_point_head.wait_for_server() # TODO: Is wait_for_server correct here? Don't we want to wait for result? + rospy.loginfo('done aclient_point_head')