diff --git a/controllers/evolo/evolo_controllers/evolo_controllers/control.py b/controllers/evolo/evolo_controllers/evolo_controllers/control.py new file mode 100755 index 000000000..225be6871 --- /dev/null +++ b/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() diff --git a/controllers/evolo/evolo_controllers/evolo_controllers/yaw_control.py b/controllers/evolo/evolo_controllers/evolo_controllers/yaw_control.py deleted file mode 100755 index 6d80990db..000000000 --- a/controllers/evolo/evolo_controllers/evolo_controllers/yaw_control.py +++ /dev/null @@ -1,86 +0,0 @@ -#! /usr/bin/env python3 - -import math -import rclpy -from rclpy.node import Node -from std_msgs.msg import String, Float32 -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 -import json - -#Basic PID regulator -class yaw_control(Node): - def __init__(self): - super().__init__("yaw_control") - self.logger = self.get_logger() - self.logger.info("yaw_control!") - - self.declare_node_parameters() - - self.update_rate = float(self.get_parameter("update_rate").value) - self.logger.info(f"update rate: {self.update_rate}") - self.robot_name = self.get_parameter("robot_name").value - - - self.yaw_setpoint = None - self.yaw_setpoint_time = None - - #Control inputs. - self.create_subscription(Float32, - f"{ControlTopics.CONTROL_YAW_TOPIC}", self.yaw_cb, 1) - #Outputs - self.evolo_pub = self.create_publisher(String, - f"{evoloTopics.EVOLO_CAPTAIN_TO}", 1) - - def time_now(self): - return self.get_clock().now().nanoseconds * 1e-9 - - def declare_node_parameters(self): - self.declare_parameter("update_rate", 1) - self.declare_parameter("robot_name", "evolo") - - def yaw_cb(self, msg): - self.yaw_setpoint = msg.data - self.yaw_setpoint_time = self.time_now() - - - def update(self): - now = self.time_now() - - if self.yaw_setpoint_time is not None and now-self.yaw_setpoint_time < 1 and self.yaw_setpoint is not None: - #Convert yaw to NED and degrees - target_course = -math.degrees(self.yaw_setpoint) + 90 - while(target_course < 0): - target_course+=360 - while(target_course >= 360): - target_course -= 360 - - #TODO send YAW command to evolo - msg = String() - - target = {"ctt": target_course,"dtt": 100, "sogAim": "fly"} - msg.data = json.dumps({"setTarget": target}) - self.evolo_pub.publish(msg) - - - self.logger.info(f"sending target course={target_course}") - - else: - msg = String() - target = {"sogAim": "stop"} - msg.data = json.dumps({"setTarget": target}) - self.evolo_pub.publish(msg) - self.logger.info(f"sending stop") - - -def main(args=None, namespace=None): - rclpy.init(args=args) - control_node = yaw_control() - - control_node.create_timer(1.0/control_node.update_rate, control_node.update) - executor = MultiThreadedExecutor() - executor.add_node(control_node) - executor.spin() diff --git a/controllers/evolo/evolo_controllers/setup.py b/controllers/evolo/evolo_controllers/setup.py index a02e629dc..a14ac0c04 100644 --- a/controllers/evolo/evolo_controllers/setup.py +++ b/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', ], }, ) diff --git a/messages/evolo_msgs/msg/Topics.msg b/messages/evolo_msgs/msg/Topics.msg index 76bb48997..c14a5e32a 100644 --- a/messages/evolo_msgs/msg/Topics.msg +++ b/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 diff --git a/scripts/smarc_bringups/scripts/evolo_bringup.sh b/scripts/smarc_bringups/scripts/evolo_bringup.sh index 97892ebb9..e96a750d1 100755 --- a/scripts/smarc_bringups/scripts/evolo_bringup.sh +++ b/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