Object viz working

CMakeLists.txt

@@ -16,15 +16,48 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 # find dependencies
-find_package(ament_cmake REQUIRED)
+if ($ENV{ROS_VERSION} EQUAL 1)
+  find_package(catkin REQUIRED
+    COMPONENTS
+      roscpp
+      message_generation
+  )
+else()
+  find_package(ament_cmake REQUIRED)
+  find_package(rclcpp REQUIRED)
+endif()
+
 find_package(socket_can REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
 
 include_directories(include)
 include_directories(${socket_can_INCLUDE_DIRS})
 
+if ($ENV{ROS_VERSION} EQUAL 1)
+  add_message_files(DIRECTORY msg
+    FILES
+    Object.msg
+    ObjectList.msg
+  )
+
+  generate_messages(DEPENDENCIES std_msgs geometry_msgs)
+
+  catkin_package(
+    INCLUDE_DIRS
+      include
+    LIBRARIES
+      ${PROJECT_NAME}_can
+      ${PROJECT_NAME}_msg
+    CATKIN_DEPENDS
+      message_runtime
+      roscpp
+      socket_can
+  )
+  include_directories(${catkin_INCLUDE_DIRS})
+endif()
+
 add_library(continental_radar_msg
   src/radar_state.cpp
   src/radar_cfg.cpp
@@ -33,32 +66,37 @@ add_library(continental_radar_msg
   src/object_list.cpp
 )
 
-add_executable(continental_radar_can
+add_library(continental_radar_can
   src/continental_radar_can.cpp
 )
 
-target_link_libraries(continental_radar_can continental_radar_msg)
+target_link_libraries(continental_radar_can continental_radar_msg ${socket_can_LIBRARIES})
 
-target_link_libraries(continental_radar_can ${socket_can_LIBRARIES})
+add_executable(continental_radar_ros
+  src/continental_radar_ros.cpp
+)
 
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
+add_executable(continental_radar_rviz
+  src/radar_viz.cpp
+)
+target_link_libraries(continental_radar_rviz ${catkin_LIBRARIES})
+
+if ($ENV{ROS_VERSION} EQUAL 1)
+  add_dependencies(continental_radar_ros ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+  target_link_libraries(continental_radar_ros continental_radar_can ${catkin_LIBRARIES})
 endif()
 
 install(
   TARGETS
     continental_radar_can
     continental_radar_msg
+    continental_radar_ros
+    continental_radar_rviz
   ARCHIVE DESTINATION lib/${PROJECT_NAME}
   LIBRARY DESTINATION lib/${PROJECT_NAME}
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
-ament_package()
+if ($ENV{ROS_VERSION} EQUAL 2)
+  ament_package()
+endif()

include/continental_radar/continental_radar_can.hpp

@@ -12,36 +12,63 @@
 #include "continental_radar/cluster_list.hpp"
 #include "continental_radar/object_list.hpp"
 
-#include <stdint.h>
+#include <cstdint>
+#include <string>
 
 namespace continental_radar
 {
 class ContinentalRadarCAN {
  public:
   ContinentalRadarCAN();
 
+  ContinentalRadarCAN(std::string port);
+
   ~ContinentalRadarCAN();
 
   virtual bool receive_radar_data();
 
-  virtual bool send_radar_data();
+  virtual bool send_radar_data(uint32_t frame_id);
+
+  radar_state::RadarState * get_radar_state();
+
+  radar_cfg::RadarCfg * get_radar_cfg();
+
+  cluster_list::Cluster_0_Status * get_cluster_0_status();
+
+  cluster_list::Cluster_1_General * get_cluster_1_general();
+
+  cluster_list::Cluster_2_Quality * get_cluster_2_quality();
+
+  object_list::Object_0_Status * get_object_0_status();
+
+  object_list::Object_1_General * get_object_1_general();
+
+  object_list::Object_3_Extended * get_object_3_extended();
+
+  virtual void send_object_0_status() {};
+
+  virtual void send_object_1_general() {};
+
+  virtual void send_object_3_extended() {};
 
  private:
-  socket_can::SocketCAN can;
+  socket_can::SocketCAN can_;
+
+  radar_state::RadarState radar_state_;
 
-  radar_state::RadarState radar_state;
+  radar_cfg::RadarCfg radar_cfg_;
 
-  radar_cfg::RadarCfg radar_cfg;
+  cluster_list::Cluster_0_Status cluster_0_status_;
 
-  cluster_list::Cluster_0_Status cluster_0_status;
+  cluster_list::Cluster_1_General cluster_1_general_;
 
-  cluster_list::Cluster_1_General cluster_1_general;
+  cluster_list::Cluster_2_Quality cluster_2_quality_;
 
-  object_list::Object_0_Status object_0_status;
+  object_list::Object_0_Status object_0_status_;
 
-  object_list::Object_1_General object_1_general;
+  object_list::Object_1_General object_1_general_;
 
-  object_list::Object_3_Extended object_3_extended;
+  object_list::Object_3_Extended object_3_extended_;
 };
 }
 

include/continental_radar/continental_radar_ros.hpp

@@ -0,0 +1,46 @@
+//
+// Created by shivesh on 9/13/19.
+//
+
+#ifndef CONTINENTAL_RADAR_CONTINENTAL_RADAR_ROS_HPP
+#define CONTINENTAL_RADAR_CONTINENTAL_RADAR_ROS_HPP
+
+#include <ros/ros.h>
+#include <thread>
+
+#include "continental_radar/Object.h"
+#include "continental_radar/ObjectList.h"
+#include "continental_radar/continental_radar_can.hpp"
+
+namespace continental_radar
+{
+class ContinentalRadarROS : public ContinentalRadarCAN {
+ public:
+  ContinentalRadarROS();
+
+  ~ContinentalRadarROS();
+
+  void receive_data();
+
+  void run();
+
+  void send_object_0_status() override;
+
+  void send_object_1_general() override;
+
+  void send_object_3_extended() override;
+
+ private:
+  ros::NodeHandle nh_;
+
+  ros::Publisher objects_data_pub_;
+
+  ObjectList object_list;
+
+  int object_id_;
+
+  std::thread receive_data_thread;
+};
+}
+
+#endif //CONTINENTAL_RADAR_CONTINENTAL_RADAR_ROS_HPP

msg/Object.msg

@@ -0,0 +1,11 @@
+uint8 id
+
+geometry_msgs/Point position
+geometry_msgs/Vector3 relative_velocity
+
+float32 length
+float32 width
+float32 orientation_angle
+
+int8 dynamic_property
+int8 class_type

msg/ObjectList.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+
+continental_radar/Object[] objects

package.xml

@@ -7,14 +7,23 @@
   <maintainer email="shiveshkhaitan@gmail.com">shiveshkhaitan</maintainer>
   <license>TODO: License declaration</license>
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend condition="$ROS_VERSION==1">catkin</buildtool_depend>
+  <buildtool_depend condition="$ROS_VERSION==2">ament_cmake</buildtool_depend>
 
   <depend>socket_can</depend>
+  <depend>visualization_msgs</depend>
+  <depend condition="$ROS_VERSION == 1">roscpp</depend>
+  <depend condition="$ROS_VERSION == 2">rclcpp</depend>
 
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
+  <build_depend condition="$ROS_VERSION == 1">message_generation</build_depend>
+  <build_export_depend condition="$ROS_VERSION == 1">message_runtime</build_export_depend>
+  <exec_depend condition="$ROS_VERSION == 1">message_runtime</exec_depend>
+
+  <test_depend condition="$ROS_VERSION == 2">ament_lint_auto</test_depend>
+  <test_depend condition="$ROS_VERSION == 2">ament_lint_common</test_depend>
 
   <export>
-    <build_type>ament_cmake</build_type>
+    <build_type condition="$ROS_VERSION == 1">catkin</build_type>
+    <build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
   </export>
 </package>

src/cluster_list.cpp

@@ -93,5 +93,13 @@ cluster_1_general * Cluster_1_General::get_cluster_1_general()
 {
   return & cluster_1_general_msg;
 }
+
+Cluster_2_Quality::Cluster_2_Quality()
+{
+}
+
+Cluster_2_Quality::~Cluster_2_Quality()
+{
+}
 }
 }