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 _door_detector_actions_alg_node_h_ 00026 #define _door_detector_actions_alg_node_h_ 00027 00028 #include <iri_base_algorithm/iri_base_algorithm.h> 00029 #include "door_detector_actions_alg.h" 00030 00031 // [publisher subscriber headers] 00032 #include <visualization_msgs/Marker.h> 00033 #include <std_msgs/Int8.h> 00034 #include <geometry_msgs/PoseStamped.h> 00035 #include <arm_navigation_msgs/MoveArmAction.h> 00036 #include <arm_navigation_msgs/utils.h> 00037 #include <tf/transform_listener.h> 00038 #include <tf/transform_datatypes.h> 00039 00040 // [service client headers] 00041 00042 // [action server client headers] 00043 #include <iri_action_server/iri_action_server.h> 00044 #include <iri_door_detector/FindADoorAction.h> 00045 00050 class DoorDetectorActionsAlgNode : public algorithm_base::IriBaseAlgorithm<DoorDetectorActionsAlgorithm> 00051 { 00052 private: 00053 // [publisher attributes] 00054 ros::Publisher arm_poses_marker_publisher_; 00055 visualization_msgs::Marker Marker_msg_; 00056 ros::Publisher door_action_start_publisher_; 00057 std_msgs::Int8 Int8_msg_; 00058 00059 // [subscriber attributes] 00060 CMutex arm_poses_marker_mutex_; 00061 ros::Subscriber door_handle_subscriber_; 00062 void door_handle_callback(const geometry_msgs::PoseStamped::ConstPtr& msg); 00063 CMutex door_handle_mutex_; 00064 ros::Subscriber door_centroid_subscriber_; 00065 void door_centroid_callback(const geometry_msgs::PoseStamped::ConstPtr& msg); 00066 CMutex door_centroid_mutex_; 00067 00068 // [algorithm variables] 00069 int closed_door; 00070 int open_door; 00071 int start; 00072 int voting_rule_approval; 00073 float side_open; 00074 float side_closed; 00075 std::string tf_original_frame; 00076 std::string tf_base_frame; 00077 std::string tf_arm_frame; 00078 std::string tf_target_frame; 00079 std::string tf_target_frame_2; 00080 std::string current_planner; 00081 tf::TransformListener tf_listener; 00082 std_msgs::Int8 action_start; 00083 visualization_msgs::Marker marker; 00084 geometry_msgs::PoseStamped base_door_centroid; 00085 geometry_msgs::PoseStamped base_door_handle; 00086 geometry_msgs::PoseStamped marker_pose; 00087 arm_navigation_msgs::MotionPlanRequest arm_door_centroid; 00088 arm_navigation_msgs::MotionPlanRequest arm_door_handle; 00089 00090 // [service attributes] 00091 00092 // [client attributes] 00093 00094 // [action server attributes] 00095 IriActionServer<iri_door_detector::FindADoorAction> find_a_door_aserver_; 00096 void find_a_doorStartCallback(const iri_door_detector::FindADoorGoalConstPtr& goal); 00097 void find_a_doorStopCallback(void); 00098 bool find_a_doorIsFinishedCallback(void); 00099 bool find_a_doorHasSucceedCallback(void); 00100 void find_a_doorGetResultCallback(iri_door_detector::FindADoorResultPtr& result); 00101 void find_a_doorGetFeedbackCallback(iri_door_detector::FindADoorFeedbackPtr& feedback); 00102 00103 // [action client attributes] 00104 00105 // [reconfigurable parameters] 00106 double base_distance_before_closed_door; 00107 double base_distance_after_open_door; 00108 double arm_distance_before_closed_door; 00109 double z_arm_offset; 00110 double y_offset; 00111 bool no_simulator; 00112 00113 public: 00120 DoorDetectorActionsAlgNode(void); 00121 00128 ~DoorDetectorActionsAlgNode(void); 00129 00130 protected: 00143 void mainNodeThread(void); 00144 00157 void node_config_update(Config &config, uint32_t level); 00158 00165 void addNodeDiagnostics(void); 00166 00172 geometry_msgs::PoseStamped transformGoalBase (geometry_msgs::PoseStamped pose, std::string traget_frame); 00173 00179 arm_navigation_msgs::MotionPlanRequest transformGoalArm (geometry_msgs::PoseStamped pose, std::string traget_frame); 00180 00186 arm_navigation_msgs::MotionPlanRequest liftArm (geometry_msgs::PoseStamped pose, std::string target_frame); 00187 00193 visualization_msgs::Marker ArrowMarker(std_msgs::Header header, geometry_msgs::Pose pose, int alpha, int color, const char arrow_tag []); 00194 00195 // [diagnostic functions] 00196 00197 // [test functions] 00198 }; 00199 00200 #endif