Initial arm bridge for the simulator (#580)

* Init

* Make pub and sub

* Cleaner

CMakeLists.txt

@@ -241,12 +241,14 @@ else ()
 
         add_cpp_interface_library_macro(moteus deps/moteus/lib/cpp/mjbots)
 
-        add_gazebo_plugin_macro(differential_drive_plugin_6w src/gazebo/differential_drive_6w.cpp src)
+        add_gazebo_plugin_macro(differential_drive_plugin_6w src/simulator/differential_drive_6w.cpp src)
 
-        add_gazebo_plugin_macro(kinect_plugin src/gazebo/gazebo_ros_openni_kinect.cpp src/gazebo)
+        add_gazebo_plugin_macro(kinect_plugin src/simulator/gazebo_ros_openni_kinect.cpp src/simulator)
         target_link_libraries(kinect_plugin PRIVATE gazebo_ros_camera_utils DepthCameraPlugin Eigen3::Eigen)
         set_target_properties(kinect_plugin PROPERTIES CXX_CLANG_TIDY "")
 
+        add_cpp_node_macro(sim_arm_bridge src/simulator/arm_bridge/*.cpp)
+
         if (ZED_FOUND)
             add_cpp_nodelet_macro(zed_nodelet src/perception/zed_wrapper/*.c* src/perception/zed_wrapper)
             target_precompile_headers(zed_nodelet PRIVATE src/perception/zed_wrapper/pch.hpp)

src/simulator/arm_bridge/sim_arm_bridge.cpp

@@ -0,0 +1,30 @@
+#include "sim_arm_bridge.hpp"
+
+#include <ros/init.h>
+#include <ros/node_handle.h>
+#include <sensor_msgs/JointState.h>
+
+ros::Subscriber subscriber;
+
+ros::Publisher jointStatePublisher;
+
+constexpr std::string_view ARM_THROTTLE_CMD_TOPIC = "arm_throttle_cmd";
+constexpr std::string_view ARM_VELOCITY_CMD_TOPIC = "arm_velocity_cmd";
+constexpr std::string_view ARM_POSITION_CMD_TOPIC = "arm_position_cmd";
+
+int main(int argc, char* argv[]) {
+    ros::init(argc, argv, "sim_arm_bridge");
+    ros::NodeHandle nh;
+
+    subscriber = nh.subscribe(std::string{ARM_POSITION_CMD_TOPIC}, 1, positionCallback);
+
+    ros::spin();
+    return EXIT_SUCCESS;
+}
+
+void positionCallback(mrover::Position const& positions) {
+    sensor_msgs::JointState jointState;
+    jointState.name = positions.names;
+    jointState.position.assign(positions.positions.begin(), positions.positions.end());
+    jointStatePublisher.publish(jointState);
+}

src/simulator/arm_bridge/sim_arm_bridge.hpp

@@ -0,0 +1,5 @@
+#pragma once
+
+#include <mrover/Position.h>
+
+void positionCallback(mrover::Position const& positions);