Skip to content

Commit

Permalink
Fix Code Style On base_station_poc (#2192)
Browse files Browse the repository at this point in the history
automated style fixes

Co-authored-by: N8BWert <[email protected]>
  • Loading branch information
github-actions[bot] and N8BWert authored Feb 19, 2024
1 parent 7dff367 commit 755cb27
Show file tree
Hide file tree
Showing 6 changed files with 41 additions and 46 deletions.
20 changes: 8 additions & 12 deletions soccer/src/soccer/radio/network_radio.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,9 +16,7 @@ using ip::udp;

namespace radio {

NetworkRadio::NetworkRadio()
: base_station_socket_(io_service_),
send_buffers_(kNumShells) {
NetworkRadio::NetworkRadio() : base_station_socket_(io_service_), send_buffers_(kNumShells) {
base_station_socket_.open(udp::v4());
base_station_socket_.bind(udp::endpoint(udp::v4(), kBaseStationBindPort));

Expand All @@ -30,20 +28,19 @@ void NetworkRadio::start_receive() {
boost::asio::buffer(robot_status_buffer_), robot_status_endpoint_,
[this](const boost::system::error_code& error, size_t num_bytes) {
receive_robot_status(error, num_bytes);
}
);
});

base_station_socket_.async_receive_from(
boost::asio::buffer(alive_robots_buffer_), alive_robots_endpoint_,
[this](const boost::system::error_code& error, size_t num_bytes) {
receive_alive_robots(error, num_bytes);
}
);
});
}

void NetworkRadio::send_control_message(uint8_t robot_id, const rj_msgs::msg::MotionSetpoint& motion,
const rj_msgs::msg::ManipulatorSetpoint& manipulator,
strategy::Positions role) {
void NetworkRadio::send_control_message(uint8_t robot_id,
const rj_msgs::msg::MotionSetpoint& motion,
const rj_msgs::msg::ManipulatorSetpoint& manipulator,
strategy::Positions role) {
// Build the control packet for this robot.
std::array<uint8_t, sizeof(rtp::ControlMessage)>& forward_packet_buffer =
send_buffers_[robot_id];
Expand All @@ -59,8 +56,7 @@ void NetworkRadio::send_control_message(uint8_t robot_id, const rj_msgs::msg::Mo
if (static_cast<bool>(error)) {
SPDLOG_ERROR("Error Sending: {}", error.message());

Check warning on line 57 in soccer/src/soccer/radio/network_radio.cpp

View workflow job for this annotation

GitHub Actions / build-and-test

inside a lambda, '__FUNCTION__' expands to the name of the function call operator; consider capturing the name of the enclosing function explicitly
}
}
);
});
}

void NetworkRadio::poll_receive() {
Expand Down
20 changes: 10 additions & 10 deletions soccer/src/soccer/radio/network_radio.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,9 +29,9 @@ class NetworkRadio : public Radio {
protected:
// Send Control Message through the Base Station to the Robots
void send_control_message(uint8_t robot_id, const rj_msgs::msg::MotionSetpoint& motion,
const rj_msgs::msg::ManipulatorSetpoint& manipulator,
strategy::Positions role) override;
const rj_msgs::msg::ManipulatorSetpoint& manipulator,
strategy::Positions role) override;

// Poll the asynchronous boost::asio receiver
void poll_receive() override;

Expand All @@ -42,23 +42,23 @@ class NetworkRadio : public Radio {
/**
* @brief Setup the base_station_socket_ to receive robot status and alive robot messages
* from the base station
*
*
*/
void start_receive();

/**
* @brief Parse the alive robots from a packet received via the base station.
*
* @param error
* @param num_bytes
*
* @param error
* @param num_bytes
*/
void receive_robot_status(const boost::system::error_code& error, size_t num_bytes);

/**
* @brief Parse the alive robots from a packet received via the base station.
*
* @param error
* @param num_bytes
*
* @param error
* @param num_bytes
*/
void receive_alive_robots(const boost::system::error_code& error, size_t num_bytes);

Expand Down
4 changes: 2 additions & 2 deletions soccer/src/soccer/radio/radio.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,8 @@ Radio::Radio()
});
}

alive_robots_pub_ = create_publisher<rj_msgs::msg::AliveRobots>(
"/alive_robots", rclcpp::QoS(1));
alive_robots_pub_ =
create_publisher<rj_msgs::msg::AliveRobots>("/alive_robots", rclcpp::QoS(1));

tick_timer_ = create_wall_timer(std::chrono::milliseconds(100), [this]() { tick(); });
}
Expand Down
31 changes: 15 additions & 16 deletions soccer/src/soccer/radio/radio.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,13 @@
#include <rclcpp/rclcpp.hpp>

#include <rj_constants/topic_names.hpp>
#include <rj_msgs/msg/alive_robots.hpp>
#include <rj_msgs/msg/manipulator_setpoint.hpp>
#include <rj_msgs/msg/motion_setpoint.hpp>
#include <rj_msgs/msg/robot_status.hpp>
#include <rj_msgs/msg/team_color.hpp>
#include <rj_param_utils/param.hpp>
#include <rj_param_utils/ros2_local_param_provider.hpp>
#include <rj_msgs/msg/alive_robots.hpp>

