Changed evolo controller to use twist and minor changes to evolo bringup script

controllers/evolo/evolo_controllers/evolo_controllers/control.py

@@ -0,0 +1,115 @@
+#! /usr/bin/env python3
+
+## This node converts TwistStamped to 
+# steering (Float32) and speed (Float32) for 
+# evolo flight controller
+
+import math
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String, Float32
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import TwistStamped
+from rclpy.executors import MultiThreadedExecutor
+
+from evolo_msgs.msg import Topics as evoloTopics
+from smarc_msgs.msg import Topics as SmarcTopics
+from smarc_control_msgs.msg import Topics as ControlTopics
+
+#Evolo speed and steering controller
+class twist_control(Node):
+    def __init__(self):
+        super().__init__("evolo_control")
+        self.logger = self.get_logger()
+
+        self.declare_parameter("update_rate", 10)
+        self.update_rate = float(self.get_parameter("update_rate").value)
+
+        self.declare_parameter("robot_name", "evolo")
+        self.robot_name = self.get_parameter("robot_name").value
+
+        #limits
+        self.declare_parameter("max_turnrate", 10.0) #Deg/s
+        self.max_turnrate = float(self.get_parameter("max_turnrate").value)
+        self.max_turnrate_rad = math.radians(self.max_turnrate)
+
+        self.declare_parameter("max_speed", 6) #m/s ~12 knots
+        self.max_speed = self.get_parameter("max_speed").value
+
+        #Open or closed loop control
+        self.declare_parameter("closed_loop_control", False)
+        self.closed_loop_ctrl = self.get_parameter("closed_loop_control").value
+
+        #Open loop gain
+        self.declare_parameter("open_loop_gain", 1.0)
+        self.open_loop_gain = self.get_parameter("open_loop_gain").value
+
+        self.twist_setpoint = None
+        self.twist_setpoint_time = None
+
+        self.odom_feedback = None
+        self.odom_feedback_time = None
+
+        #Control inputs.
+        self.create_subscription(TwistStamped, evoloTopics.EVOLO_TWIST_SETPOINT , self.twist_cb, 1)
+
+        #Feedback
+        self.create_subscription(Odometry, SmarcTopics.ODOM_TOPIC , self.odom_cb, 1)
+
+        #Outputs
+        self.steering_pub = self.create_publisher(Float32, evoloTopics.EVOLO_STEERING_SETPOINT, 1)
+        self.speed_pub = self.create_publisher(Float32, evoloTopics.EVOLO_SPEED_SETPOINT, 1)
+
+    def time_now(self):
+        return self.get_clock().now().nanoseconds * 1e-9
+
+    def odom_cb(self,msg):
+        self.odom_feedback = msg
+        self.odom_feedback_time = self.time_now()
+
+    def twist_cb(self, msg):
+        self.twist_setpoint = msg
+        self.twist_setpoint_time = self.time_now()
+
+
+    def update(self):
+        now = self.time_now()
+
+        setpoint_OK = self.twist_setpoint_time is not None and now-self.twist_setpoint_time < 1 and self.twist_setpoint is not None
+        feedback_OK = self.odom_feedback is not None and now - self.odom_feedback_time < 1
+
+        #TODO closed loop control in the future
+        if(self.closed_loop_ctrl):
+            self.logger.info(f"Closed loop control is not implemented")
+        else:    
+            #Open loop control
+            if(setpoint_OK):
+
+                #speed = math.sqrt((self.twist_setpoint.twist.linear.x * self.twist_setpoint.twist.linear.x) + 
+                #                (self.twist_setpoint.twist.linear.y*self.twist_setpoint.twist.linear.y))
+                #steering = math.atan2(self.twist_setpoint.twist.linear.y, self.twist_setpoint.twist.linear.x) # Not used?
+
+                speed = max(0, min( self.max_speed, self.twist_setpoint.twist.linear.x)) #m/s
+                turnRate = max(-self.max_turnrate_rad, min(self.max_turnrate_rad, self.twist_setpoint.twist.angular.z * self.open_loop_gain)) #rad/s
+
+                steering_msg = Float32()
+                steering_msg.data = float(turnRate)
+                self.steering_pub.publish(steering_msg)
+
+                speed_msg = Float32()
+                speed_msg.data = float(speed)
+                self.speed_pub.publish(speed_msg)
+
+                self.logger.info(f"Open loop control")
+            else:
+                self.logger.info(f"No setpoint")
+
+
+def main(args=None, namespace=None):
+    rclpy.init(args=args)
+    control_node = twist_control()
+
+    control_node.create_timer(1.0/control_node.update_rate, control_node.update)
+    executor = MultiThreadedExecutor()
+    executor.add_node(control_node)
+    executor.spin()

controllers/evolo/evolo_controllers/setup.py

@@ -23,7 +23,7 @@
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'yaw_controller = evolo_controllers.yaw_control:main',
+            'controller = evolo_controllers.control:main',
         ],
     },
 )

messages/evolo_msgs/msg/Topics.msg

@@ -1,6 +1,11 @@
 # This file defines all the topics used on Evolo
 # The goal is to have a single location that all cpp/py scripts can easily access
 
