00001 // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC. 00002 // Author 00003 // All rights reserved. 00004 // 00005 // This file is part of iri-ros-pkg 00006 // iri-ros-pkg is free software: you can redistribute it and/or modify 00007 // it under the terms of the GNU Lesser General Public License as published by 00008 // the Free Software Foundation, either version 3 of the License, or 00009 // at your option) any later version. 00010 // 00011 // This program is distributed in the hope that it will be useful, 00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 // GNU Lesser General Public License for more details. 00015 // 00016 // You should have received a copy of the GNU Lesser General Public License 00017 // along with this program. If not, see <http://www.gnu.org/licenses/>. 00018 // 00019 // IMPORTANT NOTE: This code has been generated through a script from the 00020 // iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness 00021 // of the scripts. ROS topics can be easly add by using those scripts. Please 00022 // refer to the IRI wiki page for more information: 00023 // http://wikiri.upc.es/index.php/Robotics_Lab 00024 00025 #ifndef _arm_movements_by_pose_alg_node_h_ 00026 #define _arm_movements_by_pose_alg_node_h_ 00027 00028 #include <iri_base_algorithm/iri_base_algorithm.h> 00029 #include "arm_movements_by_pose_alg.h" 00030 00031 // [publisher subscriber headers] 00032 #include <visualization_msgs/MarkerArray.h> 00033 #include <visualization_msgs/Marker.h> 00034 00035 // [service client headers] 00036 #include <iri_wam_common_msgs/joints_move.h> 00037 #include <iri_wam_common_msgs/wamInverseKinematics.h> 00038 #include <iri_wam_common_msgs/pose_move.h> 00039 #include <estirabot_msgs/ArmMovementsPosesSrv.h> 00040 00041 // [action server client headers] 00042 #include <actionlib/client/simple_action_client.h> 00043 #include <actionlib/client/terminal_state.h> 00044 #include <arm_navigation_msgs/MoveArmAction.h> 00045 00046 00047 // other 00048 #include <arm_navigation_msgs/utils.h> 00049 //#include <geometry_msgs/PoseStamped.h> 00050 00051 // logfile 00052 #include <iostream> 00053 #include <fstream> 00054 00059 class ArmMovementsByPoseAlgNode : public algorithm_base::IriBaseAlgorithm<ArmMovementsByPoseAlgorithm> 00060 { 00061 private: 00062 // [publisher attributes] 00063 ros::Publisher visualization_movement_markers_publisher_; 00064 visualization_msgs::MarkerArray MarkerArray_msg_; 00065 00066 // [subscriber attributes] 00067 00068 // [service attributes] 00069 ros::ServiceServer clean_movement_server_; 00070 bool clean_movementCallback(estirabot_msgs::ArmMovementsPosesSrv::Request &req, estirabot_msgs::ArmMovementsPosesSrv::Response &res); 00071 CMutex clean_movement_mutex_; 00072 00073 // [client attributes] 00074 ros::ServiceClient wam_pose_move_client_; 00075 iri_wam_common_msgs::pose_move wam_pose_move_srv_; 00076 ros::ServiceClient move_in_joints_client_; 00077 iri_wam_common_msgs::joints_move move_in_joints_srv_; 00078 ros::ServiceClient get_joints_from_pose_client_; 00079 iri_wam_common_msgs::wamInverseKinematics get_joints_from_pose_srv_; 00080 ros::ServiceClient pose_move_client_; 00081 iri_wam_common_msgs::pose_move pose_move_srv_; 00082 00083 // [action server attributes] 00084 00085 // [action client attributes] 00086 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_right_arm_client_; 00087 arm_navigation_msgs::MoveArmGoal move_right_arm_goal_; 00088 bool move_right_armMakeActionRequest(geometry_msgs::PoseStamped pose); 00089 void move_right_armDone(const actionlib::SimpleClientGoalState& state, const arm_navigation_msgs::MoveArmResultConstPtr& result); 00090 void move_right_armActive(); 00091 void move_right_armFeedback(const arm_navigation_msgs::MoveArmFeedbackConstPtr& feedback); 00092 00093 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_left_arm_client_; 00094 arm_navigation_msgs::MoveArmGoal move_left_arm_goal_; 00095 bool move_left_armMakeActionRequest(geometry_msgs::PoseStamped pose); 00096 void move_left_armDone(const actionlib::SimpleClientGoalState& state, const arm_navigation_msgs::MoveArmResultConstPtr& result); 00097 void move_left_armActive(); 00098 void move_left_armFeedback(const arm_navigation_msgs::MoveArmFeedbackConstPtr& feedback); 00099 00100 // generic acction client make action request 00101 bool move_armMakeActionRequest(bool right_arm, arm_navigation_msgs::SimplePoseConstraint desired_pose, std::string group_name); 00102 00103 // Other 00104 bool fake_movements; 00105 bool robot_wam; 00106 bool log_movements; 00107 double roll_rotation, yaw_rotation, pitch_rotation; 00108 double robot_movement_speed_; // currently only supported by WAM 00109 00110 // log file 00111 std::ofstream logfile; 00112 00118 void get_poses_markers(std::vector<geometry_msgs::PoseStamped> poses, std::vector< uint8_t> secondary_arm); 00119 00125 std::vector< double > get_wam_movement_speed(); 00126 00127 public: 00134 ArmMovementsByPoseAlgNode(void); 00135 00142 ~ArmMovementsByPoseAlgNode(void); 00143 00144 protected: 00157 void mainNodeThread(void); 00158 00171 void node_config_update(Config &config, uint32_t level); 00172 00179 void addNodeDiagnostics(void); 00180 00181 // [diagnostic functions] 00182 00183 // [test functions] 00184 }; 00185 00186 #endif