Skip to content

Commit

Permalink
added code for receiving on udp
Browse files Browse the repository at this point in the history
  • Loading branch information
@davidcalderon03
davidcalderon03 committed Nov 6, 2024
1 parent b726cce commit 68a8fae
Show file tree
Hide file tree
Showing 4 changed files with 98 additions and 69 deletions.
1 change: 1 addition & 0 deletions external/vectornav
Submodule vectornav added at 2859fe
1 change: 0 additions & 1 deletion urc_hw/include/async_sockets/udpsocket.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,7 +133,6 @@ class UDPSocket : public BaseSocket
this->Connect(host.c_str(), port, onError);
}

private:
static void Receive(UDPSocket * udpSocket)
{
char tempBuffer[BUFFER_SIZE + 1];
Expand Down
150 changes: 82 additions & 68 deletions urc_hw/src/hardware_interfaces/rover_drivetrain.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,59 +1,54 @@
#include "urc_hw/hardware_interfaces/rover_drivetrain.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "pluginlib/class_list_macros.hpp"
#include <cstddef>
#include <cstdint>
#include <arpa/inet.h>
#include <cstdio>
#include <iostream>
#include <iterator>
#include <ostream>
#include <netinet/in.h>
#include <pb.h>
#include <pb_encode.h>
#include <pb_decode.h>
#include <pb_encode.h>
#include <rclcpp/clock.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/time.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <string>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
#include <urc_nanopb/urc.pb.h>

PLUGINLIB_EXPORT_CLASS(
urc_hardware::hardware_interfaces::RoverDrivetrain,
hardware_interface::SystemInterface);
PLUGINLIB_EXPORT_CLASS(urc_hardware::hardware_interfaces::RoverDrivetrain,
hardware_interface::SystemInterface);

namespace urc_hardware::hardware_interfaces
{
namespace urc_hardware::hardware_interfaces {

RoverDrivetrain::RoverDrivetrain()
: hardware_interface_name("Rover Drivetrain")
, velocity_rps_commands(2, 0)
, velocity_rps_states(2, 0)
, velocity_rps_breakdown(6, 0)
{
}
: hardware_interface_name("Rover Drivetrain"), velocity_rps_commands(2, 0),
velocity_rps_states(2, 0), velocity_rps_breakdown(6, 0) {}
RoverDrivetrain::~RoverDrivetrain() = default;

hardware_interface::CallbackReturn RoverDrivetrain::on_init(
const hardware_interface::HardwareInfo & info)
{
hardware_interface::CallbackReturn
RoverDrivetrain::on_init(const hardware_interface::HardwareInfo &info) {
if (hardware_interface::SystemInterface::on_init(info) !=
hardware_interface::CallbackReturn::SUCCESS)
{
hardware_interface::CallbackReturn::SUCCESS) {
return hardware_interface::CallbackReturn::ERROR;
}

if (info_.hardware_parameters.find("udp_address") == info_.hardware_parameters.end()) {
RCLCPP_ERROR(
rclcpp::get_logger(hardware_interface_name),
"Error during initialization: 'udp_address' configuration not "
"found. Expect to enter the udp server address.");
if (info_.hardware_parameters.find("udp_address") ==
info_.hardware_parameters.end()) {
RCLCPP_ERROR(rclcpp::get_logger(hardware_interface_name),
"Error during initialization: 'udp_address' configuration not "
"found. Expect to enter the udp server address.");
return hardware_interface::CallbackReturn::ERROR;
}
if (info_.hardware_parameters.find("udp_port") == info_.hardware_parameters.end()) {
RCLCPP_ERROR(
rclcpp::get_logger(hardware_interface_name),
"Error during initialization: 'udp_port' configuration not "
"found. Expect to enter the port number.");
if (info_.hardware_parameters.find("udp_port") ==
info_.hardware_parameters.end()) {
RCLCPP_ERROR(rclcpp::get_logger(hardware_interface_name),
"Error during initialization: 'udp_port' configuration not "
"found. Expect to enter the port number.");
return hardware_interface::CallbackReturn::ERROR;
}

Expand All @@ -63,66 +58,86 @@ hardware_interface::CallbackReturn RoverDrivetrain::on_init(
return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn RoverDrivetrain::on_configure(const rclcpp_lifecycle::State &)
{
hardware_interface::CallbackReturn
RoverDrivetrain::on_configure(const rclcpp_lifecycle::State &) {
std::fill(velocity_rps_commands.begin(), velocity_rps_commands.end(), 0.0);
std::fill(velocity_rps_states.begin(), velocity_rps_states.end(), 0.0);
std::fill(velocity_rps_breakdown.begin(), velocity_rps_breakdown.end(), 0.0);
return hardware_interface::CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::CommandInterface> RoverDrivetrain::export_command_interfaces()
{
std::vector<hardware_interface::CommandInterface>
RoverDrivetrain::export_command_interfaces() {
std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.emplace_back("left_wheel", "velocity", &this->velocity_rps_commands[0]);
command_interfaces.emplace_back("right_wheel", "velocity", &this->velocity_rps_commands[1]);
command_interfaces.emplace_back("left_wheel", "velocity",
&this->velocity_rps_commands[0]);
command_interfaces.emplace_back("right_wheel", "velocity",
&this->velocity_rps_commands[1]);
return command_interfaces;
}

std::vector<hardware_interface::StateInterface> RoverDrivetrain::export_state_interfaces()
{
std::vector<hardware_interface::StateInterface>
RoverDrivetrain::export_state_interfaces() {
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.emplace_back("left_wheel", "velocity", &this->velocity_rps_states[0]);
state_interfaces.emplace_back("right_wheel", "velocity", &this->velocity_rps_states[1]);
state_interfaces.emplace_back("left_wheel", "velocity.front", &this->velocity_rps_breakdown[0]);
state_interfaces.emplace_back("left_wheel", "velocity.mid", &this->velocity_rps_breakdown[1]);
state_interfaces.emplace_back("left_wheel", "velocity.back", &this->velocity_rps_breakdown[2]);
state_interfaces.emplace_back("right_wheel", "velocity.front", &this->velocity_rps_breakdown[3]);
state_interfaces.emplace_back("right_wheel", "velocity.mid", &this->velocity_rps_breakdown[4]);
state_interfaces.emplace_back("right_wheel", "velocity.back", &this->velocity_rps_breakdown[5]);
state_interfaces.emplace_back("left_wheel", "velocity",
&this->velocity_rps_states[0]);
state_interfaces.emplace_back("right_wheel", "velocity",
&this->velocity_rps_states[1]);
state_interfaces.emplace_back("left_wheel", "velocity.front",
&this->velocity_rps_breakdown[0]);
state_interfaces.emplace_back("left_wheel", "velocity.mid",
&this->velocity_rps_breakdown[1]);
state_interfaces.emplace_back("left_wheel", "velocity.back",
&this->velocity_rps_breakdown[2]);
state_interfaces.emplace_back("right_wheel", "velocity.front",
&this->velocity_rps_breakdown[3]);
state_interfaces.emplace_back("right_wheel", "velocity.mid",
&this->velocity_rps_breakdown[4]);
state_interfaces.emplace_back("right_wheel", "velocity.back",
&this->velocity_rps_breakdown[5]);
return state_interfaces;
}

hardware_interface::CallbackReturn RoverDrivetrain::on_activate(const rclcpp_lifecycle::State &)
{
hardware_interface::CallbackReturn
RoverDrivetrain::on_activate(const rclcpp_lifecycle::State &) {
udp_ = std::make_shared<UDPSocket<128>>(true);
udp_->Connect(udp_address, std::stoi(udp_port));
RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "Rover Drivetrain activated!");
RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name),
"Rover Drivetrain activated!");
return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn RoverDrivetrain::on_deactivate(const rclcpp_lifecycle::State &)
{
hardware_interface::CallbackReturn
RoverDrivetrain::on_deactivate(const rclcpp_lifecycle::State &) {
udp_->Close();
RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name), "Rover Drivetrain deactivated!");
RCLCPP_INFO(rclcpp::get_logger(hardware_interface_name),
"Rover Drivetrain deactivated!");
return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::return_type RoverDrivetrain::read(
const rclcpp::Time &,
const rclcpp::Duration &)
{

// RCLCPP_INFO(rclcpp::get_logger("test"),
// "%.5f, %.5f", velocity_rps_commands[0], velocity_rps_commands[1]);

hardware_interface::return_type
RoverDrivetrain::read(const rclcpp::Time &, const rclcpp::Duration &) {
int sockfd;
char bufferRecv[256];
memset(bufferRecv, '\0', sizeof(bufferRecv));
if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
RCLCPP_ERROR(rclcpp::get_logger(hardware_interface_name),
"Cannot create socket.");
return hardware_interface::return_type::ERROR;
}
struct sockaddr_in servAddr, cliAddr;
socklen_t cLen = sizeof(cliAddr);
servAddr.sin_family = AF_INET;
servAddr.sin_port = htons(8443);
servAddr.sin_addr.s_addr = inet_addr("192.168.1.113");
ssize_t rByte = recvfrom(sockfd, bufferRecv, sizeof(bufferRecv), 0,
(struct sockaddr *)&cliAddr, &cLen);
return hardware_interface::return_type::OK;
}

hardware_interface::return_type RoverDrivetrain::write(
const rclcpp::Time & time,
const rclcpp::Duration & duration)
{
hardware_interface::return_type
RoverDrivetrain::write(const rclcpp::Time &time,
const rclcpp::Duration &duration) {
if (duration.seconds() < 0.001) {
return hardware_interface::return_type::OK;
}
Expand All @@ -139,7 +154,6 @@ hardware_interface::return_type RoverDrivetrain::write(
message.m5Setpoint = velocity_rps_commands[1] * ENCODER_CPR * -1;
message.m6Setpoint = velocity_rps_commands[1] * ENCODER_CPR * -1;


// Fill Required Fields
message.redEnabled = 0;
message.blueEnabled = 0;
Expand All @@ -161,4 +175,4 @@ hardware_interface::return_type RoverDrivetrain::write(
return hardware_interface::return_type::OK;
}

} // namespace urc_hardware::hardware_interfaces
} // namespace urc_hardware::hardware_interfaces
15 changes: 15 additions & 0 deletions urc_nanopb/proto/urc.proto
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,21 @@ message RequestMessage {
required int32 timestamp = 6;
}

message DrivetrainResponse {
required int32 m1Feedback = 1;
required uint32 m1Current = 2;
required int32 m2Feedback = 3;
required uint32 m2Current = 4;
required int32 m3Feedback = 5;
required uint32 m3Current = 6;
required int32 m4Feedback = 7;
required uint32 m4Current = 8;
required int32 m5Feedback = 9;
required uint32 m5Current = 10;
required int32 m6Feedback = 11;
required uint32 m6Current = 12;
}

message IMUMessage {
required int32 timestamp = 1;

Expand Down

0 comments on commit 68a8fae

Please sign in to comment.