Skip to content

Commit

Permalink
Format
Browse files Browse the repository at this point in the history
  • Loading branch information
@yambati03
yambati03 committed May 30, 2024
1 parent 70c7cd1 commit 43ee7cb
Show file tree
Hide file tree
Showing 6 changed files with 351 additions and 365 deletions.
86 changes: 40 additions & 46 deletions urc_bt_nodes/include/urc_bt_nodes/actions/status_light_publisher.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,59 +14,53 @@
namespace behavior::actions
{

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 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;
}

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 color!");
}

throw std::invalid_argument("Invalid state!");
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;
}

class StatusLightPublisher : public BT::RosTopicPubNode<urc_msgs::msg::StatusLightCommand>
{
public:
StatusLightPublisher(
const std::string &instance_name, const BT::NodeConfig &conf,
const BT::RosNodeParams &params)
: BT::RosTopicPubNode<urc_msgs::msg::StatusLightCommand>(instance_name, conf, params) {}
throw std::invalid_argument("Invalid state!");
}

static BT::PortsList providedPorts()
{
return providedBasicPorts(
{BT::InputPort<std::string>("color"),
BT::InputPort<std::string>("state")});
}
class StatusLightPublisher : public BT::RosTopicPubNode<urc_msgs::msg::StatusLightCommand>
{
public:
StatusLightPublisher(
const std::string & instance_name, const BT::NodeConfig & conf,
const BT::RosNodeParams & params)
: BT::RosTopicPubNode<urc_msgs::msg::StatusLightCommand>(instance_name, conf, params) {}

static BT::PortsList providedPorts()
{
return providedBasicPorts(
{BT::InputPort<std::string>("color"),
BT::InputPort<std::string>("state")});
}

bool setMessage(urc_msgs::msg::StatusLightCommand &msg) override;
};
bool setMessage(urc_msgs::msg::StatusLightCommand & msg) override;
};
}

#endif
29 changes: 13 additions & 16 deletions urc_bt_nodes/src/actions/status_light_publisher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,24 +4,21 @@

namespace behavior::actions
{
bool StatusLightPublisher::setMessage(urc_msgs::msg::StatusLightCommand &msg)
{
std::string color = getInput<std::string>("color").value();
std::string state = getInput<std::string>("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;
}
bool StatusLightPublisher::setMessage(urc_msgs::msg::StatusLightCommand & msg)
{
std::string color = getInput<std::string>("color").value();
std::string state = getInput<std::string>("state").value();

return true;
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;
}
}

#include "behaviortree_ros2/plugins.hpp"
Expand Down
69 changes: 35 additions & 34 deletions urc_controllers/include/urc_controllers/status_light_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,51 +28,52 @@
namespace urc_controllers
{

class StatusLightController : public controller_interface::ControllerInterface
{
public:
StatusLightController();
class StatusLightController : public controller_interface::ControllerInterface
{
public:
StatusLightController();

controller_interface::InterfaceConfiguration command_interface_configuration() const override;
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
controller_interface::InterfaceConfiguration state_interface_configuration() const override;

controller_interface::return_type update(
const rclcpp::Time &time,
const rclcpp::Duration &period) override;
controller_interface::return_type update(
const rclcpp::Time & time,
const rclcpp::Duration & period) override;

controller_interface::CallbackReturn on_init() override;
controller_interface::CallbackReturn on_init() override;

controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state)
override;
controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state)
override;

controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state)
override;
controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state)
override;

controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state)
override;
controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state)
override;

controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &previous_state)
override;
controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state)
override;

controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State &previous_state)
override;
controller_interface::CallbackReturn on_error(const rclcpp_lifecycle::State & previous_state)
override;

controller_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &previous_state)
override;
controller_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state)
override;

protected:
// status_light related
std::string status_light_name;
std::shared_ptr<rclcpp::Subscription<urc_msgs::msg::StatusLightCommand>> status_light_command_subscriber_;
realtime_tools::RealtimeBuffer<uint8_t> color_command_;
realtime_tools::RealtimeBuffer<uint8_t> state_command_;
protected:
// status_light related
std::string status_light_name;
std::shared_ptr<rclcpp::Subscription<urc_msgs::msg::StatusLightCommand>>
status_light_command_subscriber_;
realtime_tools::RealtimeBuffer<uint8_t> color_command_;
realtime_tools::RealtimeBuffer<uint8_t> state_command_;

// command interfaces
std::unordered_map<std::string,
std::shared_ptr<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>>
command_interface_map;
const std::vector<std::string> STATUS_LIGHT_INTERFACES{"color", "state"};
};
// command interfaces
std::unordered_map<std::string,
std::shared_ptr<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>>
command_interface_map;
const std::vector<std::string> STATUS_LIGHT_INTERFACES{"color", "state"};
};

} // namespace urc_controllers

Expand Down
Loading

0 comments on commit 43ee7cb

Please sign in to comment.