+#Control
+string EVOLO_STEERING_SETPOINT = ctrl/steering_setpoint
+string EVOLO_SPEED_SETPOINT = ctrl/speed_setpoint
+string EVOLO_TWIST_SETPOINT = ctrl/twist_setpoint #Type: TwistStamped
+string EVOLO_TWIST_PLANNED = ctrl/twist_planned #Type: TwistStamped
 
 #communication to and from captain
 string EVOLO_CAPTAIN_TO = captain/to

scripts/smarc_bringups/scripts/evolo_bringup.sh

@@ -11,16 +11,16 @@ BT_LOG_MODE=compact # can be 'compact' or 'verbose'
 
 
 #Simulation
-SIM = False
-if ["$SIM" == "True"]; then
+SIM=True
+if [ "$SIM" = "True" ]; then
     REALSIM=simulation
-    ROBOT_NAME=evolo_v1
+    ROBOT_NAME=evolo
     USE_SIM_TIME=True
 else
     REALSIM=real
     #USE_SIM_TIME=False
-    USE_SIM_TIME=False #Useful for rosbags
-    LOCATION_SOURCE=MQTT #[SBG MQTT SERIAL]
+    USE_SIM_TIME=True #Useful for rosbags
+    LOCATION_SOURCE=SBG #[SBG MQTT SERIAL]
 fi
 
 #Low controllers
@@ -53,7 +53,7 @@ tmux send-keys "sleep 4; ros2 run evolo_move_path move_path_server --ros-args -r
 #tmux send-keys "sleep 4; ros2 run lolo_loiter server --ros-args -r __ns:=/$ROBOT_NAME -p use_sim_time:=$USE_SIM_TIME" C-m
 
 # Mqtt bridge.
-tmux new-window -t $SESSION:3 -n 'mqtt_bridge'
+tmux new-window -t $SESSION:3 -n 'mqtt'
 tmux select-window -t $SESSION:3
 
 # To connect to smarc MQTT broker
@@ -72,7 +72,7 @@ if [ "$REALSIM" = "real" ]; then
     #Connection to evolo captain
     tmux new-window -t $SESSION:4 -n 'Evolo captain'
     tmux select-window -t $SESSION:4
-    tmux send-keys "ros2 launch evolo_mqtt_bridge evolo_mqtt_launch.py" C-m
+    tmux send-keys "ros2 launch evolo_mqtt_bridge evolo_mqtt_launch.py"
     #tmux send-keys "ros2 launch evolo_serial_bridge evolo_serial_launch.py"
 
     #SBG driver
@@ -81,7 +81,7 @@ if [ "$REALSIM" = "real" ]; then
     tmux send-keys "TODO: lanuch SBG driver" C-m
 
     #Odom
-    tmux new-window -t $SESSION:6 -n 'SBG to Odom'
+    tmux new-window -t $SESSION:6 -n 'Localization to Odom'
     tmux select-window -t $SESSION:6
     tmux split-window -h -t $SESSION:6.0
 
@@ -98,11 +98,11 @@ if [ "$REALSIM" = "real" ]; then
     if [ "$LOCATION_SOURCE" = "MQTT" ] || [ "$LOCATION_SOURCE" = "SERIAL" ]; then
         #Odom initializer
         tmux select-pane -t $SESSION:6.0
-        tmux send-keys "ros2 launch evolo_captain_interface evolo_captain_odom_initializer_launch.py" C-m
+        tmux send-keys "ros2 launch evolo_captain_interface evolo_captain_odom_initializer_launch.py use_sim_time:=$USE_SIM_TIME" C-m
 
         #sbg to odom node
         tmux select-pane -t $SESSION:6.1
-        tmux send-keys "ros2 launch evolo_captain_interface evolo_captain_odom_launch.py" C-m
+        tmux send-keys "ros2 launch evolo_captain_interface evolo_captain_odom_launch.py use_sim_time:=$USE_SIM_TIME" C-m
     fi
 
 
@@ -128,7 +128,7 @@ if [ "$REALSIM" = "real" ]; then
 else #Sim
     tmux new-window -t $SESSION:4 -n 'tcp-endpoint'
     tmux select-window -t $SESSION:4
-    tmux send-keys "ros2 run ros_tcp_endpoint default_server_endpoint --ros-args -p ROS_IP:=127.0.0.1"
+    tmux send-keys "ros2 run ros_tcp_endpoint default_server_endpoint --ros-args -p ROS_IP:=127.0.0.1" C-m
 fi
 
 if [ "$REALSIM" = "real" ]; then
@@ -171,7 +171,7 @@ tmux split-window -h -t $SESSION:13.0
 
 #Pointcloud preprocessing
 tmux select-pane -t $SESSION:13.0
-tmux send-keys "ros2 launch pointcloud_preprocessing pointcloud_preprocessing_launch_boat.py" C-m
+tmux send-keys "ros2 launch pointcloud_preprocessing pointcloud_preprocessing_launch_evolo.py use_sim_time:=$USE_SIM_TIME" C-m
 #occupancy grid
 tmux select-pane -t $SESSION:13.1
 #tmux send-keys "ros2 run clustering_segmentation clustering_segmentation --ros-args -p use_sim_time:=$USE_SIM_TIME" C-m