Starting emergency manager

lolo_action_servers/CMakeLists.txt

@@ -165,21 +165,21 @@ include_directories(
 
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
- install(PROGRAMS
-   scripts/geometry.py
-   scripts/lolo.py
-   scripts/ros_lolo.py
-   scripts/goto_waypoint.py
-   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
- )
-
-catkin_install_python(PROGRAMS 
-   scripts/geometry.py
-   scripts/lolo.py
-   scripts/ros_lolo.py
-   scripts/goto_waypoint.py
-   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
+#  install(PROGRAMS
+#    scripts/geometry.py
+#    scripts/lolo.py
+#    scripts/ros_lolo.py
+#    scripts/goto_waypoint.py
+#    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+#  )
+
+# catkin_install_python(PROGRAMS 
+#    scripts/geometry.py
+#    scripts/lolo.py
+#    scripts/ros_lolo.py
+#    scripts/goto_waypoint.py
+#    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
 
 ## Mark executables and/or libraries for installation
 # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node

lolo_action_servers/launch/config.yaml

@@ -4,15 +4,15 @@ action_update_freq : 10
 # update frequency of servo controls
 controller_update_freq : 10
 # duh
-max_rpm : 2000
+max_rpm : 500
 max_fin_radians : 0.6
 
 # How aggressive lolo will be when using control surfaces
 # 0 = dont use at all, 999 = bang-bang
-# 50 = a nice number tuned in sim
 # if set to 0, the only way lolo can turn is with thrusters
 rudder_Kp : 50
 elevator_Kp : 100
+thruster_drive_Kp : 60
 
 # A 2D cone placed on the yaw plane 
 # in front of lolo where it will turn using just the rudders
@@ -27,3 +27,13 @@ rudder_cone_degrees : 18
 # make spirals to dive or rise towards the depth of the point until the point is
 # within the cone again
 forward_cone_degrees : 3
+
+# Obvious
+enable_spiral : false
+enable_thruster_turn : true
+
+# how deep does lolo need to be so that the rudders actually work
+# below this depth, it will reduce thrust on one side to turn
+# above this depth, it will set both thrusters to given RPM
+# and use the rudders to turn
+useless_rudder_depth : 0.8

lolo_action_servers/scripts/goto_waypoint.py → lolo_action_servers/src/goto_waypoint.py

@@ -40,16 +40,21 @@ class LoloGotoWP(object):
     def __init__(self):
 
         # see launch/config.yaml
-        self.lolo = Lolo(max_rpm = get_param("max_rpm", 2000),
+        self.lolo = Lolo(max_rpm = get_param("max_rpm", 500),
                          max_fin_radians = get_param("max_fin_radians", 0.6),
                          rudder_Kp = get_param("rudder_Kp", 50),
                          elevator_Kp = get_param("elevator_Kp", 50),
+                         thruster_drive_Kp = get_param("thruster_drive_Kp", 10),
                          rudder_cone_degrees = get_param("rudder_cone_degrees", 5.72),
-                         forward_cone_degrees = get_param("forward_cone_degrees", 10))
+                         forward_cone_degrees = get_param("forward_cone_degrees", 10),
+                         enable_spiral = get_param("enable_spiral", False),
+                         enable_thruster_turn = get_param("enable_thruster_turn", True),
+                         useless_rudder_depth = get_param("useless_rudder_depth", 0.8))
 
         self.ros_lolo = ROSLolo(lolo = self.lolo,
                                 robot_name = get_param("robot_name", "lolo"),
-                                update_freq = get_param("controller_update_freq", 10))
+                                update_freq = get_param("controller_update_freq", 10),
+                                max_rpm = get_param("max_rpm", 500))
 
         self.update_freq = get_param("action_update_freq", 10)
 

lolo_action_servers/scripts/lolo.py → lolo_action_servers/src/lolo.py

@@ -51,8 +51,12 @@ def __init__(self,
                  max_fin_radians = 0.6,
                  rudder_Kp = 50,
                  elevator_Kp = 50,
+                 thruster_drive_Kp = 10,
                  rudder_cone_degrees = 5.72,
-                 forward_cone_degrees = 10):
+                 forward_cone_degrees = 10,
+                 enable_spiral = False,
+                 enable_thruster_turn = True,
+                 useless_rudder_depth = 0.8):
         """
         A container object that abstracts away ros-related stuff for a nice abstract vehicle
         pose is in NED, x = north, y = east, z = down/depth
@@ -62,8 +66,12 @@ def __init__(self,
         self.max_fin_radians = max_fin_radians
         self.rudder_Kp = rudder_Kp
         self.elevator_Kp = elevator_Kp
+        self.thruster_drive_Kp = thruster_drive_Kp
         self.rudder_cone_degrees = rudder_cone_degrees
         self.forward_cone_degrees = forward_cone_degrees
+        self.enable_spiral = enable_spiral
+        self.enable_thruster_turn = enable_thruster_turn
+        self.useless_rudder_depth = useless_rudder_depth
 
         self.goal = None
         self.control_mode = Lolo.IDLE
@@ -119,11 +127,14 @@ def update(self):
         # Change mode according to where the goal is relative to us
         ####
         # depth control priority
-        if np.abs(pitch_diff) > self.forward_cone_degrees and np.abs(depth_diff) > self.goal.tolerance:
-            self._change_mode(Lolo.BACKWARDS_SPIRAL)
+        if np.abs(pitch_diff) > self.forward_cone_degrees and\
+           np.abs(depth_diff) > self.goal.tolerance and\
+           self.enable_spiral:
+                self._change_mode(Lolo.BACKWARDS_SPIRAL)
         # thruster-turn second priority
-        elif np.abs(yaw_diff) > self.rudder_cone_degrees:
-            self._change_mode(Lolo.THRUSTER_TURN)
+        elif np.abs(yaw_diff) > self.rudder_cone_degrees and\
+             self.enable_thruster_turn:
+                self._change_mode(Lolo.THRUSTER_TURN)
         # no special mode needed, just drive to goal
         else:
             self._change_mode(Lolo.DRIVE)
@@ -196,8 +207,8 @@ def _drive_to_depth(self, pitch_direction, pitch_mag):
 
     def _thruster_turn_to_yaw(self, turn_direction, turn_mag):
         self.desired_rudder_angle = turn_direction * turn_mag * self.max_fin_radians * self.rudder_Kp
-        self.desired_rpms[0] = -turn_direction * self.max_rpm
-        self.desired_rpms[1] =  turn_direction * self.max_rpm
+        self.desired_rpms[0] = -turn_direction * self.goal.rpm
+        self.desired_rpms[1] =  turn_direction * self.goal.rpm
 
 
     def _drive_to_yaw(self, turn_direction, turn_mag):
@@ -208,9 +219,18 @@ def _drive_to_yaw(self, turn_direction, turn_mag):
         # super simple P controller for rudder
         self.desired_rudder_angle = turn_direction * turn_mag * self.max_fin_radians * self.rudder_Kp
         # and just drive forward
-        # TODO possibly tune these for some light P control too
         self.desired_rpms[0] = self.goal.rpm
         self.desired_rpms[1] = self.goal.rpm
+        # if we are not deep enough for rudders to be useful, help with thrusters
+        if self.depth < self.useless_rudder_depth:
+            if turn_direction < 0:
+                self.desired_rpms[0] = self.goal.rpm
+                self.desired_rpms[1] = self.goal.rpm * (1-(self.thruster_drive_Kp*turn_mag))
+            elif turn_direction > 0:
+                self.desired_rpms[0] = self.goal.rpm * (1-(self.thruster_drive_Kp*turn_mag))
+                self.desired_rpms[1] = self.goal.rpm
+
+
 
 
 

lolo_action_servers/scripts/ros_lolo.py → lolo_action_servers/src/ros_lolo.py

@@ -31,7 +31,8 @@ def __init__(self,
                  control_thrusters=True,
                  control_elevons=True,
                  control_rudder=True,
-                 control_elevator=True):
+                 control_elevator=True,
+                 max_rpm = 500):
         """
         Reads the state of a lolo from ROS topics and updates the given lolo object
         Reads the desired angles and such from the lolo object and publishes setpoints
