diff --git a/SubjuGator/command/subjugator_launch/launch/subsystems/nav_box.launch b/SubjuGator/command/subjugator_launch/launch/subsystems/nav_box.launch index fe18b292d..9cfddd633 100644 --- a/SubjuGator/command/subjugator_launch/launch/subsystems/nav_box.launch +++ b/SubjuGator/command/subjugator_launch/launch/subsystems/nav_box.launch @@ -17,10 +17,11 @@ - - - - + + + + + diff --git a/SubjuGator/command/subjugator_launch/package.xml b/SubjuGator/command/subjugator_launch/package.xml index e3f6ef4ba..3244541cd 100644 --- a/SubjuGator/command/subjugator_launch/package.xml +++ b/SubjuGator/command/subjugator_launch/package.xml @@ -31,6 +31,7 @@ mil_passive_sonar mil_pneumatic_actuator mil_blueview_driver + nav_tube_driver subjugator_perception yolov7_ros catkin diff --git a/SubjuGator/drivers/nav_tube_driver/CMakeLists.txt b/SubjuGator/drivers/nav_tube_driver/CMakeLists.txt new file mode 100644 index 000000000..b4ae30d54 --- /dev/null +++ b/SubjuGator/drivers/nav_tube_driver/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.0.2) +project(nav_tube_driver) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + mil_msgs + nodelet + roscpp +) +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES nav_tube_driver +# CATKIN_DEPENDS mil_msgs nodelet roscpp +# DEPENDS system_lib +) +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_node src/nav_tube_driver.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +install(TARGETS ${PROJECT_NAME}_node + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/SubjuGator/drivers/nav_tube_driver/package.xml b/SubjuGator/drivers/nav_tube_driver/package.xml new file mode 100644 index 000000000..a08cb5772 --- /dev/null +++ b/SubjuGator/drivers/nav_tube_driver/package.xml @@ -0,0 +1,15 @@ + + + nav_tube_driver + 0.5.0 + nav_tube_driver + andrew + MIT + catkin + mil_msgs + roscpp + mil_msgs + roscpp + mil_msgs + roscpp + diff --git a/SubjuGator/drivers/nav_tube_driver/src/nav_tube_driver.cpp b/SubjuGator/drivers/nav_tube_driver/src/nav_tube_driver.cpp new file mode 100644 index 000000000..5451370f3 --- /dev/null +++ b/SubjuGator/drivers/nav_tube_driver/src/nav_tube_driver.cpp @@ -0,0 +1,220 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +// modeled after mil_passive_sonar/sylphase_ros_bridge + +using tcp = boost::asio::ip::tcp; + +class NavTubeDriver +{ +private: + ros::NodeHandle nh_; + ros::NodeHandle private_nh_; + + ros::Publisher pub_; + + std::string ip_; + int port_; + std::string frame_id_; + uint16_t hz_; + + uint64_t acceptable_frequency; + + ros::Time prev; + + std::thread timer_thread; + + std::mutex m; + + bool running = true; + + bool initialized = false; + + static const uint8_t sync1 = 0x37; + static const uint8_t sync2 = 0x01; + + uint8_t heartbeat_packet[2 + sizeof(hz_)]; + + boost::shared_ptr connect(); + + void send_heartbeat(boost::shared_ptr socket); + + void read_messages(boost::shared_ptr socket); + + double calculate_pressure(uint16_t analog_input); + +public: + NavTubeDriver(ros::NodeHandle nh, ros::NodeHandle private_nh); + + ~NavTubeDriver(); + + void run(); +}; + +NavTubeDriver::NavTubeDriver(ros::NodeHandle nh, ros::NodeHandle private_nh) : nh_(nh), private_nh_(private_nh) +{ + pub_ = nh.advertise("depth", 10); + ip_ = private_nh.param("ip", std::string("192.168.37.61")); + port_ = private_nh.param("port", 33056); + frame_id_ = private_nh.param("frame_id", "/depth"); + + int hz__ = private_nh.param("hz", 20); + + if (hz__ > std::numeric_limits::max()) + { + ROS_WARN_STREAM("Depth polling frequency is greater than 16 bits!"); + } + + hz_ = hz__; + + acceptable_frequency = (1'000'000'000 * 1.25) / (uint64_t)hz_; + + heartbeat_packet[0] = sync1; + heartbeat_packet[1] = sync2; + uint16_t nw_ordering = htons(hz_); + reinterpret_cast(&heartbeat_packet[2])[0] = nw_ordering; +} + +NavTubeDriver::~NavTubeDriver() +{ + { + std::lock_guard lock(m); + running = false; + } + timer_thread.join(); +} + +boost::shared_ptr NavTubeDriver::connect() +{ + using ip_address = boost::asio::ip::address; + tcp::endpoint endpoint(ip_address::from_string(ip_), port_); + + ROS_INFO_STREAM("Connecting to Depth Server"); + boost::asio::io_service io_service; + boost::shared_ptr socket = boost::make_shared(io_service); + socket->connect(endpoint); + ROS_INFO_STREAM("Connection to Depth Server established"); + + return socket; +} + +void NavTubeDriver::run() +{ + while (ros::ok()) + { + try + { + prev = ros::Time::now(); + boost::shared_ptr socket; + + socket = connect(); + timer_thread = std::thread(&NavTubeDriver::send_heartbeat, this, socket); + initialized = true; + read_messages(socket); + } + catch (boost::system::system_error const& e) + { + ros::Duration wait_time(5); + ROS_WARN_STREAM("Error with NavTube Depth driver TCP socket " << e.what() << ". Trying again in " + << wait_time.toSec() << " seconds"); + + if (initialized) + timer_thread.join(); + initialized = false; + wait_time.sleep(); + } + } +} + +void NavTubeDriver::send_heartbeat(boost::shared_ptr socket) +{ + try + { + while (true) + { + boost::asio::write(*socket, boost::asio::buffer(heartbeat_packet)); + + { + std::lock_guard lock(m); + if (!running) + return; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(250)); + } + } + catch (boost::system::system_error const& e) + { + } +} + +void NavTubeDriver::read_messages(boost::shared_ptr socket) +{ + mil_msgs::DepthStamped msg; + msg.header.frame_id = frame_id_; + msg.header.seq = 0; + + uint8_t backing[10]; + + auto buffer = boost::asio::buffer(backing, sizeof(backing)); + + while (ros::ok()) + { + if (ros::Time::now().toNSec() - prev.toNSec() > acceptable_frequency) + { + ROS_WARN_STREAM("Depth sampling rate is falling behind."); + } + + if (!boost::asio::buffer_size(buffer)) + { + // Bytes are out of sync so try and resync + if (backing[0] != sync1 || backing[1] != sync2) + { + for (int i = 0; i < (sizeof(backing) / sizeof(backing[0])) - 1; i++) + { + backing[i] = backing[i + 1]; + } + buffer = boost::asio::buffer(backing + (sizeof(backing) / sizeof(backing[0])) - sizeof(backing[0]), + sizeof(backing[0])); + } + else + { + ++msg.header.seq; + msg.header.stamp = ros::Time::now(); + + uint64_t bits = be64toh(*reinterpret_cast(&backing[2])); + double pressure = *reinterpret_cast(&bits); + msg.depth = pressure; + + pub_.publish(msg); + buffer = boost::asio::buffer(backing, sizeof(backing)); + } + } + + size_t bytes_read = socket->read_some(buffer); + + buffer = boost::asio::buffer(buffer + bytes_read); + prev = ros::Time::now(); + } +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "nav_tube_driver"); + + ros::NodeHandle nh; + ros::NodeHandle private_nh("~"); + + NavTubeDriver node(nh, private_nh); + node.run(); +}