From 8c01785641e77ca3cce97b6948d4236672451b7e Mon Sep 17 00:00:00 2001 From: Amy Phung Date: Tue, 26 May 2020 16:29:13 -0700 Subject: [PATCH 1/4] Add joy as a build dependency, create Dockerfile --- Dockerfile | 34 ++++++++++++++++++++++++++++++++++ package.xml | 2 ++ 2 files changed, 36 insertions(+) create mode 100644 Dockerfile 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/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 From 4f2f62a73f405551b393a089a8faf6e042dfbdb9 Mon Sep 17 00:00:00 2001 From: Amy Phung Date: Wed, 27 May 2020 08:56:19 -0700 Subject: [PATCH 2/4] Add patch for faulty hitch commands on startup --- src/Teleop/Teleop.cpp | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/Teleop/Teleop.cpp b/src/Teleop/Teleop.cpp index ce15504..026cf26 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); From 656afb3b0902d422154976b5d15ab5fac65e4dc2 Mon Sep 17 00:00:00 2001 From: Amy Phung Date: Wed, 27 May 2020 09:06:45 -0700 Subject: [PATCH 3/4] Update README.md --- README.md | 26 ++++++++++++++++++++++++++ launch/mainstate.launch | 10 +++++----- 2 files changed, 31 insertions(+), 5 deletions(-) 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 --> - - + + - - - + + + From 13d7aa51a55a145f51d9003ff8b5bbdcc62b53ba Mon Sep 17 00:00:00 2001 From: Amy Phung Date: Wed, 27 May 2020 11:57:57 -0700 Subject: [PATCH 4/4] Change hitch movement rate --- src/Teleop/Teleop.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Teleop/Teleop.cpp b/src/Teleop/Teleop.cpp index 026cf26..cf9c5e8 100644 --- a/src/Teleop/Teleop.cpp +++ b/src/Teleop/Teleop.cpp @@ -206,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