WIP ROS1 / ROS2 refactorization

Author: joydeep-b

CMakeLists.txt

@@ -1,15 +1,22 @@
 PROJECT(ut_automata)
-CMAKE_MINIMUM_REQUIRED(VERSION 3.1.0)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.6)
+
+if(DEFINED ENV{ROS_VERSION})
+  set(ROS_VERSION $ENV{ROS_VERSION})
+else()
+  message(FATAL_ERROR "ROS_VERSION is not defined")
+endif()
 
 MESSAGE(STATUS "Compilers found: ${CMAKE_CXX_COMPILER_LIST}")
 MESSAGE(STATUS "Using compiler: ${CMAKE_CXX_COMPILER}")
 MESSAGE(STATUS "Build Type: ${CMAKE_BUILD_TYPE}")
 MESSAGE(STATUS "Build Mode: ${CMAKE_BUILD_MODE}")
 MESSAGE(STATUS "Arch: ${CMAKE_SYSTEM_PROCESSOR}")
+MESSAGE(STATUS "ROS_VERSION: ${ROS_VERSION}")
 
-SET(CMAKE_AUTOMOC ON)
-SET(CMAKE_AUTORCC ON)
-SET(CMAKE_AUTOUIC ON)
+# SET(CMAKE_AUTOMOC ON)
+# SET(CMAKE_AUTORCC ON)
+# SET(CMAKE_AUTOUIC ON)
 
 IF(CMAKE_VERSION VERSION_LESS "3.7.0")
     SET(CMAKE_INCLUDE_CURRENT_DIR ON)
@@ -23,20 +30,37 @@ IF(${CMAKE_BUILD_TYPE} MATCHES "Release")
 ELSEIF(${CMAKE_BUILD_TYPE} MATCHES "Debug")
   MESSAGE(STATUS "Additional Flags for Debug mode")
   SET(CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -g")
-  SET(BUILD_SPECIFIC_LIBRARIES "")
 ENDIF()
 
-INCLUDE($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-ROSBUILD_INIT()
-SET(ROS_BUILD_STATIC_LIBS true)
-SET(ROS_BUILD_SHARED_LIBS false)
+if(${ROS_VERSION} EQUAL "1")
+  INCLUDE($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+  ROSBUILD_INIT()
+  SET(ROS_BUILD_STATIC_LIBS true)
+  SET(ROS_BUILD_SHARED_LIBS false)
+  ROSBUILD_GENMSG()
+  SET(libs roslib roscpp glog gflags amrl_shared_lib rosbag X11 lua5.1 boost_system)
+elseif(${ROS_VERSION} EQUAL "2")
+  find_package(ament_cmake REQUIRED)
+  find_package(geometry_msgs REQUIRED)
+  find_package(nav_msgs REQUIRED)
+  find_package(std_msgs REQUIRED)
+  find_package(rclcpp REQUIRED)
+  find_package(amrl_msgs REQUIRED)
+  find_package(rosidl_default_generators REQUIRED)
+  rosidl_generate_interfaces(${PROJECT_NAME}
+    "msg/CarStatusMsg.msg"
+    "msg/VescState.msg"
+    "msg/VescStateStamped.msg"
+    DEPENDENCIES std_msgs
+  )
+  SET(libs glog gflags amrl_shared_lib X11 lua5.1)
+endif()
 
 FIND_PACKAGE(Qt5 COMPONENTS Core Widgets Gui WebSockets OpenGL REQUIRED)
 SET(CMAKE_INCLUDE_CURRENT_DIR ON)
 
 MESSAGE(STATUS "ROS-Overrride Build Type: ${CMAKE_BUILD_TYPE}")
 MESSAGE(STATUS "CXX Flags: ${CMAKE_CXX_FLAGS}")
-MESSAGE(STATUS "Build-Specific Libraries: ${BUILD_SPECIFIC_LIBRARIES}")
 
 SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
 SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
@@ -48,16 +72,10 @@ INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src)
 INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/include)
 INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/submodules/config_reader/include)
 
-ROSBUILD_GENMSG()
-
 ADD_SUBDIRECTORY(src/shared)
 INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src/shared)
 
-SET(libs roslib roscpp glog gflags amrl_shared_lib
-    ${BUILD_SPECIFIC_LIBRARIES} rosbag X11 lua5.1 boost_system)
-
-IF(${CMAKE_BUILD_MODE} MATCHES "Hardware")
-  MESSAGE(STATUS "Building hardware drivers")
+if(${ROS_VERSION} EQUAL "1")
   rosbuild_add_executable(vesc_driver
                           src/vesc_driver/serial.cc
                           src/vesc_driver/vesc_driver_node.cpp
@@ -83,29 +101,29 @@ IF(${CMAKE_BUILD_MODE} MATCHES "Hardware")
     src/joystick/joystick.cc
     src/joystick/joystick_driver.cc)
   TARGET_LINK_LIBRARIES(joystick ${libs})
