Merge pull request #48 from Ashuh/ds4-controller-rework

Ds4 controller rework

f1tenth_racecar/package.xml

@@ -53,7 +53,7 @@
   <exec_depend>f1tenth_racecar_bringup</exec_depend>
   <exec_depend>f1tenth_msgs</exec_depend>
   <exec_depend>ds4_driver</exec_depend>
-  <exec_depend>ds4_controller</exec_depend>
+  <exec_depend>f1tenth_teleop_ds4</exec_depend>
   <exec_depend>collision_warning_system</exec_depend>
   <exec_depend>drive_controller</exec_depend>
   <exec_depend>drive_mode_selector</exec_depend>

f1tenth_racecar_bringup/config/f1tenth_teleop_ds4.yaml

@@ -0,0 +1,5 @@
+timeout: 0.5
+min_speed: 0.5
+max_speed_crawl: 1
+max_speed_normal: 5
+max_speed_race: 15

f1tenth_racecar_bringup/launch/f1tenth_teleop.launch

@@ -3,7 +3,7 @@
         <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/ds4_driver.yaml"/>
     </node>
 
-    <node name="ds4_controller" pkg="ds4_controller" type="ds4_controller_node" output="screen">
-        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/ds4_controller.yaml"/>
+    <node name="f1tenth_teleop_ds4" pkg="f1tenth_teleop_ds4" type="f1tenth_teleop_ds4_node" output="screen">
+        <rosparam command="load" file="$(find f1tenth_racecar_bringup)/config/f1tenth_teleop_ds4.yaml"/>
     </node>
 </launch>

f1tenth_racecar_bringup/package.xml

@@ -10,7 +10,7 @@
 
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>f1tenth_msgs</exec_depend>
-  <exec_depend>ds4_controller</exec_depend>
+  <exec_depend>f1tenth_teleop_ds4</exec_depend>
   <exec_depend>ds4_driver</exec_depend>
   <exec_depend>path_recorder</exec_depend>
   <exec_depend>lidar_obstacle_detector</exec_depend>

ds4_controller/CMakeLists.txt → f1tenth_teleop_ds4/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.0.2)
-project(ds4_controller)
+project(f1tenth_teleop_ds4)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
@@ -8,10 +8,11 @@ project(ds4_controller)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
-  roscpp
   ackermann_msgs
-  f1tenth_msgs
   ds4_driver
+  f1tenth_msgs
+  f1tenth_utils
+  roscpp
 )
 
 ## System dependencies are found with CMake's conventions
@@ -105,7 +106,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
-#  LIBRARIES ds4_controller
+#  LIBRARIES f1tenth_teleop_ds4
 #  CATKIN_DEPENDS roscpp
 #  DEPENDS system_lib
 )
