From 79438ed5589805e335fefa81787e90d2084dcce4 Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Fri, 3 Feb 2017 18:46:53 +0100 Subject: [PATCH 1/2] rework package: now using virtual KDL chain and FJT action client --- cob_lookat_action/CMakeLists.txt | 34 +- cob_lookat_action/action/LookAt.action | 24 +- .../cob_lookat_action_server.h | 61 +++ cob_lookat_action/package.xml | 6 +- .../scripts/cob_lookat_action_client.py | 39 ++ .../scripts/cob_lookat_target_publisher.py | 24 + .../src/cob_lookat_action_client.py | 85 --- .../src/cob_lookat_action_server.cpp | 498 +++++++----------- .../src/cob_lookat_action_server_node.cpp | 21 + .../src/cob_monitor_arm_client.py | 63 --- 10 files changed, 374 insertions(+), 481 deletions(-) create mode 100644 cob_lookat_action/include/cob_lookat_action/cob_lookat_action_server.h create mode 100755 cob_lookat_action/scripts/cob_lookat_action_client.py create mode 100755 cob_lookat_action/scripts/cob_lookat_target_publisher.py delete mode 100755 cob_lookat_action/src/cob_lookat_action_client.py create mode 100644 cob_lookat_action/src/cob_lookat_action_server_node.cpp delete mode 100755 cob_lookat_action/src/cob_monitor_arm_client.py diff --git a/cob_lookat_action/CMakeLists.txt b/cob_lookat_action/CMakeLists.txt index a86dcee6..41b6e100 100644 --- a/cob_lookat_action/CMakeLists.txt +++ b/cob_lookat_action/CMakeLists.txt @@ -1,7 +1,19 @@ cmake_minimum_required(VERSION 2.8.3) project(cob_lookat_action) -find_package(catkin REQUIRED COMPONENTS actionlib_msgs actionlib control_msgs geometry_msgs message_generation moveit_msgs roscpp tf trajectory_msgs) +find_package(catkin REQUIRED COMPONENTS + actionlib_msgs + actionlib + control_msgs + geometry_msgs + kdl_conversions + kdl_parser + message_generation + roscpp + tf + tf_conversions + trajectory_msgs +) ### Message Generation ### add_action_files( @@ -15,22 +27,32 @@ generate_messages( catkin_package( CATKIN_DEPENDS actionlib_msgs geometry_msgs message_runtime + LIBRARIES cob_lookat_action_server + INCLUDE_DIRS include ) ### BUILD ### -include_directories(${catkin_INCLUDE_DIRS}) +include_directories(include ${catkin_INCLUDE_DIRS}) -add_executable(cob_lookat_action_server src/cob_lookat_action_server.cpp) +add_library(cob_lookat_action_server src/cob_lookat_action_server.cpp) add_dependencies(cob_lookat_action_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(cob_lookat_action_server ${catkin_LIBRARIES}) +add_executable(cob_lookat_action_server_node src/cob_lookat_action_server_node.cpp) +add_dependencies(cob_lookat_action_server_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(cob_lookat_action_server_node cob_lookat_action_server ${catkin_LIBRARIES}) + ### INSTALL ### -install(TARGETS cob_lookat_action_server +install(TARGETS cob_lookat_action_server cob_lookat_action_server_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(PROGRAMS src/cob_lookat_action_client.py src/cob_monitor_arm_client.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/src +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(PROGRAMS scripts/cob_lookat_action_client.py scripts/cob_lookat_target_publisher.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts ) diff --git a/cob_lookat_action/action/LookAt.action b/cob_lookat_action/action/LookAt.action index 72524d91..4b2c98cb 100644 --- a/cob_lookat_action/action/LookAt.action +++ b/cob_lookat_action/action/LookAt.action @@ -1,17 +1,25 @@ ###goal definition -#the target to look at -geometry_msgs/PoseStamped target +string target_frame +string pointing_frame + +#geometry_msgs/Transform target_trans +#geometry_msgs/Transform pointing_offset + +uint8 X_POSITIVE = 0 +uint8 Y_POSITIVE = 1 +uint8 Z_POSITIVE = 2 +uint8 X_NEGATIVE = 3 +uint8 Y_NEGATIVE = 4 +uint8 Z_NEGATIVE = 5 + +uint8 pointing_axis_type -##some more parameters from pr2_point_head action -#geometry_msgs/Vector3 pointing_axis -#string pointing_frame -#duration min_duration -#float64 max_velocity --- #result definition bool success +string message --- #feedback bool status -#float64 pointing_angle_error +string message diff --git a/cob_lookat_action/include/cob_lookat_action/cob_lookat_action_server.h b/cob_lookat_action/include/cob_lookat_action/cob_lookat_action_server.h new file mode 100644 index 00000000..c28b926d --- /dev/null +++ b/cob_lookat_action/include/cob_lookat_action/cob_lookat_action_server.h @@ -0,0 +1,61 @@ +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +class CobLookAtAction +{ +protected: + + ros::NodeHandle nh_; + + actionlib::SimpleActionClient *fjt_ac_; + actionlib::SimpleActionServer *lookat_as_; + std::string fjt_name_; + std::string lookat_name_; + cob_lookat_action::LookAtFeedback lookat_fb_; + cob_lookat_action::LookAtResult lookat_res_; + + std::vector joint_names_; + std::string chain_base_link_; + std::string chain_tip_link_; + + KDL::Chain chain_main_; + boost::shared_ptr fk_solver_pos_; + boost::shared_ptr ik_solver_vel_; + boost::shared_ptr ik_solver_pos_; + + tf::TransformListener tf_listener_; + +public: + + CobLookAtAction(std::string action_name) : + fjt_name_("joint_trajectory_controller/follow_joint_trajectory"), + lookat_name_(action_name) + {} + + ~CobLookAtAction(void) {} + + bool init(); + void goalCB(const cob_lookat_action::LookAtGoalConstPtr &goal); +}; diff --git a/cob_lookat_action/package.xml b/cob_lookat_action/package.xml index 2a7c26b4..2a905093 100644 --- a/cob_lookat_action/package.xml +++ b/cob_lookat_action/package.xml @@ -20,11 +20,15 @@ actionlib_msgs actionlib + boost control_msgs geometry_msgs - moveit_msgs + kdl_conversions + kdl_parser + orocos_kdl roscpp tf + tf_conversions trajectory_msgs rospy diff --git a/cob_lookat_action/scripts/cob_lookat_action_client.py b/cob_lookat_action/scripts/cob_lookat_action_client.py new file mode 100755 index 00000000..e4a3d5b8 --- /dev/null +++ b/cob_lookat_action/scripts/cob_lookat_action_client.py @@ -0,0 +1,39 @@ +#! /usr/bin/env python + +import random + +import rospy +import actionlib + +import cob_lookat_action.msg + +def cob_lookat_action_client(): + client = actionlib.SimpleActionClient('lookat_action', cob_lookat_action.msg.LookAtAction) + print "Waiting for Server..." + client.wait_for_server() + print "...done!" + + # Creates a goal to send to the action server. + goal = cob_lookat_action.msg.LookAtGoal() + goal.target_frame = "lookat_target" + goal.pointing_frame = "sensorring_base_link" + goal.pointing_axis_type = 0 # X_POSITIVE + #print "GOAL: ", goal + + # Sends the goal to the action server. + client.send_goal(goal) + + # Waits for the server to finish performing the action. + client.wait_for_result() + + result = client.get_result() + print result + + return result.success + +if __name__ == '__main__': + try: + rospy.init_node('cob_lookat_action_client') + result = cob_lookat_action_client() + except rospy.ROSInterruptException: + print "program interrupted before completion" diff --git a/cob_lookat_action/scripts/cob_lookat_target_publisher.py b/cob_lookat_action/scripts/cob_lookat_target_publisher.py new file mode 100755 index 00000000..1ef41dc1 --- /dev/null +++ b/cob_lookat_action/scripts/cob_lookat_target_publisher.py @@ -0,0 +1,24 @@ +#!/usr/bin/python + +import random +import rospy +import tf + +if __name__ == '__main__': + rospy.init_node("lookat_target_publisher") + br = tf.TransformBroadcaster() + rate = rospy.Rate(10.0) + pose = (random.uniform(0.2, 1.5), random.uniform(-1.0, 1.0), 1.4+random.uniform(-0.5, 0.5)) + while not rospy.is_shutdown(): + br.sendTransform( pose, + (0.0, 0.0, 0.0, 1.0), + rospy.Time.now(), + "lookat_target", + "base_link") + + try: + rate.sleep() + except rospy.ROSTimeMovedBackwardsException, e: + rospy.logwarn("ROSTimeMovedBackwardsException during sleep(). Continue anyway...") + except rospy.exceptions.ROSInterruptException as e: + pass diff --git a/cob_lookat_action/src/cob_lookat_action_client.py b/cob_lookat_action/src/cob_lookat_action_client.py deleted file mode 100755 index 2ca0e8ad..00000000 --- a/cob_lookat_action/src/cob_lookat_action_client.py +++ /dev/null @@ -1,85 +0,0 @@ -#! /usr/bin/env python - -import random - -import rospy -import actionlib - -from geometry_msgs.msg import PoseStamped -from moveit_msgs.srv import * -from moveit_msgs.msg import * -import cob_lookat_action.msg - -def cob_lookat_action_client(): - # Creates the SimpleActionClient, passing the type of the action - # (FibonacciAction) to the constructor. - client = actionlib.SimpleActionClient('lookat_action', cob_lookat_action.msg.LookAtAction) - print "Waiting for Server..." - client.wait_for_server() - print "...done!" - - focus_pub = rospy.Publisher('/lookat_target', PoseStamped) - rospy.sleep(1.0) - - #print "Waiting for IK-Services..." - #rospy.wait_for_service('/compute_fk') - #print "...done!" - #get_fk = rospy.ServiceProxy('/compute_fk', GetPositionFK) - - #fk_req = moveit_msgs.srv.GetPositionFKRequest() - - #fk_req.header.stamp = rospy.Time.now() - #fk_req.header.frame_id = "base_link" - #fk_req.fk_link_names = ["lookat_focus_frame"] - #fk_req.robot_state.joint_state.name = ["torso_lower_neck_tilt_joint","torso_pan_joint","torso_upper_neck_tilt_joint","lookat_lin_joint","lookat_x_joint","lookat_y_joint","lookat_z_joint"] - #fk_req.robot_state.joint_state.position = [random.uniform(-0.25,0.25), random.uniform(-0.2,0.2), random.uniform(-0.4,0.4), random.uniform(-5.0,5.0), random.uniform(-3.141,3.141), random.uniform(-3.141,3.141), random.uniform(-3.141,3.141)] - ##print "REQUEST: ", fk_req - - #fk_res = moveit_msgs.srv.GetPositionFKResponse() - #try: - #fk_res = get_fk(fk_req) - ##print "RESPONSE:", fk_res - #except rospy.ServiceException, e: - #print "Service call failed: %s"%e - #return False - - - # Creates a goal to send to the action server. - goal = cob_lookat_action.msg.LookAtGoal() - #goal.target = fk_res.pose_stamped[0] - focus = PoseStamped() - focus.header.stamp = rospy.Time.now() - focus.header.frame_id = "base_link" - focus.pose.position.x = random.uniform(-5.0, 5.0) - focus.pose.position.y = random.uniform(-1.0, 1.0) - focus.pose.position.z = random.uniform(0.0, 3.0) - focus.pose.orientation.x = 0.0 - focus.pose.orientation.y = 0.0 - focus.pose.orientation.z = 0.0 - focus.pose.orientation.w = 1.0 - goal.target = focus - #print "GOAL: ", goal - - #Debug - focus_pub.publish(goal.target) - rospy.sleep(1.0) - - # Sends the goal to the action server. - client.send_goal(goal) - - # Waits for the server to finish performing the action. - client.wait_for_result() - - result = client.get_result() - print result - - return result.success - -if __name__ == '__main__': - try: - # Initializes a rospy node so that the SimpleActionClient can - # publish and subscribe over ROS. - rospy.init_node('cob_lookat_action_client') - result = cob_lookat_action_client() - except rospy.ROSInterruptException: - print "program interrupted before completion" diff --git a/cob_lookat_action/src/cob_lookat_action_server.cpp b/cob_lookat_action/src/cob_lookat_action_server.cpp index 1520b742..39c3741d 100644 --- a/cob_lookat_action/src/cob_lookat_action_server.cpp +++ b/cob_lookat_action/src/cob_lookat_action_server.cpp @@ -1,366 +1,228 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include -#include -#include - -class CobLookAtAction +bool CobLookAtAction::init() { -protected: - - ros::NodeHandle nh; - - ros::ServiceClient ik_client; - actionlib::SimpleActionClient *torso_ac; - actionlib::SimpleActionClient *head_ac; - - // NodeHandle instance must be created before this line. Otherwise strange error may occur. - actionlib::SimpleActionServer *lookat_as; - std::string lookat_action_name; - - // create messages that are used to published feedback/result - cob_lookat_action::LookAtFeedback lookat_fb; - cob_lookat_action::LookAtResult lookat_res; - - bool torso_available; - bool head_available; - std::vector torso_joints; - std::vector head_joints; - std::vector lookat_joints; - -public: - - CobLookAtAction(std::string action_name) : - lookat_action_name(action_name) {} - - ~CobLookAtAction(void) {} - - bool init() + /// get parameters from parameter server + if (!nh_.getParam("joint_names", joint_names_)) { - torso_available = false; - head_available = false; - - torso_ac = new actionlib::SimpleActionClient(nh, "/torso_controller/follow_joint_trajectory", true); - head_ac = new actionlib::SimpleActionClient(nh,"/head_controller/follow_joint_trajectory", true); - - ROS_WARN("Waiting for Torso-AS"); - torso_available = torso_ac->waitForServer(ros::Duration(10.0)); - ROS_WARN("Waiting for Head-AS"); - head_available = head_ac->waitForServer(ros::Duration(10.0)); - - if(torso_available) - { - XmlRpc::XmlRpcValue tj; - nh.getParam("/torso_controller/joint_names", tj); - ROS_ASSERT(tj.getType() == XmlRpc::XmlRpcValue::TypeArray); - - for (int32_t i = 0; i < tj.size(); ++i) - { - ROS_ASSERT(tj[i].getType() == XmlRpc::XmlRpcValue::TypeString); - torso_joints.push_back(tj[i]); - } - - if(head_available) - { - XmlRpc::XmlRpcValue hj; - nh.getParam("/head_controller/JointName", hj); - ROS_ASSERT(hj.getType() == XmlRpc::XmlRpcValue::TypeString); - head_joints.push_back(hj); - - //nh.getParam("/head_controller/joints", hj); - //ROS_ASSERT(hj.getType() == XmlRpc::XmlRpcValue::TypeArray); - - //for (int32_t i = 0; i < hj.size(); ++i) - //{ - // ROS_ASSERT(hj[i].getType() == XmlRpc::XmlRpcValue::TypeString); - // head_joints.push_back(hj[i]); - //} - } - - //additional lookat joints - lookat_joints.clear(); - lookat_joints = torso_joints; - lookat_joints.push_back("lookat_lin_joint"); - lookat_joints.push_back("lookat_x_joint"); - lookat_joints.push_back("lookat_y_joint"); - lookat_joints.push_back("lookat_z_joint"); - - ROS_INFO("Starting up..."); - ik_client = nh.serviceClient("/compute_ik"); - - if(head_available) - lookat_as = new actionlib::SimpleActionServer(nh, lookat_action_name, boost::bind(&CobLookAtAction::goalCB, this, _1), false); - else - lookat_as = new actionlib::SimpleActionServer(nh, lookat_action_name, boost::bind(&CobLookAtAction::goalCB_torso, this, _1), false); - - lookat_as->start(); - - return true; - } - - ROS_ERROR("No torso_controller loaded!"); + ROS_ERROR("Parameter 'joint_names' not set"); return false; } - void goalCB(const cob_lookat_action::LookAtGoalConstPtr &goal) + if (!nh_.getParam("chain_base_link", chain_base_link_)) { - // helper variables - bool success = false; - - // publish info to the console for the user - //ROS_INFO("%s: New Goal!", lookat_action_name.c_str()); + ROS_ERROR("Parameter 'chain_base_link' not set"); + return false; + } - moveit_msgs::GetPositionIK srv; - srv.request.ik_request.group_name = "lookat"; - srv.request.ik_request.ik_link_name = "lookat_focus_frame"; - srv.request.ik_request.timeout = ros::Duration(0.1); - srv.request.ik_request.attempts = 1; - srv.request.ik_request.pose_stamped = goal->target; + if (!nh_.getParam("chain_tip_link", chain_tip_link_)) + { + ROS_ERROR("Parameter 'chain_tip_link' not set"); + return false; + } - std::vector joint_seed (lookat_joints.size(), 0.0); + /// parse robot_description and generate KDL chains + KDL::Tree tree; + if (!kdl_parser::treeFromParam("/robot_description", tree)) + { + ROS_ERROR("Failed to construct kdl tree"); + return false; + } - moveit_msgs::RobotState seed; - seed.joint_state.header = goal->target.header; - seed.joint_state.name = lookat_joints; - seed.joint_state.position = joint_seed; - srv.request.ik_request.robot_state = seed; + tree.getChain(chain_base_link_, chain_tip_link_, chain_main_); + if (chain_main_.getNrOfJoints() == 0) + { + ROS_ERROR("Failed to initialize kinematic chain"); + return false; + } - if (ik_client.call(srv)) - { - //ROS_DEBUG("IK Result: %d", srv.response.error_code.val); - if(srv.response.error_code.val == 1) - { - ROS_INFO("IK Solution found"); - success = true; - } - else - { - ROS_ERROR("No IK Solution found"); - success = false; - //lookat_res.success = success; - //// set the action state to aborted - //lookat_as->setAborted(lookat_res); - //return; - } - } - else - { - ROS_ERROR("IK-Call failed"); - success = false; - lookat_res.success = success; - // set the action state to aborted - lookat_as->setAborted(lookat_res); - return; - } - std::vector torso_config; - torso_config.resize(torso_joints.size()); + ROS_WARN_STREAM("Waiting for ActionServer: " << fjt_name_); + fjt_ac_ = new actionlib::SimpleActionClient(nh_, fjt_name_, true); + fjt_ac_->waitForServer(ros::Duration(10.0)); - std::vector head_config; - head_config.resize(head_joints.size()); + lookat_as_ = new actionlib::SimpleActionServer(nh_, lookat_name_, boost::bind(&CobLookAtAction::goalCB, this, _1), false); + lookat_as_->start(); - if(success) - { - for(unsigned int i=0, j=0; i= 0) - head_config[0] = 0.0; //look backwards - else - head_config[0] = -3.1415926; //lock forwards - } - } + return true; +} - // send a goal to the action - control_msgs::FollowJointTrajectoryGoal torso_goal; - torso_goal.trajectory.header.stamp = ros::Time::now(); - torso_goal.trajectory.header.frame_id = "base_link"; - torso_goal.trajectory.joint_names = torso_joints; - trajectory_msgs::JointTrajectoryPoint torso_point; - torso_point.positions = torso_config; - torso_point.time_from_start = ros::Duration(1.0); - torso_goal.trajectory.points.push_back(torso_point); - control_msgs::FollowJointTrajectoryGoal head_goal; - head_goal.trajectory.header.stamp = ros::Time::now(); - head_goal.trajectory.header.frame_id = "base_link"; - head_goal.trajectory.joint_names = head_joints; - trajectory_msgs::JointTrajectoryPoint head_point; - head_point.positions = head_config; - head_point.time_from_start = ros::Duration(1.0); - head_goal.trajectory.points.push_back(head_point); +void CobLookAtAction::goalCB(const cob_lookat_action::LookAtGoalConstPtr &goal) +{ + bool success = true; + std::string message; - torso_ac->sendGoal(torso_goal); - head_ac->sendGoal(head_goal); + /// set up lookat chain + KDL::Chain chain_lookat, chain_full; + KDL::Vector lookat_lin_axis(0.0, 0.0, 0.0); + switch (goal->pointing_axis_type) + { + case cob_lookat_action::LookAtGoal::X_POSITIVE: + lookat_lin_axis.x(1.0); + break; + case cob_lookat_action::LookAtGoal::Y_POSITIVE: + lookat_lin_axis.y(1.0); + break; + case cob_lookat_action::LookAtGoal::Z_POSITIVE: + lookat_lin_axis.z(1.0); + break; + case cob_lookat_action::LookAtGoal::X_NEGATIVE: + lookat_lin_axis.x(-1.0); + break; + case cob_lookat_action::LookAtGoal::Y_NEGATIVE: + lookat_lin_axis.y(-1.0); + break; + case cob_lookat_action::LookAtGoal::Z_NEGATIVE: + lookat_lin_axis.z(-1.0); + break; + default: + ROS_ERROR("PointingAxisType %d not defined! Using default: 'X_POSITIVE'!", goal->pointing_axis_type); + lookat_lin_axis.x(1.0); + break; + } + KDL::Joint lookat_lin_joint("lookat_lin_joint", KDL::Vector(), lookat_lin_axis, KDL::Joint::TransAxis); - bool finished_before_timeout = torso_ac->waitForResult(ros::Duration(5.0)) && head_ac->waitForResult(ros::Duration(5.0)); + /// transform pointing_frame to offset + KDL::Frame offset; + tf::StampedTransform offset_transform; + bool transformed = false; - if (finished_before_timeout) + do + { + try { - ROS_INFO("Both Actions finished"); - success = true; + ros::Time now = ros::Time::now(); + tf_listener_.waitForTransform(chain_tip_link_, goal->pointing_frame, now, ros::Duration(0.1)); + tf_listener_.lookupTransform(chain_tip_link_, goal->pointing_frame, now, offset_transform); + transformed = true; } - else + catch (tf::TransformException& ex) { - ROS_INFO("At leas one Action did not finish before timeout."); - success = false; + ROS_ERROR("LookatAction: %s", ex.what()); + ros::Duration(0.1).sleep(); } + } while (!transformed && ros::ok()); + tf::transformTFToKDL(offset_transform, offset); + //tf::transformMsgToKDL(goal->pointing_offset, offset); - if(success) - { - //ROS_INFO("%s: Succeeded", lookat_action_name.c_str()); - lookat_res.success = success; + KDL::Segment lookat_rotx_link("lookat_rotx_link", lookat_lin_joint, offset); + chain_lookat.addSegment(lookat_rotx_link); - // set the action state to succeeded - lookat_as->setSucceeded(lookat_res); - } - else - { - //ROS_INFO("%s: Failed to execute", lookat_action_name.c_str()); - lookat_res.success = success; + KDL::Vector lookat_rotx_axis(1.0, 0.0, 0.0); + KDL::Joint lookat_rotx_joint("lookat_rotx_joint", KDL::Vector(), lookat_rotx_axis, KDL::Joint::RotAxis); + KDL::Segment lookat_roty_link("lookat_roty_link", lookat_rotx_joint); + chain_lookat.addSegment(lookat_roty_link); - // set the action state to aborted - lookat_as->setAborted(lookat_res); - } - } - - void goalCB_torso(const cob_lookat_action::LookAtGoalConstPtr &goal) - { - // helper variables - bool success = false; + KDL::Vector lookat_roty_axis(0.0, 1.0, 0.0); + KDL::Joint lookat_roty_joint("lookat_roty_joint", KDL::Vector(), lookat_roty_axis, KDL::Joint::RotAxis); + KDL::Segment lookat_rotz_link("lookat_rotz_link", lookat_roty_joint); + chain_lookat.addSegment(lookat_rotz_link); - // publish info to the console for the user - //ROS_INFO("%s: New Goal!", lookat_action_name.c_str()); + KDL::Vector lookat_rotz_axis(0.0, 0.0, 1.0); + KDL::Joint lookat_rotz_joint("lookat_rotz_joint", KDL::Vector(), lookat_rotz_axis, KDL::Joint::RotAxis); + KDL::Segment lookat_focus_frame("lookat_focus_frame", lookat_rotz_joint); + chain_lookat.addSegment(lookat_focus_frame); - moveit_msgs::GetPositionIK srv; - srv.request.ik_request.group_name = "lookat"; - srv.request.ik_request.ik_link_name = "lookat_focus_frame"; - srv.request.ik_request.timeout = ros::Duration(0.1); - srv.request.ik_request.attempts = 1; - srv.request.ik_request.pose_stamped = goal->target; + chain_full = chain_main_; + chain_full.addChain(chain_lookat); - std::vector joint_seed (lookat_joints.size(), 0.0); + /// set up solver + fk_solver_pos_.reset(new KDL::ChainFkSolverPos_recursive(chain_full)); + ik_solver_vel_.reset(new KDL::ChainIkSolverVel_pinv(chain_full)); + ik_solver_pos_.reset(new KDL::ChainIkSolverPos_NR(chain_full, *fk_solver_pos_, *ik_solver_vel_)); - moveit_msgs::RobotState seed; - seed.joint_state.header = goal->target.header; - seed.joint_state.name = lookat_joints; - seed.joint_state.position = joint_seed; - srv.request.ik_request.robot_state = seed; + /// transform target_frame to p_in + KDL::Frame p_in; + tf::StampedTransform transform_in; + transformed = false; - if (ik_client.call(srv)) - { - //ROS_DEBUG("IK Result: %d", srv.response.error_code.val); - if(srv.response.error_code.val == 1) - { - ROS_INFO("IK Solution found"); - success = true; - } - else - { - ROS_ERROR("No IK Solution found"); - success = false; - //lookat_res.success = success; - //// set the action state to aborted - //lookat_as->setAborted(lookat_res); - //return; - } - } - else + do + { + try { - ROS_ERROR("IK-Call failed"); - success = false; - lookat_res.success = success; - // set the action state to aborted - lookat_as->setAborted(lookat_res); - return; + ros::Time now = ros::Time::now(); + tf_listener_.waitForTransform(chain_base_link_, goal->target_frame, now, ros::Duration(0.1)); + tf_listener_.lookupTransform(chain_base_link_, goal->target_frame, now, transform_in); + transformed = true; } - - std::vector torso_config; - torso_config.resize(torso_joints.size()); - - if(success) + catch (tf::TransformException& ex) { - for(unsigned int i=0, j=0; isendGoal(torso_goal); + tf::transformTFToKDL(transform_in, p_in); + KDL::JntArray q_init(chain_full.getNrOfJoints()); + KDL::JntArray q_out(chain_full.getNrOfJoints()); + int result = ik_solver_pos_->CartToJnt(q_init, p_in, q_out); - bool finished_before_timeout = torso_ac->waitForResult(ros::Duration(5.0)); - - if (finished_before_timeout) - { - ROS_INFO("Action finished"); - success = true; - } - else - { - ROS_INFO("Action did not finish before timeout."); - success = false; - } + /// solution valid? + if (result != KDL::SolverI::E_NOERROR) + { + success = false; + message = "Failed to find solution"; + ROS_ERROR_STREAM(lookat_name_ << ": " << message); + lookat_res_.success = success; + lookat_res_.message = message; + lookat_as_->setAborted(lookat_res_); + return; + } + /// execute solution as FJT + control_msgs::FollowJointTrajectoryGoal lookat_traj; + lookat_traj.trajectory.header.stamp = ros::Time::now(); + lookat_traj.trajectory.header.frame_id = chain_base_link_; + lookat_traj.trajectory.joint_names = joint_names_; + trajectory_msgs::JointTrajectoryPoint traj_point; + for(unsigned int i = 0; i < chain_main_.getNrOfJoints(); i++) + { + traj_point.positions.push_back(q_out(i)); + } + traj_point.time_from_start = ros::Duration(3.0); + lookat_traj.trajectory.points.push_back(traj_point); - if(success) - { - ROS_INFO("%s: Succeeded", lookat_action_name.c_str()); - lookat_res.success = success; + fjt_ac_->sendGoal(lookat_traj); - // set the action state to succeeded - lookat_as->setSucceeded(lookat_res); - } - else + bool finished_before_timeout = fjt_ac_->waitForResult(ros::Duration(5.0)); + + /// fjt action successful? + if (finished_before_timeout) + { + actionlib::SimpleClientGoalState state = fjt_ac_->getState(); + if(state != actionlib::SimpleClientGoalState::SUCCEEDED) { - ROS_INFO("%s: Failed to execute", lookat_action_name.c_str()); - lookat_res.success = success; - - // set the action state to aborted - lookat_as->setAborted(lookat_res); + success = false; + message = "FJT finished: " + state.toString(); + ROS_ERROR_STREAM(lookat_name_ << ": " << message); + lookat_res_.success = success; + lookat_res_.message = message; + lookat_as_->setAborted(lookat_res_); + return; } } + else + { + success = false; + message = "FJT did not finish before the time out."; + ROS_ERROR_STREAM(lookat_name_ << ": " << message); + lookat_res_.success = success; + lookat_res_.message = message; + lookat_as_->setAborted(lookat_res_); + return; + } -}; - - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "cob_lookat_action"); - - CobLookAtAction *lookat = new CobLookAtAction("lookat_action"); - if(lookat->init()) - ros::spin(); - return 0; + /// lookat action successful? + success = true; + message = "Lookat finished successful."; + ROS_ERROR_STREAM(lookat_name_ << ": " << message); + lookat_res_.success = success; + lookat_res_.message = message; + lookat_as_->setSucceeded(lookat_res_); + return; } diff --git a/cob_lookat_action/src/cob_lookat_action_server_node.cpp b/cob_lookat_action/src/cob_lookat_action_server_node.cpp new file mode 100644 index 00000000..cc1d5104 --- /dev/null +++ b/cob_lookat_action/src/cob_lookat_action_server_node.cpp @@ -0,0 +1,21 @@ +#include +#include + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "lookat_action"); + + CobLookAtAction *lookat = new CobLookAtAction("lookat_action"); + if(lookat->init()) + { + ROS_INFO_STREAM("lookat_action running..."); + ros::spin(); + } + else + { + ROS_ERROR_STREAM("Failed to initialize lookat_action"); + } + + return 0; +} diff --git a/cob_lookat_action/src/cob_monitor_arm_client.py b/cob_lookat_action/src/cob_monitor_arm_client.py deleted file mode 100755 index f22bca32..00000000 --- a/cob_lookat_action/src/cob_monitor_arm_client.py +++ /dev/null @@ -1,63 +0,0 @@ -#! /usr/bin/env python - -import random - -import rospy -import actionlib - -from geometry_msgs.msg import PoseStamped -from moveit_msgs.srv import * -from moveit_msgs.msg import * -import cob_lookat_action.msg - -def cob_lookat_action_client(): - # Creates the SimpleActionClient, passing the type of the action - # (FibonacciAction) to the constructor. - client = actionlib.SimpleActionClient('lookat_action', cob_lookat_action.msg.LookAtAction) - - focus_pub = rospy.Publisher('/lookat_target', PoseStamped) - - # Waits until the action server has started up and started - # listening for goals. - client.wait_for_server() - - # Creates a goal to send to the action server. - goal = cob_lookat_action.msg.LookAtGoal() - focus = PoseStamped() - focus.header.stamp = rospy.Time.now() - focus.header.frame_id = "arm_7_link" - focus.pose.orientation.w = 1.0 - goal.target = focus - #print "GOAL: ", goal - - while not rospy.is_shutdown(): - #client.cancel_goal() - - focus.header.stamp = rospy.Time.now() - - ##Debug - #focus_pub.publish(goal.target) - #rospy.sleep(2.0) - - # Sends the goal to the action server. - client.send_goal(goal) - - #rospy.sleep(0.1) - - - # Waits for the server to finish performing the action. - client.wait_for_result() - - result = client.get_result() - print result - - return result.success - -if __name__ == '__main__': - try: - # Initializes a rospy node so that the SimpleActionClient can - # publish and subscribe over ROS. - rospy.init_node('cob_lookat_action_client') - result = cob_lookat_action_client() - except rospy.ROSInterruptException: - print "program interrupted before completion" From caae2f842c93630c75f60e15ef62e8d54f85a0db Mon Sep 17 00:00:00 2001 From: Felix Messmer Date: Wed, 8 Feb 2017 09:39:10 +0100 Subject: [PATCH 2/2] fix install tags --- cob_lookat_action/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cob_lookat_action/CMakeLists.txt b/cob_lookat_action/CMakeLists.txt index 41b6e100..6494c573 100644 --- a/cob_lookat_action/CMakeLists.txt +++ b/cob_lookat_action/CMakeLists.txt @@ -54,5 +54,5 @@ install(DIRECTORY include/${PROJECT_NAME}/ ) install(PROGRAMS scripts/cob_lookat_action_client.py scripts/cob_lookat_target_publisher.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}/scripts + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} )