From dd8169d37186f9dfd818749296a8850a7ae426d3 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 13 Apr 2017 10:48:59 +0200 Subject: [PATCH] Factory and setup improvements --- CMakeLists.txt | 5 +- include/ur_modern_driver/event_counter.h | 86 ++++++++++++++++++++++++ include/ur_modern_driver/ur/factory.h | 4 +- src/ros_main.cpp | 64 +++++++++++++++--- 4 files changed, 145 insertions(+), 14 deletions(-) create mode 100644 include/ur_modern_driver/event_counter.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 7f1badf..944df13 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -159,13 +159,14 @@ target_link_libraries(ur_hardware_interface ## Declare a C++ executable set(${PROJECT_NAME}_SOURCES src/ros/rt_publisher.cpp + src/ros/mb_publisher.cpp + src/ros/robot_hardware.cpp src/ur/stream.cpp + src/ur/commander.cpp src/ur/robot_mode.cpp src/ur/master_board.cpp src/ur/rt_state.cpp src/ur/messages.cpp - #src/ros_main.cpp - #src/ur_ros_wrapper.cpp src/ur_driver.cpp src/ur_realtime_communication.cpp src/ur_communication.cpp diff --git a/include/ur_modern_driver/event_counter.h b/include/ur_modern_driver/event_counter.h new file mode 100644 index 0000000..2c8ff31 --- /dev/null +++ b/include/ur_modern_driver/event_counter.h @@ -0,0 +1,86 @@ +#pragma once + +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/consumer.h" + + +class EventCounter : public URRTPacketConsumer +{ +private: + typedef std::chrono::high_resolution_clock Clock; + Clock::time_point events_[250]; + size_t idx_ = 0; + + + Clock::time_point last_; + +public: + void trigger() + { + //auto now = Clock::now(); + //LOG_INFO("Time diff: %d ms", std::chrono::duration_cast(now - last_)); + //last_ = now; + //return; + + events_[idx_] = Clock::now(); + idx_ += 1; + + if(idx_ > 250) + { + std::chrono::time_point t_min = + std::chrono::time_point::max(); + std::chrono::time_point t_max = + std::chrono::time_point::min(); + + for(auto const& e : events_) + { + if(e < t_min) + t_min = e; + if(e > t_max) + t_max = e; + } + + auto diff = t_max - t_min; + auto secs = std::chrono::duration_cast(diff).count(); + auto ms = std::chrono::duration_cast(diff).count(); + std::chrono::duration test(t_max - t_min); + LOG_INFO("Recieved 250 messages at %f Hz", (250.0/test.count())); + idx_ = 0; + } + } + +public: + bool consume(RTState_V1_6__7& state) + { + trigger(); + return true; + } + bool consume(RTState_V1_8& state) + { + trigger(); + return true; + } + bool consume(RTState_V3_0__1& state) + { + trigger(); + return true; + } + bool consume(RTState_V3_2__3& state) + { + trigger(); + return true; + } + + void setupConsumer() + { + last_ = Clock::now(); + } + void teardownConsumer() + { + } + void stopConsumer() + { + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/factory.h b/include/ur_modern_driver/ur/factory.h index 83d89ab..83f0cd6 100644 --- a/include/ur_modern_driver/ur/factory.h +++ b/include/ur_modern_driver/ur/factory.h @@ -9,6 +9,8 @@ #include "ur_modern_driver/ur/state_parser.h" #include "ur_modern_driver/ur/stream.h" +static const int UR_PRIMARY_PORT = 30001; + class URFactory : private URMessagePacketConsumer { private: @@ -42,7 +44,7 @@ private: } public: - URFactory(std::string& host) : stream_(host, 30001) + URFactory(std::string& host) : stream_(host, UR_PRIMARY_PORT) { URProducer prod(stream_, parser_); std::vector> results; diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 09b142a..1c660dd 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -7,6 +7,10 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/pipeline.h" #include "ur_modern_driver/ros/rt_publisher.h" +#include "ur_modern_driver/ros/mb_publisher.h" +#include "ur_modern_driver/ros/io_service.h" +#include "ur_modern_driver/ros/ros_controller.h" +#include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/factory.h" #include "ur_modern_driver/ur/messages.h" #include "ur_modern_driver/ur/parser.h" @@ -17,9 +21,14 @@ static const std::string IP_ADDR_ARG("~robot_ip_address"); static const std::string REVERSE_PORT_ARG("~reverse_port"); static const std::string SIM_TIME_ARG("~use_sim_time"); +static const std::string ROS_CONTROL_ARG("~use_ros_control"); static const std::string PREFIX_ARG("~prefix"); static const std::string BASE_FRAME_ARG("~base_frame"); static const std::string TOOL_FRAME_ARG("~tool_frame"); +static const std::string JOINT_NAMES_PARAM("hardware_interface/joints"); + +static const int UR_SECONDARY_PORT = 30002; +static const int UR_RT_PORT = 30003; struct ProgArgs { @@ -28,8 +37,12 @@ public: std::string prefix; std::string base_frame; std::string tool_frame; + std::vector joint_names; + double max_acceleration; + double max_velocity; int32_t reverse_port; bool use_sim_time; + bool use_ros_control; }; bool parse_args(ProgArgs& args) @@ -41,12 +54,16 @@ bool parse_args(ProgArgs& args) } ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); ros::param::param(SIM_TIME_ARG, args.use_sim_time, false); + ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false); ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); + ros::param::get(JOINT_NAMES_PARAM, args.joint_names); return true; } +#include "ur_modern_driver/event_counter.h" + int main(int argc, char** argv) { ros::init(argc, argv, "ur_driver"); @@ -58,22 +75,47 @@ int main(int argc, char** argv) } URFactory factory(args.host); - auto parser = factory.getRTParser(); + //RT packets + auto rt_parser = factory.getRTParser(); + URStream rt_stream(args.host, UR_RT_PORT); + URProducer rt_prod(rt_stream, *rt_parser); + RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame); + EventCounter rt_ec; - URStream s2(args.host, 30003); - URProducer p2(s2, *parser); - RTPublisher pub(args.prefix, args.base_frame, args.tool_frame); + URCommander rt_commander(rt_stream); + vector*> rt_vec; - Pipeline pl(p2, pub); - - pl.run(); - - while (ros::ok()) + if(args.use_ros_control) { - std::this_thread::sleep_for(std::chrono::milliseconds(33)); + rt_vec.push_back(new ROSController(rt_commander, args.joint_names)); } - pl.stop(); + //rt_vec.push_back(&rt_pub); + + MultiConsumer rt_cons(rt_vec); + Pipeline rt_pl(rt_prod, rt_cons); + + //Message packets + auto state_parser = factory.getStateParser(); + URStream state_stream(args.host, UR_SECONDARY_PORT); + URProducer state_prod(state_stream, *state_parser); + MBPublisher state_pub; + vector*> state_vec{&state_pub}; + MultiConsumer state_cons(state_vec); + Pipeline state_pl(state_prod, state_cons); + + LOG_INFO("Starting main loop"); + + rt_pl.run(); + state_pl.run(); + + URCommander state_commander(state_stream); + IOService service(state_commander); + + ros::spin(); + + rt_pl.stop(); + state_pl.stop(); return EXIT_SUCCESS; } \ No newline at end of file