Added TakeOff and Landing emulator

CMakeLists.txt

@@ -144,6 +144,8 @@ add_executable(firedetectionemulator src/firedetection_action_emulator_node.cpp)
 add_executable(makeplanemulator src/makeplan_action_emulator_node.cpp)
 add_executable(fireextinguishemulator src/fireextinguish_action_emulator_node.cpp)
 add_executable(windowdetectionemulator src/windowdetection_action_emulator_node.cpp)
+add_executable(takeoffemulator src/takeoff_action_emulator_node.cpp)
+add_executable(landingemulator src/landing_action_emulator_node.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -157,6 +159,8 @@ add_dependencies(firedetectionemulator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${cat
 add_dependencies(makeplanemulator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(fireextinguishemulator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(windowdetectionemulator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(takeoffemulator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(landingemulator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(firedetectionemulator
@@ -175,6 +179,13 @@ target_link_libraries(fireextinguishemulator
    ${catkin_LIBRARIES}
  )
 
+target_link_libraries(takeoffemulator
+   ${catkin_LIBRARIES}
+ )
+
+target_link_libraries(landingemulator
+   ${catkin_LIBRARIES}
+ )
 
 #############
 ## Install ##

launch/emulators.launch

@@ -27,20 +27,20 @@
 
   </group>
 
-  <group ns="firefly1">
+  <!--<group ns="firefly1">-->
 	  <node pkg="upo_actions" type="firedetectionemulator" name ="fire_detection_node" output="screen">
 
 	    <param name="success_percentaje" value="1.0"/>
 	    <param name="duration" value="10.0"/>
 
 	  </node>
 
-	  <!--<node pkg="upo_actions" type="makeplanemulator" name ="make_plan_node" output="screen">
+	  <node pkg="upo_actions" type="makeplanemulator" name ="make_plan_node" output="screen">
 
 	    <param name="success_percentaje" value="1.0"/>
 	    <param name="duration" value="5.0"/>
 
-	  </node>-->
+	  </node>
 
 	 <node pkg="upo_actions" type="fireextinguishemulator" name ="fire_extinguish_node" output="screen">
 
@@ -56,9 +56,22 @@
 
 	  </node>
 
+	<node pkg="upo_actions" type="takeoffemulator" name ="takeoff_node" output="screen">
+
+	    <param name="success_percentaje" value="1.0"/>
+	    <param name="duration" value="2.0"/>
+
+	  </node>
+
+	<node pkg="upo_actions" type="landingemulator" name ="landing_node" output="screen">
+
+	    <param name="success_percentaje" value="1.0"/>
+	    <param name="duration" value="2.0"/>
+
+	  </node>
 
 
-  </group>
+ <!-- </group>-->
 
 </launch>
 

src/landing_action_emulator_node.cpp

@@ -0,0 +1,24 @@
+#include "upo_actions_emulator.hpp"
+
+using namespace UPOActionsEmulators;
+
+int main(int argc, char** argv)
+{
+  //One for each robot
+  ros::init(argc, argv, "makeplan_emulator_node");
+  ros::NodeHandle nh;
+
+  LandingActionEmulator landingROS(nh);
+
+  ros::Rate r(10);
+
+  while(ros::ok())
+  {
+	landingROS.process();
+
+	ros::spinOnce();
+	r.sleep();
+  }
+
+  return 0;
+}

src/takeoff_action_emulator_node.cpp

@@ -0,0 +1,24 @@
+#include "upo_actions_emulator.hpp"
+
+using namespace UPOActionsEmulators;
+
+int main(int argc, char** argv)
+{
+  //One for each robot
+  ros::init(argc, argv, "makeplan_emulator_node");
+  ros::NodeHandle nh;
+
+  TakeOffActionEmulator takeoffROS(nh);
+
+  ros::Rate r(10);
+
+  while(ros::ok())
+  {
+	takeoffROS.process();
+
+	ros::spinOnce();
+	r.sleep();
+  }
+
+  return 0;
+}

src/upo_actions_emulator.hpp

@@ -12,6 +12,8 @@
 #include <upo_actions/MakePlanAction.h>
 #include <upo_actions/FireExtinguishAction.h>
 #include <upo_actions/WindowDetectionAction.h>
+#include <upo_actions/TakeOffAction.h>
+#include <upo_actions/LandingAction.h>
 
 namespace UPOActionsEmulators
 {
@@ -379,6 +381,234 @@ void preemptCB()
 };
 
 
+
+class TakeOffActionEmulator
+{
+
+public:
+
+ TakeOffActionEmulator(ros::NodeHandle nh): action_(nh, "TakeOff", false)
+{
+	ros::NodeHandle nhp("~");
+	float duration_of_action;
+  	//Get parameters from configuration file
+  	nhp.param<float>("success_percentaje", success_rate_,1.0);
+	nhp.param<float>("duration", duration_of_action,10.0);
+
+	action_duration_ = ros::Duration(duration_of_action);
+
+	//Start action servers
+	//register the goal and feeback callbacks
+  	action_.registerGoalCallback(boost::bind(&TakeOffActionEmulator::goalCB, this));
+  	action_.registerPreemptCallback(boost::bind(&TakeOffActionEmulator::preemptCB, this));
+
+  	action_.start();
+
+}
+
+
+void process(void)
+{
+	upo_actions::TakeOffFeedback action_feedback_;
+  	upo_actions::TakeOffResult action_result_;
+
+	//ROS_INFO("Processing...");
+	if (action_.isActive())
+	{
+		ros::Duration action_current_duration_ = ros::Time::now() - action_start_time_;
+
+		//Return the adequate feedback, if any		
+		//fdas_feedback_.current_time.data = fdas_current_duration_;
+		//fdas_.publishFeedback(fdas_feedback_);
+
+		//
+		if(action_current_duration_ > action_duration_)
+		{
+			action_result_.extra_info = "Took off";
+
+			if(success_rate_>random_number_)
+			{
+				action_result_.success = true;
+				action_.setSucceeded(action_result_);
+			}else
+			{
+				action_result_.success = false;
+				action_.setAborted(action_result_);
+			}
+		}
+
+	}
+
+}
+
+
+private:
+
+  //Defined success rate of the node
+  float success_rate_;
+  //Random number to emulate it
+  float random_number_;
+
+
+  actionlib::SimpleActionServer<upo_actions::TakeOffAction> action_;
+  //Duration of the action. After this time, if the random_number is over the success_rate_ the action will succeed. It will fail otherwise
+  ros::Duration action_duration_;
+  ros::Time action_start_time_;
+
+
+
+//! Goal received
+void goalCB()
+  {
+    // reset helper variables
+    action_start_time_ = ros::Time::now();
+
+    // accept the new goal
+   // action_duration_ = action_.acceptNewGoal()->detect_time.data;
+    action_.acceptNewGoal();
+
+	//TODO: improve this
+    std::default_random_engine generator;
+    std::uniform_real_distribution<float> distribution(0.0,1.0);
+    random_number_ = distribution(generator);
+
+    ROS_INFO("TakeOff emulator: Goal Received");
+
+
+    //Preempts received for the new goal between checking if isNewGoalAvailable or invocation of a goal callback and the acceptNewGoal call will not trigger a preempt callback. This means, isPreemptRequested should be called after accepting the goal even for callback-based implementations to make sure the new goal does not have a pending preempt request.
+
+   if(action_.isPreemptRequested())
+	action_.setPreempted();
+
+
+  }
+
+
+void preemptCB()
+  {
+    //ROS_INFO("%s: Preempted", action_name_.c_str());
+    // set the action state to preempted
+    action_.setPreempted();
+  }
+
+
+};
+
+
+class LandingActionEmulator
+{
+
+public:
+
+ LandingActionEmulator(ros::NodeHandle nh): action_(nh, "Landing", false)
+{
+	ros::NodeHandle nhp("~");
+	float duration_of_action;
+  	//Get parameters from configuration file
+  	nhp.param<float>("success_percentaje", success_rate_,1.0);
+	nhp.param<float>("duration", duration_of_action,10.0);
+
+	action_duration_ = ros::Duration(duration_of_action);
+
+	//Start action servers
+	//register the goal and feeback callbacks
+  	action_.registerGoalCallback(boost::bind(&LandingActionEmulator::goalCB, this));
+  	action_.registerPreemptCallback(boost::bind(&LandingActionEmulator::preemptCB, this));
+
+  	action_.start();
+
+}
+
+
+void process(void)
+{
+	upo_actions::LandingFeedback action_feedback_;
+  	upo_actions::LandingResult action_result_;
+
+	//ROS_INFO("Processing...");
+	if (action_.isActive())
+	{
+		ros::Duration action_current_duration_ = ros::Time::now() - action_start_time_;
+
+		//Return the adequate feedback, if any		
+		//fdas_feedback_.current_time.data = fdas_current_duration_;
+		//fdas_.publishFeedback(fdas_feedback_);
+
+		//
+		if(action_current_duration_ > action_duration_)
+		{
+			action_result_.extra_info = "Took off";
+
+			if(success_rate_>random_number_)
+			{
+				action_result_.success = true;
+				action_.setSucceeded(action_result_);
+			}else
+			{
+				action_result_.success = false;
+				action_.setAborted(action_result_);
+			}
+		}
+
+	}
+
+}
+
+
+private:
+
+  //Defined success rate of the node
+  float success_rate_;
+  //Random number to emulate it
+  float random_number_;
+
+
+  actionlib::SimpleActionServer<upo_actions::LandingAction> action_;
+  //Duration of the action. After this time, if the random_number is over the success_rate_ the action will succeed. It will fail otherwise
+  ros::Duration action_duration_;
+  ros::Time action_start_time_;
+
+
+
+//! Goal received
+void goalCB()
+  {
+    // reset helper variables
+    action_start_time_ = ros::Time::now();
+
+    // accept the new goal
+   // action_duration_ = action_.acceptNewGoal()->detect_time.data;
+    action_.acceptNewGoal();
+
+	//TODO: improve this
+    std::default_random_engine generator;
+    std::uniform_real_distribution<float> distribution(0.0,1.0);
+    random_number_ = distribution(generator);
+
+    ROS_INFO("Landing emulator: Goal Received");
+
+
+    //Preempts received for the new goal between checking if isNewGoalAvailable or invocation of a goal callback and the acceptNewGoal call will not trigger a preempt callback. This means, isPreemptRequested should be called after accepting the goal even for callback-based implementations to make sure the new goal does not have a pending preempt request.
+
+   if(action_.isPreemptRequested())
+	action_.setPreempted();
+
+
+  }
+
+
+void preemptCB()
+  {
+    //ROS_INFO("%s: Preempted", action_name_.c_str());
+    // set the action state to preempted
+    action_.setPreempted();
+  }
+
+
+};
+
+
+
 }