Merge pull request #98 from ipa-fxm/rework_lookat_action

rework package: now using virtual KDL chain and FJT action client

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}
 )

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

cob_lookat_action/include/cob_lookat_action/cob_lookat_action_server.h

@@ -0,0 +1,61 @@
+#include <string>
+#include <vector>
+#include <math.h>
+
+#include <ros/ros.h>
+
+#include <actionlib/client/simple_action_client.h>
+#include <actionlib/server/simple_action_server.h>
+#include <actionlib/client/terminal_state.h>
+
+#include <cob_lookat_action/LookAtAction.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <trajectory_msgs/JointTrajectory.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+
+#include <tf/transform_listener.h>
+#include <tf/transform_datatypes.h>
+#include <tf_conversions/tf_kdl.h>
+#include <kdl_conversions/kdl_msg.h>
+#include <kdl_parser/kdl_parser.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainiksolvervel_pinv.hpp>
+#include <kdl/chainiksolverpos_nr.hpp>
+
+
+class CobLookAtAction
+{
+protected:
+
+    ros::NodeHandle nh_;
+
+    actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> *fjt_ac_;
+    actionlib::SimpleActionServer<cob_lookat_action::LookAtAction> *lookat_as_;
+    std::string fjt_name_;
+    std::string lookat_name_;
+    cob_lookat_action::LookAtFeedback lookat_fb_;
+    cob_lookat_action::LookAtResult lookat_res_;
+
+    std::vector<std::string> joint_names_;
+    std::string chain_base_link_;
+    std::string chain_tip_link_;
+
+    KDL::Chain chain_main_;
+    boost::shared_ptr<KDL::ChainFkSolverPos_recursive> fk_solver_pos_;
+    boost::shared_ptr<KDL::ChainIkSolverVel_pinv> ik_solver_vel_;
+    boost::shared_ptr<KDL::ChainIkSolverPos_NR> 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);
+};

cob_lookat_action/package.xml

@@ -20,11 +20,15 @@
 
   <depend>actionlib_msgs</depend>
   <depend>actionlib</depend>
+  <depend>boost</depend>
   <depend>control_msgs</depend>
   <depend>geometry_msgs</depend>
-  <depend>moveit_msgs</depend>
+  <depend>kdl_conversions</depend>
+  <depend>kdl_parser</depend>
+  <depend>orocos_kdl</depend>
   <depend>roscpp</depend>
   <depend>tf</depend>
+  <depend>tf_conversions</depend>
   <depend>trajectory_msgs</depend>
 
   <exec_depend>rospy</exec_depend>

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"

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