#include "robot_intent.hpp"
#include "robot_status.hpp"
Expand All @@ -29,7 +29,7 @@ DECLARE_FLOAT64(kRadioParamModule, timeout);
* @details This is the abstract superclass for NetworkRadio and SimRadio, which do
* the actual work - this just declares the interface and handles sending stop commands when no new
* commands come in for a while.
*
*
* The radio should handle:
* 1. Sending Control Messages to the Robots
* * Alive Robots should be receiving real commands
Expand All @@ -44,52 +44,51 @@ class Radio : public rclcpp::Node {
protected:
/**
* @brief Send a control message to the corresponding robot.
*
*
* @param robot_id The robot to send to
* @param motion The (x (m/s), y (m/s), z (rad/s)) velocities for the robot to move at
* @param motion The (x (m/s), y (m/s), z (rad/s)) velocities for the robot to move at
* @param manipulator The Shoot Mode, Trigger Mode, Kick Speed, and Dribbler Speed for the Robot
* @param role The position for the robot
*/
virtual void send_control_message(uint8_t robot_id, const rj_msgs::msg::MotionSetpoint& motion,
const rj_msgs::msg::ManipulatorSetpoint& manipulator,
strategy::Positions role) = 0;
const rj_msgs::msg::ManipulatorSetpoint& manipulator,
strategy::Positions role) = 0;

/**
* @brief Poll the receiver service for Messages.
*
*
*/
virtual void poll_receive() = 0;

/**
* @brief Switch the team this radio is sending data to.
*
* @param blue_team
*
* @param blue_team
*/
virtual void switch_team(bool blue_team) = 0;

/**
* @brief Wrapper over the local publisher to publish a robot status for a given robot
*
*
* @param robot_id The robot id of the robot, whose status to publish
* @param robot_status The status of the robot
*/
void publish_robot_status(int robot_id, const rj_msgs::msg::RobotStatus& robot_status);

/**
* @brief Wrapper over the private publisher to publish a message containing alive robots
*
*
* @param alive_robots A message containing the alive robots
*/
void publish_alive_robots(const rj_msgs::msg::AliveRobots& alive_robots);

const bool blue_team();


private:
/**
* @brief Poll the receiver and send empty motion commands to robots that software has
* not updated for a long time.
*
*
*/
void tick();
// Time between consecutive calls to tick().
Expand All @@ -106,7 +105,7 @@ class Radio : public rclcpp::Node {

// Ros publisher to update alive robots
rclcpp::Publisher<rj_msgs::msg::AliveRobots>::SharedPtr alive_robots_pub_;

// Ros subscribers to receive velocity commands, which are sent to the robot
std::array<rclcpp::Subscription<rj_msgs::msg::MotionSetpoint>::SharedPtr, kNumShells>
motion_subs_;
Expand All @@ -115,8 +114,8 @@ class Radio : public rclcpp::Node {
// Cached last velocity command
std::array<rj_msgs::msg::MotionSetpoint::SharedPtr, kNumShells> motions_;

// Ros subscribers to receive auxillary control (i.e. shoot_mode, trigger_mode, kick_speed, and dribbler_speed)
// which are stored and sent to the robot
// Ros subscribers to receive auxillary control (i.e. shoot_mode, trigger_mode, kick_speed, and
// dribbler_speed) which are stored and sent to the robot
std::array<rclcpp::Subscription<rj_msgs::msg::ManipulatorSetpoint>::SharedPtr, kNumShells>
manipulator_subs_;
// Cached auxillary control information
Expand Down
6 changes: 3 additions & 3 deletions soccer/src/soccer/radio/sim_radio.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ SimRadio::SimRadio(bool blue_team)
alive_message.alive_robots = alive_robots_;
publish_alive_robots(alive_message);
});

for (uint8_t robot_id = 0; robot_id < kNumShells; robot_id++) {
alive_robots_[robot_id] = true;
}
Expand All @@ -120,8 +120,8 @@ SimRadio::SimRadio(bool blue_team)
}

void SimRadio::send_control_message(uint8_t robot_id, const rj_msgs::msg::MotionSetpoint& motion,
const rj_msgs::msg::ManipulatorSetpoint& manipulator,
strategy::Positions role) {
const rj_msgs::msg::ManipulatorSetpoint& manipulator,
strategy::Positions role) {

Check warning on line 124 in soccer/src/soccer/radio/sim_radio.cpp

View workflow job for this annotation

GitHub Actions / build-and-test

parameter 'role' is unused
RobotControl sim_packet;

// Send a sim packet with a single robot. The simulator can handle many robots, but our commands
Expand Down
6 changes: 3 additions & 3 deletions soccer/src/soccer/radio/sim_radio.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,10 +22,10 @@ class SimRadio : public Radio {
SimRadio(bool blue_team = false);

protected:
//
//
void send_control_message(uint8_t robot_id, const rj_msgs::msg::MotionSetpoint& motion,
const rj_msgs::msg::ManipulatorSetpoint& manipulator,
strategy::Positions role) override;
const rj_msgs::msg::ManipulatorSetpoint& manipulator,
strategy::Positions role) override;

// Poll the asynchronous receiver
void poll_receive() override;
Expand Down

0 comments on commit 755cb27

Please sign in to comment.