Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef _darwin_autocharge_alg_node_h_
00026 #define _darwin_autocharge_alg_node_h_
00027
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "darwin_autocharge_alg.h"
00030 #include "eventserver.h"
00031 #include <iostream>
00032 #include "stdlib.h"
00033 #include "math.h"
00034 #include <tf/transform_listener.h>
00035
00036
00037 #include <std_msgs/Int32.h>
00038 #include <geometry_msgs/Twist.h>
00039 #include <sensor_msgs/JointState.h>
00040 #include <std_msgs/Float64MultiArray.h>
00041 #include <ar_pose/ARMarkers.h>
00042 #include <geometry_msgs/PoseStamped.h>
00043
00044
00045 #include <kinematics_msgs/GetPositionIK.h>
00046
00047
00048 #include <arm_navigation_msgs/MoveArmAction.h>
00049 #include <iri_darwin_robot/tracking_headAction.h>
00050 #include <actionlib/client/simple_action_client.h>
00051 #include <actionlib/client/terminal_state.h>
00052 #include <control_msgs/FollowJointTrajectoryAction.h>
00053
00054 typedef enum {searching=0,searching2,rotate,new_goal,tracking,walking,initial_pose,search_left_marker,search_left_marker2,search_right_marker,search_right_marker2,move_back,transform_left_pose,transform_right_pose,make_decision,make_decision2,open_gripper,open_gripper2,take_connectors,take_connectors2,close_gripper,correct_position,tracking_left,tracking_left2,tracking_right,tracking_right2,go_forward,go_back,turn_right,turn_left,go_right,go_left} TStates;
00055
00060 class DarwinAutochargeAlgNode : public algorithm_base::IriBaseAlgorithm<DarwinAutochargeAlgorithm>
00061 {
00062 private:
00063
00064 ros::Publisher execute_action_publisher_;
00065 std_msgs::Int32 Int32_msg_;
00066 ros::Publisher cmd_vel_publisher_;
00067 geometry_msgs::Twist Twist_msg_;
00068 ros::Publisher head_target_publisher_;
00069 std_msgs::Float64MultiArray Float64MultiArray_msg_;
00070
00071
00072 ros::Subscriber adc_channels_subscriber_;
00073 void adc_channels_callback(const std_msgs::Float64MultiArray::ConstPtr& msg);
00074 CMutex adc_channels_mutex_;
00075 double current_voltage;
00076 ros::Subscriber joint_states_subscriber_;
00077 void joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg);
00078 CMutex joint_states_mutex_;
00079 sensor_msgs::JointState current_state;
00080 ros::Subscriber marker_subscriber_;
00081 void marker_callback(const ar_pose::ARMarkers::ConstPtr& msg);
00082 CMutex marker_mutex_;
00083 ar_pose::ARMarkers msg_;
00084 geometry_msgs::PoseStamped marker_;
00085 geometry_msgs::PoseStamped conv_msg_;
00086 geometry_msgs::PoseStamped right_pose_;
00087 geometry_msgs::PoseStamped left_pose_;
00088 bool marker_found;
00089 bool marker_really_founded;
00090
00091
00092
00093 ros::ServiceClient get_ik_client_;
00094 kinematics_msgs::GetPositionIK get_ik_srv_;
00095
00096
00097
00098
00099 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_left_arm_client_;
00100 arm_navigation_msgs::MoveArmGoal move_left_arm_goal_;
00101 void move_left_armMakeActionRequest();
00102 void move_left_armDone(const actionlib::SimpleClientGoalState& state, const arm_navigation_msgs::MoveArmResultConstPtr& result);
00103 void move_left_armActive();
00104 void move_left_armFeedback(const arm_navigation_msgs::MoveArmFeedbackConstPtr& feedback);
00105 bool left_arm_moving;
00106
00107 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_right_arm_client_;
00108 arm_navigation_msgs::MoveArmGoal move_right_arm_goal_;
00109 void move_right_armMakeActionRequest();
00110 void move_right_armDone(const actionlib::SimpleClientGoalState& state, const arm_navigation_msgs::MoveArmResultConstPtr& result);
00111 void move_right_armActive();
00112 void move_right_armFeedback(const arm_navigation_msgs::MoveArmFeedbackConstPtr& feedback);
00113 bool right_arm_moving;
00114
00115 actionlib::SimpleActionClient<iri_darwin_robot::tracking_headAction> tracking_head_client_;
00116 iri_darwin_robot::tracking_headGoal tracking_head_goal_;
00117 void tracking_headMakeActionRequest();
00118 void tracking_headDone(const actionlib::SimpleClientGoalState& state, const iri_darwin_robot::tracking_headResultConstPtr& result);
00119 void tracking_headActive();
00120 void tracking_headFeedback(const iri_darwin_robot::tracking_headFeedbackConstPtr& feedback);
00121 bool head_is_tracking;
00122
00123 actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> move_joints_client_;
00124 control_msgs::FollowJointTrajectoryGoal move_joints_goal_;
00125 void move_jointsMakeActionRequest();
00126 void move_jointsDone(const actionlib::SimpleClientGoalState& state, const control_msgs::FollowJointTrajectoryResultConstPtr& result);
00127 void move_jointsActive();
00128 void move_jointsFeedback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback);
00129 bool joints_moving;
00130
00131 tf::TransformListener tf_listener;
00132 int n,k,m,p;
00133 double current_pan,current_tilt;
00134 bool ready;
00135 bool close_to_station;
00136 bool left_marker_found;
00137 bool right_marker_found;
00138 bool right_data_ready;
00139 bool left_data_ready;
00140 bool left_done;
00141 bool ready_for_check;
00142 bool left_ok;
00143 bool right_ok;
00144 bool prev_left_ok;
00145 bool prev_right_ok;
00146 bool left_ready_tf;
00147 bool right_ready_tf;
00148 bool first_goal;
00149 bool success;
00150
00151 TStates state;
00152
00153 public:
00160 DarwinAutochargeAlgNode(void);
00161
00168 ~DarwinAutochargeAlgNode(void);
00169
00170 protected:
00183 void mainNodeThread(void);
00184
00197 void node_config_update(Config &config, uint32_t level);
00198
00205 void addNodeDiagnostics(void);
00206
00207
00208
00209
00210 };
00211
00212 #endif