From 42bbe04b9f5a6258afe8730e402f48e8b860126a Mon Sep 17 00:00:00 2001 From: Yashas Ambati <32080306+yambati03@users.noreply.github.com> Date: Sat, 1 Jun 2024 14:51:45 -0600 Subject: [PATCH 1/2] [hw] refactor status light (#199) * Refactor status light * Format --- urc_bringup/launch/bt.launch.py | 4 +- .../launch/test_status_light.launch.py | 56 +++++++++++++++++++ urc_bringup/strategies/bt_test.xml | 7 --- urc_bringup/strategies/status_light.xml | 51 +++++++++++++++++ urc_bringup/strategies/urc_strategies.btproj | 6 +- .../actions/status_light_publisher.hpp | 49 +++++++++++++--- .../src/actions/status_light_publisher.cpp | 26 ++++----- .../status_light_controller.hpp | 15 ++--- .../src/status_light_controller.cpp | 39 +++++-------- .../hardware_interfaces/status_light.hpp | 23 ++++++-- .../src/hardware_interfaces/status_light.cpp | 44 ++++++++++----- urc_hw_description/urdf/ros2_control.xacro | 4 +- urc_msgs/CMakeLists.txt | 1 + urc_msgs/msg/StatusLightCommand.msg | 10 ++++ 14 files changed, 247 insertions(+), 88 deletions(-) create mode 100644 urc_bringup/launch/test_status_light.launch.py create mode 100644 urc_bringup/strategies/status_light.xml create mode 100644 urc_msgs/msg/StatusLightCommand.msg diff --git a/urc_bringup/launch/bt.launch.py b/urc_bringup/launch/bt.launch.py index df0fe7bb..ce70c620 100644 --- a/urc_bringup/launch/bt.launch.py +++ b/urc_bringup/launch/bt.launch.py @@ -15,7 +15,7 @@ def generate_launch_description(): "libbt_call_trigger.so", "libbt_follow_path.so", "libbt_update_current_pose.so", - "libbt_status_light_publisher.so" + "libbt_status_light_publisher.so", ] node_lib_path_base = os.path.join( Path(get_package_share_directory("urc_bt_nodes")).parent.parent.absolute(), @@ -28,7 +28,7 @@ def generate_launch_description(): ros_lib_paths = [ os.path.join(node_lib_path_base, lib_name) for lib_name in ros_lib_names ] - bt_file_name = "bt_test.xml" + bt_file_name = "status_light.xml" enable_color = SetEnvironmentVariable(name="RCUTILS_COLORIZED_OUTPUT", value="1") diff --git a/urc_bringup/launch/test_status_light.launch.py b/urc_bringup/launch/test_status_light.launch.py new file mode 100644 index 00000000..3c6db9c1 --- /dev/null +++ b/urc_bringup/launch/test_status_light.launch.py @@ -0,0 +1,56 @@ +import os +from xacro import process_file +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from launch.actions import IncludeLaunchDescription, RegisterEventHandler +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.event_handlers import OnProcessExit + + +def generate_launch_description(): + pkg_urc_bringup = get_package_share_directory("urc_bringup") + + controller_config_file_dir = os.path.join( + pkg_urc_bringup, "config", "controller_config.yaml" + ) + xacro_file = os.path.join( + get_package_share_directory("urc_hw_description"), "urdf/walli.xacro" + ) + assert os.path.exists(xacro_file), "urdf path doesnt exist in " + str(xacro_file) + robot_description_config = process_file( + xacro_file, mappings={"use_simulation": "false"} + ) + robot_desc = robot_description_config.toxml() + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[controller_config_file_dir, {"robot_description": robot_desc}], + output="both", + ) + + load_status_light_controller = Node( + package="controller_manager", + executable="spawner", + arguments=["-p", controller_config_file_dir, "status_light_controller"], + ) + + bt_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_urc_bringup, "launch", "bt.launch.py") + ) + ) + + return LaunchDescription( + [ + control_node, + load_status_light_controller, + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_status_light_controller, + on_exit=[bt_launch], + ) + ), + ] + ) diff --git a/urc_bringup/strategies/bt_test.xml b/urc_bringup/strategies/bt_test.xml index 845c9bb1..a824057a 100644 --- a/urc_bringup/strategies/bt_test.xml +++ b/urc_bringup/strategies/bt_test.xml @@ -7,8 +7,6 @@ - - @@ -52,11 +50,6 @@ Message to log. - - - - diff --git a/urc_bringup/strategies/status_light.xml b/urc_bringup/strategies/status_light.xml new file mode 100644 index 00000000..66de4e72 --- /dev/null +++ b/urc_bringup/strategies/status_light.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urc_bringup/strategies/urc_strategies.btproj b/urc_bringup/strategies/urc_strategies.btproj index 0203af58..7a84c081 100644 --- a/urc_bringup/strategies/urc_strategies.btproj +++ b/urc_bringup/strategies/urc_strategies.btproj @@ -1,6 +1,7 @@ + @@ -35,8 +36,9 @@ - - + + + diff --git a/urc_bt_nodes/include/urc_bt_nodes/actions/status_light_publisher.hpp b/urc_bt_nodes/include/urc_bt_nodes/actions/status_light_publisher.hpp index 3bf6d6fe..61a6d5da 100644 --- a/urc_bt_nodes/include/urc_bt_nodes/actions/status_light_publisher.hpp +++ b/urc_bt_nodes/include/urc_bt_nodes/actions/status_light_publisher.hpp @@ -1,34 +1,65 @@ #ifndef STATUS_LIGHT_PUBLISHER_HPP__ #define STATUS_LIGHT_PUBLISHER_HPP__ +#include + #include "behaviortree_cpp/basic_types.h" #include "behaviortree_cpp/tree_node.h" #include "behaviortree_cpp/tree_node.h" #include "behaviortree_ros2/ros_node_params.hpp" #include "behaviortree_ros2/bt_topic_pub_node.hpp" -#include "string" -#include "std_msgs/msg/int8.hpp" + +#include "urc_msgs/msg/status_light_command.hpp" namespace behavior::actions { -class StatusLightPublisher : public BT::RosTopicPubNode + +int stringToColor(std::string color) +{ + if (color == "red") { + return urc_msgs::msg::StatusLightCommand::RED; + } + if (color == "green") { + return urc_msgs::msg::StatusLightCommand::GREEN; + } + if (color == "blue") { + return urc_msgs::msg::StatusLightCommand::BLUE; + } + + throw std::invalid_argument("Invalid color!"); +} + +int stringToState(std::string state) +{ + if (state == "on") { + return urc_msgs::msg::StatusLightCommand::ON; + } + if (state == "off") { + return urc_msgs::msg::StatusLightCommand::OFF; + } + if (state == "blink") { + return urc_msgs::msg::StatusLightCommand::BLINK; + } + + throw std::invalid_argument("Invalid state!"); +} + +class StatusLightPublisher : public BT::RosTopicPubNode { public: StatusLightPublisher( const std::string & instance_name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) - : BT::RosTopicPubNode(instance_name, conf, params) {} + : BT::RosTopicPubNode(instance_name, conf, params) {} static BT::PortsList providedPorts() { return providedBasicPorts( - { - BT::InputPort("value"), - }); + {BT::InputPort("color"), + BT::InputPort("state")}); } - bool setMessage(std_msgs::msg::Int8 & msg) override; - + bool setMessage(urc_msgs::msg::StatusLightCommand & msg) override; }; } diff --git a/urc_bt_nodes/src/actions/status_light_publisher.cpp b/urc_bt_nodes/src/actions/status_light_publisher.cpp index 31b3075f..f4c7a0d2 100644 --- a/urc_bt_nodes/src/actions/status_light_publisher.cpp +++ b/urc_bt_nodes/src/actions/status_light_publisher.cpp @@ -4,24 +4,22 @@ namespace behavior::actions { -bool StatusLightPublisher::setMessage(std_msgs::msg::Int8 & msg) +bool StatusLightPublisher::setMessage(urc_msgs::msg::StatusLightCommand & msg) { - msg = getInput("value").value(); + std::string color = getInput("color").value(); + std::string state = getInput("state").value(); + + try { + msg.color = stringToColor(color); + msg.state = stringToState(state); + } catch (std::invalid_argument & e) { + RCLCPP_ERROR(node_->get_logger(), "Invalid argument: %s", e.what()); + return false; + } + return true; } } -namespace BT -{ -template<> -inline std_msgs::msg::Int8 convertFromString(StringView str) -{ - std_msgs::msg::Int8 output; - output.data = convertFromString(str); - return output; -} -} // namespace BT - - #include "behaviortree_ros2/plugins.hpp" CreateRosNodePlugin(behavior::actions::StatusLightPublisher, "StatusLightPublisher"); diff --git a/urc_controllers/include/urc_controllers/status_light_controller.hpp b/urc_controllers/include/urc_controllers/status_light_controller.hpp index 96b26394..6171af05 100644 --- a/urc_controllers/include/urc_controllers/status_light_controller.hpp +++ b/urc_controllers/include/urc_controllers/status_light_controller.hpp @@ -21,6 +21,7 @@ #include #include "std_msgs/msg/int8.hpp" #include +#include #include #include @@ -62,18 +63,18 @@ class StatusLightController : public controller_interface::ControllerInterface protected: // status_light related std::string status_light_name; - std::shared_ptr> color_command_subscriber_; - std::shared_ptr> display_command_subscriber_; - realtime_tools::RealtimeBuffer color_command_; - realtime_tools::RealtimeBuffer display_command_; + std::shared_ptr> + status_light_command_subscriber_; + realtime_tools::RealtimeBuffer color_command_; + realtime_tools::RealtimeBuffer state_command_; // command interfaces std::unordered_map>> command_interface_map; - const std::vector STATUS_LIGHT_INTERFACES{"color", "display"}; + const std::vector STATUS_LIGHT_INTERFACES{"color", "state"}; }; -} // namespace urc_controllers +} // namespace urc_controllers -#endif // URC_CONTROLLERS__IMU_BROADCASTER_HPP_ +#endif // URC_CONTROLLERS__IMU_BROADCASTER_HPP_ diff --git a/urc_controllers/src/status_light_controller.cpp b/urc_controllers/src/status_light_controller.cpp index a553aae0..7602f6d5 100644 --- a/urc_controllers/src/status_light_controller.cpp +++ b/urc_controllers/src/status_light_controller.cpp @@ -20,10 +20,7 @@ namespace urc_controllers { StatusLightController::StatusLightController() -: color_command_subscriber_(nullptr) - , display_command_subscriber_(nullptr) - , color_command_(0.0) - , display_command_(0.0) {} +: status_light_command_subscriber_(nullptr), color_command_(0), state_command_(0) {} controller_interface::CallbackReturn StatusLightController::on_init() { @@ -51,7 +48,7 @@ const controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_configuration.names.push_back(status_light_name + "/" + "color"); - command_interfaces_configuration.names.push_back(status_light_name + "/" + "display"); + command_interfaces_configuration.names.push_back(status_light_name + "/" + "state"); return command_interfaces_configuration; } @@ -84,15 +81,13 @@ controller_interface::CallbackReturn StatusLightController::on_activate( return controller_interface::CallbackReturn::ERROR; } - color_command_subscriber_ = get_node()->create_subscription( - "/cmd_statuslight_color", rclcpp::SystemDefaultsQoS(), - [this](std::shared_ptr message) { - color_command_.writeFromNonRT(static_cast(message->data)); - }); - display_command_subscriber_ = get_node()->create_subscription( - "/cmd_statuslight_display", rclcpp::SystemDefaultsQoS(), - [this](std::shared_ptr message) { - display_command_.writeFromNonRT(static_cast(message->data)); + status_light_command_subscriber_ = + get_node()->create_subscription( + "/command/status_light", rclcpp::SystemDefaultsQoS(), + [this](std::shared_ptr msg) + { + color_command_.writeFromNonRT(static_cast(msg->color)); + state_command_.writeFromNonRT(static_cast(msg->state)); }); RCLCPP_INFO(get_node()->get_logger(), "StatusLightController activated!"); @@ -102,32 +97,28 @@ controller_interface::CallbackReturn StatusLightController::on_activate( controller_interface::CallbackReturn StatusLightController::on_deactivate( const rclcpp_lifecycle::State &) { - color_command_subscriber_.reset(); - display_command_subscriber_.reset(); + status_light_command_subscriber_.reset(); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn StatusLightController::on_cleanup( const rclcpp_lifecycle::State &) { - color_command_subscriber_.reset(); - display_command_subscriber_.reset(); + status_light_command_subscriber_.reset(); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn StatusLightController::on_error( const rclcpp_lifecycle::State &) { - color_command_subscriber_.reset(); - display_command_subscriber_.reset(); + status_light_command_subscriber_.reset(); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn StatusLightController::on_shutdown( const rclcpp_lifecycle::State &) { - color_command_subscriber_.reset(); - display_command_subscriber_.reset(); + status_light_command_subscriber_.reset(); return controller_interface::CallbackReturn::SUCCESS; } @@ -136,8 +127,8 @@ controller_interface::return_type StatusLightController::update( const rclcpp::Duration &) { command_interface_map["color"]->get().set_value(*color_command_.readFromRT()); - command_interface_map["display"]->get().set_value(*display_command_.readFromRT()); + command_interface_map["state"]->get().set_value(*state_command_.readFromRT()); return controller_interface::return_type::OK; } -} // namespace urc_controllers +} // namespace urc_controllers diff --git a/urc_hw/include/urc_hw/hardware_interfaces/status_light.hpp b/urc_hw/include/urc_hw/hardware_interfaces/status_light.hpp index f541b991..09afdc05 100644 --- a/urc_hw/include/urc_hw/hardware_interfaces/status_light.hpp +++ b/urc_hw/include/urc_hw/hardware_interfaces/status_light.hpp @@ -18,6 +18,19 @@ namespace urc_hardware::hardware_interfaces { +std::string num_to_state(uint8_t num) +{ + switch (num) { + case 0: + return "off"; + case 1: + return "on"; + case 2: + return "blinking"; + default: + return "unknown"; + } +} class StatusLight : public hardware_interface::SystemInterface { @@ -46,7 +59,7 @@ class StatusLight : public hardware_interface::SystemInterface // basic info const std::string hardware_interface_name; // states - std::vector signals; // [0]: color choice, [1]: display mode(e.g. flashing / idle / double flashing) + std::vector signals; // [0]: color selection, [1]: state (on, off, blinking) // hardware resources std::shared_ptr> udp_; @@ -57,11 +70,9 @@ class StatusLight : public hardware_interface::SystemInterface uint8_t buffer[TeensyMessage_size]; size_t message_length; - // private info for lights - uint8_t currentLight = 0; - uint8_t lightModes[3]; // current pattern for each of 3 lights + uint8_t lightStates[3]; // current state of each light (red, green, blue). }; -} // namespace urc_hardware::hardware_interfaces +} // namespace urc_hardware::hardware_interfaces -#endif // URC_HW__STATUS_LIGHT_HW_INTERFACE_HPP +#endif // URC_HW__STATUS_LIGHT_HW_INTERFACE_HPP diff --git a/urc_hw/src/hardware_interfaces/status_light.cpp b/urc_hw/src/hardware_interfaces/status_light.cpp index 5a4a8fe2..50667202 100644 --- a/urc_hw/src/hardware_interfaces/status_light.cpp +++ b/urc_hw/src/hardware_interfaces/status_light.cpp @@ -54,7 +54,8 @@ hardware_interface::CallbackReturn StatusLight::on_init( if (info.gpios.size() == 0) { RCLCPP_ERROR( rclcpp::get_logger( - hardware_interface_name), "Should have at least one gpio to be the Status Light."); + hardware_interface_name), + "Should have at least one gpio to be the Status Light."); return CallbackReturn::ERROR; } @@ -69,9 +70,14 @@ hardware_interface::CallbackReturn StatusLight::on_init( if (!find_light) { RCLCPP_ERROR( rclcpp::get_logger( - hardware_interface_name), "Not able to find sensor named 'status_light'."); + hardware_interface_name), + "Not able to find sensor named 'status_light'."); return CallbackReturn::ERROR; } + + RCLCPP_INFO( + rclcpp::get_logger(hardware_interface_name), + "Status light HW interface initialized!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -85,7 +91,7 @@ std::vector StatusLight::export_command_in { std::vector command_interfaces; command_interfaces.emplace_back("status_light", "color", &this->signals[0]); - command_interfaces.emplace_back("status_light", "display", &this->signals[1]); + command_interfaces.emplace_back("status_light", "state", &this->signals[1]); return command_interfaces; } @@ -99,19 +105,25 @@ hardware_interface::CallbackReturn StatusLight::on_activate(const rclcpp_lifecyc { udp_ = std::make_shared>(true); udp_->Connect(udp_address, std::stoi(udp_port)); - RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "StatusLight activated!"); + RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "Status light activated!"); return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn StatusLight::on_deactivate(const rclcpp_lifecycle::State &) { udp_->Close(); - RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "StatusLight deactivated!"); + RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "Status light deactivated!"); return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::return_type StatusLight::read(const rclcpp::Time &, const rclcpp::Duration &) { + RCLCPP_INFO( + rclcpp::get_logger( + hardware_interface_name), "Red: %s, Green: %s, Blue: %s", num_to_state( + lightStates[0]).c_str(), num_to_state(lightStates[1]).c_str(), num_to_state( + lightStates[2]).c_str()); + return hardware_interface::return_type::OK; } @@ -121,17 +133,18 @@ hardware_interface::return_type StatusLight::write(const rclcpp::Time &, const r TeensyMessage message = TeensyMessage_init_zero; pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer)); - currentLight = signals[0]; - if (currentLight >= 0 && currentLight < 3 && signals[1] >= 0 && signals[1] < 3) { - lightModes[currentLight] = signals[1]; + uint8_t selected_color = static_cast(signals[0]); + uint8_t selected_state = static_cast(signals[1]); + if (selected_color >= 0 && selected_color <= 2 && selected_state >= 0 && selected_state <= 2) { + lightStates[selected_color] = selected_state; } - message.redEnabled = (lightModes[0] != 0); - message.redBlink = (lightModes[0] == 2); - message.blueEnabled = (lightModes[1] != 0); - message.blueBlink = (lightModes[1] == 2); - message.greenEnabled = (lightModes[2] != 0); - message.greenBlink = (lightModes[2] == 2); + message.redEnabled = (lightStates[0] != 0); + message.redBlink = (lightStates[0] == 2); + message.greenEnabled = (lightStates[1] != 0); + message.greenBlink = (lightStates[1] == 2); + message.blueEnabled = (lightStates[2] != 0); + message.blueBlink = (lightStates[2] == 2); // Fill Required Fields message.m1Setpoint = 0; @@ -141,6 +154,7 @@ hardware_interface::return_type StatusLight::write(const rclcpp::Time &, const r message.m5Setpoint = 0; message.m6Setpoint = 0; + // Set message id to status light message.messageID = 1; bool status = pb_encode(&stream, TeensyMessage_fields, &message); @@ -153,4 +167,4 @@ hardware_interface::return_type StatusLight::write(const rclcpp::Time &, const r return hardware_interface::return_type::OK; } -} // namespace urc_hardware::hardware_interfaces +} // namespace urc_hardware::hardware_interfaces diff --git a/urc_hw_description/urdf/ros2_control.xacro b/urc_hw_description/urdf/ros2_control.xacro index 1e826fd0..b4318f31 100644 --- a/urc_hw_description/urdf/ros2_control.xacro +++ b/urc_hw_description/urdf/ros2_control.xacro @@ -175,7 +175,7 @@ - + @@ -224,4 +224,4 @@ - + \ No newline at end of file diff --git a/urc_msgs/CMakeLists.txt b/urc_msgs/CMakeLists.txt index 788b69c2..0f8a8901 100644 --- a/urc_msgs/CMakeLists.txt +++ b/urc_msgs/CMakeLists.txt @@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BatteryInfo.msg" "msg/Waypoint.msg" "msg/NavigationStatus.msg" + "msg/StatusLightCommand.msg" "msg/GridLocation.msg" "srv/GeneratePlan.srv" "action/FollowPath.action" diff --git a/urc_msgs/msg/StatusLightCommand.msg b/urc_msgs/msg/StatusLightCommand.msg new file mode 100644 index 00000000..ede94b93 --- /dev/null +++ b/urc_msgs/msg/StatusLightCommand.msg @@ -0,0 +1,10 @@ +uint8 RED=0 +uint8 GREEN=1 +uint8 BLUE=2 + +uint8 OFF=0 +uint8 ON=1 +uint8 BLINK=2 + +uint8 color +uint8 state \ No newline at end of file From e9776738549e3f3833cca745a0eeb5aae874d119 Mon Sep 17 00:00:00 2001 From: Yashas Ambati <32080306+yambati03@users.noreply.github.com> Date: Sat, 1 Jun 2024 14:56:38 -0600 Subject: [PATCH 2/2] [following] add lethal cost threshold config (#200) * Add lethal cost threshold config * Format --- .../trajectory_following/config/pure_pursuit_config.yaml | 1 + .../trajectory_following/src/follower_action_server.cpp | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml b/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml index 1c06c810..aab639fc 100644 --- a/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml +++ b/urc_navigation/trajectory_following/config/pure_pursuit_config.yaml @@ -2,6 +2,7 @@ follower_action_server: ros__parameters: lookahead_distance: 0.9 desired_linear_velocity: 0.5 + lethal_cost_threshold: 10 cmd_vel_stamped: true cmd_vel_topic: "/rover_drivetrain_controller/cmd_vel" odom_topic: "/rover_drivetrain_controller/odom" diff --git a/urc_navigation/trajectory_following/src/follower_action_server.cpp b/urc_navigation/trajectory_following/src/follower_action_server.cpp index 0d1f3433..d2f763d0 100644 --- a/urc_navigation/trajectory_following/src/follower_action_server.cpp +++ b/urc_navigation/trajectory_following/src/follower_action_server.cpp @@ -19,6 +19,7 @@ FollowerActionServer::FollowerActionServer(const rclcpp::NodeOptions & options) declare_parameter("map_frame", "map"); declare_parameter("goal_tolerance", 0.1); declare_parameter("cmd_vel_stamped", false); + declare_parameter("lethal_cost_threshold", 50); tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -219,7 +220,7 @@ void FollowerActionServer::execute( break; } else if (getCost( current_costmap_, output.lookahead_point.point.x, - output.lookahead_point.point.y) > 0) + output.lookahead_point.point.y) > get_parameter("lethal_cost_threshold").as_int()) { result->error_code = urc_msgs::action::FollowPath::Result::OBSTACLE_DETECTED; goal_handle->abort(result);