Support for opentera online (#27)

* Support for opentera online * Reduced Realsense frequency to 15 Hz * Added real robot_status * Fix firmware * Fix robot_status * Replace isBatteryCharging by isPsuConnected. * Add isBatteryCharging * Fix ControlPanel * Add the step for the teensy udev rule * Fix base_status in robot_status * Add localisation arg to the rtabmap launch file * WIP rtabmap and rviz * WIP rtabmap * Fix odas dynamic linking error * Fix voltage and current inversion * WIP * Removed map stuff (rtabmap, map_image_generator, labels_manager) for OpenTera * Fix launch file argument * Added launchfile output arguments * Update odas_ros * Added explicit value false for is_stand_alone in ttop_opentera_online * Update odas_ros * Update odas_ros * WIP * Updated opentera-webrtc-ros * Cleanup in message format documentation comment in .ino file Co-authored-by: mamaheux <[email protected]> * Replace DoAction srv with opentera_webrtc_ros_msgs/SetString srv * Fix code review elements * Update frontend * Add remap for head actions in teleop * Update submodule * Update submodule * Fix problem with echo cancellation * Added scripts and documentation to setup ca certificates * Updated documentation * Updated submodule * Fix some code review elements Co-authored-by: Marc-Antoine Maheux <[email protected]> Co-authored-by: mamaheux <[email protected]>
introlab · Apr 20, 2022 · fce1a6f · fce1a6f
1 parent 9886dff
commit fce1a6f
Show file tree

Hide file tree

Showing 48 changed files with 2,368 additions and 188 deletions.
diff --git a/documentation/assembly/01_COMPUTER_CONFIGURATION.md b/documentation/assembly/01_COMPUTER_CONFIGURATION.md
@@ -9,7 +9,8 @@ TODO
 ### A. OpenCR Dev Rule
 
 1. Copy [100-opencr-custom.rules](../../firmwares/opencr_firmware/100-opencr-custom.rules) in `/etc/udev/rules.d/`.
-2. Add the user to the `dialout` group.
+2. Copy [100-teensy.rules](../../firmwares/psu_control/100-teensy.rules) in `/etc/udev/rules.d/`.
+3. Add the user to the `dialout` group.
 
 ```bash
 sudo usermod -a -G dialout $USER

diff --git a/firmwares/opencr_firmware/PsuControlCommandHandler.cpp b/firmwares/opencr_firmware/PsuControlCommandHandler.cpp
@@ -43,13 +43,14 @@ void PsuControlCommandHandler::handleMessage()
         case MESSAGE_TYPE_STATUS:
             if (m_statusCommandHandler != nullptr)
             {
-                bool isBatteryCharging = m_buffer[2];
+                bool isPsuConnected = m_buffer[2];
+                bool isBatteryCharging = m_buffer[3];
                 float stateOfCharge, current, voltage;
 
-                memcpy(&stateOfCharge, m_buffer + 3, sizeof(float));
-                memcpy(&current, m_buffer + 7, sizeof(float));
-                memcpy(&voltage, m_buffer + 11, sizeof(float));
-                m_statusCommandHandler(isBatteryCharging, stateOfCharge, current, voltage);
+                memcpy(&stateOfCharge, m_buffer + 4, sizeof(float));
+                memcpy(&current, m_buffer + 8, sizeof(float));
+                memcpy(&voltage, m_buffer + 12, sizeof(float));
+                m_statusCommandHandler(isPsuConnected, isBatteryCharging, stateOfCharge, current, voltage);
             }
             break;
     }

diff --git a/firmwares/opencr_firmware/PsuControlCommandHandler.h b/firmwares/opencr_firmware/PsuControlCommandHandler.h
@@ -3,7 +3,12 @@
 
 #include <cstdint>
 
-typedef void (*StatusCommandHandler)(bool isBatteryCharging, float stateOfCharge, float current, float voltage);
+typedef void (*StatusCommandHandler)(
+    bool isPsuConnected,
+    bool isBatteryCharging,
+    float stateOfCharge,
+    float current,
+    float voltage);
 
 class PsuControlCommandHandler
 {

diff --git a/firmwares/opencr_firmware/opencr_firmware.ino b/firmwares/opencr_firmware/opencr_firmware.ino
@@ -50,14 +50,11 @@ float rawImuMsgData[RAW_IMU_MSG_DATA_LENGTH];
 std_msgs::Float32MultiArray rawImuMsg;
 ros::Publisher rawImuPub("opencr/raw_imu", &rawImuMsg);
 
-constexpr int STATE_OF_CHARGE_VOLTAGE_CURRENT_MSG_DATA_LENGTH = 3;
-float stateOfChargeVoltageCurrentMsgData[STATE_OF_CHARGE_VOLTAGE_CURRENT_MSG_DATA_LENGTH];
-std_msgs::Float32MultiArray stateOfChargeVoltageCurrentMsg;
-ros::Publisher
-    stateOfChargeVoltageCurrentPub("opencr/state_of_charge_voltage_current", &stateOfChargeVoltageCurrentMsg);
-
-std_msgs::Bool isBatteryChargingMsg;
-ros::Publisher isBatteryChargingPub("opencr/is_battery_charging", &isBatteryChargingMsg);
+// Message format : stateOfCharge, current, voltage, isPsuConnected, isBatteryCharging
+constexpr int BASE_STATUS_MSG_DATA_LENGTH = 5;
+float baseStatusMsgData[BASE_STATUS_MSG_DATA_LENGTH];
+std_msgs::Float32MultiArray baseStatusMsg;
+ros::Publisher baseStatusPub("opencr/base_status", &baseStatusMsg);
 
 // Robot
 MPU9250 imu(SPI_IMU, BDPIN_SPI_CS_IMU);
@@ -102,8 +99,7 @@ void setupRos()
     nh.advertise(currentHeadPosePub);
     nh.advertise(isHeadPoseReachablePub);
     nh.advertise(rawImuPub);
-    nh.advertise(stateOfChargeVoltageCurrentPub);
-    nh.advertise(isBatteryChargingPub);
+    nh.advertise(baseStatusPub);
 }
 
 void setupImu()
@@ -204,13 +200,15 @@ void onImuTimer()
     rawImuPub.publish(&rawImuMsg);
 }
 
-void onStatusCommand(bool isBatteryCharging, float stateOfCharge, float current, float voltage)
+void onStatusCommand(bool isPsuConnected, bool isBatteryCharging, float stateOfCharge, float current, float voltage)
 {
-    stateOfChargeVoltageCurrentMsgData[0] = stateOfCharge;
-    stateOfChargeVoltageCurrentMsgData[1] = current;
-    stateOfChargeVoltageCurrentMsgData[2] = voltage;
-
-    stateOfChargeVoltageCurrentMsg.data = &stateOfChargeVoltageCurrentMsgData[0];
-    stateOfChargeVoltageCurrentMsg.data_length = STATE_OF_CHARGE_VOLTAGE_CURRENT_MSG_DATA_LENGTH;
-    stateOfChargeVoltageCurrentPub.publish(&stateOfChargeVoltageCurrentMsg);
+    baseStatusMsgData[0] = stateOfCharge;
+    baseStatusMsgData[1] = voltage;
+    baseStatusMsgData[2] = current;
+    baseStatusMsgData[3] = isPsuConnected;
+    baseStatusMsgData[4] = isBatteryCharging;
+
+    baseStatusMsg.data = baseStatusMsgData;
+    baseStatusMsg.data_length = BASE_STATUS_MSG_DATA_LENGTH;
+    baseStatusPub.publish(&baseStatusMsg);
 }
diff --git a/firmwares/psu_control/100-teensy.rules b/firmwares/psu_control/100-teensy.rules
@@ -0,0 +1,39 @@
+# UDEV Rules for Teensy boards, http://www.pjrc.com/teensy/
+#
+# The latest version of this file may be found at:
+#   http://www.pjrc.com/teensy/00-teensy.rules
+#
+# This file must be placed at:
+#
+# /etc/udev/rules.d/00-teensy.rules    (preferred location)
+#   or
+# /lib/udev/rules.d/00-teensy.rules    (req'd on some broken systems)
+#
+# To install, type this command in a terminal:
+#   sudo cp 00-teensy.rules /etc/udev/rules.d/00-teensy.rules
+#
+# After this file is installed, physically unplug and reconnect Teensy.
+#
+ATTRS{idVendor}=="16c0", ATTRS{idProduct}=="04*", ENV{ID_MM_DEVICE_IGNORE}="1", ENV{ID_MM_PORT_IGNORE}="1"
+ATTRS{idVendor}=="16c0", ATTRS{idProduct}=="04[789a]*", ENV{MTP_NO_PROBE}="1"
+KERNEL=="ttyACM*", ATTRS{idVendor}=="16c0", ATTRS{idProduct}=="04*", GROUP:="dialout", RUN:="/bin/stty -F /dev/%k raw -echo"
+KERNEL=="hidraw*", ATTRS{idVendor}=="16c0", ATTRS{idProduct}=="04*", GROUP:="dialout"
+SUBSYSTEMS=="usb", ATTRS{idVendor}=="16c0", ATTRS{idProduct}=="04*", GROUP:="dialout"
+KERNEL=="hidraw*", ATTRS{idVendor}=="1fc9", ATTRS{idProduct}=="013*", GROUP:="dialout"
+SUBSYSTEMS=="usb", ATTRS{idVendor}=="1fc9", ATTRS{idProduct}=="013*", GROUP:="dialout"
+
+#
+# If you share your linux system with other users, or just don't like the
+# idea of write permission for everybody, you can replace MODE:="0666" with
+# OWNER:="yourusername" to create the device owned by you, or with
+# GROUP:="somegroupname" and mange access using standard unix groups.
+#
+# ModemManager tends to interfere with USB Serial devices like Teensy.
+# Problems manifest as the Arduino Serial Monitor missing some incoming
+# data, and "Unable to open /dev/ttyACM0 for reboot request" when
+# uploading.  If you experience these problems, disable or remove
+# ModemManager from your system.  If you must use a modem, perhaps
+# try disabling the "MM_FILTER_RULE_TTY_ACM_INTERFACE" ModemManager
+# rule.  Changing ModemManager's filter policy from "strict" to "default"
+# may also help.  But if you don't use a modem, completely removing
+# the troublesome ModemManager is the most effective solution.
diff --git a/firmwares/psu_control/include/OpencrCommandSender.h b/firmwares/psu_control/include/OpencrCommandSender.h
@@ -10,7 +10,12 @@ class OpencrCommandSender
 public:
     OpencrCommandSender();
 
-    void sendStatusCommand(bool isBatteryCharging, float stateOfCharge, float current, float voltage);
+    void sendStatusCommand(
+        bool isPsuConnected,
+        bool isBatteryCharging,
+        float stateOfCharge,
+        float current,
+        float voltage);
 
 private:
     void sendCommand();

diff --git a/firmwares/psu_control/src/OpencrCommandSender.cpp b/firmwares/psu_control/src/OpencrCommandSender.cpp
@@ -10,15 +10,21 @@ OpencrCommandSender::OpencrCommandSender()
     memset(m_buffer, 0, UINT8_MAX);
 }
 
-void OpencrCommandSender::sendStatusCommand(bool isBatteryCharging, float stateOfCharge, float current, float voltage)
+void OpencrCommandSender::sendStatusCommand(
+    bool isPsuConnected,
+    bool isBatteryCharging,
+    float stateOfCharge,
+    float current,
+    float voltage)
 {
-    m_buffer[0] = sizeof(float) * 3 + 3;
+    m_buffer[0] = sizeof(float) * 3 + 4;
     m_buffer[1] = MESSAGE_TYPE_STATUS;
-    m_buffer[2] = static_cast<uint8_t>(isBatteryCharging);
+    m_buffer[2] = static_cast<uint8_t>(isPsuConnected);
+    m_buffer[3] = static_cast<uint8_t>(isBatteryCharging);
 
-    memcpy(m_buffer + 3, &stateOfCharge, sizeof(float));
-    memcpy(m_buffer + 7, &current, sizeof(float));
-    memcpy(m_buffer + 11, &voltage, sizeof(float));
+    memcpy(m_buffer + 4, &stateOfCharge, sizeof(float));
+    memcpy(m_buffer + 8, &current, sizeof(float));
+    memcpy(m_buffer + 12, &voltage, sizeof(float));
 
     sendCommand();
 }

diff --git a/firmwares/psu_control/src/mainNormal.cpp b/firmwares/psu_control/src/mainNormal.cpp
@@ -98,7 +98,10 @@ void onStatusTicker()
     float current = currentVoltageSensor.readCurrent();
     float voltage = currentVoltageSensor.readVoltage();
 
+    bool isPsuConnected = charger.isPsuConnected();
 
+    DEBUG_SERIAL.print("isPsuConnected=");
+    DEBUG_SERIAL.println(isPsuConnected);
     DEBUG_SERIAL.print("isBatteryCharging=");
     DEBUG_SERIAL.println(isBatteryCharging);
     DEBUG_SERIAL.print("stateOfCharge=");
@@ -113,7 +116,7 @@ void onStatusTicker()
     DEBUG_SERIAL.println(externalTemperatureCelsius);
     DEBUG_SERIAL.println("Send status");
     DEBUG_SERIAL.println();
-    opencrCommandSender.sendStatusCommand(isBatteryCharging, stateOfCharge, current, voltage);
+    opencrCommandSender.sendStatusCommand(isPsuConnected, isBatteryCharging, stateOfCharge, current, voltage);
 }
 
 #endif
diff --git a/ros/behaviors/teleoperation/CMakeLists.txt b/ros/behaviors/teleoperation/CMakeLists.txt
@@ -8,13 +8,13 @@ project(teleoperation)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
-        hbba_lite
-        rospy
-        std_msgs
-        t_top
-        geometry_msgs
-        message_generation
-        )
+    hbba_lite
+    rospy
+    std_msgs
+    t_top
+    geometry_msgs
+    opentera_webrtc_ros_msgs
+)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -57,10 +57,10 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate services in the 'srv' folder
-add_service_files(
-        FILES
-        DoAction.srv
-)
+# add_service_files(
+#         FILES
+#         DoAction.srv
+# )
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -70,7 +70,7 @@ add_service_files(
 # )
 
 ## Generate added messages and services with any dependencies listed here
-generate_messages()
+# generate_messages()
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -104,7 +104,7 @@ generate_messages()
 catkin_package(
         #  INCLUDE_DIRS include
         #  LIBRARIES gesture
-        #  CATKIN_DEPENDS hbba_lite rospy std_msgs t_top
+         CATKIN_DEPENDS hbba_lite rospy std_msgs t_top opentera_webrtc_ros_msgs
         #  DEPENDS system_lib
 )
 

diff --git a/ros/behaviors/teleoperation/package.xml b/ros/behaviors/teleoperation/package.xml
@@ -54,18 +54,18 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>t_top</build_depend>
   <build_depend>geometry_msgs</build_depend>
-  <build_depend>message_generation</build_depend>
+  <build_depend>opentera_webrtc_ros_msgs</build_depend>
   <build_export_depend>hbba_lite</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <build_export_depend>t_top</build_export_depend>
   <build_export_depend>geometry_msgs</build_export_depend>
-  <build_export_depend>message_generation</build_export_depend>
+  <build_export_depend>opentera_webrtc_ros_msgs</build_export_depend>
   <exec_depend>hbba_lite</exec_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>t_top</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>opentera_webrtc_ros_msgs</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

diff --git a/ros/behaviors/teleoperation/scripts/teleoperation_node.py b/ros/behaviors/teleoperation/scripts/teleoperation_node.py
@@ -5,7 +5,7 @@
 from contextlib import contextmanager
 
 from t_top import MovementCommands, HEAD_ZERO_Z
-from teleoperation.srv import DoAction, DoActionRequest, DoActionResponse
+from opentera_webrtc_ros_msgs.srv import SetString, SetStringRequest, SetStringResponse
 
 
 def twist_is_null(twist: Twist) -> bool:
@@ -46,7 +46,7 @@ def __init__(self):
             self._movement_commands.sleep_time), self._on_timer_cb)
 
         self._origin_serv = rospy.Service(
-            "teleop_do_action", DoAction, self._do_action_cb)
+            "teleop_do_action", SetString, self._do_action_cb)
 
     def _on_twist_cb(self, msg):
         if self._movement_commands.is_filtering_all_messages:
@@ -90,7 +90,7 @@ def _move(self, msg: Twist) -> None:
         self._state.current_speed = TeleopAngles(
             torso_angle=msg.angular.z/0.15, head_angle=-1.1*msg.linear.x/0.15)
 
-    def _do_action_cb(self, req: DoActionRequest) -> DoActionResponse:
+    def _do_action_cb(self, req: SetStringRequest) -> SetStringResponse:
         jump_table = {
             "goto_origin": self._goto_origin,
             "do_yes": self._do_yes,
@@ -100,15 +100,15 @@ def _do_action_cb(self, req: DoActionRequest) -> DoActionResponse:
             "goto_origin_torso": self._goto_origin_torso,
         }
 
-        if req.name in jump_table:
+        if req.data in jump_table:
             if self._movement_commands.is_filtering_all_messages:
-                return DoActionResponse(success=False, message="HBBA filter is active")
+                return SetStringResponse(success=False, message="HBBA filter is active")
 
-            jump_table[req.name]()
-            return DoActionResponse(success=True, message="")
+            jump_table[req.data]()
+            return SetStringResponse(success=True, message="")
 
         else:
-            return DoActionResponse(success=False, message=f"'{req.name}' not in {list(jump_table.keys())}")
+            return SetStringResponse(success=False, message=f"'{req.data}' not in {list(jump_table.keys())}")
 
     @contextmanager
     def _pause_moving(self):

diff --git a/ros/behaviors/teleoperation/srv/DoAction.srv b/ros/behaviors/teleoperation/srv/DoAction.srv
diff --git a/ros/demos/README.md b/ros/demos/README.md
@@ -7,3 +7,4 @@ This folder contains ROS packages performing robot demonstrations.
 - The [control_panel](control_panel) folder contains a graphical user interface to control T-Top's features manually.
 - The [smart_speaker](smart_speaker) folder contains a node that makes T-Top a smart speaker that can tell the current
   weather, the weather forecast or a story. Also, it can dance to the ambiant music or play a song and dance.
+- The [ttop_opentera](ttop_opentera) folder contains a node that makes T-Top a telepresence robot using OpenTera.
diff --git a/ros/demos/control_panel/launch/control_panel.launch b/ros/demos/control_panel/launch/control_panel.launch
@@ -1,10 +1,13 @@
 <launch>
     <arg name="language" default="en"/>  <!-- Options: fr or en -->
 
-    <node pkg="control_panel" type="control_panel_node" name="control_panel_node" output="screen"/>
+    <arg name="output" default="log"/>
+
+    <node pkg="control_panel" type="control_panel_node" name="control_panel_node" output="$(arg output)"/>
 
     <include file="$(find t_top)/launch/bringup.launch">
         <arg name="language" value="$(arg language)"/>
         <arg name="slam" value="false"/>
+        <arg name="output" value="$(arg output)"/>
     </include>
 </launch>
diff --git a/ros/demos/control_panel/src/widgets/ControlPanel.cpp b/ros/demos/control_panel/src/widgets/ControlPanel.cpp
@@ -23,7 +23,7 @@ ControlPanel::ControlPanel(ros::NodeHandle& nodeHandle, shared_ptr<DesireSet> de
     createUi();
 
     m_batterySubscriber = nodeHandle.subscribe(
-        "opencr/state_of_charge_voltage_current",
+        "opencr/base_status",
         1,
         &ControlPanel::batterySubscriberCallback,
         this);
@@ -38,7 +38,7 @@ void ControlPanel::onVolumeChanged(int volume)
 
 void ControlPanel::batterySubscriberCallback(const std_msgs::Float32MultiArray::ConstPtr& msg)
 {
-    if (msg->data.size() == 3)
+    if (msg->data.size() == 4)
     {
         int battery = static_cast<int>(msg->data[0]);
         invokeLater([this, battery]() { m_batteryLevel->display(QString::number(battery)); });

diff --git a/ros/demos/smart_speaker/launch/smart_speaker_rss.launch b/ros/demos/smart_speaker/launch/smart_speaker_rss.launch
@@ -1,7 +1,9 @@
 <launch>
     <arg name="language" default="en"/>  <!-- Options: fr or en -->
 
-    <node pkg="smart_speaker" type="smart_speaker_rss_node" name="smart_speaker_rss_node" output="screen" launch-prefix="bash -c 'sleep 60; $0 $@' ">
+    <arg name="output" default="log"/>
+
+    <node pkg="smart_speaker" type="smart_speaker_rss_node" name="smart_speaker_rss_node" output="$(arg output)" launch-prefix="bash -c 'sleep 60; $0 $@' ">
         <param name="language" value="$(arg language)"/>
         <param name="story_path_en" value="$(find smart_speaker)/story_en.txt"/>
         <param name="story_path_fr" value="$(find smart_speaker)/story_fr.txt"/>
@@ -12,5 +14,6 @@
     <include file="$(find t_top)/launch/bringup.launch">
         <arg name="language" value="$(arg language)"/>
         <arg name="slam" value="false"/>
+        <arg name="output" value="$(arg output)"/>
     </include>
 </launch>