Merge pull request #2 from MahimanaGIT/eli/main

Eli/main

operations/CMakeLists.txt

@@ -23,6 +23,7 @@ find_package(catkin REQUIRED COMPONENTS
   srcp2_msgs
   tf2
   tf2_ros
+  perception
 )
 
 ## System dependencies are found with CMake's conventions
@@ -76,6 +77,8 @@ add_message_files(
 add_action_files(DIRECTORY action
   FILES
   Navigation.action
+  ResourceLocaliser.action
+  NavigationVision.action
 )
 
 generate_messages(
@@ -119,7 +122,7 @@ generate_messages(
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES operations
- CATKIN_DEPENDS roscpp rospy actionlib_msgs std_msgs message_runtime utils geometry_msgs srcp2_msgs
+ CATKIN_DEPENDS roscpp rospy actionlib_msgs std_msgs message_runtime utils geometry_msgs srcp2_msgs perception
  #  DEPENDS system_lib
 )
 
@@ -141,6 +144,7 @@ include_directories(
 # Declare a C++ library
 add_library(${PROJECT_NAME}
   src/navigation_algorithm.cpp
+  src/scout_state_machine.cpp
 )
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 
@@ -185,6 +189,13 @@ target_link_libraries(navigation_client
   ${catkin_LIBRARIES}
 )
 
+# Drives both robots forwards in task 2. Example node
+add_executable(resource_localiser src/resource_localiser.cpp)
+add_dependencies(resource_localiser ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(resource_localiser
+${catkin_LIBRARIES}
+)
+
 add_executable(trajectory_processor src/trajectory_processor.cpp)
 add_dependencies(trajectory_processor ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(trajectory_processor
@@ -197,9 +208,9 @@ target_link_libraries(wheel_speed_processing
   ${catkin_LIBRARIES}
 )
 
-add_executable(navigation_vision src/navigation_vision.cpp)
-add_dependencies(navigation_vision ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-target_link_libraries(navigation_vision
+add_executable(navigation_vision_server src/navigation_vision_server.cpp)
+add_dependencies(navigation_vision_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(navigation_vision_server
   ${catkin_LIBRARIES}
 )
 

operations/action/Navigation.action

@@ -1,11 +1,9 @@
 # Define the goal
 float32 forward_velocity  # For manual driving
-float32 angular_velocity  # For manual driving
-
-geometry_msgs/PointStamped point # Will not contain orientation information
+float32 angular_velocity  # For manual driving. Positive is counterclockwise, negative is clockwise
 
 geometry_msgs/PoseStamped pose   # Will contain orientation information
-bool manual_driving              # Whether or not this is a manual drive
+uint32 drive_mode              # Whether or not this is a manual drive. Corresponds to COMMON_NAMES::NAV_TYPE
 ---
 # Define the result
 # See COMMON_NAMES::NAV_RESULT for possible values

operations/action/NavigationVision.action

@@ -0,0 +1,8 @@
+# Define the goal
+string desired_object_label  
+---
+# Define the result
+bool object_reached
+---
+# Define a feedback message
+bool centered

operations/action/ResourceLocaliser.action

@@ -0,0 +1,8 @@
+# Define the goal
+
+---
+# Define the result
+uint32 resource_final_distance
+---
+# Define a feedback message
+float32 percent_complete

operations/include/operations/navigation_algorithm.h

@@ -10,6 +10,10 @@
 #include <tf2_ros/transform_listener.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
+#include <utils/common_names.h>
+
+using namespace COMMON_NAMES;
+
 class NavigationAlgo
 {
 private:
@@ -165,6 +169,8 @@ class NavigationAlgo
    */
   static double changeInPosition(const geometry_msgs::PoseStamped& current_robot_pose, const geometry_msgs::PoseStamped& target_robot_pose);
 
+  static double changeInOrientation(const geometry_msgs::PoseStamped& desired_pose, const std::string& robot_name, const tf2_ros::Buffer& tf_buffer);
+
 };
 
 #endif

operations/include/operations/scout_state_machine.h

@@ -0,0 +1,95 @@
+#ifndef SCOUT_STATE_MACHINE_H
+#define SCOUT_STATE_MACHINE_H
+
+#include <ros/ros.h>
+#include <iostream>
+#include <operations/NavigationAction.h> 
+#include <operations/ResourceLocaliserAction.h> 
+#include <actionlib/client/simple_action_client.h>
+#include <srcp2_msgs/VolSensorMsg.h>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <std_msgs/Empty.h>
+#include <utils/common_names.h>
+#include <mutex>
+
+using namespace COMMON_NAMES;
+
+enum LOCATOR_STATES
+{
+  INIT = 0,   // At the start of each session or after recharging, 
+              // we would like to be at a certain location. Hence this state to get there
+  SEARCH ,    // SEARCH ALGORITHM
+  LOCATE,     // Rotate and drive until distance minimises
+  FOUND,      // Stop at the location for leading Excavator to the location
+  MOVE_OUT,    // Make way for Excavator
+  RECHARGE,    // Find and go to Recharge Station
+  WAIT_FOR_STATE_UPDATE   // Do nothing
+};
+
+
+class ScoutStateMachine
+{
+
+private:
+  ros::NodeHandle nh_;
+
+  LOCATOR_STATES robot_state_ = LOCATOR_STATES::INIT;
+  std::string robot_name_;
+  bool state_machine_continue_ = true;
+
+  typedef actionlib::SimpleActionClient<operations::NavigationAction> NavigationClient_;
+  NavigationClient_* navigation_client_;
+  operations::NavigationGoal navigation_action_goal_;
+
+  typedef actionlib::SimpleActionClient<operations::ResourceLocaliserAction> ResourceLocaliserClient_;
+  ResourceLocaliserClient_* resource_localiser_client_;
+  operations::ResourceLocaliserGoal resource_localiser_goal_;
+
+  // Listens to the volatile sensor
+  ros::Subscriber volatile_sub_;
+
+  // Publishes the exact location of a localised volatile
+  ros::Publisher  volatile_pub_;
+
+  // Listens for excavators indicating they are ready to pick up a volatile.
+  ros::Subscriber excavator_ready_sub_;
+
+  // Listens for robot odometry. Used to publish volatile location in FOUND
+  ros::Subscriber robot_odom_sub_;
+
+  void processVolatileMessage(const srcp2_msgs::VolSensorMsg::ConstPtr& vol_msg);
+  void processOdomMessage(const nav_msgs::Odometry::ConstPtr& odom_msg);
+  void processExcavatorMessage(const std_msgs::Empty::ConstPtr& excavator_msg);
+
+  // Whether or not we are on the first iteration of a state.
+  // Reset to true on state transition, set to false after first iteration.
+  bool first_iter_ = true;
+
+  // Whether a volatile is currently detected. Reset to false at the beginning of SEARCH
+  bool vol_detected_ = false;
+
+  // Whether an excavator has arrived and is ready to pick up a volatile. Reset to false at the beginning of FOUND
+  bool excavator_ready_ = false;
+
+  // Most recent robot pose from odometry
+  geometry_msgs::PoseStamped robot_pose_;
+
+  // Mutex-protected way to get robot_pose_
+  geometry_msgs::PoseStamped getRobotPose();
+
+  // Mutexex to protect flags set in subscribers and read in the SM
+  std::mutex vol_flag_mutex_;
+  std::mutex robot_pose_mutex_;
+  std::mutex excavator_ready_mutex_;
+
+public:
+  ScoutStateMachine(ros::NodeHandle nh, const std::string& robot_name);
+
+  ~ScoutStateMachine();
+
+  void startStateMachine();
+  void stopStateMachine(); // Do we need it though?
+};
+
+#endif // SCOUT_STATE_MACHINE_H

operations/launch/nav_vision_test.launch

@@ -0,0 +1,10 @@
+<launch>
+    <arg name="robot_name" default="small_scout_1" />
+
+    <include file="$(find operations)/launch/wheel_controller.launch">
+        <arg name="robot_name" value="$(arg robot_name)" />
+    </include>
+
+    <node name="navigation_actionlib_server" pkg="operations" type="navigation_actionlib_server" args="$(arg robot_name)" output="screen"/>
+    <node name="wheel_speed_processing" pkg="operations" type="wheel_speed_processing" args="$(arg robot_name)" />
+</launch>

operations/package.xml

@@ -65,6 +65,7 @@
 
   <build_depend>geometry_msgs</build_depend>
   <build_depend>srcp2_msgs</build_depend>
+  <build_depend>perception</build_depend>
 
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
@@ -77,6 +78,7 @@
   <build_export_depend>geometry_msgs</build_export_depend>
   <build_export_depend>perception</build_export_depend>
   <build_export_depend>srcp2_msgs</build_export_depend>
+  <build_export_depend>perception</build_export_depend>
 
   <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
@@ -89,6 +91,7 @@
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>perception</exec_depend>
   <exec_depend>srcp2_msgs</exec_depend>
+  <exec_depend>perception</exec_depend>
 
   <test_depend>rosunit</test_depend>