chore: minor enhancements

go2_robot_sdk/go2_robot_sdk/go2_driver_node.py

@@ -55,7 +55,13 @@ class RobotBaseNode(Node):
 
     def __init__(self):
         super().__init__('go2_driver_node')
+
+        self.declare_parameter('robot_ip', os.getenv('ROBOT_IP', os.getenv('GO2_IP')))
+        self.declare_parameter('token', os.getenv('ROBOT_TOKEN', os.getenv('GO2_TOKEN','')))
 
+        self.robot_ip = self.get_parameter('robot_ip').get_parameter_value().string_value
+        self.token = self.get_parameter('token').get_parameter_value().string_value
+
         self.conn = None
         qos_profile = QoSProfile(depth=10)
         self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile)
@@ -242,20 +248,20 @@ def publish_robot_state(self):
             go2_state.mode = self.robot_sport_state["data"]["mode"]
             go2_state.progress = self.robot_sport_state["data"]["progress"]
             go2_state.gait_type = self.robot_sport_state["data"]["gait_type"]
-            go2_state.position = self.robot_sport_state["data"]["position"]
-            go2_state.body_height = self.robot_sport_state["data"]["body_height"]
+            go2_state.position = list(map(float,self.robot_sport_state["data"]["position"]))
+            go2_state.body_height = float(self.robot_sport_state["data"]["body_height"])
             go2_state.velocity = self.robot_sport_state["data"]["velocity"]
             go2_state.range_obstacle = list(map(float, self.robot_sport_state["data"]["range_obstacle"]))
             go2_state.foot_force = self.robot_sport_state["data"]["foot_force"]
-            go2_state.foot_position_body = self.robot_sport_state["data"]["foot_position_body"]
-            go2_state.foot_speed_body = self.robot_sport_state["data"]["foot_speed_body"]
+            go2_state.foot_position_body = list(map(float,self.robot_sport_state["data"]["foot_position_body"]))
+            go2_state.foot_speed_body = list(map(float, self.robot_sport_state["data"]["foot_speed_body"]))
             self.go2_state_pub.publish(go2_state) 
 
             imu = IMU()
-            imu.quaternion = self.robot_sport_state["data"]["imu_state"]["quaternion"]
+            imu.quaternion = list(map(float,self.robot_sport_state["data"]["imu_state"]["quaternion"]))
             imu.accelerometer = list(map(float,self.robot_sport_state["data"]["imu_state"]["accelerometer"]))
             imu.gyroscope = list(map(float,self.robot_sport_state["data"]["imu_state"]["gyroscope"]))
-            imu.rpy = self.robot_sport_state["data"]["imu_state"]["rpy"]
+            imu.rpy = list(map(float,self.robot_sport_state["data"]["imu_state"]["rpy"]))
             imu.temperature = self.robot_sport_state["data"]["imu_state"]["temperature"]
             self.imu_pub.publish(imu) 
 
@@ -293,7 +299,8 @@ def _spin(node: Node,
 async def start_node():
     base_node = RobotBaseNode()
     conn = Go2Connection(
-        os.environ.get('ROBOT_IP'),
+        base_node.robot_ip,
+        token=base_node.token,
         on_validated=base_node.on_validated,
         on_message=base_node.on_data_channel_message,
 

go2_robot_sdk/launch/robot.launch.py

@@ -24,6 +24,7 @@
 import os
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
+from launch.conditions import UnlessCondition
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
@@ -32,6 +33,7 @@
 def generate_launch_description():
 
     use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    no_rviz2 = LaunchConfiguration('no_rviz2', default='false')
 
     urdf_file_name = 'go2.urdf'
     urdf = os.path.join(
@@ -82,6 +84,7 @@ def generate_launch_description():
             package='rviz2',
             namespace='',
             executable='rviz2',
+            condition=UnlessCondition(no_rviz2),
             name='rviz2',
             arguments=['-d' + os.path.join(get_package_share_directory('go2_robot_sdk'), 'config', 'conf.rviz')]
         )

go2_robot_sdk/launch/robot_on_steroids.launch.py

@@ -24,6 +24,7 @@
 import os
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
+from launch.conditions import UnlessCondition
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
@@ -32,6 +33,7 @@
 def generate_launch_description():
 
     use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    no_rviz2 = LaunchConfiguration('no_rviz2', default='false')
 
     urdf_file_name = 'go2_on_steroids.urdf'
     urdf = os.path.join(
@@ -82,6 +84,7 @@ def generate_launch_description():
             package='rviz2',
             namespace='',
             executable='rviz2',
+            condition=UnlessCondition(no_rviz2),
             name='rviz2',
             arguments=['-d' + os.path.join(get_package_share_directory('go2_robot_sdk'), 'config', 'conf.rviz')]
         )

go2_robot_sdk/package.xml

@@ -8,6 +8,7 @@
   <license>Apache-2.0</license>
 
   <depend>rclpy</depend>
+  <depend>go2_interfaces</depend>
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>

go2_robot_sdk/scripts/webrtc_driver.py

@@ -45,14 +45,16 @@
 class Go2Connection():
     def __init__(
             self, 
-            robot_ip=None, 
+            robot_ip=None,
+            token="",
             on_validated=None, 
             on_message=None, 
             on_open=None
             ):
 
         self.pc = RTCPeerConnection()
         self.robot_ip = robot_ip
+        self.token = token
         self.robot_validation = "PENDING"
         self.on_validated = on_validated
         self.on_message = on_message
@@ -124,7 +126,7 @@ async def connect(self):
                 "sdp": offer,
                 "id": "STA_localNetwork",
                 "type": "offer",
-                "token": "",
+                "token": self.token,
             }
             async with session.post(url, json=data, headers=headers) as resp:
                 if resp.status == 200: