creating test branch

catkin_ws/src/f1tenth_modules/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
+  geometry_msgs
   message_generation
   roslaunch
   tf2
@@ -59,6 +60,7 @@ add_message_files(
   FILES
   PointDist.msg
   PidInfo.msg
+  JoyButtons.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -113,7 +115,7 @@ generate_messages(
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES f1tenth_modules
- CATKIN_DEPENDS roscpp sensor_msgs std_msgs message_runtime
+ CATKIN_DEPENDS roscpp sensor_msgs std_msgs geometry_msgs message_runtime
 #  DEPENDS system_lib
 )
 

catkin_ws/src/f1tenth_modules/include/f1tenth_modules/States.hh

@@ -36,7 +36,7 @@ namespace States {
 
     namespace Autonmous
     {
-        const std::string NAME = "AUTONMOUS";
+        const std::string NAME = "AUTONOMOUS";
         const std::string DRIVE_TOPIC = "/auto_drive";
     } // namespace Autonmous
 

catkin_ws/src/f1tenth_modules/launch/system.launch

@@ -12,13 +12,27 @@
         <param name="coalesce_interval" value="0.025" />
     </node>
 
+    <node pkg="f1tenth_modules" name="JoyStick" type="JoyStick" output="screen"/>
+
+    <!-- RPLidar-->
+    <node pkg="rplidar_ros" type="rplidarNode" name="rplidar_node" />
+
+    <!-- VESC-->
+    <arg name="vesc_config" default="$(find f1tenth_hardware)/config/vesc.yaml" />
+    <rosparam file="$(arg vesc_config)" command="load" />
+
+    <node pkg="vesc_ackermann" type="ackermann_to_vesc_node" name="ackermann_to_vesc" >
+      <remap from="ackermann_cmd" to="vesc_cmd" />
+    </node>
+
+    <node pkg="vesc_driver" type="vesc_driver_node" name="vesc_driver" />
+
     <!-- MUX-->
     <node pkg="f1tenth_modules" name="MuxNode" type="MuxNode" output="screen">
         <!-- ??rosparams??-->
     </node>
 
-    <node pkg="f1tenth_modules" name="JoyStick" type="JoyStick" output="screen"/>
-
+
     <!-- Wallfollowing -->
     <arg name="wallfollowing" default="0"/>
     <node pkg="f1tenth_modules" name="WallFollowing" type="WallFollowing" output="screen" if="$(eval arg('wallfollowing') == 1)">

catkin_ws/src/f1tenth_modules/launch/tests/mux_test.launch

@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- Custom default arguments used to launch specific nodes-->
+    <arg name="use_simulator" default="0"/>
+    <arg name="log_waypoints" default="0"/>
+    <arg name="wallfollowing" default="0"/>
+    <arg name="gapfollowing" default="0"/>
+
+    <node pkg="f1tenth_modules" name="KeyInput" type="KeyInput" output="screen"/>
+
+    <!-- Launch a map from the maps folder-->
+    <arg name="map" default="$(find f1tenth_simulator)/maps/levine.yaml" if="$(eval arg('use_simulator') == 1)"/>
+    <node pkg="map_server" name="map_server" type="map_server" args="$(arg map)"/>
+
+    <!-- Launch the racecar model -->
+    <include file="$(find f1tenth_simulator)/launch/racecar_model.launch" if="$(eval arg('use_simulator') == 1)"/>
+
+    <!-- Begin the simulator with the parameters from params.yaml -->
+    <node pkg="f1tenth_simulator" name="f1tenth_simulator" type="simulator" output="screen" if="$(eval arg('use_simulator') == 1)">
+        <rosparam command="load" file="$(find f1tenth_simulator)/params.yaml"/>
+    </node>
+
+    <!-- Joystick-->
+    <arg name="dev" default="/dev/input/js0" />
+    <node pkg="joy" name="joy" type="joy_node">
+        <param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->
+        <param name="deadzone" value="0.2" />
+        <param name="autorepeat_rate" value="40" />
+        <param name="coalesce_interval" value="0.025" />
+    </node>
+    <node pkg="f1tenth_modules" name="JoyStick" type="JoyStick" output="screen"/>
+
+    <!-- Launch Mux Node -->
+    <node pkg="f1tenth_modules" name="MuxNode" type="MuxNode" output="screen">
+        <param name="useSimulator" type="bool" value="true" if="$(eval arg('use_simulator') == 1)"/>
+    </node>
+
+    <!-- Wall Following -->
+    <node pkg="f1tenth_modules" name="WallFollowing" type="WallFollowing" output="screen" if="$(eval arg('wallfollowing') == 1)">
+        <rosparam command="load" file="$(find f1tenth_modules)/params.yaml"/>
+    </node>
+
+    <!-- GapFollowing -->
+    <node pkg="f1tenth_modules" name="GapFollowing" type="GapFollowing" output="screen" if="$(eval arg('gapfollowing') == 1)">
+        <rosparam command="load" file="$(find f1tenth_modules)/params.yaml"/>
+    </node>
+
+    <!-- Launch RVIZ -->
+    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find f1tenth_simulator)/launch/simulator.rviz" output="screen" if="$(eval arg('use_simulator') == 1)"/>
+
+    <!-- Launch waypoint logger-->
+    <node pkg="f1tenth_modules" name="waypoint_logger" type="waypoint_logger.py" output="screen" if="$(eval arg('log_waypoints') == 1)"/>
+
+</launch>

catkin_ws/src/f1tenth_modules/msg/JoyButtons.msg

@@ -0,0 +1,4 @@
+bool a
+bool b
+bool x
+bool y

catkin_ws/src/f1tenth_modules/node/GapFollowing.cc

@@ -39,7 +39,7 @@ class GapFollowing
         double dispThreshold, dispBufferAngle;
         int muxIdx;
         int scanStartIdx, scanEndIdx;
-        
+
         bool enabled;
         bool use_simulator;
 
@@ -59,7 +59,7 @@ class GapFollowing
             exit(-1);
 
         n.getParam("gap_follow_idx", muxIdx);
-        n.getParam("gap_follow_topic", driveTopic);
+        n.getParam("autonomous_drive_topic", driveTopic);
         n.getParam("rb", rb);
         n.getParam("disparity_threshold", dispThreshold);
         n.getParam("disparity_buffer_angle", dispBufferAngle);
@@ -75,7 +75,7 @@ class GapFollowing
             muxSub = n.subscribe("/mux", 1, &GapFollowing::mux_cb, this);
             ROS_INFO("(GAP FOLLOWING): not ussing simulator.");
         }
-        else 
+        else
         {
             muxSub = n.subscribe("/input", 1, &GapFollowing::key_input, this);
             ROS_INFO("(GAP FOLLOWING): not using simulator.");
@@ -84,10 +84,10 @@ class GapFollowing
         scanStartIdx = getScanIdx((-M_PI/3.0), lidarData);
         scanEndIdx = getScanIdx((M_PI/3.0), lidarData);
 
-        ROS_INFO("");
-        ROS_INFO("Start index of scan : %d", scanStartIdx);
-        ROS_INFO("End index of scan: %d", scanEndIdx);
-        ROS_INFO("");
+        // ROS_INFO("");
+        // ROS_INFO("Start index of scan : %d", scanStartIdx);
+        // ROS_INFO("End index of scan: %d", scanEndIdx);
+        // ROS_INFO("");
 
         // Rviz configuration
         geometry_msgs::Pose pose;
@@ -96,7 +96,9 @@ class GapFollowing
         pose.orientation.w = 1.0;
         scale.x = scale.y = 0.1;
 
-        rvizOpts opts = {
+        if (use_simulator)
+        {
+            rvizOpts opts = {
                             .color=0xff0000,
                             .frame_id="laser",
                             .ns="point",
@@ -105,19 +107,20 @@ class GapFollowing
                             .topic="/dynamic_viz"
                         };
 
-        // These points will be green
-        opts.color=0x00ff00;
-        cp = std::make_unique<RvizPoint>(n, opts);
-        bufferPoints = std::make_unique<RvizLineList>(n, opts);
+            // These points will be green
+            opts.color=0x00ff00;
+            cp = std::make_unique<RvizPoint>(n, opts);
+            bufferPoints = std::make_unique<RvizLineList>(n, opts);
 
-        opts.color=0xff0000;
-        fp = std::make_unique<RvizLine>(n, opts);
-        bubble = std::make_unique<RvizPoint>(n, opts);
+            opts.color=0xff0000;
+            fp = std::make_unique<RvizLine>(n, opts);
+            bubble = std::make_unique<RvizPoint>(n, opts);
 
-        cp->addTransformPair("base_link", "laser");
-        fp->addTransformPair("base_link", "laser");
-        bubble->addTransformPair("base_link", "laser");
-        bufferPoints->addTransformPair("base_link", "laser");
+            cp->addTransformPair("base_link", "laser");
+            fp->addTransformPair("base_link", "laser");
+            bubble->addTransformPair("base_link", "laser");
+            bufferPoints->addTransformPair("base_link", "laser");
+        }
     }
 
     void mux_cb(const std_msgs::Int32MultiArray &msg)
@@ -128,7 +131,10 @@ class GapFollowing
     void key_input(const std_msgs::UInt8 &msg)
     {
         if (msg.data == States::GapFollowing::INPUT_CHAR)
+        {
+            ROS_INFO("Gap Following enabled");
             enabled = true;
+        }
         else
             enabled = false;
     }
@@ -172,10 +178,13 @@ class GapFollowing
             };
 
         // Rviz
-        closestPoint.p.x = closestPoint.dist*std::cos(closestPoint.angle);
-        closestPoint.p.y = closestPoint.dist*std::sin(closestPoint.angle);
-        closestPoint.p.z = 0.0;
-        cp->addTranslation(closestPoint.p);
+        if (use_simulator)
+        {
+            closestPoint.p.x = closestPoint.dist*std::cos(closestPoint.angle);
+            closestPoint.p.y = closestPoint.dist*std::sin(closestPoint.angle);
+            closestPoint.p.z = 0.0;
+            cp->addTranslation(closestPoint.p);
+        }
 
         // calculate start and end range of the bubble within the scan
         if (closestPoint.dist < rb)
@@ -298,7 +307,8 @@ class GapFollowing
             }
         }
 
-        bufferPoints->addTranslation(bp);
+        if (use_simulator)
+            bufferPoints->addTranslation(bp);
         //////////////
 
         // Find the largest point away from us within the max sequence
@@ -324,9 +334,12 @@ class GapFollowing
         };
 
         // Rviz
-        furthestPoint.p.x = furthestPoint.dist*std::cos(furthestPoint.angle);
-        furthestPoint.p.y = furthestPoint.dist*std::sin(furthestPoint.angle);
-        fp->addTranslation(furthestPoint.p);
+        if (use_simulator)
+        {
+            furthestPoint.p.x = furthestPoint.dist*std::cos(furthestPoint.angle);
+            furthestPoint.p.y = furthestPoint.dist*std::sin(furthestPoint.angle);
+            fp->addTranslation(furthestPoint.p);
+        }
 
         // Set the steering angle to the farthest point
         // (TODO) Set this up to be a helper function with custom structs
@@ -337,7 +350,10 @@ class GapFollowing
         drive.drive.speed = 1.5;
 
         if (enabled)
+        {
+            ROS_INFO("Publishing drive message");
             drivePub.publish(drive);
+        }
     }
 };
 

catkin_ws/src/f1tenth_modules/node/JoyStick.cc

@@ -1,6 +1,7 @@
 #include <ros/ros.h>
 #include <ackermann_msgs/AckermannDriveStamped.h>
 #include <sensor_msgs/Joy.h>