@@ -41,6 +42,7 @@ def __init__(self,
         self.base_link = robot_name + "/base_link"
         self.reference_link = reference_link
         self.update_freq = update_freq
+        self.max_rpm = max_rpm
 
         self.tf_listener = tf.TransformListener()
         while not rospy.is_shutdown():
@@ -105,8 +107,12 @@ def update(self, timer_event=None):
         self.update_tf()
 
         if self.control_thrusters:
-            self.t1_pub.publish(int(self.lolo.desired_rpms[0]))
-            self.t2_pub.publish(int(self.lolo.desired_rpms[1]))
+            t1m = min(self.max_rpm, abs(self.lolo.desired_rpms[0]))
+            t2m = min(self.max_rpm, abs(self.lolo.desired_rpms[1]))
+            t1 = int(np.sign(self.lolo.desired_rpms[0]) * t1m)
+            t2 = int(np.sign(self.lolo.desired_rpms[1]) * t2m)
+            self.t1_pub.publish(t1)
+            self.t2_pub.publish(t2)
 
         if self.control_elevons:
             self.elevon_p_pub.publish(self.lolo.desired_elevon_angles[0])

sam_action_servers/CMakeLists.txt

@@ -22,6 +22,8 @@ find_package(catkin REQUIRED COMPONENTS
   actionlib
   #math
   ddynamic_reconfigure_python
+  ros_type_introspection
+  topic_tools
 )
 
 ## System dependencies are found with CMake's conventions
@@ -144,7 +146,7 @@ include_directories(
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/bezier_planner_node.cpp)
+add_executable(emergency_manager_node src/emergency_manager.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -154,12 +156,14 @@ include_directories(
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(emergency_manager_node 
+${${PROJECT_NAME}_EXPORTED_TARGETS} 
+${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
+target_link_libraries(emergency_manager_node
+  ${catkin_LIBRARIES}
+)
 
 #############
 ## Install ##

sam_action_servers/src/emergency_manager.cpp

@@ -0,0 +1,76 @@
+#include "ros/ros.h"
+#include "ros/callback_queue.h"
+#include <std_msgs/Bool.h>
+#include "sensor_msgs/Image.h"
+#include "sensor_msgs/LaserScan.h"
+#include "geometry_msgs/TwistStamped.h"
+
+#include "ros_type_introspection/ros_introspection.hpp"
+#include <topic_tools/shape_shifter.h>
+#include <thread>
+
+using namespace RosIntrospection;
+using topic_tools::ShapeShifter;
+
+class GenericSensorMonitor
+{
+
+public:
+
+    std::string topic_name_;
+    double rate_;
+    ros::CallbackQueue queue;
+    ros::Subscriber subscriber_;
+
+    GenericSensorMonitor(std::string &topic, double rate): topic_name_(topic), rate_(rate)
+    {
+        ros::NodeHandle nh;
+        nh.setCallbackQueue(&queue);
+        subscriber_ = nh.subscribe(topic_name_, 10, &GenericSensorMonitor::topicCB, this);
+        std::thread(&GenericSensorMonitor::CheckQueue, this).detach();
+    }
+
+    void CheckQueue()
+    {
+        ros::Rate r(rate_);
+        bool queue_init = false;
+        while(ros::ok())
+        {
+            if(!queue.isEmpty())
+            {
+                queue.callOne(ros::WallDuration(0.01));
+                ROS_INFO_STREAM("Callback " << topic_name_);
+                queue_init = true;
+            }
+            else
+            {
+                if(queue_init)
+                {
+                    ROS_ERROR_STREAM("Data not coming in " << topic_name_);
+                }
+                // break;
+            }
+            r.sleep();
+        }    
+    }
+
+    // Callbacks
+    void topicCB(const ShapeShifter::ConstPtr& msg)
+    {
+    }
+
+};
+
+
+int main(int argn, char* args[])
+{
+    ros::init(argn, args, "arm_to_fpga");
+    std::string topic = "/bool/test";
+    std::shared_ptr<GenericSensorMonitor> monitor(new GenericSensorMonitor(topic, 10.));
+    std::string topic2 = "/bool/test2";
+    std::shared_ptr<GenericSensorMonitor> monitor2(new GenericSensorMonitor(topic2, 100.));
+
+    ros::spin();
+
+    return 0;
+}

smarc_bt/CMakeLists.txt

@@ -219,6 +219,7 @@ include_directories(
 #   # myfile2
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )
+
 #install(DIRECTORY launch/
 	#DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
   #PATTERN ".svn" EXCLUDE

smarc_bt/launch/config.yaml

@@ -37,9 +37,9 @@ EMERGENCY_ACTION_NAMESPACE : 'ctrl/emergency_surface_action'
 # the sams in these should be subst_value'd in the launch file!
 # or over-written with params
 LATLONTOUTM_SERVICE : 'dr/lat_lon_to_utm'
-UTM_TO_LATLON_SERVICE : 'dr/utm_to_lat_lon'
-# in cases where the above service couldnt be found for some reason
-LATLONTOUTM_SERVICE_ALTERNATIVE : 'lat_lon_to_utm'
+LATLONTOUTM_SERVICE_ALT : 'lat_lon_to_utm'
+UTMTOLATLON_SERVICE : 'dr/utm_to_lat_lon'
+UTMTOLATLON_SERVICE_ALT : 'utm_to_lat_lon'
 
 # tf frame names
 BASE_LINK : 'base_link'

smarc_bt/msg/GotoWaypoint.msg

@@ -4,7 +4,6 @@
 geometry_msgs/PoseStamped pose
 # required proximity in meters to waypoint in
 # order to consider the waypoint as reached
-# (should be at least 1m)
 float64 goal_tolerance
 
 # Defines the type of z control used going there
@@ -27,4 +26,7 @@ float64 travel_speed
 
 float64 lat
 float64 lon
+float64 arrival_heading
+bool use_heading
+
 string name