nppilot: added the ROS package skeleton

ros/src/nppilot/CMakeLists.txt

@@ -0,0 +1,199 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(nppilot)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES nppilot
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/nppilot.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## 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/nppilot_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## 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})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_nppilot.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

ros/src/nppilot/config/v_pid.yaml

@@ -0,0 +1,11 @@
+Kp: 0
+Ki: 0
+Kd: 0
+
+upper_limit: 0.5
+lower_limit: 0
+windup_limit: 0.5
+
+setpoint_topic: /v/sp
+topic_from_plant: /v/pv
+topic_from_controller: /v/u

ros/src/nppilot/config/yaw_rate_pid.yaml

@@ -0,0 +1,11 @@
+Kp: 0
+Ki: 0
+Kd: 0
+
+upper_limit: 1
+lower_limit: -1
+windup_limit: 0.5
+
+setpoint_topic: /yaw_rate/sp
+topic_from_plant: /yaw_rate/pv
+topic_from_controller: /yaw_rate/u

ros/src/nppilot/launch/rover1.launch

@@ -0,0 +1,10 @@
+<launch>
+  <node name="yaw_rate_pid" pkg="pid" type="controller">
+    <rosparam command="load" file="$(find nppilot)/config/yaw_rate_pid.yaml"/>
+  </node>
+  <node name="v_pid" pkg="pid" type="controller">
+    <rosparam command="load" file="$(find nppilot)/config/v_pid.yaml"/>
+  </node>
+  <node pkg="nppilot" name="revo" type="revo.py" output="screen">
+  </node>
+</launch>

ros/src/nppilot/package.xml

@@ -0,0 +1,70 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>nppilot</name>
+  <version>0.0.0</version>
+  <description>The nppilot package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="michaelh@todo.todo">michaelh</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/nppilot</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>rosflight_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>rosflight_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ros/src/nppilot/scripts/revo.py

@@ -0,0 +1,186 @@
+#!/usr/bin/env python
+"""Revo adapts from high level demands into calibrated demands for the
+rover.
+"""
+
+from __future__ import division, with_statement, print_function
+
+import time
+import types
+import Queue as queue
+import enum
+
+import rospy
+import std_msgs
+
+from std_msgs.msg import Float32, Float64, Bool
+from geometry_msgs.msg import Vector3Stamped
+from rosflight_msgs.msg import Command, RCRaw, Status
+
+
+class RCChannel(enum.IntEnum):
+    roll = 0
+    pitch = 1
+    throttle = 2
+    yaw = 3
+    mode = 5
+
+
+def _scale(x, scale):
+    """Scale a -1 to 1 number to the given (low, zero, high) range.
+
+    >>> _scale(-0.5, (-3, 0, 4))
+    -1.5
+    >>> _scale(-0.5, (0.1, 0.3, 0.9))
+    0.2
+    >>> round(_scale(0.5, (0.1, 0.3, 0.9)), 5)
+    0.6
+    >>> round(_scale(0.8, (-0.8, 0.1, 0.9)), 5)
+    0.74
+    """
+    low, zero, high = scale
+    if x <= -1:
+        return low
+    if x >= 1:
+        return high
+    if x < 0:
+        return low + (low - zero) * x
+    return zero + (high - zero) * x
+
+
+def _scale_rc(x):
+    """Scale a RC time in us to [-1..1].
+
+    >>> round(_scale_rc(1500), 5)
+    0.0
+    >>> round(_scale_rc(1100), 5)
+    -0.8
+    >>> round(_scale_rc(1800), 5)
+    0.6
+    """
+    if x <= 1000:
+        return -1
+    if x >= 2000:
+        return 1
+    return -1 + (x - 1000) / 500
+
+
+class SlewLimiter:
+    def __init__(self, rate, now=time.time):
+        self._rate = rate
+        self._a = None
+        self._t = None
+        self._now = now
+
+    def __call__(self, v):
+        if self._rate == None:
+            return v
+
+        now = self._now()
+        if self._t == None:
+            self._v = v
+        else:
+            limit = self._rate * (now - self._t)
+            delta = min(limit, max(-limit, v - self._v))
+            self._v += delta
+
+        self._t = now
+        return self._v
+
+
+class Controller:
+    def __init__(self):
+        rospy.init_node('revo', anonymous=True)
+        self._command_pub = rospy.Publisher('command', Command, queue_size=10)
+        self._mode = rospy.Publisher('mode', Float64, queue_size=10)
+        self._override_z = rospy.Publisher('override_z', Bool, queue_size=10)
+        self._override_f = rospy.Publisher('override_f', Bool, queue_size=10)
+        self._cmd = [0] * 4
+        self._scales = [
+            (-0.9, 0, 0.9),
+            (-0.9, 0, 0.9),
+            (-0.9, 0, 0.9),
+            (0.1, 0.3, 0.9),
+        ]
+        max_accel = [
+            None,
+            None,
+            None,
+            1,
+        ]
+        self._accel_limiter = [SlewLimiter(x) for x in max_accel]
+        self._rc_trim = [0] * 8
+        self._rc = [0] * 8
+        self._armed = False
+
+    def cmd_z(self, msg):
+        self._cmd[2] = msg.data
+
+    def cmd_f(self, msg):
+        self._cmd[3] = msg.data
+
+    def rc_raw(self, msg):
+        rc = [_scale_rc(x) for x in msg.values]
+        rc = [x + trim for x, trim in zip(rc, self._rc_trim)]
+        self._rc = rc
+
+    def status(self, msg):
+        self._armed = msg.armed
+
+    def send_command(self):
+        h = std_msgs.msg.Header()
+        h.stamp = rospy.Time.now()
+        cmd = [_scale(x, s) for x, s in zip(self._cmd, self._scales)]
+        cmd = [f(x) for x, f in zip(cmd, self._accel_limiter)]
+        self._command_pub.publish(Command(h, 0, 0, *cmd))
+
+    def send_override(self):
+        self._override_z.publish(abs(self._rc[RCChannel.roll]) > 0.1)
+        self._override_f.publish((abs(self._rc[RCChannel.throttle]) > 0.1)
+                                 or not self._armed)
+
+    def send_mode(self):
+        switch = self._rc[RCChannel.mode]
+        if switch < -0.3:
+            mode = 0
+        elif switch < 0.3:
+            mode = 1
+        else:
+            mode = 2
+        self._mode.publish(mode)
+
+    def run(self):
+        q = queue.Queue()
+
+        rospy.Subscriber('cmd_z', Float64, lambda x: q.put((self.cmd_z, x)))
+        rospy.Subscriber('cmd_f', Float64, lambda x: q.put((self.cmd_f, x)))
+        rospy.Subscriber('rc_raw', RCRaw, lambda x: q.put((self.rc_raw, x)))
+        rospy.Subscriber('status', Status, lambda x: q.put((self.status, x)))
+
+        rate = rospy.Rate(20)
+        cycle = 0
+
+        while not rospy.is_shutdown():
+            try:
+                while True:
+                    f, msg = q.get_nowait()
+                    f(msg)
+            except queue.Empty:
+                pass
+            self.send_command()
+            if cycle % 5 == 0:
+                self.send_override()
+                self.send_mode()
+            cycle += 1
+            if cycle >= 100:
+                cycle = 0
+            rate.sleep()
+
+
+def main():
+    c = Controller()
+    c.run()
+
+
+if __name__ == '__main__':
+    main()