@@ -123,7 +124,7 @@ include_directories(
 
 ## Declare a C++ library
 # add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/ds4_controller.cpp
+#   src/${PROJECT_NAME}/f1tenth_teleop_ds4.cpp
 # )
 
 ## Add cmake target dependencies of the library
@@ -134,7 +135,10 @@ include_directories(
 ## 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/ds4_controller.cpp)
+add_executable(${PROJECT_NAME}_node 
+  src/f1tenth_teleop_ds4.cpp
+  src/f1tenth_teleop_ds4_node.cpp
+)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -198,7 +202,7 @@ target_link_libraries(${PROJECT_NAME}_node
 #############
 
 ## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_ds4_controller.cpp)
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_f1tenth_teleop_ds4.cpp)
 # if(TARGET ${PROJECT_NAME}-test)
 #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
 # endif()
@@ -209,4 +213,4 @@ target_link_libraries(${PROJECT_NAME}_node
 if (CATKIN_ENABLE_TESTING)
   find_package(roslaunch REQUIRED)
   roslaunch_add_file_check(launch)
-endif()
+endif()

ds4_controller/include/ds4_controller/ds4_controller.h → f1tenth_teleop_ds4/include/f1tenth_teleop_ds4/f1tenth_teleop_ds4.h

@@ -1,38 +1,44 @@
-#ifndef DS4_CONTROLLER_DS4_CONTROLLER_H
-#define DS4_CONTROLLER_DS4_CONTROLLER_H
+#ifndef F1TENTH_TELEOP_DS4_F1TENTH_TELEOP_DS4_H
+#define F1TENTH_TELEOP_DS4_F1TENTH_TELEOP_DS4_H
 
 #include <string>
+#include <vector>
 
 #include <ros/ros.h>
 
 #include "ds4_driver/Status.h"
 
-namespace f1tenth_racecar
-{
-namespace utils
-{
-class DS4Controller
+class TeleopDS4
 {
+public:
+  TeleopDS4();
+
 private:
-  ros::NodeHandle nh_;
+  enum Mode
+  {
+    CRAWL = 0,
+    NORMAL = 1,
+    RACE = 2
+  };
 
+  ros::NodeHandle nh_;
   ros::Timer timer_;
 
   ros::Subscriber status_sub_;
-
   ros::Publisher feedback_pub_;
   ros::Publisher drive_pub_;
   ros::Publisher drive_mode_pub_;
 
-  ros::Time last_message_time_;
-  double timeout_;
-
-  double battery_percentage_;
+  ds4_driver::Status prev_status_;
 
+  double timeout_;
+  std::vector<double> max_speeds_;
+  double min_speed_;
   double max_steering_angle_;
-  double max_speed_;
+  double battery_percentage_;
+  Mode mode_;
 
-  void statusCallback(const ds4_driver::Status status_msg);
+  void statusCallback(const ds4_driver::Status& status_msg);
 
   void publishDriveMsg(const double throttle_axis, const double steering_axis);
 
@@ -44,13 +50,7 @@ class DS4Controller
 
   void batteryToRGB(float& r, float& g, float& b);
 
-  template <typename T>
-  void getParam(const std::string& key, T& result);
-
-public:
-  DS4Controller();
+  bool buttonIsPressed(const ds4_driver::Status& status, int32_t ds4_driver::Status::*button) const;
 };
-}  // namespace utils
-}  // namespace f1tenth_racecar
 
-#endif  // DS4_CONTROLLER_DS4_CONTROLLER_H
+#endif  // F1TENTH_TELEOP_DS4_F1TENTH_TELEOP_DS4_H

f1tenth_teleop_ds4/launch/f1tenth_teleop_ds4.launch

@@ -0,0 +1,17 @@
+<launch>
+  <arg name="timeout" default="0.5"/>
+  <arg name="min_speed" default="0.5"/>
+  <arg name="max_speed_crawl" default="1"/>
+  <arg name="max_speed_normal" default="5"/>
+  <arg name="max_speed_race" default="15"/>
+  <arg name="max_steering_angle" default="0.4"/>
+
+  <node name="f1tenth_teleop_ds4" pkg="f1tenth_teleop_ds4" type="f1tenth_teleop_ds4_node" output="screen">
+    <param name="timeout" value="$(arg timeout)" />
+    <param name="min_speed" value="$(arg min_speed)"/>
+    <param name="max_speed_crawl" value="$(arg max_speed_crawl)"/>
+    <param name="max_speed_normal" value="$(arg max_speed_normal)"/>
+    <param name="max_speed_race" value="$(arg max_speed_race)"/>
+    <param name="max_steering_angle" value="$(arg max_steering_angle)"/>
+  </node>
+</launch>

ds4_controller/package.xml → f1tenth_teleop_ds4/package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>ds4_controller</name>
+  <name>f1tenth_teleop_ds4</name>
   <version>0.0.0</version>
-  <description>The ds4_controller package</description>
+  <description>The f1tenth_teleop_ds4 package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
@@ -19,7 +19,7 @@
   <!-- 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/ds4_controller</url> -->
+  <!-- <url type="website">http://wiki.ros.org/f1tenth_teleop_ds4</url> -->
 
 
   <!-- Author tags are optional, multiple are allowed, one per tag -->
@@ -55,9 +55,9 @@
   <build_export_depend>roscpp</build_export_depend>
   <exec_depend>roscpp</exec_depend>
   <depend>ackermann_msgs</depend>
-  <depend>f1tenth_msgs</depend>
-  <depend>tf2_ros</depend>
   <depend>ds4_driver</depend>
+  <depend>f1tenth_msgs</depend>
+  <depend>f1tenth_utils</depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->