From 44751cfb2a11042bfe096c1c6fb83166adea090b Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 1 Apr 2019 16:27:43 +0200 Subject: [PATCH] removed unused files --- CMakeLists.txt | 40 +- include/ur_rtde_driver/ros/action_server.h | 96 ----- .../action_trajectory_follower_interface.h | 55 --- include/ur_rtde_driver/ros/controller.h | 113 ----- .../ur_rtde_driver/ros/hardware_interface.h | 104 ----- .../ros/lowbandwidth_trajectory_follower.h | 59 --- include/ur_rtde_driver/ros/mb_publisher.h | 82 ---- include/ur_rtde_driver/ros/rt_publisher.h | 95 ---- .../ur_rtde_driver/ros/trajectory_follower.h | 70 --- include/ur_rtde_driver/ros/urscript_handler.h | 44 -- src/ros/action_server.cpp | 372 ---------------- src/ros/controller.cpp | 152 ------- src/ros/hardware_interface.cpp | 109 ----- src/ros/lowbandwidth_trajectory_follower.cpp | 407 ------------------ src/ros/mb_publisher.cpp | 135 ------ src/ros/rt_publisher.cpp | 139 ------ src/ros/trajectory_follower.cpp | 264 ------------ src/ros/urscript_handler.cpp | 65 --- src/ros_main.cpp | 250 ----------- test_move.py | 168 -------- 20 files changed, 9 insertions(+), 2810 deletions(-) delete mode 100644 include/ur_rtde_driver/ros/action_server.h delete mode 100644 include/ur_rtde_driver/ros/action_trajectory_follower_interface.h delete mode 100644 include/ur_rtde_driver/ros/controller.h delete mode 100644 include/ur_rtde_driver/ros/hardware_interface.h delete mode 100644 include/ur_rtde_driver/ros/lowbandwidth_trajectory_follower.h delete mode 100644 include/ur_rtde_driver/ros/mb_publisher.h delete mode 100644 include/ur_rtde_driver/ros/rt_publisher.h delete mode 100644 include/ur_rtde_driver/ros/trajectory_follower.h delete mode 100644 include/ur_rtde_driver/ros/urscript_handler.h delete mode 100644 src/ros/action_server.cpp delete mode 100644 src/ros/controller.cpp delete mode 100644 src/ros/hardware_interface.cpp delete mode 100644 src/ros/lowbandwidth_trajectory_follower.cpp delete mode 100644 src/ros/mb_publisher.cpp delete mode 100644 src/ros/rt_publisher.cpp delete mode 100644 src/ros/trajectory_follower.cpp delete mode 100644 src/ros/urscript_handler.cpp delete mode 100644 src/ros_main.cpp delete mode 100755 test_move.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 5cb31fe..459aef7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ catkin_package( INCLUDE_DIRS include LIBRARIES - ur_rtde_hardware_interface + ur_rtde_driver CATKIN_DEPENDS actionlib control_msgs @@ -60,33 +60,16 @@ add_compile_options(-Wall) add_compile_options(-Wextra) add_compile_options(-Wno-unused-parameter) -# support indigo's ros_control - This can be removed upon EOL indigo -if("${controller_manager_msgs_VERSION}" VERSION_LESS "0.10.0") - add_definitions(-DUR_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) -endif() include_directories( include ${catkin_INCLUDE_DIRS} ) -# Hardware Interface -add_library(ur_rtde_hardware_interface - src/ros/controller.cpp - src/ros/hardware_interface.cpp) -target_link_libraries(ur_rtde_hardware_interface ${catkin_LIBRARIES}) - - -set(${PROJECT_NAME}_SOURCES +add_library(ur_rtde_driver src/comm/stream.cpp src/comm/tcp_socket.cpp - src/ros/action_server.cpp - src/ros/lowbandwidth_trajectory_follower.cpp - src/ros/mb_publisher.cpp - src/ros/rt_publisher.cpp src/ros/service_stopper.cpp - src/ros/trajectory_follower.cpp - src/ros/urscript_handler.cpp src/ur/commander.cpp src/ur/master_board.cpp src/ur/messages.cpp @@ -94,24 +77,19 @@ set(${PROJECT_NAME}_SOURCES src/ur/rt_state.cpp src/ur/server.cpp ) - -add_executable(ur_rtde_driver ${${PROJECT_NAME}_SOURCES} src/ros_main.cpp) +target_link_libraries(ur_rtde_driver ${catkin_LIBRARIES}) add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(ur_rtde_driver - ur_rtde_hardware_interface - ${catkin_LIBRARIES} - ) - -install(DIRECTORY config launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -install(TARGETS ur_rtde_driver ur_rtde_hardware_interface +install(TARGETS ur_rtde_driver ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +install(DIRECTORY config launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + + install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" @@ -125,5 +103,5 @@ if (CATKIN_ENABLE_TESTING) tests/ur/rt_state.cpp) catkin_add_gtest(ur_rtde_driver_test ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_TEST_SOURCES} tests/main.cpp) - target_link_libraries(ur_rtde_driver_test ur_rtde_hardware_interface ${catkin_LIBRARIES}) + target_link_libraries(ur_rtde_driver_test ur_rtde_driver ${catkin_LIBRARIES}) endif() diff --git a/include/ur_rtde_driver/ros/action_server.h b/include/ur_rtde_driver/ros/action_server.h deleted file mode 100644 index 6a77a6f..0000000 --- a/include/ur_rtde_driver/ros/action_server.h +++ /dev/null @@ -1,96 +0,0 @@ -/* - * Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower) - * - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "ur_rtde_driver/log.h" -#include "ur_rtde_driver/ros/service_stopper.h" -#include "ur_rtde_driver/ros/trajectory_follower.h" -#include "ur_rtde_driver/ur/consumer.h" -#include "ur_rtde_driver/ur/master_board.h" -#include "ur_rtde_driver/ur/state.h" - -namespace ur_rtde_driver -{ -class ActionServer : public Service, public URRTPacketConsumer -{ -private: - typedef control_msgs::FollowJointTrajectoryAction Action; - typedef control_msgs::FollowJointTrajectoryResult Result; - typedef actionlib::ServerGoalHandle GoalHandle; - typedef actionlib::ActionServer Server; - - ros::NodeHandle nh_; - Server as_; - - std::vector joint_names_; - std::set joint_set_; - double max_velocity_; - - GoalHandle curr_gh_; - std::atomic interrupt_traj_; - std::atomic has_goal_, running_; - std::mutex tj_mutex_; - std::condition_variable tj_cv_; - std::thread tj_thread_; - - ActionTrajectoryFollowerInterface& follower_; - - RobotState state_; - std::array q_actual_, qd_actual_; - - void onGoal(GoalHandle gh); - void onCancel(GoalHandle gh); - - bool validate(GoalHandle& gh, Result& res); - bool validateState(GoalHandle& gh, Result& res); - bool validateJoints(GoalHandle& gh, Result& res); - bool validateTrajectory(GoalHandle& gh, Result& res); - - bool try_execute(GoalHandle& gh, Result& res); - void interruptGoal(GoalHandle& gh); - - std::vector reorderMap(std::vector goal_joints); - - void trajectoryThread(); - bool updateState(RTShared& data); - -public: - ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector& joint_names, double max_velocity); - - void start(); - virtual void onRobotStateChange(RobotState state); - - virtual bool consume(RTState_V1_6__7& state); - virtual bool consume(RTState_V1_8& state); - virtual bool consume(RTState_V3_0__1& state); - virtual bool consume(RTState_V3_2__3& state); -}; -} // namespace ur_rtde_driver diff --git a/include/ur_rtde_driver/ros/action_trajectory_follower_interface.h b/include/ur_rtde_driver/ros/action_trajectory_follower_interface.h deleted file mode 100644 index 13cd484..0000000 --- a/include/ur_rtde_driver/ros/action_trajectory_follower_interface.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - * Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower) - * - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace ur_rtde_driver -{ -struct TrajectoryPoint -{ - std::array positions; - std::array velocities; - std::chrono::microseconds time_from_start; - - TrajectoryPoint() - { - } - - TrajectoryPoint(std::array& pos, std::array& vel, std::chrono::microseconds tfs) - : positions(pos), velocities(vel), time_from_start(tfs) - { - } -}; - -class ActionTrajectoryFollowerInterface -{ -public: - virtual bool start() = 0; - virtual bool execute(std::vector& trajectory, std::atomic& interrupt) = 0; - virtual void stop() = 0; - virtual ~ActionTrajectoryFollowerInterface(){}; -}; -} // namespace ur_rtde_driver diff --git a/include/ur_rtde_driver/ros/controller.h b/include/ur_rtde_driver/ros/controller.h deleted file mode 100644 index 4c6e5f1..0000000 --- a/include/ur_rtde_driver/ros/controller.h +++ /dev/null @@ -1,113 +0,0 @@ -/* - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#pragma once -#include -#include -#include -#include -#include -#include -#include -#include "ur_rtde_driver/log.h" -#include "ur_rtde_driver/ros/hardware_interface.h" -#include "ur_rtde_driver/ros/service_stopper.h" -#include "ur_rtde_driver/ur/commander.h" -#include "ur_rtde_driver/ur/consumer.h" -#include "ur_rtde_driver/ur/rt_state.h" - -namespace ur_rtde_driver -{ -class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service -{ -private: - ros::NodeHandle nh_; - ros::Time lastUpdate_; - controller_manager::ControllerManager controller_; - bool robot_state_received_; - - // state interfaces - JointInterface joint_interface_; - WrenchInterface wrench_interface_; - // controller interfaces - PositionInterface position_interface_; - VelocityInterface velocity_interface_; - - // currently activated controller - HardwareInterface* active_interface_; - // map of switchable controllers - std::map available_interfaces_; - - std::atomic service_enabled_; - std::atomic service_cooldown_; - - // helper functions to map interfaces - template - void registerInterface(T* interface) - { - RobotHW::registerInterface(interface); - } - template - void registerControllereInterface(T* interface) - { - registerInterface(interface); - available_interfaces_[T::INTERFACE_NAME] = interface; - } - - void read(RTShared& state); - bool update(); - bool write(); - void reset(); - -public: - ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector& joint_names, - double max_vel_change, std::string tcp_link); - virtual ~ROSController() - { - } - // from RobotHW - void doSwitch(const std::list& start_list, - const std::list& stop_list); - // from URRTPacketConsumer - virtual void setupConsumer(); - virtual bool consume(RTState_V1_6__7& state) - { - read(state); - return update(); - } - virtual bool consume(RTState_V1_8& state) - { - read(state); - return update(); - } - virtual bool consume(RTState_V3_0__1& state) - { - read(state); - return update(); - } - virtual bool consume(RTState_V3_2__3& state) - { - read(state); - return update(); - } - - virtual void onTimeout(); - - virtual void onRobotStateChange(RobotState state); -}; -} // namespace ur_rtde_driver diff --git a/include/ur_rtde_driver/ros/hardware_interface.h b/include/ur_rtde_driver/ros/hardware_interface.h deleted file mode 100644 index 6152c14..0000000 --- a/include/ur_rtde_driver/ros/hardware_interface.h +++ /dev/null @@ -1,104 +0,0 @@ -/* - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#pragma once -#include -#include -#include -#include -#include -#include "ur_rtde_driver/ros/trajectory_follower.h" -#include "ur_rtde_driver/ur/commander.h" -#include "ur_rtde_driver/ur/rt_state.h" - -namespace ur_rtde_driver -{ -class HardwareInterface -{ -public: - virtual bool write() = 0; - virtual void start() - { - } - virtual void stop() - { - } - virtual void reset() - { - } -}; - -using hardware_interface::JointHandle; - -class JointInterface : public hardware_interface::JointStateInterface -{ -private: - std::array velocities_, positions_, efforts_; - -public: - JointInterface(std::vector& joint_names); - void update(RTShared& packet); - - typedef hardware_interface::JointStateInterface parent_type; - static const std::string INTERFACE_NAME; -}; - -class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface -{ - std::array tcp_; - -public: - WrenchInterface(std::string tcp_link); - void update(RTShared& packet); - typedef hardware_interface::ForceTorqueSensorInterface parent_type; - static const std::string INTERFACE_NAME; -}; - -class VelocityInterface : public HardwareInterface, public hardware_interface::VelocityJointInterface -{ -private: - URCommander& commander_; - std::array velocity_cmd_, prev_velocity_cmd_; - double max_vel_change_; - -public: - VelocityInterface(URCommander& commander, hardware_interface::JointStateInterface& js_interface, - std::vector& joint_names, double max_vel_change); - virtual bool write(); - virtual void reset(); - typedef hardware_interface::VelocityJointInterface parent_type; - static const std::string INTERFACE_NAME; -}; - -class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface -{ -private: - TrajectoryFollower& follower_; - std::array position_cmd_; - -public: - PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface& js_interface, - std::vector& joint_names); - virtual bool write(); - virtual void start(); - virtual void stop(); - - typedef hardware_interface::PositionJointInterface parent_type; - static const std::string INTERFACE_NAME; -}; -} // namespace ur_rtde_driver diff --git a/include/ur_rtde_driver/ros/lowbandwidth_trajectory_follower.h b/include/ur_rtde_driver/ros/lowbandwidth_trajectory_follower.h deleted file mode 100644 index 54e68d5..0000000 --- a/include/ur_rtde_driver/ros/lowbandwidth_trajectory_follower.h +++ /dev/null @@ -1,59 +0,0 @@ -/* - * Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include "ur_rtde_driver/log.h" -#include "ur_rtde_driver/ros/action_trajectory_follower_interface.h" -#include "ur_rtde_driver/ur/commander.h" -#include "ur_rtde_driver/ur/server.h" - -namespace ur_rtde_driver -{ -class LowBandwidthTrajectoryFollower : public ActionTrajectoryFollowerInterface -{ -private: - std::atomic running_; - std::array last_positions_; - URCommander& commander_; - URServer server_; - - double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, servoj_gain_, servoj_lookahead_time_, - max_joint_difference_; - - std::string program_; - - bool execute(const std::array& positions, const std::array& velocities, double sample_number, - double time_in_seconds); - -public: - LowBandwidthTrajectoryFollower(URCommander& commander, std::string& reverse_ip, int reverse_port, bool version_3); - - bool start(); - bool execute(std::vector& trajectory, std::atomic& interrupt); - void stop(); - - virtual ~LowBandwidthTrajectoryFollower(){}; -}; -} // namespace ur_rtde_driver diff --git a/include/ur_rtde_driver/ros/mb_publisher.h b/include/ur_rtde_driver/ros/mb_publisher.h deleted file mode 100644 index 76c0c18..0000000 --- a/include/ur_rtde_driver/ros/mb_publisher.h +++ /dev/null @@ -1,82 +0,0 @@ -/* - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#pragma once - -#include -#include -#include -#include -#include - -#include "ur_rtde_driver/ur/consumer.h" - -namespace ur_rtde_driver -{ -using namespace ros; - -class MBPublisher : public URStatePacketConsumer -{ -private: - NodeHandle nh_; - Publisher io_pub_; - Publisher status_pub_; - - template - inline void appendDigital(std::vector& vec, std::bitset bits) - { - for (size_t i = 0; i < N; i++) - { - ur_msgs::Digital digi; - digi.pin = static_cast(i); - digi.state = bits.test(i); - vec.push_back(digi); - } - } - - void publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data); - void publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const; - void publishRobotStatus(const RobotModeData_V1_X& data) const; - void publishRobotStatus(const RobotModeData_V3_0__1& data) const; - -public: - MBPublisher() - : io_pub_(nh_.advertise("ur_driver/io_states", 1)) - , status_pub_(nh_.advertise("ur_driver/robot_status", 1)) - { - } - - virtual bool consume(MasterBoardData_V1_X& data); - virtual bool consume(MasterBoardData_V3_0__1& data); - virtual bool consume(MasterBoardData_V3_2& data); - - virtual bool consume(RobotModeData_V1_X& data); - virtual bool consume(RobotModeData_V3_0__1& data); - virtual bool consume(RobotModeData_V3_2& data); - - virtual void setupConsumer() - { - } - virtual void teardownConsumer() - { - } - virtual void stopConsumer() - { - } -}; -} // namespace ur_rtde_driver diff --git a/include/ur_rtde_driver/ros/rt_publisher.h b/include/ur_rtde_driver/ros/rt_publisher.h deleted file mode 100644 index bdc5ca8..0000000 --- a/include/ur_rtde_driver/ros/rt_publisher.h +++ /dev/null @@ -1,95 +0,0 @@ -/* - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ur_rtde_driver/ur/consumer.h" - -namespace ur_rtde_driver -{ -using namespace ros; -using namespace tf; - -const std::string JOINTS[] = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", - "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" }; - -class RTPublisher : public URRTPacketConsumer -{ -private: - NodeHandle nh_; - Publisher joint_pub_; - Publisher wrench_pub_; - Publisher tool_vel_pub_; - Publisher joint_temperature_pub_; - TransformBroadcaster transform_broadcaster_; - std::vector joint_names_; - std::string base_frame_; - std::string tool_frame_; - bool temp_only_; - - bool publishJoints(RTShared& packet, Time& t); - bool publishWrench(RTShared& packet, Time& t); - bool publishTool(RTShared& packet, Time& t); - bool publishTransform(RTShared& packet, Time& t); - bool publishTemperature(RTShared& packet, Time& t); - - bool publish(RTShared& packet); - -public: - RTPublisher(std::string& joint_prefix, std::string& base_frame, std::string& tool_frame, bool temp_only = false) - : joint_pub_(nh_.advertise("joint_states", 1)) - , wrench_pub_(nh_.advertise("wrench", 1)) - , tool_vel_pub_(nh_.advertise("tool_velocity", 1)) - , joint_temperature_pub_(nh_.advertise("joint_temperature", 1)) - , base_frame_(base_frame) - , tool_frame_(tool_frame) - , temp_only_(temp_only) - { - for (auto const& j : JOINTS) - { - joint_names_.push_back(joint_prefix + j); - } - } - - virtual bool consume(RTState_V1_6__7& state); - virtual bool consume(RTState_V1_8& state); - virtual bool consume(RTState_V3_0__1& state); - virtual bool consume(RTState_V3_2__3& state); - - virtual void setupConsumer() - { - } - virtual void teardownConsumer() - { - } - virtual void stopConsumer() - { - } -}; -} // namespace ur_rtde_driver diff --git a/include/ur_rtde_driver/ros/trajectory_follower.h b/include/ur_rtde_driver/ros/trajectory_follower.h deleted file mode 100644 index d1cb659..0000000 --- a/include/ur_rtde_driver/ros/trajectory_follower.h +++ /dev/null @@ -1,70 +0,0 @@ -/* - * Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower) - * - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include "ur_rtde_driver/log.h" -#include "ur_rtde_driver/ros/action_trajectory_follower_interface.h" -#include "ur_rtde_driver/ur/commander.h" -#include "ur_rtde_driver/ur/server.h" - -namespace ur_rtde_driver -{ -class TrajectoryFollower : public ActionTrajectoryFollowerInterface -{ -private: - std::atomic running_; - std::array last_positions_; - URCommander& commander_; - URServer server_; - - double servoj_time_, servoj_lookahead_time_, servoj_gain_; - std::string program_; - - template - size_t append(uint8_t* buffer, T& val) - { - size_t s = sizeof(T); - std::memcpy(buffer, &val, s); - return s; - } - - bool execute(std::array& positions, bool keep_alive); - double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel); - -public: - TrajectoryFollower(URCommander& commander, std::string& reverse_ip, int reverse_port, bool version_3); - - bool start(); - bool execute(std::array& positions); - bool execute(std::vector& trajectory, std::atomic& interrupt); - void stop(); - - virtual ~TrajectoryFollower(){}; -}; -} // namespace ur_rtde_driver diff --git a/include/ur_rtde_driver/ros/urscript_handler.h b/include/ur_rtde_driver/ros/urscript_handler.h deleted file mode 100644 index bac501d..0000000 --- a/include/ur_rtde_driver/ros/urscript_handler.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#pragma once - -#include -#include - -#include "std_msgs/String.h" -#include "ur_rtde_driver/log.h" -#include "ur_rtde_driver/ros/service_stopper.h" -#include "ur_rtde_driver/ur/commander.h" - -namespace ur_rtde_driver -{ -class URScriptHandler : public Service -{ -private: - ros::NodeHandle nh_; - URCommander& commander_; - ros::Subscriber urscript_sub_; - RobotState state_; - -public: - URScriptHandler(URCommander& commander); - void urscriptInterface(const std_msgs::String::ConstPtr& msg); - void onRobotStateChange(RobotState state); -}; -} // namespace ur_rtde_driver diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp deleted file mode 100644 index 5f9a495..0000000 --- a/src/ros/action_server.cpp +++ /dev/null @@ -1,372 +0,0 @@ -/* - * Copyright 2018 Jarek Potiuk (low bandwidth trajectory follower) - * - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_rtde_driver/ros/action_server.h" -#include - -namespace ur_rtde_driver -{ -ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector& joint_names, - double max_velocity) - : as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1), - boost::bind(&ActionServer::onCancel, this, _1), false) - , joint_names_(joint_names) - , joint_set_(joint_names.begin(), joint_names.end()) - , max_velocity_(max_velocity) - , interrupt_traj_(false) - , has_goal_(false) - , running_(false) - , follower_(follower) - , state_(RobotState::Error) -{ -} - -void ActionServer::start() -{ - if (running_) - return; - - LOG_INFO("Starting ActionServer"); - running_ = true; - tj_thread_ = std::thread(&ActionServer::trajectoryThread, this); - as_.start(); -} - -void ActionServer::onRobotStateChange(RobotState state) -{ - state_ = state; - - // don't interrupt if everything is fine - if (state == RobotState::Running) - return; - - // don't retry interrupts - if (interrupt_traj_ || !has_goal_) - return; - - // on successful lock we're not executing a goal so don't interrupt - if (tj_mutex_.try_lock()) - { - tj_mutex_.unlock(); - return; - } - - interrupt_traj_ = true; - // wait for goal to be interrupted and automagically unlock when going out of scope - std::lock_guard lock(tj_mutex_); - - Result res; - res.error_code = -100; - res.error_string = "Robot safety stop"; - curr_gh_.setAborted(res, res.error_string); -} - -bool ActionServer::updateState(RTShared& data) -{ - q_actual_ = data.q_actual; - qd_actual_ = data.qd_actual; - return true; -} - -bool ActionServer::consume(RTState_V1_6__7& state) -{ - return updateState(state); -} -bool ActionServer::consume(RTState_V1_8& state) -{ - return updateState(state); -} -bool ActionServer::consume(RTState_V3_0__1& state) -{ - return updateState(state); -} -bool ActionServer::consume(RTState_V3_2__3& state) -{ - return updateState(state); -} - -void ActionServer::onGoal(GoalHandle gh) -{ - Result res; - res.error_code = -100; - - LOG_INFO("Received new goal"); - - if (!validate(gh, res) || !try_execute(gh, res)) - { - LOG_WARN("Goal error: %s", res.error_string.c_str()); - gh.setRejected(res, res.error_string); - } -} - -void ActionServer::onCancel(GoalHandle gh) -{ - interrupt_traj_ = true; - // wait for goal to be interrupted - std::lock_guard lock(tj_mutex_); - - Result res; - res.error_code = -100; - res.error_string = "Goal cancelled by client"; - gh.setCanceled(res); -} - -bool ActionServer::validate(GoalHandle& gh, Result& res) -{ - return validateState(gh, res) && validateJoints(gh, res) && validateTrajectory(gh, res); -} - -bool ActionServer::validateState(GoalHandle& gh, Result& res) -{ - switch (state_) - { - case RobotState::EmergencyStopped: - res.error_string = "Robot is emergency stopped"; - return false; - - case RobotState::ProtectiveStopped: - res.error_string = "Robot is protective stopped"; - return false; - - case RobotState::Error: - res.error_string = "Robot is not ready, check robot_mode"; - return false; - - case RobotState::Running: - return true; - - default: - res.error_string = "Undefined state"; - return false; - } -} - -bool ActionServer::validateJoints(GoalHandle& gh, Result& res) -{ - auto goal = gh.getGoal(); - auto const& joints = goal->trajectory.joint_names; - std::set goal_joints(joints.begin(), joints.end()); - - if (goal_joints == joint_set_) - return true; - - res.error_code = Result::INVALID_JOINTS; - res.error_string = "Invalid joint names for goal\n"; - res.error_string += "Expected: "; - std::for_each(goal_joints.begin(), goal_joints.end(), - [&res](std::string joint) { res.error_string += joint + ", "; }); - res.error_string += "\nFound: "; - std::for_each(joint_set_.begin(), joint_set_.end(), [&res](std::string joint) { res.error_string += joint + ", "; }); - return false; -} - -bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res) -{ - auto goal = gh.getGoal(); - res.error_code = Result::INVALID_GOAL; - - // must at least have one point - if (goal->trajectory.points.size() < 1) - return false; - - for (auto const& point : goal->trajectory.points) - { - if (point.velocities.size() != joint_names_.size()) - { - res.error_code = Result::INVALID_GOAL; - res.error_string = "Received a goal with an invalid number of velocities"; - return false; - } - - if (point.positions.size() != joint_names_.size()) - { - res.error_code = Result::INVALID_GOAL; - res.error_string = "Received a goal with an invalid number of positions"; - return false; - } - - for (auto const& velocity : point.velocities) - { - if (!std::isfinite(velocity)) - { - res.error_string = "Received a goal with infinities or NaNs in velocity"; - return false; - } - if (std::fabs(velocity) > max_velocity_) - { - res.error_string = - "Received a goal with velocities that are higher than max_velocity_ " + std::to_string(max_velocity_); - return false; - } - } - for (auto const& position : point.positions) - { - if (!std::isfinite(position)) - { - res.error_string = "Received a goal with infinities or NaNs in positions"; - return false; - } - } - } - - // todo validate start position? - - return true; -} - -inline std::chrono::microseconds convert(const ros::Duration& dur) -{ - return std::chrono::duration_cast(std::chrono::seconds(dur.sec) + - std::chrono::nanoseconds(dur.nsec)); -} - -bool ActionServer::try_execute(GoalHandle& gh, Result& res) -{ - if (!running_) - { - res.error_string = "Internal error"; - return false; - } - if (!tj_mutex_.try_lock()) - { - interrupt_traj_ = true; - res.error_string = "Received another trajectory"; - curr_gh_.setAborted(res, res.error_string); - tj_mutex_.lock(); - // todo: make configurable - std::this_thread::sleep_for(std::chrono::milliseconds(250)); - } - // locked here - curr_gh_ = gh; - interrupt_traj_ = false; - has_goal_ = true; - tj_mutex_.unlock(); - tj_cv_.notify_one(); - return true; -} - -std::vector ActionServer::reorderMap(std::vector goal_joints) -{ - std::vector indecies; - for (auto const& aj : joint_names_) - { - size_t j = 0; - for (auto const& gj : goal_joints) - { - if (aj == gj) - break; - j++; - } - indecies.push_back(j); - } - return indecies; -} - -void ActionServer::trajectoryThread() -{ - LOG_INFO("Trajectory thread started"); - - while (running_) - { - std::unique_lock lk(tj_mutex_); - if (!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&] { return running_ && has_goal_; })) - continue; - - LOG_INFO("Trajectory received and accepted"); - curr_gh_.setAccepted(); - - auto goal = curr_gh_.getGoal(); - std::vector trajectory; - trajectory.reserve(goal->trajectory.points.size() + 1); - - // joint names of the goal might have a different ordering compared - // to what URScript expects so need to map between the two - auto mapping = reorderMap(goal->trajectory.joint_names); - - LOG_INFO("Translating trajectory"); - - auto const& fp = goal->trajectory.points[0]; - auto fpt = convert(fp.time_from_start); - - // make sure we have a proper t0 position - if (fpt > std::chrono::microseconds(0)) - { - LOG_INFO("Trajectory without t0 recieved, inserting t0 at currrent position"); - trajectory.push_back(TrajectoryPoint(q_actual_, qd_actual_, std::chrono::microseconds(0))); - } - - for (auto const& point : goal->trajectory.points) - { - std::array pos, vel; - for (size_t i = 0; i < 6; i++) - { - size_t idx = mapping[i]; - pos[idx] = point.positions[i]; - vel[idx] = point.velocities[i]; - } - auto t = convert(point.time_from_start); - trajectory.push_back(TrajectoryPoint(pos, vel, t)); - } - - double t = - std::chrono::duration_cast>(trajectory[trajectory.size() - 1].time_from_start) - .count(); - LOG_INFO("Executing trajectory with %zu points and duration of %4.3fs", trajectory.size(), t); - - Result res; - - LOG_INFO("Attempting to start follower %p", &follower_); - if (follower_.start()) - { - if (follower_.execute(trajectory, interrupt_traj_)) - { - // interrupted goals must be handled by interrupt trigger - if (!interrupt_traj_) - { - LOG_INFO("Trajectory executed successfully"); - res.error_code = Result::SUCCESSFUL; - curr_gh_.setSucceeded(res); - } - else - LOG_INFO("Trajectory interrupted"); - } - else - { - LOG_INFO("Trajectory failed"); - res.error_code = -100; - res.error_string = "Connection to robot was lost"; - curr_gh_.setAborted(res, res.error_string); - } - - follower_.stop(); - } - else - { - LOG_ERROR("Failed to start trajectory follower!"); - res.error_code = -100; - res.error_string = "Robot connection could not be established"; - curr_gh_.setAborted(res, res.error_string); - } - - has_goal_ = false; - lk.unlock(); - } -} -} // namespace ur_rtde_driver diff --git a/src/ros/controller.cpp b/src/ros/controller.cpp deleted file mode 100644 index 554ecaa..0000000 --- a/src/ros/controller.cpp +++ /dev/null @@ -1,152 +0,0 @@ -/* - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_rtde_driver/ros/controller.h" - -namespace ur_rtde_driver -{ -ROSController::ROSController(URCommander& commander, TrajectoryFollower& follower, - std::vector& joint_names, double max_vel_change, std::string tcp_link) - : controller_(this, nh_) - , robot_state_received_(false) - , joint_interface_(joint_names) - , wrench_interface_(tcp_link) - , position_interface_(follower, joint_interface_, joint_names) - , velocity_interface_(commander, joint_interface_, joint_names, max_vel_change) -{ - registerInterface(&joint_interface_); - registerInterface(&wrench_interface_); - registerControllereInterface(&position_interface_); - registerControllereInterface(&velocity_interface_); -} - -void ROSController::setupConsumer() -{ - lastUpdate_ = ros::Time::now(); -} - -void ROSController::doSwitch(const std::list& start_list, - const std::list& stop_list) -{ - LOG_INFO("Switching hardware interface"); - - if (active_interface_ != nullptr && stop_list.size() > 0) - { - LOG_INFO("Stopping active interface"); - active_interface_->stop(); - active_interface_ = nullptr; - } - - for (auto const& ci : start_list) - { - std::string requested_interface(""); - -#if defined(UR_ROS_CONTROL_INTERFACE_OLD_ROS_CONTROL) - requested_interface = ci.hardware_interface; -#else - if (!ci.claimed_resources.empty()) - requested_interface = ci.claimed_resources[0].hardware_interface; -#endif - - auto ait = available_interfaces_.find(requested_interface); - - if (ait == available_interfaces_.end()) - continue; - - auto new_interface = ait->second; - - LOG_INFO("Starting %s", ci.name.c_str()); - - active_interface_ = new_interface; - new_interface->start(); - - return; - } - - if (start_list.size() > 0) - LOG_WARN("Failed to start interface!"); -} - -bool ROSController::write() -{ - if (active_interface_ == nullptr) - return true; - - return active_interface_->write(); -} - -void ROSController::reset() -{ - if (active_interface_ == nullptr) - return; - - active_interface_->reset(); -} - -void ROSController::read(RTShared& packet) -{ - joint_interface_.update(packet); - wrench_interface_.update(packet); - robot_state_received_ = true; -} - -bool ROSController::update() -{ - // don't run controllers if we haven't received robot state yet - if (!robot_state_received_) - return true; - - auto time = ros::Time::now(); - auto diff = time - lastUpdate_; - lastUpdate_ = time; - - controller_.update(time, diff, !service_enabled_); - - // emergency stop and such should not kill the pipeline - // but still prevent writes - if (!service_enabled_) - { - reset(); - return true; - } - - // allow the controller to update x times before allowing writes again - if (service_cooldown_ > 0) - { - service_cooldown_ -= 1; - return true; - } - - return write(); -} - -void ROSController::onTimeout() -{ - update(); -} - -void ROSController::onRobotStateChange(RobotState state) -{ - bool next = (state == RobotState::Running); - if (next == service_enabled_) - return; - - service_enabled_ = next; - service_cooldown_ = 125; -} -} // namespace ur_rtde_driver diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp deleted file mode 100644 index 636c1ea..0000000 --- a/src/ros/hardware_interface.cpp +++ /dev/null @@ -1,109 +0,0 @@ -/* - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_rtde_driver/ros/hardware_interface.h" -#include "ur_rtde_driver/log.h" - -namespace ur_rtde_driver -{ -const std::string JointInterface::INTERFACE_NAME = "hardware_interface::JointStateInterface"; -JointInterface::JointInterface(std::vector& joint_names) -{ - for (size_t i = 0; i < 6; i++) - { - registerHandle(hardware_interface::JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i])); - } -} - -void JointInterface::update(RTShared& packet) -{ - positions_ = packet.q_actual; - velocities_ = packet.qd_actual; - efforts_ = packet.i_actual; -} - -const std::string WrenchInterface::INTERFACE_NAME = "hardware_interface::ForceTorqueSensorInterface"; -WrenchInterface::WrenchInterface(std::string tcp_link) -{ - registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", tcp_link, tcp_.begin(), tcp_.begin() + 3)); -} - -void WrenchInterface::update(RTShared& packet) -{ - tcp_ = packet.tcp_force; -} - -const std::string VelocityInterface::INTERFACE_NAME = "hardware_interface::VelocityJointInterface"; -VelocityInterface::VelocityInterface(URCommander& commander, hardware_interface::JointStateInterface& js_interface, - std::vector& joint_names, double max_vel_change) - : commander_(commander), max_vel_change_(max_vel_change), prev_velocity_cmd_({ 0, 0, 0, 0, 0, 0 }) -{ - for (size_t i = 0; i < 6; i++) - { - registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &velocity_cmd_[i])); - } -} - -bool VelocityInterface::write() -{ - for (size_t i = 0; i < 6; i++) - { - // clamp value to ±max_vel_change - double prev = prev_velocity_cmd_[i]; - double lo = prev - max_vel_change_; - double hi = prev + max_vel_change_; - prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi)); - } - return commander_.speedj(prev_velocity_cmd_, max_vel_change_); -} - -void VelocityInterface::reset() -{ - for (auto& val : prev_velocity_cmd_) - { - val = 0; - } -} - -const std::string PositionInterface::INTERFACE_NAME = "hardware_interface::PositionJointInterface"; -PositionInterface::PositionInterface(TrajectoryFollower& follower, - hardware_interface::JointStateInterface& js_interface, - std::vector& joint_names) - : follower_(follower) -{ - for (size_t i = 0; i < 6; i++) - { - registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i])); - } -} - -bool PositionInterface::write() -{ - return follower_.execute(position_cmd_); -} - -void PositionInterface::start() -{ - follower_.start(); -} - -void PositionInterface::stop() -{ - follower_.stop(); -} -} // namespace ur_rtde_driver diff --git a/src/ros/lowbandwidth_trajectory_follower.cpp b/src/ros/lowbandwidth_trajectory_follower.cpp deleted file mode 100644 index 87f13be..0000000 --- a/src/ros/lowbandwidth_trajectory_follower.cpp +++ /dev/null @@ -1,407 +0,0 @@ -/* - * Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_rtde_driver/ros/lowbandwidth_trajectory_follower.h" -#include -#include -#include - -namespace ur_rtde_driver -{ -static const std::array EMPTY_VALUES = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; - -static const std::string TIME_INTERVAL("{{TIME_INTERVAL}}"); -static const std::string SERVOJ_TIME("{{SERVOJ_TIME}}"); -static const std::string SERVOJ_TIME_WAITING("{{SERVOJ_TIME_WAITING}}"); -static const std::string MAX_WAITING_TIME("{{MAX_WAITING_TIME}}"); -static const std::string SERVOJ_GAIN("{{SERVOJ_GAIN}}"); -static const std::string SERVOJ_LOOKAHEAD_TIME("{{SERVOJ_LOOKAHEAD_TIME}}"); -static const std::string REVERSE_IP("{{REVERSE_IP}}"); -static const std::string REVERSE_PORT("{{REVERSE_PORT}}"); -static const std::string MAX_JOINT_DIFFERENCE("{{MAX_JOINT_DIFFERENCE}}"); -static const std::string POSITION_PROGRAM = R"( -def driveRobotLowBandwidthTrajectory(): - global JOINT_NUM = 6 - global TIME_INTERVAL = {{TIME_INTERVAL}} - global SERVOJ_TIME = {{SERVOJ_TIME}} - global SERVOJ_TIME_WAITING = {{SERVOJ_TIME_WAITING}} - global MAX_WAITING_TIME = {{MAX_WAITING_TIME}} - global EMPTY_VALUES = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - global SERVOJ_GAIN = {{SERVOJ_GAIN}} - global SERVOJ_LOOKAHEAD_TIME = {{SERVOJ_LOOKAHEAD_TIME}} - global CONNECTION_NAME = "reverse_connection" - global REVERSE_IP = "{{REVERSE_IP}}" - global REVERSE_PORT = {{REVERSE_PORT}} - global MAX_JOINT_DIFFERENCE = {{MAX_JOINT_DIFFERENCE}} - global g_position_previous = EMPTY_VALUES - global g_position_target = EMPTY_VALUES - global g_position_next = EMPTY_VALUES - global g_velocity_previous = EMPTY_VALUES - global g_velocity_target = EMPTY_VALUES - global g_velocity_next = EMPTY_VALUES - global g_time_previous = 0.0 - global g_time_target = 0.0 - global g_time_next = 0.0 - global g_num_previous = -1 - global g_num_target = -1 - global g_num_next = -1 - global g_received_waypoints_number = -1 - global g_requested_waypoints_number = -1 - global g_total_elapsed_time = 0 - global g_stopping = False - def send_message(message): - socket_send_string(message, CONNECTION_NAME) - socket_send_byte(10, CONNECTION_NAME) - end - - def is_waypoint_sentinel(waypoint): - local l_previous_index = 2 - while l_previous_index < 1 + JOINT_NUM * 2 + 2: - if waypoint[l_previous_index] != 0.0: - return False - end - l_previous_index = l_previous_index + 1 - end - return True - end - - def is_final_position_reached(position): - local l_current_position = get_actual_joint_positions() - local l_index = 0 - while l_index < JOINT_NUM: - if norm(position[l_index] - l_current_position[l_index]) > MAX_JOINT_DIFFERENCE: - return False - end - l_index = l_index + 1 - end - return True - end - - def interpolate(time_within_segment, total_segment_time, start_pos, l_end_pos, l_start_vel, end_vel): - local a = start_pos - local b = l_start_vel - local c = (-3 * a + 3 * l_end_pos - 2 * total_segment_time * b - total_segment_time * end_vel) / pow(total_segment_time, 2) - local d = (2 * a - 2 * l_end_pos + total_segment_time * b + total_segment_time * end_vel) / pow(total_segment_time, 3) - return a + b * time_within_segment + c * pow(time_within_segment, 2) + d * pow(time_within_segment, 3) - end - - def add_next_waypoint(waypoint): - enter_critical - g_position_previous = g_position_target - g_velocity_previous = g_velocity_target - g_time_previous = g_time_target - g_num_previous = g_num_target - g_position_target = g_position_next - g_velocity_target = g_velocity_next - g_time_target = g_time_next - g_num_target = g_num_next - g_num_next = waypoint[1] - g_position_next = [waypoint[2], waypoint[3], waypoint[4], waypoint[5], waypoint[6], waypoint[7]] - g_velocity_next = [waypoint[8], waypoint[9], waypoint[10], waypoint[11], waypoint[12], waypoint[13]] - g_time_next = waypoint[14] - g_received_waypoints_number = g_num_next - exit_critical - end - - thread controllingThread(): - local l_received_waypoints_number = -1 - local l_requested_waypoints_number = -1 - local l_stopped = False - local l_current_position = get_actual_joint_positions() - enter_critical - g_requested_waypoints_number = 2 - exit_critical - while True: - enter_critical - l_requested_waypoints_number = g_requested_waypoints_number - exit_critical - local l_max_waiting_time_left = MAX_WAITING_TIME - while l_received_waypoints_number < l_requested_waypoints_number and l_max_waiting_time_left > 0: - servoj(l_current_position,t=SERVOJ_TIME_WAITING,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) - enter_critical - l_received_waypoints_number = g_received_waypoints_number - exit_critical - l_max_waiting_time_left = l_max_waiting_time_left - SERVOJ_TIME_WAITING - end - if l_max_waiting_time_left <= 0: - textmsg("Closing the connection on waiting too long.") - socket_close(CONNECTION_NAME) - halt - end - enter_critical - local l_start_pos = g_position_previous - local l_start_vel = g_velocity_previous - local l_start_time = g_time_previous - local l_start_num= g_num_previous - local l_end_pos = g_position_target - local l_end_vel = g_velocity_target - local l_end_time = g_time_target - local l_end_num = g_num_target - local l_total_elapsed_time = g_total_elapsed_time - local l_stopping_after_next_interpolation = g_stopping - g_requested_waypoints_number = g_requested_waypoints_number + 1 - exit_critical - - l_current_position = l_start_pos - - local l_total_segment_time = l_end_time - l_start_time - - while l_total_elapsed_time <= l_end_time: - local l_segment_elapsed_time = l_total_elapsed_time - l_start_time - j = 0 - while j < JOINT_NUM: - l_current_position[j] = interpolate(l_segment_elapsed_time, l_total_segment_time, l_start_pos[j], l_end_pos[j], l_start_vel[j], l_end_vel[j]) - j = j + 1 - end - servoj(l_current_position,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) - enter_critical - g_total_elapsed_time = g_total_elapsed_time + TIME_INTERVAL - l_total_elapsed_time = g_total_elapsed_time - exit_critical - - end - if l_stopping_after_next_interpolation: - while not is_final_position_reached(l_end_pos): - textmsg("Catching up on final position not reached first time.") - servoj(l_end_pos,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) - end - textmsg("Finishing the controlling thread. Final position reached.") - break - end - end - end - - thread sendingThread(): - local controlling_thread = run controllingThread() - local l_sent_waypoints_number = -1 - local l_requested_waypoints_number = -1 - local l_stopping = False - - enter_critical - l_requested_waypoints_number = g_requested_waypoints_number - l_stopping = g_stopping - exit_critical - while not l_stopping: - while l_sent_waypoints_number == l_requested_waypoints_number and not l_stopping: - sleep(SERVOJ_TIME_WAITING) - - enter_critical - l_requested_waypoints_number = g_requested_waypoints_number - l_stopping = g_stopping - exit_critical - - end - if l_stopping: - break - end - send_message(l_sent_waypoints_number + 1) - l_sent_waypoints_number = l_sent_waypoints_number + 1 - end - join controlling_thread - end - - thread receivingThread(): - local sending_thread = run sendingThread() - while True: - waypoint_received = socket_read_ascii_float(14, CONNECTION_NAME) - if waypoint_received[0] == 0: - textmsg("Not received trajectory for the last 2 seconds. Quitting") - enter_critical - g_stopping = True - exit_critical - break - elif waypoint_received[0] != JOINT_NUM * 2 + 2: - textmsg("Received wrong number of floats in trajectory. This is certainly not OK.") - textmsg(waypoint_received[0]) - enter_critical - g_stopping = True - exit_critical - break - elif is_waypoint_sentinel(waypoint_received): - add_next_waypoint(waypoint_received) - enter_critical - g_stopping = True - g_received_waypoints_number = g_received_waypoints_number + 1 - exit_critical - break - end - add_next_waypoint(waypoint_received) - end - join sending_thread - end - socket_open(REVERSE_IP, REVERSE_PORT, CONNECTION_NAME) - receiving_thread = run receivingThread() - join receiving_thread - socket_close(CONNECTION_NAME) - textmsg("Exiting the program") -end - -)"; - -LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander& commander, std::string& reverse_ip, - int reverse_port, bool version_3) - : running_(false) - , commander_(commander) - , server_(reverse_port) - , time_interval_(0.008) - , servoj_time_(0.008) - , servoj_time_waiting_(0.001) - , max_waiting_time_(2.0) - , servoj_gain_(300.0) - , servoj_lookahead_time_(0.03) - , max_joint_difference_(0.01) -{ - ros::param::get("~time_interval", time_interval_); - ros::param::get("~servoj_time", servoj_time_); - ros::param::get("~servoj_time_waiting", servoj_time_waiting_); - ros::param::get("~max_waiting_time", max_waiting_time_); - ros::param::get("~servoj_gain", servoj_gain_); - ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_); - ros::param::get("~max_joint_difference", max_joint_difference_); - - std::string res(POSITION_PROGRAM); - std::ostringstream out; - if (!version_3) - { - LOG_ERROR("Low Bandwidth Trajectory Follower only works for interface version > 3"); - std::exit(-1); - } - res.replace(res.find(TIME_INTERVAL), TIME_INTERVAL.length(), std::to_string(time_interval_)); - res.replace(res.find(SERVOJ_TIME_WAITING), SERVOJ_TIME_WAITING.length(), std::to_string(servoj_time_waiting_)); - res.replace(res.find(SERVOJ_TIME), SERVOJ_TIME.length(), std::to_string(servoj_time_)); - res.replace(res.find(MAX_WAITING_TIME), MAX_WAITING_TIME.length(), std::to_string(max_waiting_time_)); - res.replace(res.find(SERVOJ_GAIN), SERVOJ_GAIN.length(), std::to_string(servoj_gain_)); - res.replace(res.find(SERVOJ_LOOKAHEAD_TIME), SERVOJ_LOOKAHEAD_TIME.length(), std::to_string(servoj_lookahead_time_)); - res.replace(res.find(REVERSE_IP), REVERSE_IP.length(), reverse_ip); - res.replace(res.find(REVERSE_PORT), REVERSE_PORT.length(), std::to_string(reverse_port)); - res.replace(res.find(MAX_JOINT_DIFFERENCE), MAX_JOINT_DIFFERENCE.length(), std::to_string(max_joint_difference_)); - program_ = res; - - if (!server_.bind()) - { - LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port); - std::exit(-1); - } - LOG_INFO("Low Bandwidth Trajectory Follower is initialized!"); -} - -bool LowBandwidthTrajectoryFollower::start() -{ - LOG_INFO("Starting LowBandwidthTrajectoryFollower"); - - if (running_) - return true; // not sure - - LOG_INFO("Uploading trajectory program to robot"); - - if (!commander_.uploadProg(program_)) - { - LOG_ERROR("Program upload failed!"); - return false; - } - - LOG_DEBUG("Awaiting incoming robot connection"); - - if (!server_.accept()) - { - LOG_ERROR("Failed to accept incoming robot connection"); - return false; - } - - LOG_DEBUG("Robot successfully connected"); - return (running_ = true); -} - -bool LowBandwidthTrajectoryFollower::execute(const std::array& positions, - const std::array& velocities, double sample_number, - double time_in_seconds) -{ - if (!running_) - return false; - - std::ostringstream out; - - out << "("; - out << sample_number << ","; - for (auto const& pos : positions) - { - out << pos << ","; - } - for (auto const& vel : velocities) - { - out << vel << ","; - } - out << time_in_seconds << ")\r\n"; - - // I know it's ugly but it's the most efficient and fastest way - // We have only ASCII characters and we can cast char -> uint_8 - const std::string tmp = out.str(); - const char* formatted_message = tmp.c_str(); - const uint8_t* buf = (uint8_t*)formatted_message; - - size_t written; - LOG_DEBUG("Sending message %s", formatted_message); - - return server_.write(buf, strlen(formatted_message) + 1, written); -} - -bool LowBandwidthTrajectoryFollower::execute(std::vector& trajectory, std::atomic& interrupt) -{ - if (!running_) - return false; - - bool finished = false; - - char* line[MAX_SERVER_BUF_LEN]; - - bool res = true; - - while (!finished && !interrupt) - { - if (!server_.readLine((char*)line, MAX_SERVER_BUF_LEN)) - { - LOG_DEBUG("Connection closed. Finishing!"); - finished = true; - break; - } - unsigned int message_num = atoi((const char*)line); - LOG_DEBUG("Received request %i", message_num); - if (message_num < trajectory.size()) - { - res = execute(trajectory[message_num].positions, trajectory[message_num].velocities, message_num, - trajectory[message_num].time_from_start.count() / 1e6); - } - else - { - res = execute(EMPTY_VALUES, EMPTY_VALUES, message_num, 0.0); - } - if (!res) - { - finished = true; - } - } - return res; -} - -void LowBandwidthTrajectoryFollower::stop() -{ - if (!running_) - return; - - server_.disconnectClient(); - running_ = false; -} -} // namespace ur_rtde_driver diff --git a/src/ros/mb_publisher.cpp b/src/ros/mb_publisher.cpp deleted file mode 100644 index 7cec1ae..0000000 --- a/src/ros/mb_publisher.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/* - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_rtde_driver/ros/mb_publisher.h" - -namespace ur_rtde_driver -{ -inline void appendAnalog(std::vector& vec, double val, uint8_t pin) -{ - ur_msgs::Analog ana; - ana.pin = pin; - ana.state = val; - vec.push_back(ana); -} - -void MBPublisher::publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data) -{ - appendAnalog(io_msg.analog_in_states, data.analog_input0, 0); - appendAnalog(io_msg.analog_in_states, data.analog_input1, 1); - appendAnalog(io_msg.analog_out_states, data.analog_output0, 0); - appendAnalog(io_msg.analog_out_states, data.analog_output1, 1); - - io_pub_.publish(io_msg); -} - -void MBPublisher::publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const -{ - // note that this is true as soon as the drives are powered, - // even if the breakes are still closed - // which is in slight contrast to the comments in the - // message definition - status.drives_powered.val = data.robot_power_on; - - status.e_stopped.val = data.emergency_stopped; - - // I found no way to reliably get information if the robot is moving - // data.programm_running would be true when using this driver to move the robot - // but it would also be true when another programm is running that might not - // in fact contain any movement commands - status.in_motion.val = industrial_msgs::TriState::UNKNOWN; - - // the error code, if any, is not transmitted by this protocol - // it can and should be fetched seperately - status.error_code = 0; - - // note that e-stop is handled by a seperate variable - status.in_error.val = data.protective_stopped; - - status_pub_.publish(status); -} - -void MBPublisher::publishRobotStatus(const RobotModeData_V1_X& data) const -{ - industrial_msgs::RobotStatus msg; - - if (data.robot_mode == robot_mode_V1_X::ROBOT_FREEDRIVE_MODE) - msg.mode.val = industrial_msgs::RobotMode::MANUAL; - else - msg.mode.val = industrial_msgs::RobotMode::AUTO; - - // todo: verify that this correct, there is also ROBOT_READY_MODE - msg.motion_possible.val = (data.robot_mode == robot_mode_V1_X::ROBOT_RUNNING_MODE) ? industrial_msgs::TriState::ON : - industrial_msgs::TriState::OFF; - - publishRobotStatus(msg, data); -} - -void MBPublisher::publishRobotStatus(const RobotModeData_V3_0__1& data) const -{ - industrial_msgs::RobotStatus msg; - - msg.motion_possible.val = - (data.robot_mode == robot_mode_V3_X::RUNNING) ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF; - - if (data.control_mode == robot_control_mode_V3_X::TEACH) - msg.mode.val = industrial_msgs::RobotMode::MANUAL; - else - msg.mode.val = industrial_msgs::RobotMode::AUTO; - - publishRobotStatus(msg, data); -} - -bool MBPublisher::consume(MasterBoardData_V1_X& data) -{ - ur_msgs::IOStates io_msg; - appendDigital(io_msg.digital_in_states, data.digital_input_bits); - appendDigital(io_msg.digital_out_states, data.digital_output_bits); - publish(io_msg, data); - return true; -} -bool MBPublisher::consume(MasterBoardData_V3_0__1& data) -{ - ur_msgs::IOStates io_msg; - appendDigital(io_msg.digital_in_states, data.digital_input_bits); - appendDigital(io_msg.digital_out_states, data.digital_output_bits); - publish(io_msg, data); - return true; -} -bool MBPublisher::consume(MasterBoardData_V3_2& data) -{ - consume(static_cast(data)); - return true; -} - -bool MBPublisher::consume(RobotModeData_V1_X& data) -{ - publishRobotStatus(data); - return true; -} -bool MBPublisher::consume(RobotModeData_V3_0__1& data) -{ - publishRobotStatus(data); - return true; -} -bool MBPublisher::consume(RobotModeData_V3_2& data) -{ - publishRobotStatus(data); - return true; -} -} // namespace ur_rtde_driver diff --git a/src/ros/rt_publisher.cpp b/src/ros/rt_publisher.cpp deleted file mode 100644 index 0ffd549..0000000 --- a/src/ros/rt_publisher.cpp +++ /dev/null @@ -1,139 +0,0 @@ -/* - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_rtde_driver/ros/rt_publisher.h" - -namespace ur_rtde_driver -{ -bool RTPublisher::publishJoints(RTShared& packet, Time& t) -{ - sensor_msgs::JointState joint_msg; - joint_msg.header.stamp = t; - - joint_msg.name.assign(joint_names_.begin(), joint_names_.end()); - joint_msg.position.assign(packet.q_actual.begin(), packet.q_actual.end()); - joint_msg.velocity.assign(packet.qd_actual.begin(), packet.qd_actual.end()); - joint_msg.effort.assign(packet.i_actual.begin(), packet.i_actual.end()); - - joint_pub_.publish(joint_msg); - - return true; -} - -bool RTPublisher::publishWrench(RTShared& packet, Time& t) -{ - geometry_msgs::WrenchStamped wrench_msg; - wrench_msg.header.stamp = t; - wrench_msg.wrench.force.x = packet.tcp_force[0]; - wrench_msg.wrench.force.y = packet.tcp_force[1]; - wrench_msg.wrench.force.z = packet.tcp_force[2]; - wrench_msg.wrench.torque.x = packet.tcp_force[3]; - wrench_msg.wrench.torque.y = packet.tcp_force[4]; - wrench_msg.wrench.torque.z = packet.tcp_force[5]; - - wrench_pub_.publish(wrench_msg); - return true; -} - -bool RTPublisher::publishTool(RTShared& packet, Time& t) -{ - geometry_msgs::TwistStamped tool_twist; - tool_twist.header.stamp = t; - tool_twist.header.frame_id = base_frame_; - tool_twist.twist.linear.x = packet.tcp_speed_actual.position.x; - tool_twist.twist.linear.y = packet.tcp_speed_actual.position.y; - tool_twist.twist.linear.z = packet.tcp_speed_actual.position.z; - tool_twist.twist.angular.x = packet.tcp_speed_actual.rotation.x; - tool_twist.twist.angular.y = packet.tcp_speed_actual.rotation.y; - tool_twist.twist.angular.z = packet.tcp_speed_actual.rotation.z; - - tool_vel_pub_.publish(tool_twist); - return true; -} - -bool RTPublisher::publishTransform(RTShared& packet, Time& t) -{ - auto tv = packet.tool_vector_actual; - - Transform transform; - transform.setOrigin(Vector3(tv.position.x, tv.position.y, tv.position.z)); - - // Create quaternion - Quaternion quat; - - double angle = std::sqrt(std::pow(tv.rotation.x, 2) + std::pow(tv.rotation.y, 2) + std::pow(tv.rotation.z, 2)); - if (angle < 1e-16) - { - quat.setValue(0, 0, 0, 1); - } - else - { - quat.setRotation(Vector3(tv.rotation.x / angle, tv.rotation.y / angle, tv.rotation.z / angle), angle); - } - - transform.setRotation(quat); - - transform_broadcaster_.sendTransform(StampedTransform(transform, t, base_frame_, tool_frame_)); - - return true; -} - -bool RTPublisher::publishTemperature(RTShared& packet, Time& t) -{ - size_t len = joint_names_.size(); - for (size_t i = 0; i < len; i++) - { - sensor_msgs::Temperature msg; - msg.header.stamp = t; - msg.header.frame_id = joint_names_[i]; - msg.temperature = packet.motor_temperatures[i]; - - joint_temperature_pub_.publish(msg); - } - return true; -} - -bool RTPublisher::publish(RTShared& packet) -{ - Time time = Time::now(); - bool res = true; - if (!temp_only_) - { - res = publishJoints(packet, time) && publishWrench(packet, time); - } - - return res && publishTool(packet, time) && publishTransform(packet, time) && publishTemperature(packet, time); -} - -bool RTPublisher::consume(RTState_V1_6__7& state) -{ - return publish(state); -} -bool RTPublisher::consume(RTState_V1_8& state) -{ - return publish(state); -} -bool RTPublisher::consume(RTState_V3_0__1& state) -{ - return publish(state); -} -bool RTPublisher::consume(RTState_V3_2__3& state) -{ - return publish(state); -} -} // namespace ur_rtde_driver diff --git a/src/ros/trajectory_follower.cpp b/src/ros/trajectory_follower.cpp deleted file mode 100644 index 1defb2c..0000000 --- a/src/ros/trajectory_follower.cpp +++ /dev/null @@ -1,264 +0,0 @@ -/* - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_rtde_driver/ros/trajectory_follower.h" -#include -#include -#include - -static const int32_t MULT_JOINTSTATE_ = 1000000; -static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}"); -static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}"); -static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}"); -static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}"); -static const std::string POSITION_PROGRAM = R"( -def driverProg(): - MULT_jointstate = {{JOINT_STATE_REPLACE}} - - SERVO_IDLE = 0 - SERVO_RUNNING = 1 - cmd_servo_state = SERVO_IDLE - cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - def set_servo_setpoint(q): - enter_critical - cmd_servo_state = SERVO_RUNNING - cmd_servo_q = q - exit_critical - end - - thread servoThread(): - state = SERVO_IDLE - while True: - enter_critical - q = cmd_servo_q - do_brake = False - if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE): - do_brake = True - end - state = cmd_servo_state - cmd_servo_state = SERVO_IDLE - exit_critical - if do_brake: - stopj(1.0) - sync() - elif state == SERVO_RUNNING: - servoj(q, {{SERVO_J_REPLACE}}) - else: - sync() - end - end - end - - socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}) - - thread_servo = run servoThread() - keepalive = 1 - while keepalive > 0: - params_mult = socket_read_binary_integer(6+1) - if params_mult[0] > 0: - q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate] - keepalive = params_mult[7] - set_servo_setpoint(q) - end - end - sleep(.1) - socket_close() - kill thread_servo -end -)"; - -namespace ur_rtde_driver -{ -TrajectoryFollower::TrajectoryFollower(URCommander& commander, std::string& reverse_ip, int reverse_port, - bool version_3) - : running_(false) - , commander_(commander) - , server_(reverse_port) - , servoj_time_(0.008) - , servoj_lookahead_time_(0.03) - , servoj_gain_(300.) -{ - ros::param::get("~servoj_time", servoj_time_); - ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_); - ros::param::get("~servoj_gain", servoj_gain_); - - std::string res(POSITION_PROGRAM); - res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_)); - - std::ostringstream out; - out << "t=" << std::fixed << std::setprecision(4) << servoj_time_; - if (version_3) - out << ", lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_; - - res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str()); - res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), reverse_ip); - res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port)); - program_ = res; - - if (!server_.bind()) - { - LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port); - std::exit(-1); - } -} - -bool TrajectoryFollower::start() -{ - if (running_) - return true; // not sure - - LOG_INFO("Uploading trajectory program to robot"); - - if (!commander_.uploadProg(program_)) - { - LOG_ERROR("Program upload failed!"); - return false; - } - - LOG_DEBUG("Awaiting incoming robot connection"); - - if (!server_.accept()) - { - LOG_ERROR("Failed to accept incoming robot connection"); - return false; - } - - LOG_DEBUG("Robot successfully connected"); - return (running_ = true); -} - -bool TrajectoryFollower::execute(std::array& positions, bool keep_alive) -{ - if (!running_) - return false; - - // LOG_INFO("servoj([%f,%f,%f,%f,%f,%f])", positions[0], positions[1], positions[2], positions[3], positions[4], - // positions[5]); - - last_positions_ = positions; - - uint8_t buf[sizeof(uint32_t) * 7]; - uint8_t* idx = buf; - - for (auto const& pos : positions) - { - int32_t val = static_cast(pos * MULT_JOINTSTATE_); - val = htobe32(val); - idx += append(idx, val); - } - - int32_t val = htobe32(static_cast(keep_alive)); - append(idx, val); - - size_t written; - return server_.write(buf, sizeof(buf), written); -} - -double TrajectoryFollower::interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel) -{ - using std::pow; - double a = p0_pos; - double b = p0_vel; - double c = (-3 * a + 3 * p1_pos - 2 * T * b - T * p1_vel) / pow(T, 2); - double d = (2 * a - 2 * p1_pos + T * b + T * p1_vel) / pow(T, 3); - return a + b * t + c * pow(t, 2) + d * pow(t, 3); -} - -bool TrajectoryFollower::execute(std::array& positions) -{ - return execute(positions, true); -} - -bool TrajectoryFollower::execute(std::vector& trajectory, std::atomic& interrupt) -{ - if (!running_) - return false; - - using namespace std::chrono; - typedef duration double_seconds; - typedef high_resolution_clock Clock; - typedef Clock::time_point Time; - - auto& last = trajectory[trajectory.size() - 1]; - auto& prev = trajectory[0]; - - Time t0 = Clock::now(); - Time latest = t0; - - std::array positions; - - for (auto const& point : trajectory) - { - // skip t0 - if (&point == &prev) - continue; - - if (interrupt) - break; - - auto duration = point.time_from_start - prev.time_from_start; - double d_s = duration_cast(duration).count(); - - // interpolation loop - while (!interrupt) - { - latest = Clock::now(); - auto elapsed = latest - t0; - - if (point.time_from_start <= elapsed) - break; - - if (last.time_from_start <= elapsed) - return true; - - double elapsed_s = duration_cast(elapsed - prev.time_from_start).count(); - for (size_t j = 0; j < positions.size(); j++) - { - positions[j] = - interpolate(elapsed_s, d_s, prev.positions[j], point.positions[j], prev.velocities[j], point.velocities[j]); - } - - if (!execute(positions, true)) - return false; - - std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.))); - } - - prev = point; - } - - // In theory it's possible the last position won't be sent by - // the interpolation loop above but rather some position between - // t[N-1] and t[N] where N is the number of trajectory points. - // To make sure this does not happen the last position is sent - return execute(last.positions, true); -} - -void TrajectoryFollower::stop() -{ - if (!running_) - return; - - // std::array empty; - // execute(empty, false); - - server_.disconnectClient(); - running_ = false; -} -} // namespace ur_rtde_driver diff --git a/src/ros/urscript_handler.cpp b/src/ros/urscript_handler.cpp deleted file mode 100644 index 20aa0cf..0000000 --- a/src/ros/urscript_handler.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/* - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "ur_rtde_driver/ros/urscript_handler.h" -#include "ur_rtde_driver/log.h" - -namespace ur_rtde_driver -{ -URScriptHandler::URScriptHandler(URCommander& commander) : commander_(commander), state_(RobotState::Error) -{ - LOG_INFO("Initializing ur_driver/URScript subscriber"); - urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &URScriptHandler::urscriptInterface, this); - LOG_INFO("The ur_driver/URScript initialized"); -} - -void URScriptHandler::urscriptInterface(const std_msgs::String::ConstPtr& msg) -{ - LOG_INFO("Message received"); - std::string str(msg->data); - if (str.back() != '\n') - str.append("\n"); - - switch (state_) - { - case RobotState::Running: - if (!commander_.uploadProg(str)) - { - LOG_ERROR("Program upload failed!"); - } - break; - case RobotState::EmergencyStopped: - LOG_ERROR("Robot is emergency stopped"); - break; - case RobotState::ProtectiveStopped: - LOG_ERROR("Robot is protective stopped"); - break; - case RobotState::Error: - LOG_ERROR("Robot is not ready, check robot_mode"); - break; - default: - LOG_ERROR("Robot is in undefined state"); - break; - } -} - -void URScriptHandler::onRobotStateChange(RobotState state) -{ - state_ = state; -} -} // namespace ur_rtde_driver diff --git a/src/ros_main.cpp b/src/ros_main.cpp deleted file mode 100644 index 1dda334..0000000 --- a/src/ros_main.cpp +++ /dev/null @@ -1,250 +0,0 @@ -/* - * Copyright 2017, 2018 Jarek Potiuk (low bandwidth trajectory follower) - * - * Copyright 2017, 2018 Simon Rasmussen (refactor) - * - * Copyright 2015, 2016 Thomas Timm Andersen (original version) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include -#include -#include -#include - -#include "ur_rtde_driver/log.h" -#include "ur_rtde_driver/comm/parser.h" -#include "ur_rtde_driver/comm/pipeline.h" -#include "ur_rtde_driver/ros/action_server.h" -#include "ur_rtde_driver/ros/controller.h" -#include "ur_rtde_driver/ros/io_service.h" -#include "ur_rtde_driver/ros/lowbandwidth_trajectory_follower.h" -#include "ur_rtde_driver/ros/mb_publisher.h" -#include "ur_rtde_driver/ros/rt_publisher.h" -#include "ur_rtde_driver/ros/service_stopper.h" -#include "ur_rtde_driver/ros/trajectory_follower.h" -#include "ur_rtde_driver/ros/urscript_handler.h" -#include "ur_rtde_driver/ur/commander.h" -#include "ur_rtde_driver/ur/factory.h" -#include "ur_rtde_driver/ur/messages.h" -#include "ur_rtde_driver/ur/producer.h" -#include "ur_rtde_driver/ur/rt_state.h" -#include "ur_rtde_driver/ur/state.h" - -static const std::string IP_ADDR_ARG("~robot_ip_address"); -static const std::string REVERSE_PORT_ARG("~reverse_port"); -static const std::string ROS_CONTROL_ARG("~use_ros_control"); -static const std::string LOW_BANDWIDTH_TRAJECTORY_FOLLOWER("~use_lowbandwidth_trajectory_follower"); -static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change"); -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 TCP_LINK_ARG("~tcp_link"); -static const std::string JOINT_NAMES_PARAM("hardware_interface/joints"); -static const std::string SHUTDOWN_ON_DISCONNECT_ARG("~shutdown_on_disconnect"); - -static const std::vector DEFAULT_JOINTS = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", - "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" }; - -static const int UR_SECONDARY_PORT = 30002; -static const int UR_RT_PORT = 30003; - -using namespace ur_rtde_driver; - -struct ProgArgs -{ -public: - std::string host; - std::string prefix; - std::string base_frame; - std::string tool_frame; - std::string tcp_link; - std::vector joint_names; - double max_acceleration; - double max_velocity; - double max_vel_change; - int32_t reverse_port; - bool use_ros_control; - bool use_lowbandwidth_trajectory_follower; - bool shutdown_on_disconnect; -}; - -class IgnorePipelineStoppedNotifier : public comm::INotifier -{ -public: - void started(std::string name) - { - LOG_INFO("Starting pipeline %s", name.c_str()); - } - void stopped(std::string name) - { - LOG_INFO("Stopping pipeline %s", name.c_str()); - } -}; - -class ShutdownOnPipelineStoppedNotifier : public comm::INotifier -{ -public: - void started(std::string name) - { - LOG_INFO("Starting pipeline %s", name.c_str()); - } - void stopped(std::string name) - { - LOG_INFO("Shutting down on stopped pipeline %s", name.c_str()); - ros::shutdown(); - exit(1); - } -}; - -bool parse_args(ProgArgs& args) -{ - if (!ros::param::get(IP_ADDR_ARG, args.host)) - { - LOG_ERROR("robot_ip_address parameter must be set!"); - return false; - } - ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001)); - ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s - ros::param::param(MAX_VEL_CHANGE_ARG, args.max_velocity, 10.0); - ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false); - ros::param::param(LOW_BANDWIDTH_TRAJECTORY_FOLLOWER, args.use_lowbandwidth_trajectory_follower, 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::param(TCP_LINK_ARG, args.tcp_link, args.prefix + "tool0"); - ros::param::param(JOINT_NAMES_PARAM, args.joint_names, DEFAULT_JOINTS); - ros::param::param(SHUTDOWN_ON_DISCONNECT_ARG, args.shutdown_on_disconnect, true); - return true; -} - -std::string getLocalIPAccessibleFromHost(std::string& host) -{ - comm::URStream stream(host, UR_RT_PORT); - return stream.connect() ? stream.getIP() : std::string(); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "ur_driver"); - - ProgArgs args; - if (!parse_args(args)) - { - return EXIT_FAILURE; - } - - // Add prefix to joint names - std::transform(args.joint_names.begin(), args.joint_names.end(), args.joint_names.begin(), - [&args](std::string name) { return args.prefix + name; }); - - std::string local_ip(getLocalIPAccessibleFromHost(args.host)); - - URFactory factory(args.host); - std::vector services; - - // RT packets - auto rt_parser = factory.getRTParser(); - comm::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, args.use_ros_control); - auto rt_commander = factory.getCommander(rt_stream); - std::vector*> rt_vec{ &rt_pub }; - - comm::INotifier* notifier(nullptr); - ROSController* controller(nullptr); - ActionServer* action_server(nullptr); - if (args.use_ros_control) - { - LOG_INFO("ROS control enabled"); - TrajectoryFollower* traj_follower = - new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); - controller = new ROSController(*rt_commander, *traj_follower, args.joint_names, args.max_vel_change, args.tcp_link); - rt_vec.push_back(controller); - services.push_back(controller); - } - else - { - LOG_INFO("ActionServer enabled"); - ActionTrajectoryFollowerInterface* traj_follower(nullptr); - if (args.use_lowbandwidth_trajectory_follower) - { - LOG_INFO("Use low bandwidth trajectory follower"); - traj_follower = - new LowBandwidthTrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); - } - else - { - LOG_INFO("Use standard trajectory follower"); - traj_follower = new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); - } - action_server = new ActionServer(*traj_follower, args.joint_names, args.max_velocity); - rt_vec.push_back(action_server); - services.push_back(action_server); - } - - URScriptHandler urscript_handler(*rt_commander); - services.push_back(&urscript_handler); - if (args.shutdown_on_disconnect) - { - LOG_INFO("Notifier: Pipeline disconnect will shutdown the node"); - notifier = new ShutdownOnPipelineStoppedNotifier(); - } - else - { - LOG_INFO("Notifier: Pipeline disconnect will be ignored."); - notifier = new IgnorePipelineStoppedNotifier(); - } - - comm::MultiConsumer rt_cons(rt_vec); - comm::Pipeline rt_pl(rt_prod, rt_cons, "RTPacket", *notifier); - - // Message packets - auto state_parser = factory.getStateParser(); - comm::URStream state_stream(args.host, UR_SECONDARY_PORT); - URProducer state_prod(state_stream, *state_parser); - MBPublisher state_pub; - - ServiceStopper service_stopper(services); - - std::vector*> state_vec{ &state_pub, &service_stopper }; - comm::MultiConsumer state_cons(state_vec); - comm::Pipeline state_pl(state_prod, state_cons, "StatePacket", *notifier); - - LOG_INFO("Starting main loop"); - - rt_pl.run(); - state_pl.run(); - - auto state_commander = factory.getCommander(state_stream); - IOService io_service(*state_commander); - - if (action_server) - action_server->start(); - - ros::spin(); - - LOG_INFO("ROS stopping, shutting down pipelines"); - - rt_pl.stop(); - state_pl.stop(); - - if (controller) - delete controller; - - LOG_INFO("Pipelines shutdown complete"); - - return EXIT_SUCCESS; -} diff --git a/test_move.py b/test_move.py deleted file mode 100755 index 566466d..0000000 --- a/test_move.py +++ /dev/null @@ -1,168 +0,0 @@ -#!/usr/bin/env python -# -# Copyright 2015, 2016 Thomas Timm Andersen (original version) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import time -import roslib; roslib.load_manifest('ur_driver') -import rospy -import actionlib -from control_msgs.msg import * -from trajectory_msgs.msg import * -from sensor_msgs.msg import JointState -from math import pi - -JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', - 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] -Q1 = [2.2,0,-1.57,0,0,0] -Q2 = [1.5,0,-1.57,0,0,0] -Q3 = [1.5,-0.2,-1.57,0,0,0] - -client = None - -def move1(): - global joints_pos - g = FollowJointTrajectoryGoal() - g.trajectory = JointTrajectory() - g.trajectory.joint_names = JOINT_NAMES - try: - joint_states = rospy.wait_for_message("joint_states", JointState) - joints_pos = joint_states.position - g.trajectory.points = [ - JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)), - JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)), - JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)), - JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))] - client.send_goal(g) - client.wait_for_result() - except KeyboardInterrupt: - client.cancel_goal() - raise - except: - raise - -def move_disordered(): - order = [4, 2, 3, 1, 5, 0] - g = FollowJointTrajectoryGoal() - g.trajectory = JointTrajectory() - g.trajectory.joint_names = [JOINT_NAMES[i] for i in order] - q1 = [Q1[i] for i in order] - q2 = [Q2[i] for i in order] - q3 = [Q3[i] for i in order] - try: - joint_states = rospy.wait_for_message("joint_states", JointState) - joints_pos = joint_states.position - g.trajectory.points = [ - JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)), - JointTrajectoryPoint(positions=q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)), - JointTrajectoryPoint(positions=q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)), - JointTrajectoryPoint(positions=q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))] - client.send_goal(g) - client.wait_for_result() - except KeyboardInterrupt: - client.cancel_goal() - raise - except: - raise - -def move_repeated(): - g = FollowJointTrajectoryGoal() - g.trajectory = JointTrajectory() - g.trajectory.joint_names = JOINT_NAMES - try: - joint_states = rospy.wait_for_message("joint_states", JointState) - joints_pos = joint_states.position - d = 2.0 - g.trajectory.points = [JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0))] - for i in range(10): - g.trajectory.points.append( - JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(d))) - d += 1 - g.trajectory.points.append( - JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(d))) - d += 1 - g.trajectory.points.append( - JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(d))) - d += 2 - client.send_goal(g) - client.wait_for_result() - except KeyboardInterrupt: - client.cancel_goal() - raise - except: - raise - -def move_interrupt(): - g = FollowJointTrajectoryGoal() - g.trajectory = JointTrajectory() - g.trajectory.joint_names = JOINT_NAMES - try: - joint_states = rospy.wait_for_message("joint_states", JointState) - joints_pos = joint_states.position - g.trajectory.points = [ - JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)), - JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)), - JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)), - JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))] - - client.send_goal(g) - time.sleep(3.0) - print "Interrupting" - joint_states = rospy.wait_for_message("joint_states", JointState) - joints_pos = joint_states.position - g.trajectory.points = [ - JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)), - JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)), - JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)), - JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))] - client.send_goal(g) - client.wait_for_result() - except KeyboardInterrupt: - client.cancel_goal() - raise - except: - raise - -def main(): - global client - try: - rospy.init_node("test_move", anonymous=True, disable_signals=True) - client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction) - print "Waiting for server..." - client.wait_for_server() - print "Connected to server" - parameters = rospy.get_param(None) - index = str(parameters).find('prefix') - if (index > 0): - prefix = str(parameters)[index+len("prefix': '"):(index+len("prefix': '")+str(parameters)[index+len("prefix': '"):-1].find("'"))] - for i, name in enumerate(JOINT_NAMES): - JOINT_NAMES[i] = prefix + name - print "This program makes the robot move between the following three poses:" - print str([Q1[i]*180./pi for i in xrange(0,6)]) - print str([Q2[i]*180./pi for i in xrange(0,6)]) - print str([Q3[i]*180./pi for i in xrange(0,6)]) - print "Please make sure that your robot can move freely between these poses before proceeding!" - inp = raw_input("Continue? y/n: ")[0] - if (inp == 'y'): - #move1() - move_repeated() - #move_disordered() - #move_interrupt() - else: - print "Halting program" - except KeyboardInterrupt: - rospy.signal_shutdown("KeyboardInterrupt") - raise - -if __name__ == '__main__': main()