+#include <f1tenth_modules/JoyButtons.h>
 
 /**
  * @brief Class to subscribe and convert joystick commands
@@ -13,21 +14,26 @@ class JoyStick
     ros::NodeHandle n;
     ros::Subscriber joySub;
     ros::Publisher joyPub;
+    ros::Publisher buttonPub;
 
     ackermann_msgs::AckermannDriveStamped drive;
     std::string id {"joy_node"};
-    std::string pubTopic {"/manual"};
+    std::string pubTopic {"/manual_drive"};
+    std::string buttonTopic {"/joy_buttons"};
     std::string subTopic {"/joy"};
 
     float maxSpeed;
     float maxSteeringAngle;
 
+    f1tenth_modules::JoyButtons buttons;
+
 public:
     JoyStick() :
         n(ros::NodeHandle("~"))
     {
         // Pub-Sub initialization
         joyPub = n.advertise<ackermann_msgs::AckermannDriveStamped>(pubTopic, 1);
+        buttonPub = n.advertise<f1tenth_modules::JoyButtons>(buttonTopic, 1);
         joySub = n.subscribe(subTopic, 1, &JoyStick::joy_callback, this);
 
         // Creating an initial, full brake drive message
@@ -41,14 +47,34 @@ class JoyStick
         // TODO: rosparam these
         maxSpeed = 7.0;
         maxSteeringAngle = 0.4189;
+
+        // Set button states
+        buttons.a = false;
+        buttons.b = false;
+        buttons.x = false;
+        buttons.y = false;
+
+        buttonPub.publish(buttons);
     }
 
     void joy_callback(const sensor_msgs::Joy &msg)
     {
+        // Publish any changes that we get from the buttons
+        if (msg.buttons[0] != buttons.x || msg.buttons[1] != buttons.a
+            || msg.buttons[2] != buttons.b || msg.buttons[3] != buttons.y)
+        {
+            buttons.x = static_cast<bool>(msg.buttons[0]);
+            buttons.a = static_cast<bool>(msg.buttons[1]);
+            buttons.b = static_cast<bool>(msg.buttons[2]);
+            buttons.y = static_cast<bool>(msg.buttons[3]);
+
+            buttonPub.publish(buttons);
+        }
+
         // Speed
         auto x = msg.axes[1];
         // Steering
-        auto y = msg.axes[3];
+        auto y = msg.axes[2];
 
         drive.header.frame_id = id;
         drive.header.stamp = ros::Time::now();

catkin_ws/src/f1tenth_modules/node/MuxNode.cc

@@ -19,7 +19,8 @@ class Mux
     ros::Subscriber muxController;
     ros::Subscriber eBrake;
 
-    std::string currState;
+    std::string currState, masterDriveTopic;
+    bool useSimulator;
 
     // main drive topic publisher
     void muxIn_cb(const ackermann_msgs::AckermannDriveStamped &msg)
@@ -31,6 +32,7 @@ class Mux
     {
         // For now, need to manually add a case statement
         // for every new state added to the system.
+        ROS_INFO("Current State: %s", currState.c_str());
         switch(msg.data)
         {
             case States::Off::INPUT_CHAR:
@@ -104,11 +106,21 @@ class Mux
 
         // Subcribers
         muxController = n.subscribe("/input", 1, &Mux::switch_cb, this);
-        muxIn = n.subscribe("", 1, &Mux::muxIn_cb, this); 
+        muxIn = n.subscribe("", 1, &Mux::muxIn_cb, this);
         eBrake = n.subscribe("/brake_bool", 1, &Mux::brake_cb, this);
 
+        n.param("useSimulator", useSimulator, false);
+
+        if (useSimulator)
+        {
+            masterDriveTopic = "/drive";
+            ROS_INFO("MUX: Using simulator drive topic /drive");
+        }
+        else
+            masterDriveTopic = "vesc_cmd";
+
         // Publishers
-        muxOut = n.advertise<ackermann_msgs::AckermannDriveStamped>("/vesc_cmd", 1);
+        muxOut = n.advertise<ackermann_msgs::AckermannDriveStamped>(masterDriveTopic, 1);
     }
 
     void brake()
@@ -118,7 +130,7 @@ class Mux
         drive.header.frame_id = States::Off::NAME;
 
         drive.drive.speed = 0.0;
-        
+
         auto i = 0;
         while (i++<50)
         {