Skip to content

Commit

Permalink
Port robot_state_helper to ROS2
Browse files Browse the repository at this point in the history
  • Loading branch information
fdurchdewald committed Feb 15, 2024
1 parent 0639943 commit 7f10ece
Show file tree
Hide file tree
Showing 5 changed files with 393 additions and 1 deletion.
13 changes: 12 additions & 1 deletion ur_robot_driver/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@ add_library(ur_robot_driver_plugin
src/dashboard_client_ros.cpp
src/hardware_interface.cpp
src/urcl_log_handler.cpp
src/robot_state_helper.cpp
)
target_link_libraries(
ur_robot_driver_plugin
Expand Down Expand Up @@ -103,13 +104,23 @@ add_executable(controller_stopper_node
)
ament_target_dependencies(controller_stopper_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})

#
# robot_state_helper
#
add_executable(robot_state_helper
src/robot_state_helper.cpp
src/robot_state_helper_node.cpp
)
target_link_libraries(robot_state_helper ${catkin_LIBRARIES} ur_client_library::urcl)
ament_target_dependencies(robot_state_helper ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})

add_executable(urscript_interface
src/urscript_interface.cpp
)
ament_target_dependencies(urscript_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})

install(
TARGETS dashboard_client ur_ros2_control_node controller_stopper_node urscript_interface
TARGETS dashboard_client ur_ros2_control_node controller_stopper_node urscript_interface robot_state_helper
DESTINATION lib/${PROJECT_NAME}
)

Expand Down
72 changes: 72 additions & 0 deletions ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
#ifndef UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_
#define UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_

#include <chrono>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/create_server.hpp"
#include "std_srvs/srv/trigger.hpp"

#include "ur_dashboard_msgs/action/set_mode.hpp"
#include "ur_dashboard_msgs/msg/safety_mode.hpp"
#include "ur_dashboard_msgs/msg/robot_mode.hpp"
#include "ur_client_library/ur/datatypes.h"

namespace ur_robot_driver
{
class RobotStateHelper
{
public:
using SetModeGoalHandle = rclcpp_action::ServerGoalHandle<ur_dashboard_msgs::action::SetMode>;

RobotStateHelper(const rclcpp::Node::SharedPtr& node);
RobotStateHelper() = delete;
virtual ~RobotStateHelper() = default;

private:
rclcpp::Node::SharedPtr node_;

void robotModeCallback(ur_dashboard_msgs::msg::RobotMode::SharedPtr msg);
void safetyModeCallback(ur_dashboard_msgs::msg::SafetyMode::SharedPtr msg);

void updateRobotState(bool called_from_thread = false);

void doTransition(bool called_from_thread = false);

bool safeDashboardTrigger(rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv, bool called_from_thread = false);

void setModeAcceptCallback(const std::shared_ptr<SetModeGoalHandle> goal_handle);
rclcpp_action::GoalResponse setModeGoalCallback(const rclcpp_action::GoalUUID& uuid,
std::shared_ptr<const ur_dashboard_msgs::action::SetMode::Goal> goal);
rclcpp_action::CancelResponse
setModeCancelCallback(const std::shared_ptr<SetModeGoalHandle> goal_handle);

void setModeExecute(const std::shared_ptr<SetModeGoalHandle> goal_handle);

void startActionServer();
bool is_started_;

std::shared_ptr<ur_dashboard_msgs::action::SetMode::Result> result_;
std::shared_ptr<ur_dashboard_msgs::action::SetMode::Feedback> feedback_;
std::shared_ptr<const ur_dashboard_msgs::action::SetMode::Goal> goal_;
std::shared_ptr<SetModeGoalHandle> current_goal_handle_;

urcl::RobotMode robot_mode_;
urcl::SafetyMode safety_mode_;

rclcpp_action::Server<ur_dashboard_msgs::action::SetMode>::SharedPtr set_mode_as_;

rclcpp::Subscription<ur_dashboard_msgs::msg::RobotMode>::SharedPtr robot_mode_sub_;
rclcpp::Subscription<ur_dashboard_msgs::msg::SafetyMode>::SharedPtr safety_mode_sub_;

rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr unlock_protective_stop_srv_;
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr restart_safety_srv_;
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr power_on_srv_;
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr power_off_srv_;
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr brake_release_srv_;
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr stop_program_srv_;
rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr play_program_srv_;
};
} // namespace ur_robot_driver

#endif // UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_
9 changes: 9 additions & 0 deletions ur_robot_driver/launch/ur_control.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -236,6 +236,14 @@ def launch_setup(context, *args, **kwargs):
parameters=[{"robot_ip": robot_ip}],
)

robot_state_helper_node = Node(
package="ur_robot_driver",
executable="robot_state_helper",
name="robot_state_helper",
output="screen",
)


tool_communication_node = Node(
package="ur_robot_driver",
condition=IfCondition(use_tool_communication),
Expand Down Expand Up @@ -354,6 +362,7 @@ def controller_spawner(name, active=True):
control_node,
ur_control_node,
dashboard_client_node,
robot_state_helper_node,
tool_communication_node,
controller_stopper_node,
urscript_interface,
Expand Down
Loading

0 comments on commit 7f10ece

Please sign in to comment.