-ENDIF()
 
-MESSAGE(STATUS "Building simulator")
-
-ROSBUILD_ADD_EXECUTABLE(simulator
-    src/simulator/simulator.cc
-    src/simulator/vector_map.cc
-    src/simulator/simulator_main.cc)
-TARGET_LINK_LIBRARIES(simulator ${libs})
-
-ROSBUILD_ADD_EXECUTABLE(step_simulator
-    src/simulator/simulator.cc
-    src/simulator/vector_map.cc
-    src/simulator/step_simulator_main.cc)
-TARGET_LINK_LIBRARIES(step_simulator ${libs})
-
-ROSBUILD_ADD_EXECUTABLE(websocket
-    src/websocket/websocket_main.cc
-    src/websocket/websocket.cc
-)
-TARGET_LINK_LIBRARIES(websocket
-    Qt5::Core
-    Qt5::Gui
-    Qt5::Widgets
-    Qt5::WebSockets
-    ${libs})
+  ROSBUILD_ADD_EXECUTABLE(simulator
+      src/simulator/simulator.cc
+      src/simulator/vector_map.cc
+      src/simulator/simulator_main.cc)
+  TARGET_LINK_LIBRARIES(simulator ${libs})
+
+  ROSBUILD_ADD_EXECUTABLE(step_simulator
+      src/simulator/simulator.cc
+      src/simulator/vector_map.cc
+      src/simulator/step_simulator_main.cc)
+  TARGET_LINK_LIBRARIES(step_simulator ${libs})
+
+  ROSBUILD_ADD_EXECUTABLE(websocket
+      src/websocket/websocket_main_ros1.cc
+      src/websocket/websocket.cc
+  )
+  TARGET_LINK_LIBRARIES(websocket
+      Qt5::Core
+      Qt5::Gui
+      Qt5::Widgets
+      Qt5::WebSockets
+      ${libs})
+elseif(${ROS_VERSION} EQUAL "2")
+  ament_package()
+endif()

config/simulator.lua

@@ -11,7 +11,8 @@ function DegToRad(d)
 end
 
 -- map_name =  "GDC1";
-map_name =  "ICRA2022_F1Tenth_Track";
+map_name =  "GDC3";
+-- map_name =  "ICRA2022_F1Tenth_Track";
 
 -- Simulator starting location.
 start_x = 1.461

manifest.xml

@@ -1,5 +1,5 @@
 <package>
-  <description brief="ut_automata">UT Automata Course Infrastructure</description>
+  <description brief="ut_automata">UT Automata Infrastructure</description>
   <author>Maintained by Joydeep Biswas</author>
   <license>LGPLv3.0</license>
   <review status="unreviewed" notes=""/>

msg/VescStateStamped.msg

@@ -1,4 +1,4 @@
 # Timestamped VESC open source motor controller state (telemetry)
 
-Header  header
+std_msgs/Header  header
 VescState state

package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ut_automata</name>
+  <version>1.0.0</version>
+  <description>UT Automata Infrastructure</description>
+  <maintainer email="joydeepb@cs.utexas.edu">joydeepb</maintainer>
+  <license>LGPLv3.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

scripts/keyboard_teleop.py

@@ -3,6 +3,7 @@
 roslib.load_manifest('ut_automata')
 import rospy
 from amrl_msgs.msg import AckermannCurvatureDriveMsg
+from sensor_msgs.msg import Joy
 
 import sys, select, termios, tty
 
