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 @@
+<launch>
+
+<!-- A launch file to independently (re-)launch the point head action for debugging -->
+
+<group ns="head_controller" >
+ <node name="point_head_action"
+ pkg="hironx_head_action" type="head_action" output="screen" >
+ <rosparam>
+ success_angle_threshold: 0.01
+ </rosparam>
+ </node>
+</group>
+
+</launch>
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 @@
+<package>
+ <name>hironx_head_action</name>
+ <version>1.1.5</version>
+ <description>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 <a href = "http://wiki.ros.org/pr2_head_action">pr2_head_action</a>
+ </description>
+ <maintainer email="dev@opensource-robotics.tokyo.jp">TORK</maintainer>
+ <license>BSD</license>
+ <url type="website">http://wiki.ros.org/hironx_head_action</url>
+ <!-- <url type="bugtracker"></url> -->
+
+ <author>Stuart Glaser</author>
+ <author email="iisaito@opensource-robotics.tokyo.jp">Isaac I. Y. Saito</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>actionlib</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>kdl_parser</build_depend>
+ <build_depend>message_filters</build_depend>
+ <build_depend>orocos_kdl</build_depend>
+ <build_depend>pr2_controllers_msgs</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>tf</build_depend>
+ <build_depend>tf_conversions</build_depend>
+ <build_depend>trajectory_msgs</build_depend>
+
+ <run_depend>actionlib</run_depend>
+ <run_depend>geometry_msgs</run_depend>
+ <run_depend>kdl_parser</run_depend>
+ <run_depend>message_filters</run_depend>
+ <run_depend>orocos_kdl</run_depend>
+ <run_depend>pr2_controllers_msgs</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+ <run_depend>tf</run_depend>
+ <run_depend>tf_conversions</run_depend>
+ <run_depend>trajectory_msgs</run_depend>
+</package>
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 <ros/ros.h>
+
+#include <vector>
+#include <boost/scoped_ptr.hpp>
+#include <boost/thread/condition.hpp>
+
+#include <actionlib/server/action_server.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/Twist.h>
+
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chain.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
+#include <kdl/frames.hpp>
+#include "kdl/chainfksolverpos_recursive.hpp"
+#include "kdl_parser/kdl_parser.hpp"
+
+#include "tf_conversions/tf_kdl.h"
+#include <message_filters/subscriber.h>
+#include <tf/message_filter.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+
+#include <sensor_msgs/JointState.h>
+#include <std_msgs/Float32MultiArray.h>
+#include <std_msgs/Int32.h>
+
+#include <urdf/model.h>
+
+#include <trajectory_msgs/JointTrajectory.h>
+#include <pr2_controllers_msgs/PointHeadAction.h>
+#include <pr2_controllers_msgs/QueryTrajectoryState.h>
+#include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
+
+
+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<pr2_controllers_msgs::PointHeadAction> 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<tf::Point> 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<KDL::ChainFkSolverPos> pose_solver_;
+ boost::scoped_ptr<KDL::ChainJntToJacSolver> 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<trajectory_msgs::JointTrajectory>("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<urdf::JointLimits> 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<double> 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 @@
+<launch>
+ <arg name="corbaport" default="2809" />
+ <arg name="GUI" default="false" />
+
+ <include file="$(find hironx_ros_bridge)/test/test-hironx-ros-bridge-common.launch">
+ <arg name="GUI" value="$(arg GUI)" />
+ <arg name="corbaport" value="$(arg corbaport)" />
+ </include>
+
+ <test type="test_head_action.py" pkg="hironx_head_action" test-name="test_head_action" />
+
+</launch>
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 @@
<node pkg="rqt_gui" type="rqt_gui" name="rqt_hironxo" respawn="false" output="screen"
args="--perspective-file $(find hironx_ros_bridge)/resource/hironx_rqt_dashboard.perspective"/>
</group>
+
+ <!-- Enable head point action https://github.com/start-jsk/rtmros_hironx/pull/427 -->
+ <include file="$(find hironx_head_action)/launch/head_action.launch"/>
+
</launch>
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')