New science board over CAN

src/Bringup/launch/science.launch.py

@@ -19,7 +19,7 @@
 
 def generate_launch_description():
     joystick_control_dir = get_package_share_directory("joystick_control")
-    joy_parameters_file = os.path.join(joystick_control_dir, "pxn.yaml")
+    joy_parameters_file = os.path.join(joystick_control_dir, "3dpro.yaml")
 
     science_drill_control = Node(
         package="joystick_control",
@@ -40,6 +40,7 @@ def generate_launch_description():
                 plugin="ros_phoenix::TalonSRX",
                 name="elevator",
                 parameters=[
+                    {"interface": "can1"},
                     {"id": 20},
                     {"max_voltage": 24.0},
                     {"brake_mode": True},
@@ -51,6 +52,7 @@ def generate_launch_description():
                 plugin="ros_phoenix::TalonSRX",
                 name="drill",
                 parameters=[
+                    {"interface": "can1"},
                     {"id": 21},
                     {"max_voltage": 24.0},
                     {"brake_mode": True},
@@ -61,28 +63,22 @@ def generate_launch_description():
         output="screen",
     )
 
-    esp_serial_bridge = Node(
+    science_sensors = Node(
         package="science_sensors",
-        executable="esp_serial_bridge",
-        name="esp_serial_bridge",
+        executable="science_sensors",
+        name="science_sensors",
+        parameters=[{"interface": "can1"}],
     )
 
     panoramic = Node(
-        package="science_sensors",
+        package="science_python",
         executable="panoramic",
         name="panoramic",
     )
 
-    polarimeter = Node(
-        package="science_sensors",
-        executable="polarimeter",
-        name="polarimeter",
-    )
-
     ld = LaunchDescription()
     ld.add_action(science_drill_control)
     ld.add_action(talon_container)
-    ld.add_action(esp_serial_bridge)
-    ld.add_action(polarimeter)
+    ld.add_action(science_sensors)
     ld.add_action(panoramic)
     return ld

src/HW-Devices/science_python/package.xml

@@ -0,0 +1,22 @@
+<?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>science_python</name>
+  <version>0.0.0</version>
+  <description>Nodes for science sensors</description>
+  <maintainer email="aj01cars@outlook.com">aydan</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>interfaces</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/HW-Devices/science_python/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/science_python
+[install]
+install_scripts=$base/lib/science_python

src/HW-Devices/science_python/setup.py

@@ -0,0 +1,33 @@
+from setuptools import find_packages, setup
+import os
+from glob import glob
+
+package_name = "science_python"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=find_packages(
+        include=["science_python", "science_python.*"], exclude=["test"]
+    ),
+    include_package_data=True,
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        (
+            os.path.join("share", package_name, "launch"),
+            glob(os.path.join("launch", "*launch.[pxy][yma]*")),
+        ),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="aydan",
+    maintainer_email="aj01cars@outlook.com",
+    description="Nodes for science sensors",
+    license="TODO: License declaration",
+    entry_points={
+        "console_scripts": [
+            "panoramic = science_python.panoramic:main",
+        ],
+    },
+)

src/HW-Devices/science_sensors/CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 3.5)
+project(science_sensors LANGUAGES CXX)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(interfaces REQUIRED)
+
+include_directories(
+  ${CMAKE_CURRENT_SOURCE_DIR}/../ros_odrive/odrive_base/include
+  ${CMAKE_CURRENT_SOURCE_DIR}/include
+)
+
+add_executable(${PROJECT_NAME}
+  "../ros_odrive/odrive_base/src/epoll_event_loop.cpp"
+  "../ros_odrive/odrive_base/src/socket_can.cpp"
+  "src/science_node.cpp"
+  "src/main.cpp"
+)
+ament_target_dependencies(${PROJECT_NAME}
+  "rclcpp"
+  "interfaces"
+)
+
+# Install Components
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME})
+
+# Install headers
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+ament_export_include_directories(
+  include/
+)
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+
+ament_package()

src/HW-Devices/science_sensors/include/science_node.hpp

@@ -0,0 +1,65 @@
+#ifndef SCIENCE_SENSORS_HPP
+#define SCIENCE_SENSORS_HPP
+
+#include <condition_variable>
+#include <mutex>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/u_int16.hpp>
+
+#include "epoll_event_loop.hpp"
+#include "interfaces/msg/dht22.hpp"
+#include "interfaces/msg/polarimeter_sweep.hpp"
+#include "interfaces/msg/science_adc.hpp"
+#include "interfaces/msg/science_motor.hpp"
+#include "interfaces/msg/science_servo.hpp"
+#include "interfaces/srv/run_polarimeter.hpp"
+#include "socket_can.hpp"
+
+#define SCAN_STEPS 48
+
+class ScienceNode : public rclcpp::Node {
+public:
+  ScienceNode(const std::string name,
+              const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
+  bool init(EpollEventLoop *event_loop);
+  void deinit();
+
+private:
+  void recv_callback(const can_frame &frame);
+  void motor_callback();
+  void servo_callback();
+  void polar_callback(
+      const std::shared_ptr<interfaces::srv::RunPolarimeter::Request> request,
+      std::shared_ptr<interfaces::srv::RunPolarimeter::Response> response);
+  void request_polar();
+
+  inline bool verify_length(const std::string &name, uint8_t expected,
+                            uint8_t length);
+
+  uint16_t node_id_;
+  SocketCanIntf can_intf_ = SocketCanIntf();
+
+  rclcpp::Publisher<interfaces::msg::ScienceADC>::SharedPtr adc_pub_;
+  rclcpp::Publisher<interfaces::msg::DHT22>::SharedPtr temp_pub_;
+  rclcpp::Publisher<std_msgs::msg::UInt16>::SharedPtr co2_pub_;
+  rclcpp::Publisher<interfaces::msg::PolarimeterSweep>::SharedPtr polar_pub_;
+
+  EpollEvent motor_evt_;
+  std::mutex motor_mutex_;
+  interfaces::msg::ScienceMotor motor_msg_ = interfaces::msg::ScienceMotor();
+  rclcpp::Subscription<interfaces::msg::ScienceMotor>::SharedPtr motor_sub_;
+
+  EpollEvent servo_evt_;
+  std::mutex servo_mutex_;
+  interfaces::msg::ScienceServo servo_msg_ = interfaces::msg::ScienceServo();
+  rclcpp::Subscription<interfaces::msg::ScienceServo>::SharedPtr servo_sub_;
+
+  EpollEvent req_polar_evt_;
+  std::condition_variable polar_cond_;
+  std::mutex polar_mutex_;
+  rclcpp::Service<interfaces::srv::RunPolarimeter>::SharedPtr polar_srv_;
+  uint16_t polar_points_[SCAN_STEPS];
+  uint8_t polar_index_;
+};
+
+#endif // SCIENCE_SENORS_HPP

src/HW-Devices/science_sensors/package.xml

@@ -7,17 +7,20 @@
   <maintainer email="aj01cars@outlook.com">aydan</maintainer>
   <license>TODO: License declaration</license>
 
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
   <depend>rclpy</depend>
+  <depend>rclcpp</depend>
   <depend>interfaces</depend>
   <depend>std_msgs</depend>
-  <exec_depend>python3-serial</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>
   <test_depend>python3-pytest</test_depend>
 
   <export>
-    <build_type>ament_python</build_type>
+    <build_type>ament_cmake</build_type>
   </export>
 </package>