diff --git a/Dockerfile b/Dockerfile
new file mode 100644
index 0000000..ec2d49c
--- /dev/null
+++ b/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 aphung@olin.edu
+
+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
diff --git a/README.md b/README.md
index 6c8e465..7469bd6 100644
--- a/README.md
+++ b/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
diff --git a/launch/mainstate.launch b/launch/mainstate.launch
index 9d0210c..d4e03b0 100644
--- a/launch/mainstate.launch
+++ b/launch/mainstate.launch
@@ -16,11 +16,11 @@ add any nodes here that you need to interface with twists
-->
-
-
+
+
-
-
-
+
+
+
diff --git a/package.xml b/package.xml
index 35a492d..2400723 100644
--- a/package.xml
+++ b/package.xml
@@ -36,10 +36,12 @@
roscpp
std_msgs
message_generation
+ joy
geometry_msgs
roscpp
std_msgs
message_generation
+ joy
geometry_msgs
roscpp
std_msgs
diff --git a/src/Teleop/Teleop.cpp b/src/Teleop/Teleop.cpp
index ce15504..cf9c5e8 100644
--- a/src/Teleop/Teleop.cpp
+++ b/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