diff --git a/.gitignore b/.gitignore index c276ad4..282879d 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ .cache/ +.clangd/ compile_commands.json *.pyc diff --git a/abb_bringup/launch/abb_moveit_no_rviz.launch.py b/abb_bringup/launch/abb_moveit_no_rviz.launch.py new file mode 100644 index 0000000..33e1748 --- /dev/null +++ b/abb_bringup/launch/abb_moveit_no_rviz.launch.py @@ -0,0 +1,196 @@ +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import os +import yaml + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def launch_setup(context, *args, **kwargs): + # Command-line arguments + robot_xacro_file = LaunchConfiguration("robot_xacro_file") + support_package = LaunchConfiguration("support_package") + moveit_config_package = LaunchConfiguration("moveit_config_package") + moveit_config_file = LaunchConfiguration("moveit_config_file") + + # Planning context + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(support_package), "urdf", robot_xacro_file] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(moveit_config_package), "config", moveit_config_file] + ), + ] + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_content.perform( + context + ) + } + + kinematics_yaml = load_yaml( + "abb_irb1200_5_90_moveit_config", "config/kinematics.yaml" + ) + + # Planning Functionality + ompl_planning_pipeline_config = { + "move_group": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + "start_state_max_bounds_error": 0.1, + } + } + ompl_planning_yaml = load_yaml( + "abb_irb1200_5_90_moveit_config", "config/ompl_planning.yaml" + ) + ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) + + # Trajectory Execution Functionality + moveit_simple_controllers_yaml = load_yaml( + "abb_irb1200_5_90_moveit_config", "config/moveit_controllers.yaml" + ) + moveit_controllers = { + "moveit_simple_controller_manager": moveit_simple_controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + } + + trajectory_execution = { + # MoveIt does not handle controller switching automatically + "moveit_manage_controllers": False, + "trajectory_execution.allowed_execution_duration_scaling": 1.2, + "trajectory_execution.allowed_goal_duration_margin": 0.5, + "trajectory_execution.allowed_start_tolerance": 0.01, + } + + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + } + + # Start the actual move_group node/action server + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + ], + ) + + # RViz + rviz_base = os.path.join( + get_package_share_directory("abb_irb1200_5_90_moveit_config"), "rviz" + ) + rviz_config = os.path.join(rviz_base, "moveit.rviz") + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config], + parameters=[ + robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + kinematics_yaml, + ], + ) + + # Static TF + static_tf_node = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"], + ) + + # Publish TF + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + nodes_to_start = [move_group_node, rviz_node, static_tf_node, robot_state_pub_node] + return nodes_to_start + + +def generate_launch_description(): + + declared_arguments = [] + + # TODO(andyz): add other options + declared_arguments.append( + DeclareLaunchArgument( + "robot_xacro_file", + description="Xacro describing the robot.", + choices=["irb1200_5_90.xacro"], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "support_package", + description="Name of the support package", + choices=["abb_irb1200_support"], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_config_package", + description="Name of the support package", + choices=["abb_irb1200_5_90_moveit_config"], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_config_file", + description="Name of the SRDF file", + choices=["abb_irb1200_5_90.srdf.xacro"], + ) + ) + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/abb_common/CMakeLists.txt b/abb_common/CMakeLists.txt new file mode 100644 index 0000000..19314cc --- /dev/null +++ b/abb_common/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 3.8) +project(abb_common) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-W -Wall -Wextra + -Wwrite-strings -Wunreachable-code -Wpointer-arith + -Winit-self -Wredundant-decls + -Wno-unused-parameter -Wno-unused-function) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + abb_egm_rws_managers + abb_egm_msgs + abb_rapid_msgs + abb_rapid_sm_addin_msgs + abb_robot_msgs + rclcpp +) + +# find dependencies +find_package(ament_cmake REQUIRED) + +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(${PROJECT_NAME} + SHARED + src/mapping.cpp + src/utilities.cpp +) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(${PROJECT_NAME}) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install( + DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/abb_common/include/abb_common/mapping.hpp b/abb_common/include/abb_common/mapping.hpp new file mode 100644 index 0000000..d6e7c73 --- /dev/null +++ b/abb_common/include/abb_common/mapping.hpp @@ -0,0 +1,265 @@ +/*********************************************************************************************************************** + * + * Copyright (c) 2020, ABB Schweiz AG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with + * or without modification, are permitted provided that + * the following conditions are met: + * + * * Redistributions of source code must retain the + * above copyright notice, this list of conditions + * and the following disclaimer. + * * Redistributions in binary form must reproduce the + * above copyright notice, this list of conditions + * and the following disclaimer in the documentation + * and/or other materials provided with the + * distribution. + * * Neither the name of ABB nor the names of its + * contributors may be used to endorse or promote + * products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************************************************************** + */ + +// This file is a modified copy from +// https://github.com/ros-industrial/abb_robot_driver/blob/master/abb_robot_cpp_utilities/include/abb_robot_cpp_utilities/mapping.h + +#pragma once + +#include +#include + +#include +#include + +#include +#include +#include + +namespace abb +{ +namespace robot +{ +namespace utilities +{ +/** + * \brief Maps RAPID task execution state to ROS representation. + * + * \param state to map. + * + * \return uint8_t containing the mapped state. + */ +uint8_t map(const rws::RWSInterface::RAPIDTaskExecutionState state); + +/** + * \brief Maps RobotWare StateMachine Add-In state to ROS representation. + * + * \param state to map. + * + * \return uint8_t containing the mapped state. + */ +uint8_t mapStateMachineState(const rws::RAPIDNum& state); + +/** + * \brief Maps RobotWare StateMachine Add-In EGM action to ROS representation. + * + * \param action to map. + * + * \return uint8_t containing the mapped state. + */ +uint8_t mapStateMachineEGMAction(const rws::RAPIDNum& action); + +/** + * \brief Maps RobotWare StateMachine Add-In SmartGripper command to RWS representation. + * + * \param command to map. + * + * \return unsigned int containing the mapped command. + * + * \throw std::runtime_error if the command is unknown. + */ +unsigned int mapStateMachineSGCommand(const unsigned int command); + +/** + * \brief Maps a RAPID 'pos' data type from RWS to ROS representation. + * + * \param rws_pos to map. + * + * \return abb_rapid_msgs::Pos containing the mapped data. + */ +abb_rapid_msgs::msg::Pos map(const rws::Pos& rws_pos); + +/** + * \brief Maps a RAPID 'orient' data type from RWS to ROS representation. + * + * \param rws_orient to map. + * + * \return abb_rapid_msgs::Orient containing the mapped data. + */ +abb_rapid_msgs::msg::Orient map(const rws::Orient& rws_orient); + +/** + * \brief Maps a RAPID 'pose' data type from RWS to ROS representation. + * + * \param rws_pose to map. + * + * \return abb_rapid_msgs::Pose containing the mapped data. + */ +abb_rapid_msgs::msg::Pose map(const rws::Pose& rws_pose); + +/** + * \brief Maps a RAPID 'loaddata' data type from RWS to ROS representation. + * + * \param rws_loaddata to map. + * + * \return abb_rapid_msgs::LoadData containing the mapped data. + */ +abb_rapid_msgs::msg::LoadData map(const rws::LoadData& rws_loaddata); + +/** + * \brief Maps a RAPID 'tooldata' data type from RWS to ROS representation. + * + * \param rws_tooldata to map. + * + * \return abb_rapid_msgs::ToolData containing the mapped data. + */ +abb_rapid_msgs::msg::ToolData map(const rws::ToolData& rws_tooldata); + +/** + * \brief Maps a RAPID 'wobjdata' data type from RWS to ROS representation. + * + * \param rws_wobjdata to map. + * + * \return abb_rapid_msgs::WObjData containing the mapped data. + */ +abb_rapid_msgs::msg::WObjData map(const rws::WObjData& rws_wobjdata); + +/** + * \brief Maps a RobotWare StateMachine Add-In RAPID 'EGMSettings' data type from RWS to ROS + * representation. + * + * \param rws_egm_settings to map. + * + * \return abb_rapid_sm_addin_msgs::EGMSettings containing the mapped data. + */ +abb_rapid_sm_addin_msgs::msg::EGMSettings map(const rws::RWSStateMachineInterface::EGMSettings& rws_egm_settings); + +/** + * \brief Maps a RAPID 'pos' data type from ROS to RWS representation. + * + * \param ros_pos to map. + * + * \return rws::Pos containing the mapped data. + */ +rws::Pos map(const abb_rapid_msgs::msg::Pos& ros_pos); + +/** + * \brief Maps a RAPID 'orient' data type from ROS to RWS representation. + * + * \param ros_orient to map. + * + * \return rws::Orient containing the mapped data. + */ +rws::Orient map(const abb_rapid_msgs::msg::Orient& ros_orient); + +/** + * \brief Maps a RAPID 'pose' data type from ROS to RWS representation. + * + * \param ros_pose to map. + * + * \return rws::Pose containing the mapped data. + */ +rws::Pose map(const abb_rapid_msgs::msg::Pose& ros_pose); + +/** + * \brief Maps a RAPID 'loaddata' data type from ROS to RWS representation. + * + * \param ros_loaddata to map. + * + * \return rws::LoadData containing the mapped data. + */ +rws::LoadData map(const abb_rapid_msgs::msg::LoadData& ros_loaddata); + +/** + * \brief Maps a RAPID 'tooldata' data type from ROS to RWS representation. + * + * \param ros_tooldata to map. + * + * \return rws::ToolData containing the mapped data. + */ +rws::ToolData map(const abb_rapid_msgs::msg::ToolData& ros_tooldata); + +/** + * \brief Maps a RAPID 'wobjdata' data type from ROS to RWS representation. + * + * \param ros_wobjdata to map. + * + * \return rws::WObjData containing the mapped data. + */ +rws::WObjData map(const abb_rapid_msgs::msg::WObjData& ros_wobjdata); + +/** + * \brief Maps a RobotWare StateMachine Add-In RAPID 'EGMSettings' data type from ROS to RWS + * representation. + * + * \param ros_egm_settings to map. + * + * \return rws::RWSStateMachineInterface::EGMSettings containing the mapped data. + */ +rws::RWSStateMachineInterface::EGMSettings map(const abb_rapid_sm_addin_msgs::msg::EGMSettings& ros_egm_settings); + +/** + * \brief Maps EGM state to ROS representation. + * + * \param state to map. + * + * \return uint8_t containing the mapped state. + */ +uint8_t map(const egm::wrapper::Status::EGMState state); + +/** + * \brief Maps motor state to ROS representation. + * + * \param state to map. + * + * \return uint8_t containing the mapped state. + */ +uint8_t map(const egm::wrapper::Status::MotorState state); + +/** + * \brief Maps RAPID execution state to ROS representation. + * + * \param state to map. + * + * \return uint8_t containing the mapped state. + */ +uint8_t map(const egm::wrapper::Status::RAPIDExecutionState rapid_execution_state); + +/** + * \brief Maps a vector to a string (e.g. for logging). + * + * \param vector to map. + * + * \return std::string of the mapped vector. + * + * \throw std::runtime if the mapping failed. + */ +template +std::string mapVectorToString(const std::vector& vector); + +} // namespace utilities +} // namespace robot +} // namespace abb diff --git a/abb_hardware_interface/include/abb_hardware_interface/utilities.hpp b/abb_common/include/abb_common/utilities.hpp similarity index 100% rename from abb_hardware_interface/include/abb_hardware_interface/utilities.hpp rename to abb_common/include/abb_common/utilities.hpp diff --git a/abb_common/package.xml b/abb_common/package.xml new file mode 100644 index 0000000..3ed0ee0 --- /dev/null +++ b/abb_common/package.xml @@ -0,0 +1,29 @@ + + + + abb_common + 0.0.0 + Package providing common function for abb_ros2 + + Grzegorz Bartyzel + + Apache2 + BSD-3-Clause + + Jon Tjerngren + ament_cmake + + rclcpp + abb_egm_rws_managers + abb_egm_msgs + abb_rapid_msgs + abb_rapid_sm_addin_msgs + abb_robot_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/abb_common/src/mapping.cpp b/abb_common/src/mapping.cpp new file mode 100644 index 0000000..46f7bd2 --- /dev/null +++ b/abb_common/src/mapping.cpp @@ -0,0 +1,439 @@ +/*********************************************************************************************************************** + * + * Copyright (c) 2020, ABB Schweiz AG + * All rights reserved. + * + * Redistribution and use in source and binary forms, with + * or without modification, are permitted provided that + * the following conditions are met: + * + * * Redistributions of source code must retain the + * above copyright notice, this list of conditions + * and the following disclaimer. + * * Redistributions in binary form must reproduce the + * above copyright notice, this list of conditions + * and the following disclaimer in the documentation + * and/or other materials provided with the + * distribution. + * * Neither the name of ABB nor the names of its + * contributors may be used to endorse or promote + * products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + *********************************************************************************************************************** + */ + +// This file is a modified copy from +// https://github.com/ros-industrial/abb_robot_driver/blob/master/abb_robot_cpp_utilities/src/mapping.cpp + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace abb +{ +namespace robot +{ +namespace utilities +{ +uint8_t map(const rws::RWSInterface::RAPIDTaskExecutionState state) +{ + switch (state) + { + case rws::RWSInterface::UNKNOWN: + return abb_robot_msgs::msg::RAPIDTaskState::EXECUTION_STATE_UNKNOWN; + + case rws::RWSInterface::READY: + return abb_robot_msgs::msg::RAPIDTaskState::EXECUTION_STATE_READY; + + case rws::RWSInterface::STOPPED: + return abb_robot_msgs::msg::RAPIDTaskState::EXECUTION_STATE_STOPPED; + + case rws::RWSInterface::STARTED: + return abb_robot_msgs::msg::RAPIDTaskState::EXECUTION_STATE_STARTED; + + case rws::RWSInterface::UNINITIALIZED: + return abb_robot_msgs::msg::RAPIDTaskState::EXECUTION_STATE_UNINITIALIZED; + + default: + return abb_robot_msgs::msg::RAPIDTaskState::EXECUTION_STATE_UNKNOWN; + } +} + +uint8_t mapStateMachineState(const rws::RAPIDNum& state) +{ + switch (static_cast(state.value)) + { + case 0: + return abb_rapid_sm_addin_msgs::msg::StateMachineState::SM_STATE_IDLE; + + case 1: + return abb_rapid_sm_addin_msgs::msg::StateMachineState::SM_STATE_INITIALIZE; + + case 2: + return abb_rapid_sm_addin_msgs::msg::StateMachineState::SM_STATE_RUN_RAPID_ROUTINE; + + case 3: + return abb_rapid_sm_addin_msgs::msg::StateMachineState::SM_STATE_RUN_EGM_ROUTINE; + + default: + return abb_rapid_sm_addin_msgs::msg::StateMachineState::SM_STATE_UNKNOWN; + } +} + +uint8_t mapStateMachineEGMAction(const rws::RAPIDNum& action) +{ + switch (static_cast(action.value)) + { + case 0: + return abb_rapid_sm_addin_msgs::msg::StateMachineState::EGM_ACTION_NONE; + + case 1: + return abb_rapid_sm_addin_msgs::msg::StateMachineState::EGM_ACTION_RUN_JOINT; + + case 2: + return abb_rapid_sm_addin_msgs::msg::StateMachineState::EGM_ACTION_RUN_POSE; + + case 3: + return abb_rapid_sm_addin_msgs::msg::StateMachineState::EGM_ACTION_STOP; + + case 4: + return abb_rapid_sm_addin_msgs::msg::StateMachineState::EGM_ACTION_START_STREAM; + + case 5: + return abb_rapid_sm_addin_msgs::msg::StateMachineState::EGM_ACTION_STOP_STREAM; + + default: + return abb_rapid_sm_addin_msgs::msg::StateMachineState::SM_STATE_UNKNOWN; + } +} + +abb_rapid_msgs::msg::Pos map(const rws::Pos& rws_pos) +{ + abb_rapid_msgs::msg::Pos ros_pos; + ros_pos.x = rws_pos.x.value; + ros_pos.y = rws_pos.y.value; + ros_pos.z = rws_pos.z.value; + return ros_pos; +} + +abb_rapid_msgs::msg::Orient map(const rws::Orient& rws_orient) +{ + abb_rapid_msgs::msg::Orient ros_orient; + ros_orient.q1 = rws_orient.q1.value; + ros_orient.q2 = rws_orient.q2.value; + ros_orient.q3 = rws_orient.q3.value; + ros_orient.q4 = rws_orient.q4.value; + return ros_orient; +} + +abb_rapid_msgs::msg::Pose map(const rws::Pose& rws_pose) +{ + abb_rapid_msgs::msg::Pose ros_pose; + ros_pose.trans = map(rws_pose.pos); + ros_pose.rot = map(rws_pose.rot); + return ros_pose; +} + +abb_rapid_msgs::msg::LoadData map(const rws::LoadData& rws_loaddata) +{ + abb_rapid_msgs::msg::LoadData ros_loaddata; + ros_loaddata.mass = rws_loaddata.mass.value; + ros_loaddata.cog = map(rws_loaddata.cog); + ros_loaddata.aom = map(rws_loaddata.aom); + ros_loaddata.ix = rws_loaddata.ix.value; + ros_loaddata.iy = rws_loaddata.iy.value; + ros_loaddata.iz = rws_loaddata.iz.value; + return ros_loaddata; +} + +abb_rapid_msgs::msg::ToolData map(const rws::ToolData& rws_tooldata) +{ + abb_rapid_msgs::msg::ToolData ros_tooldata; + ros_tooldata.robhold = rws_tooldata.robhold.value; + ros_tooldata.tframe = map(rws_tooldata.tframe); + ros_tooldata.tload = map(rws_tooldata.tload); + return ros_tooldata; +} + +abb_rapid_msgs::msg::WObjData map(const rws::WObjData& rws_wobjdata) +{ + abb_rapid_msgs::msg::WObjData ros_wobjdata; + ros_wobjdata.robhold = rws_wobjdata.robhold.value; + ros_wobjdata.ufprog = rws_wobjdata.ufprog.value; + ros_wobjdata.ufmec = rws_wobjdata.ufmec.value; + ros_wobjdata.uframe = map(rws_wobjdata.uframe); + ros_wobjdata.oframe = map(rws_wobjdata.oframe); + return ros_wobjdata; +} + +abb_rapid_sm_addin_msgs::msg::EGMSettings map(const rws::RWSStateMachineInterface::EGMSettings& rws_egm_settings) +{ + abb_rapid_sm_addin_msgs::msg::EGMSettings ros_egm_settings; + + ros_egm_settings.allow_egm_motions = rws_egm_settings.allow_egm_motions.value; + ros_egm_settings.use_presync = rws_egm_settings.use_presync.value; + + ros_egm_settings.setup_uc.use_filtering = rws_egm_settings.setup_uc.use_filtering.value; + ros_egm_settings.setup_uc.comm_timeout = rws_egm_settings.setup_uc.comm_timeout.value; + + ros_egm_settings.activate.tool = utilities::map(rws_egm_settings.activate.tool); + ros_egm_settings.activate.wobj = utilities::map(rws_egm_settings.activate.wobj); + ros_egm_settings.activate.correction_frame = utilities::map(rws_egm_settings.activate.correction_frame); + ros_egm_settings.activate.sensor_frame = utilities::map(rws_egm_settings.activate.sensor_frame); + ros_egm_settings.activate.cond_min_max = rws_egm_settings.activate.cond_min_max.value; + ros_egm_settings.activate.lp_filter = rws_egm_settings.activate.lp_filter.value; + ros_egm_settings.activate.sample_rate = rws_egm_settings.activate.sample_rate.value; + ros_egm_settings.activate.max_speed_deviation = rws_egm_settings.activate.max_speed_deviation.value; + + ros_egm_settings.run.cond_time = rws_egm_settings.run.cond_time.value; + ros_egm_settings.run.ramp_in_time = rws_egm_settings.run.ramp_in_time.value; + ros_egm_settings.run.offset = utilities::map(rws_egm_settings.run.offset); + ros_egm_settings.run.pos_corr_gain = rws_egm_settings.run.pos_corr_gain.value; + + ros_egm_settings.stop.ramp_out_time = rws_egm_settings.stop.ramp_out_time.value; + + return ros_egm_settings; +} + +unsigned int mapStateMachineSGCommand(const unsigned int command) +{ + switch (command) + { + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_NONE: + return rws::RWSStateMachineInterface::SG_COMMAND_NONE; + + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_INITIALIZE: + return rws::RWSStateMachineInterface::SG_COMMAND_INITIALIZE; + + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_CALIBRATE: + return rws::RWSStateMachineInterface::SG_COMMAND_CALIBRATE; + + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_MOVE_TO: + return rws::RWSStateMachineInterface::SG_COMMAND_MOVE_TO; + + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_GRIP_IN: + return rws::RWSStateMachineInterface::SG_COMMAND_GRIP_IN; + + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_GRIP_OUT: + return rws::RWSStateMachineInterface::SG_COMMAND_GRIP_OUT; + + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_BLOW_ON_1: + return rws::RWSStateMachineInterface::SG_COMMAND_BLOW_ON_1; + + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_BLOW_ON_2: + return rws::RWSStateMachineInterface::SG_COMMAND_BLOW_ON_2; + + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_BLOW_OFF_1: + return rws::RWSStateMachineInterface::SG_COMMAND_BLOW_OFF_1; + + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_BLOW_OFF_2: + return rws::RWSStateMachineInterface::SG_COMMAND_BLOW_OFF_2; + + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_VACUUM_ON_1: + return rws::RWSStateMachineInterface::SG_COMMAND_VACUUM_ON_1; + + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_VACUUM_ON_2: + return rws::RWSStateMachineInterface::SG_COMMAND_VACUUM_ON_2; + + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_VACUUM_OFF_1: + return rws::RWSStateMachineInterface::SG_COMMAND_VACUUM_OFF_1; + + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_VACUUM_OFF_2: + return rws::RWSStateMachineInterface::SG_COMMAND_VACUUM_OFF_2; + + case abb_rapid_sm_addin_msgs::srv::SetSGCommand::Request::SG_COMMAND_UNKNOWN: + default: + throw std::runtime_error{ "Unknown SmartGripper command" }; + } +} + +rws::Pos map(const abb_rapid_msgs::msg::Pos& ros_pos) +{ + rws::Pos rws_pos; + rws_pos.x.value = ros_pos.x; + rws_pos.y.value = ros_pos.y; + rws_pos.z.value = ros_pos.z; + return rws_pos; +} + +rws::Orient map(const abb_rapid_msgs::msg::Orient& ros_orient) +{ + rws::Orient rws_orient; + rws_orient.q1 = ros_orient.q1; + rws_orient.q2 = ros_orient.q2; + rws_orient.q3 = ros_orient.q3; + rws_orient.q4 = ros_orient.q4; + return rws_orient; +} + +rws::Pose map(const abb_rapid_msgs::msg::Pose& ros_pose) +{ + rws::Pose rws_pose; + rws_pose.pos = map(ros_pose.trans); + rws_pose.rot = map(ros_pose.rot); + return rws_pose; +} + +rws::LoadData map(const abb_rapid_msgs::msg::LoadData& ros_loaddata) +{ + rws::LoadData rws_loaddata; + rws_loaddata.mass.value = ros_loaddata.mass; + rws_loaddata.cog = map(ros_loaddata.cog); + rws_loaddata.aom = map(ros_loaddata.aom); + rws_loaddata.ix.value = ros_loaddata.ix; + rws_loaddata.iy.value = ros_loaddata.iy; + rws_loaddata.iz.value = ros_loaddata.iz; + return rws_loaddata; +} + +rws::ToolData map(const abb_rapid_msgs::msg::ToolData& ros_tooldata) +{ + rws::ToolData rws_tooldata; + rws_tooldata.robhold = ros_tooldata.robhold; + rws_tooldata.tframe = map(ros_tooldata.tframe); + rws_tooldata.tload = map(ros_tooldata.tload); + return rws_tooldata; +} + +rws::WObjData map(const abb_rapid_msgs::msg::WObjData& ros_wobjdata) +{ + rws::WObjData rws_wobjdata; + rws_wobjdata.robhold.value = ros_wobjdata.robhold; + rws_wobjdata.ufprog.value = ros_wobjdata.ufprog; + rws_wobjdata.ufmec.value = ros_wobjdata.ufmec; + rws_wobjdata.uframe = map(ros_wobjdata.uframe); + rws_wobjdata.oframe = map(ros_wobjdata.oframe); + return rws_wobjdata; +} + +rws::RWSStateMachineInterface::EGMSettings map(const abb_rapid_sm_addin_msgs::msg::EGMSettings& ros_egm_settings) +{ + rws::RWSStateMachineInterface::EGMSettings rws_egm_settings; + + rws_egm_settings.allow_egm_motions.value = ros_egm_settings.allow_egm_motions; + rws_egm_settings.use_presync.value = ros_egm_settings.use_presync; + + rws_egm_settings.setup_uc.use_filtering.value = ros_egm_settings.setup_uc.use_filtering; + rws_egm_settings.setup_uc.comm_timeout.value = ros_egm_settings.setup_uc.comm_timeout; + + rws_egm_settings.activate.tool = utilities::map(ros_egm_settings.activate.tool); + rws_egm_settings.activate.wobj = utilities::map(ros_egm_settings.activate.wobj); + rws_egm_settings.activate.correction_frame = utilities::map(ros_egm_settings.activate.correction_frame); + rws_egm_settings.activate.sensor_frame = utilities::map(ros_egm_settings.activate.sensor_frame); + rws_egm_settings.activate.cond_min_max = ros_egm_settings.activate.cond_min_max; + rws_egm_settings.activate.lp_filter = ros_egm_settings.activate.lp_filter; + rws_egm_settings.activate.sample_rate = ros_egm_settings.activate.sample_rate; + rws_egm_settings.activate.max_speed_deviation = ros_egm_settings.activate.max_speed_deviation; + + rws_egm_settings.run.cond_time = ros_egm_settings.run.cond_time; + rws_egm_settings.run.ramp_in_time = ros_egm_settings.run.ramp_in_time; + rws_egm_settings.run.offset = utilities::map(ros_egm_settings.run.offset); + rws_egm_settings.run.pos_corr_gain = ros_egm_settings.run.pos_corr_gain; + + rws_egm_settings.stop.ramp_out_time = ros_egm_settings.stop.ramp_out_time; + + return rws_egm_settings; +} + +uint8_t map(egm::wrapper::Status::EGMState state) +{ + switch (state) + { + case egm::wrapper::Status::EGM_ERROR: + return abb_egm_msgs::msg::EGMChannelState::EGM_ERROR; + + case egm::wrapper::Status::EGM_STOPPED: + return abb_egm_msgs::msg::EGMChannelState::EGM_STOPPED; + + case egm::wrapper::Status::EGM_RUNNING: + return abb_egm_msgs::msg::EGMChannelState::EGM_RUNNING; + + case egm::wrapper::Status::EGM_UNDEFINED: + default: + return abb_egm_msgs::msg::EGMChannelState::EGM_UNDEFINED; + } +} + +uint8_t map(egm::wrapper::Status::MotorState state) +{ + switch (state) + { + case egm::wrapper::Status::MOTORS_ON: + return abb_egm_msgs::msg::EGMChannelState::MOTORS_ON; + + case egm::wrapper::Status::MOTORS_OFF: + return abb_egm_msgs::msg::EGMChannelState::MOTORS_OFF; + + case egm::wrapper::Status::MOTORS_UNDEFINED: + default: + return abb_egm_msgs::msg::EGMChannelState::MOTORS_UNDEFINED; + } +} + +uint8_t map(egm::wrapper::Status::RAPIDExecutionState state) +{ + switch (state) + { + case egm::wrapper::Status::RAPID_STOPPED: + return abb_egm_msgs::msg::EGMChannelState::RAPID_STOPPED; + + case egm::wrapper::Status::RAPID_RUNNING: + return abb_egm_msgs::msg::EGMChannelState::RAPID_RUNNING; + + case egm::wrapper::Status::RAPID_UNDEFINED: + default: + return abb_egm_msgs::msg::EGMChannelState::RAPID_UNDEFINED; + } +} + +template +std::string mapVectorToString(const std::vector& vector) +{ + std::stringstream ss; + + ss << "["; + for (size_t i = 0; i < vector.size(); ++i) + { + ss << vector[i]; + + if (ss.fail()) + { + std::string error_message{ "Failed to map vector to string" }; + throw std::runtime_error{ error_message }; + } + + ss << (i < vector.size() - 1 ? ", " : ""); + } + ss << "]"; + + return ss.str(); +} + +template std::string mapVectorToString(const std::vector& vector); +template std::string mapVectorToString(const std::vector& vector); +template std::string mapVectorToString(const std::vector& vector); +template std::string mapVectorToString(const std::vector& vector); + +} // namespace utilities +} // namespace robot +} // namespace abb diff --git a/abb_hardware_interface/src/utilities.cpp b/abb_common/src/utilities.cpp similarity index 98% rename from abb_hardware_interface/src/utilities.cpp rename to abb_common/src/utilities.cpp index 5a94675..ce7c91d 100644 --- a/abb_hardware_interface/src/utilities.cpp +++ b/abb_common/src/utilities.cpp @@ -39,7 +39,7 @@ // https://github.com/ros-industrial/abb_robot_driver/blob/master/abb_robot_cpp_utilities/src/initialization.cpp // https://github.com/ros-industrial/abb_robot_driver/blob/master/abb_robot_cpp_utilities/src/verification.cpp -#include +#include #include #include diff --git a/abb_hardware_interface/CMakeLists.txt b/abb_hardware_interface/CMakeLists.txt index 6adc616..49c6dfc 100644 --- a/abb_hardware_interface/CMakeLists.txt +++ b/abb_hardware_interface/CMakeLists.txt @@ -19,6 +19,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() set(THIS_PACKAGE_INCLUDE_DEPENDS + abb_common abb_egm_rws_managers hardware_interface pluginlib @@ -41,7 +42,6 @@ add_library( ${PROJECT_NAME} SHARED src/abb_hardware_interface.cpp - src/utilities.cpp ) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_include_directories( diff --git a/abb_hardware_interface/package.xml b/abb_hardware_interface/package.xml index e518439..d12e31f 100644 --- a/abb_hardware_interface/package.xml +++ b/abb_hardware_interface/package.xml @@ -12,10 +12,10 @@ BSD-3-Clause Dmitri Ignakov - Jon Tjerngren + Jon Tjerngren ament_cmake + abb_common abb_egm_rws_managers hardware_interface pluginlib diff --git a/abb_hardware_interface/src/abb_hardware_interface.cpp b/abb_hardware_interface/src/abb_hardware_interface.cpp index 43ef979..922f820 100644 --- a/abb_hardware_interface/src/abb_hardware_interface.cpp +++ b/abb_hardware_interface/src/abb_hardware_interface.cpp @@ -14,7 +14,7 @@ // limitations under the License. #include -#include +#include using namespace std::chrono_literals; diff --git a/abb_rws_client/CMakeLists.txt b/abb_rws_client/CMakeLists.txt index 7a78054..f85b211 100644 --- a/abb_rws_client/CMakeLists.txt +++ b/abb_rws_client/CMakeLists.txt @@ -19,12 +19,12 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() set(THIS_PACKAGE_INCLUDE_DEPENDS + abb_common abb_egm_rws_managers abb_egm_msgs abb_robot_msgs abb_rapid_msgs abb_rapid_sm_addin_msgs - abb_hardware_interface rclcpp sensor_msgs ) @@ -43,7 +43,6 @@ endforeach() add_library(rws_client_lib src/rws_service_provider_ros.cpp src/rws_state_publisher_ros.cpp - src/mapping.cpp ) ament_target_dependencies(rws_client_lib ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_include_directories( diff --git a/abb_rws_client/package.xml b/abb_rws_client/package.xml index a167d0c..265711a 100644 --- a/abb_rws_client/package.xml +++ b/abb_rws_client/package.xml @@ -3,18 +3,24 @@ abb_rws_client 0.0.0 - TODO: Package description + Package for receiving data and sending commands from ABB robots via RWS interface. + Grzegorz Bartyzel + Apache2 + BSD-3-Clause - rclcpp + Jon Tjerngren + ament_cmake + + abb_common abb_egm_rws_managers - sensor_msgs abb_egm_msgs abb_rapid_msgs - abb_robot_msgs abb_rapid_sm_addin_msgs - abb_hardware_interface + abb_robot_msgs + rclcpp + sensor_msgs ament_cmake_gtest diff --git a/abb_rws_client/src/rws_service_provider_ros.cpp b/abb_rws_client/src/rws_service_provider_ros.cpp index 65cea04..3970a9e 100644 --- a/abb_rws_client/src/rws_service_provider_ros.cpp +++ b/abb_rws_client/src/rws_service_provider_ros.cpp @@ -42,8 +42,8 @@ #include -#include -#include +#include +#include using RAPIDSymbols = abb::rws::RWSStateMachineInterface::ResourceIdentifiers::RAPID::Symbols; diff --git a/abb_rws_client/src/rws_state_publisher_ros.cpp b/abb_rws_client/src/rws_state_publisher_ros.cpp index 2b1b365..557b38e 100644 --- a/abb_rws_client/src/rws_state_publisher_ros.cpp +++ b/abb_rws_client/src/rws_state_publisher_ros.cpp @@ -40,8 +40,8 @@ #include -#include -#include +#include +#include namespace {