@@ -14,6 +15,17 @@
 CTRL-C to quit
 """
 
+joystick_msg = Joy()
+joystick_msg.header.seq = 0
+joystick_msg.header.frame_id = "base_link"
+joystick_msg.axes = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+joystick_msg.buttons = [0, 0, 0, 0, 0, 0, 0, 0]
+kThrottleAxis = 4
+kSteeringAxis = 0
+kManualButton = 4
+kAutoButton = 5
+kPersistentAutoButton = 7
+
 keyBindings = {
   'w':(1,0),
   'd':(1,-1),
@@ -40,6 +52,7 @@ def vels(speed,turn):
   pub = rospy.Publisher('ackermann_curvature_drive',
                         AckermannCurvatureDriveMsg,
                         queue_size=5)
+  joypub = rospy.Publisher('joystick', Joy, queue_size=5)
   rospy.init_node('keyop')
 
   x = 0
@@ -48,23 +61,36 @@ def vels(speed,turn):
 
   try:
     while(1):
+       joystick_msg.axes = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+       joystick_msg.buttons = [0, 0, 0, 0, 0, 0, 0, 0]
        key = getKey()
        if key in keyBindings.keys():
           x = keyBindings[key][0]
           th = keyBindings[key][1]
+          joystick_msg.buttons[kManualButton] = 1
        else:
           x = 0
           th = 0
           if (key == '\x03'):
              break
+       if key == ' ':
+          print('Persistent auto')
+          joystick_msg.buttons[kPersistentAutoButton] = 1
+       elif key == '.':
+          print('Auto')
+          joystick_msg.buttons[kAutoButton] = 1
+
        msg = AckermannCurvatureDriveMsg()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = "base_link"
-
        msg.velocity = x*speed
        msg.curvature = th*turn
 
-       pub.publish(msg)
+       joystick_msg.axes[kThrottleAxis] = x
+       joystick_msg.axes[kSteeringAxis] = th
+       joystick_msg.header.stamp = rospy.Time.now()
+       joypub.publish(joystick_msg)
+      #  pub.publish(msg)
 
   except:
     print('error')

src/websocket/websocket.cc

@@ -1,4 +1,4 @@
-//========================================================================
+  //========================================================================
 //  This software is free: you can redistribute it and/or modify
 //  it under the terms of the GNU Lesser General Public License Version 3,
 //  as published by the Free Software Foundation.
@@ -186,78 +186,6 @@ QByteArray DataMessage::ToByteArray() const {
   return data;
 }
 
-DataMessage DataMessage::FromRosMessages(
-      const LaserScan& laser_msg,
-      const VisualizationMsg& local_msg,
-      const VisualizationMsg& global_msg,
-      const Localization2DMsg& localization_msg) {
-  static const bool kDebug = false;
-  DataMessage msg;
-  for (size_t i = 0; i < sizeof(msg.header.map); ++i) {
-    msg.header.map[i] = 0;
-  }
-  msg.header.loc_x = localization_msg.pose.x;
-  msg.header.loc_y = localization_msg.pose.y;
-  msg.header.loc_r = localization_msg.pose.theta;
-  strncpy(msg.header.map,
-          localization_msg.map.data(),
-          std::min(sizeof(msg.header.map) - 1, localization_msg.map.size()));
-  msg.header.laser_min_angle = laser_msg.angle_min;
-  msg.header.laser_max_angle = laser_msg.angle_max;
-  msg.header.num_laser_rays = laser_msg.ranges.size();
-  msg.laser_scan.resize(laser_msg.ranges.size());
-  for (size_t i = 0; i < laser_msg.ranges.size(); ++i) {
-    if (laser_msg.ranges[i] <= laser_msg.range_min || 
-        laser_msg.ranges[i] >= laser_msg.range_max) {
-      msg.laser_scan[i] = 0;
-    } else {
-      msg.laser_scan[i] = static_cast<uint32_t>(laser_msg.ranges[i] * 1000.0);
-    }
-  }
-
-  msg.points = local_msg.points;
-  msg.header.num_local_points = local_msg.points.size();
-  msg.points.insert(msg.points.end(),
-                    global_msg.points.begin(),
-                    global_msg.points.end());
-
-  msg.lines = local_msg.lines;
-  msg.header.num_local_lines = local_msg.lines.size();
-  msg.lines.insert(msg.lines.end(),
-                   global_msg.lines.begin(),
-                   global_msg.lines.end());
-
-  msg.arcs = local_msg.arcs;
-  msg.header.num_local_arcs = local_msg.arcs.size();
-  msg.arcs.insert(msg.arcs.end(),
-                  global_msg.arcs.begin(),
-                  global_msg.arcs.end());
-
-  msg.header.num_points = msg.points.size();
-  msg.header.num_lines = msg.lines.size();
-  msg.header.num_arcs = msg.arcs.size();
-
-  if (kDebug) {
-    printf("nonce: %d "
-           "num_points: %d "
-           "num_lines: %d "
-           "num_arcs: %d "
-           "num_laser_rays: %d "
-           "num_local_points: %d "
-           "num_local_lines: %d "
-           "num_local_arcs: %d\n",
-           msg.header.nonce,
-           msg.header.num_points,
-           msg.header.num_lines,
-           msg.header.num_arcs,
-           msg.header.num_laser_rays,
-           msg.header.num_local_points,
-           msg.header.num_local_lines,
-           msg.header.num_local_arcs);
-  }
-  return msg;
-}
-
 void RobotWebSocket::SendError(const QString& error_val) {
   for (auto c: clients_) {
     CHECK_NOTNULL(c);