Melodic devel

Dockerfile

@@ -0,0 +1,34 @@
+ARG ROS_DISTRO=melodic
+
+FROM ros:$ROS_DISTRO
+
+ARG DOCKER_USER=kubo
+ARG LIBSBP_V=2.3.10
+
+MAINTAINER Amy Phung [email protected]
+
+RUN bash -c \
+    'useradd -lmG video $DOCKER_USER \
+    && mkdir -p /home/$DOCKER_USER/catkin_ws/src/state_controller'
+
+COPY . /home/$DOCKER_USER/catkin_ws/src/state_controller/
+
+RUN bash -c \
+# General Setup
+    'apt-get update \
+    && apt-get upgrade -y \
+    && apt-get install -y wget \
+    && apt-get install -y sudo \
+# Set up catkin workspace
+    && cd /home/$DOCKER_USER/catkin_ws \
+    && rosdep update \
+    && source /opt/ros/$ROS_DISTRO/setup.bash \
+    && rosdep install -iry --from-paths src \
+    && cd /home/$DOCKER_USER/catkin_ws \
+# Build package
+    && catkin_make -j1'
+
+WORKDIR /home/$DOCKER_USER/catkin_ws
+USER $DOCKER_USER
+
+WORKDIR /home/$DOCKER_USER

README.md

@@ -1,6 +1,24 @@
 # Overview
 The state controller is designed to act as the midbrain of a robot - facilitating easy integration of and switching between safety protocols, remote control, and fully autonomous behaviors. The controller can be configured to collect commands from an arbitrary number of nodes and send the appropriate messages through to the hindbrain. The state controller also handles activating/disactivating the robot and interacts with the softestop.
 
+# Setup
+1. Clone this repository to your catkin workspace
+```
+git clone https://github.com/olinrobotics/state_controller.git
+```
+2. Resolve dependencies
+```
+rosdep install -iry --from-paths src
+```
+
+# Files
+Launch files:
++ `teleop.launch` - starts up the joy and teleop nodes
++ `mainstate.launch` - the primary launch file - sets up behaviors, starts up mainstate node, and runs teleop launch file.
+  + Args:
+    + `behaviors_file`: A .yaml file with set behaviors corresponding priorities. Defaults to behaviors listed in `config/example_behaviors`, but this should be changed for different projects
+
+
 # ROS Structure
 ([Diagram of the ROS Nodes](https://photos.app.goo.gl/ZgS1Ykb9EHDQ4bWTA))
 
@@ -18,3 +36,11 @@ _graphic created using_ `rosrun rqt_graph rqt_graph`
 
 # Writing a Behavior Node
 See the GitHub Wiki for tutorials and example code covering Python and C++ ([link](https://github.com/olinrobotics/state_controller/wiki))
+
+# Known Issues
++ The Joy node assumes all axes default to 0 and will publish
+0s for the two triggers until they are pulled. The trigger's "resting"
+state is 1, not 0, so this becomes problematic and causes "runaway"
+behavior when only one of the triggers have been pulled. To get around
+this, the operator must pull both triggers to update the joy node
+before the hitch will be activated

launch/mainstate.launch

@@ -16,11 +16,11 @@ add any nodes here that you need to interface with twists
 -->
 <launch>
 
-<rosparam command="delete" param="/behaviors"/>
-<arg name="behaviors_file" default="$(find state_controller)/config/example_behaviors.yaml"/>
+  <rosparam command="delete" param="/behaviors"/>
+  <arg name="behaviors_file" default="$(find state_controller)/config/example_behaviors.yaml"/>
 
-<rosparam command="load" file="$(arg behaviors_file)" />
-<node pkg="state_controller" type="MainState" name="mainstate" output="screen"/>
-<include file="$(find state_controller)/launch/teleop.launch"/>
+  <rosparam command="load" file="$(arg behaviors_file)" />
+  <node pkg="state_controller" type="MainState" name="mainstate" output="screen"/>
+  <include file="$(find state_controller)/launch/teleop.launch"/>
 
 </launch>

package.xml

@@ -36,10 +36,12 @@
   <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>message_generation</build_depend>
+  <build_depend>joy</build_depend>
   <build_export_depend>geometry_msgs</build_export_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <build_export_depend>message_generation</build_export_depend>
+  <build_export_depend>joy</build_export_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>std_msgs</exec_depend>

src/Teleop/Teleop.cpp

@@ -88,6 +88,13 @@ void Teleop::joyCB(const sensor_msgs::Joy::ConstPtr &joy){
    *
    * @param[in] joy Message read from /joy topic
    */
+
+  if (joy->axes[5] == 0 || joy->axes[2] == 0) {
+    ROS_INFO_ONCE("Hitch has not been initialized. Please pull both triggers to initialize hitch controls");
+  } else if (joy->axes[5] != 0 && joy->axes[2] != 0) {
+    ROS_INFO_ONCE("Hitch successfully initialized. Hitch controls active");
+  }
+
   //check for estop
   if(joy->buttons[estopButton] && !estop && !estopButtonFlag){
     activate(false);
@@ -165,9 +172,18 @@ void Teleop::joyCB(const sensor_msgs::Joy::ConstPtr &joy){
         driveMsg.twist.linear.x =  joy->axes[1];
         driveMsgPub.publish(driveMsg);
       }
-      // // generate and send hitch message
-      if (hitchMsg.pose.position.z != computeZPosition(joy->axes[5], joy->axes[2]) ||
-          hitchMsg.pose.orientation.y != computeYOrientation(joy->buttons[5], joy->buttons[4])) {
+      // if triggers haven't been moved from their default position, do nothing
+      // Note: The Joy node assumes all axes default to 0 and will publish
+      // 0s for the two triggers until they are pulled. The trigger's "resting"
+      // state is 1, not 0, so this becomes problematic and causes "runaway"
+      // behavior when only one of the triggers have been pulled. To get around
+      // this, the operator must pull both triggers to update the joy node
+      // before the hitch will be activated
+      // TODO: Fix this bug
+      if (joy->axes[5] == 0 || joy->axes[2] == 0) { }
+      // generate and send hitch message if unique
+      else if (hitchMsg.pose.position.z != computeZPosition(joy->axes[5], joy->axes[2]) ||
+               hitchMsg.pose.orientation.y != computeYOrientation(joy->buttons[5], joy->buttons[4])) {
         hitchMsg.pose.position.z = computeZPosition(joy->axes[5], joy->axes[2]);
         hitchMsg.pose.orientation.y = computeYOrientation(joy->buttons[5], joy->buttons[4]);
         hitchMsgPub.publish(hitchMsg);
@@ -190,11 +206,11 @@ float Teleop::computeZPosition(float up_axis, float down_axis) {
      return priorHitchPositionZ;
    } else if (up_axis < 1) {
      // Increment height by 0.1
-     priorHitchPositionZ = priorHitchPositionZ + 0.0005;
+     priorHitchPositionZ = priorHitchPositionZ + 0.001;
      return priorHitchPositionZ;
    } else if (down_axis < 1){
      // Decrement height by 0.1
-     priorHitchPositionZ = priorHitchPositionZ - 0.0005;
+     priorHitchPositionZ = priorHitchPositionZ - 0.001;
      return priorHitchPositionZ;
    } else{
      // If neither are pressed, do nothing