diff --git a/iisz.urdf b/iisz.urdf new file mode 100644 index 0000000..cd9918d --- /dev/null +++ b/iisz.urdf @@ -0,0 +1,400 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + ign_ros2_control/IgnitionSystem + + + + + + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + + + + + /home/ros/workspace/install/iisy_config/share/iisy_config/config/iisy_controllers.yml + + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Grey + + diff --git a/src/controller_test/CMakeLists.txt b/src/controller_test/CMakeLists.txt new file mode 100644 index 0000000..16ae478 --- /dev/null +++ b/src/controller_test/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) +project(controller_test) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(control_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(move src/test.cpp) +set_property(TARGET move PROPERTY CXX_STANDARD 17) +ament_target_dependencies(move rclcpp moveit_ros_planning_interface control_msgs) + +install(TARGETS + move + DESTINATION lib/${PROJECT_NAME}) + +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/src/controller_test/package.xml b/src/controller_test/package.xml new file mode 100644 index 0000000..0f4f1d8 --- /dev/null +++ b/src/controller_test/package.xml @@ -0,0 +1,23 @@ + + + + controller_test + 0.1.0 + + Test for Moveit on IISY + + bastian + bastian + TODO: License declaration + + ament_cmake + + rclcpp + geometry_msgs + moveit_ros_planning_interface + + + ament_cmake + + + diff --git a/src/controller_test/src/test.cpp b/src/controller_test/src/test.cpp new file mode 100644 index 0000000..f715544 --- /dev/null +++ b/src/controller_test/src/test.cpp @@ -0,0 +1,196 @@ +#include +#include +#include + +#include +#include + +#include + +std::shared_ptr node; +bool common_goal_accepted = false; +rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN; +int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL; + +void common_goal_response( + rclcpp_action::ClientGoalHandle + ::SharedPtr future) +{ + RCLCPP_DEBUG( + node->get_logger(), "common_goal_response time: %f", + rclcpp::Clock().now().seconds()); + auto goal_handle = future.get(); + if (!goal_handle) { + common_goal_accepted = false; + printf("Goal rejected\n"); + } else { + common_goal_accepted = true; + printf("Goal accepted\n"); + } +} + +void common_result_response( + const rclcpp_action::ClientGoalHandle + ::WrappedResult & result) +{ + printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds()); + common_resultcode = result.code; + common_action_result_code = result.result->error_code; + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + printf("SUCCEEDED result code\n"); + break; + case rclcpp_action::ResultCode::ABORTED: + printf("Goal was aborted\n"); + return; + case rclcpp_action::ResultCode::CANCELED: + printf("Goal was canceled\n"); + return; + default: + printf("Unknown result code\n"); + return; + } +} + +void common_feedback( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) +{ + std::cout << "feedback->desired.positions :"; + for (auto & x : feedback->desired.positions) { + std::cout << x << "\t"; + } + std::cout << std::endl; + std::cout << "feedback->desired.velocities :"; + for (auto & x : feedback->desired.velocities) { + std::cout << x << "\t"; + } + std::cout << std::endl; +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + node = std::make_shared("trajectory_test_node"); + + std::cout << "node created" << std::endl; + + rclcpp_action::Client::SharedPtr action_client; + action_client = rclcpp_action::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + "/arm_controller/follow_joint_trajectory"); + + bool response = + action_client->wait_for_action_server(std::chrono::seconds(1)); + if (!response) { + throw std::runtime_error("could not get action server"); + } + std::cout << "Created action server" << std::endl; + + std::vector joint_names = { + "base_rot", + "base_link1_joint", + "link1_link2_joint", + "link2_rot", + "link2_link3_joint", + "link3_rot" + }; + + std::vector points; + trajectory_msgs::msg::JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap + point.positions.resize(joint_names.size()); + + point.positions[0] = 0.0; + point.positions[1] = 0.0; + point.positions[2] = 0.0; + point.positions[3] = 0.0; + point.positions[4] = 0.0; + point.positions[5] = 0.0; + + trajectory_msgs::msg::JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(1.0); + point2.positions.resize(joint_names.size()); + + point2.positions[0] = -1.0; + point2.positions[1] = 0.0; + point2.positions[2] = 0.0; + point2.positions[3] = 0.0; + point2.positions[4] = 0.0; + point2.positions[5] = 0.0; + + trajectory_msgs::msg::JointTrajectoryPoint point3; + point3.time_from_start = rclcpp::Duration::from_seconds(2.0); + point3.positions.resize(joint_names.size()); + + point3.positions[0] = 1.0; + point3.positions[1] = 0.0; + point3.positions[2] = 0.0; + point3.positions[3] = 0.0; + point3.positions[4] = 0.0; + point3.positions[5] = 0.0; + + trajectory_msgs::msg::JointTrajectoryPoint point4; + point4.time_from_start = rclcpp::Duration::from_seconds(3.0); + point4.positions.resize(joint_names.size()); + + point4.positions[0] = 0.0; + point4.positions[1] = 0.0; + point4.positions[2] = 0.0; + point4.positions[3] = 0.0; + point4.positions[4] = 0.0; + point4.positions[5] = 0.0; + + points.push_back(point); + points.push_back(point2); + points.push_back(point3); + points.push_back(point4); + + rclcpp_action::Client::SendGoalOptions opt; + opt.goal_response_callback = std::bind(common_goal_response, std::placeholders::_1); + opt.result_callback = std::bind(common_result_response, std::placeholders::_1); + opt.feedback_callback = std::bind(common_feedback, std::placeholders::_1, std::placeholders::_2); + + control_msgs::action::FollowJointTrajectory_Goal goal_msg; + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0); + goal_msg.trajectory.joint_names = joint_names; + goal_msg.trajectory.points = points; + + auto goal_handle_future = action_client->async_send_goal(goal_msg, opt); + + if (rclcpp::spin_until_future_complete(node, goal_handle_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "send goal call failed :("); + return 1; + } + RCLCPP_ERROR(node->get_logger(), "send goal call ok :)"); + + rclcpp_action::ClientGoalHandle::SharedPtr + goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); + return 1; + } + RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server"); + + // Wait for the server to be done with the goal + auto result_future = action_client->async_get_result(goal_handle); + RCLCPP_INFO(node->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node, result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "get result call failed :("); + return 1; + } + + std::cout << "async_send_goal" << std::endl; + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/src/ign_world/launch/gazebo_controller_launch.py b/src/ign_world/launch/gazebo_controller_launch.py new file mode 100644 index 0000000..85bec5d --- /dev/null +++ b/src/ign_world/launch/gazebo_controller_launch.py @@ -0,0 +1,81 @@ +import os +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch_ros.actions import Node, SetParameter +from launch.launch_description_sources import PythonLaunchDescriptionSource +from moveit_configs_utils import MoveItConfigsBuilder +import xacro + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs() + + load_joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], + output='screen' + ) + + load_joint_trajectory_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'arm_controller'], + output='screen' + ) + + ignition_spawn_entity = Node( + package='ros_gz_sim', + executable='create', + arguments=[ + '-name', 'iisy', + '-allow_renaming', 'true', + '-string', xacro.process(os.path.join(get_package_share_directory('iisy_config_2'), 'config', 'iisy.urdf.xacro')), + ], + output='screen' + ) + + return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')), + launch_arguments={'gz_args': '-v 4 -r /home/ros/workspace/src/ign_world/world/gaz_new_test.sdf'}.items(), + ), + + Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"], + output='both', + ), + + SetParameter(name="use_sim_time", value=True), + + Node( + package='ros_actor_action_server', + executable='ros_actor_action_server' + ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(moveit_config.package_path / "launch/rsp.launch.py") + ), + ), + + #IncludeLaunchDescription( + # PythonLaunchDescriptionSource( + # str(moveit_config.package_path / "launch/move_group.launch.py") + # ), + #), + + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=ignition_spawn_entity, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_joint_trajectory_controller], + ) + ), + + ignition_spawn_entity, + ]) \ No newline at end of file diff --git a/src/ign_world/launch/moveit_launch.py b/src/ign_world/launch/moveit_launch.py index 197498f..7e284bb 100644 --- a/src/ign_world/launch/moveit_launch.py +++ b/src/ign_world/launch/moveit_launch.py @@ -1,15 +1,17 @@ import os import subprocess - import yaml + from launch import LaunchDescription -from launch_ros.actions import Node,SetParameter -from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, RegisterEventHandler, UnsetEnvironmentVariable +from launch_ros.actions import Node, SetParameter +from launch.actions import IncludeLaunchDescription, RegisterEventHandler, LogInfo, ExecuteProcess, ExecuteLocal, TimerAction from launch_ros.substitutions import FindPackageShare -from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory from launch.event_handlers import OnProcessExit +from launch.launch_description_entity import LaunchDescriptionEntity +from pathlib import Path def load_file(package_name, file_path): @@ -49,6 +51,19 @@ def load_yaml(package_name, file_path): return None +def launch_chain(nodes: list[ExecuteLocal]): + generatedNodes: list[LaunchDescriptionEntity] = [] + for index in range(1, len(nodes)): + generatedNodes.append(RegisterEventHandler( + OnProcessExit( + target_action=nodes[index-1], + on_exit=nodes[index] + ) + )) + generatedNodes.append(nodes[0]) + return generatedNodes + + def generate_launch_description(): robot_description_config = load_xacro('iisy_config', 'urdf/iisy.urdf') @@ -60,10 +75,9 @@ def generate_launch_description(): kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yml') robot_description_kinematics = {'robot_description_kinematics': kinematics_yaml} - moveit_controllers_yaml = load_yaml("iisy_config", "config/iisy_moveit_controllers.yaml") - moveit_controllers = { - "moveit_simple_controller_manager": moveit_controllers_yaml, - "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + controllers = { + "moveit_simple_controller_manager": load_yaml("iisy_config", "config/iisy_controllers.yml"), + "moveit_controller_manager": "moveit_ros_control_interface/Ros2ControlMultiManager", } ompl_planning_pipeline_config = { @@ -86,7 +100,7 @@ def generate_launch_description(): } trajectory_execution = { - 'moveit_manage_controllers': True, + '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 @@ -126,11 +140,71 @@ def generate_launch_description(): robot_description_kinematics, planning, trajectory_execution, - moveit_controllers, + controllers, planning_scene_monitor_parameters, ], ) + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + robot_description, + str(Path(get_package_share_directory("iisy_config")+"/config/iisy_controllers.yaml")), + ], + ) + + load_joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='both' + ) + + load_joint_trajectory_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_trajectory_controller'], + output='both' + ) + + move_group = Node( + package='moveit_ros_move_group', + executable='move_group', + # prefix='xterm -fs 10 -e gdb --ex run --args', + output='both', + parameters=[ + robot_description, + robot_description_semantic, + robot_description_planning, + ompl_planning_pipeline_config, + robot_description_kinematics, + planning, + trajectory_execution, + controllers, + planning_scene_monitor_parameters, + # octomap_config, + # octomap_updater_config + ] + ) + + test = Node( + package='moveit_test', + executable='move', + output='both', + parameters=[ + robot_description, + robot_description_semantic, + robot_description_planning, + ompl_planning_pipeline_config, + robot_description_kinematics, + planning, + trajectory_execution, + controllers, + planning_scene_monitor_parameters, + # octomap_config, + # octomap_updater_config + ], + ) + # octomap_config = {'octomap_frame': 'camera_rgb_optical_frame', # 'octomap_resolution': 0.05, # 'max_range': 5.0} @@ -139,19 +213,55 @@ def generate_launch_description(): pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') - return LaunchDescription([ - SetParameter(name="use_sim_time",value=True), + + #IncludeLaunchDescription( + # PythonLaunchDescriptionSource(os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), + # launch_arguments={'gz_args': '-r /home/ros/workspace/src/ign_world/world/gaz_new_test.sdf'}.items(), + #), + + Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=["/clock[rosgraph_msgs/msg/Clock@ignition.msgs.Clock"], + output='both', + ), + + SetParameter(name="use_sim_time", value=True), + + move_group, + + #RegisterEventHandler( + # OnProcessExit( + # target_action=spawn_robot, + # on_exit=[load_joint_state_broadcaster] + # ) + #), + + controller_manager, + + RegisterEventHandler( + OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_joint_trajectory_controller] + ) + ), + RegisterEventHandler( + OnProcessExit( + target_action=load_joint_trajectory_controller, + on_exit=TimerAction(period=10.0,actions=[test]) + ) + ), + + #spawn_robot, + load_joint_state_broadcaster, + #load_joint_trajectory_controller, + rviz, Node( package='ros_actor_action_server', executable='ros_actor_action_server'), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), - launch_arguments={'gz_args': '-r /home/ros/workspace/src/ign_world/world/gaz_new_test.sdf'}.items(), - ), - Node( package='tf2_ros', executable='static_transform_publisher', @@ -182,20 +292,15 @@ def generate_launch_description(): "--child-frame-id", "lidar_2_link"] ), - Node( - package='ros_gz_bridge', - executable='parameter_bridge', - arguments=["/clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock"], - ), - - Node( - package='ros_gz_bridge', - executable='parameter_bridge', - arguments=["/world/default/model/iisy/joint_state@sensor_msgs/msg/JointState@ignition.msgs.Model"], - remappings=[ - ('/world/default/model/iisy/joint_state', '/joint_states'), - ] - ), + #Node( + # package='ros_gz_bridge', + # executable='parameter_bridge', + # arguments=["/world/default/model/iisy/joint_state[sensor_msgs/msg/JointState@ignition.msgs.Model"], + # output='both', + # remappings=[ + # ('/world/default/model/iisy/joint_state', '/joint_states'), + # ] + #), Node( package='robot_state_publisher', @@ -203,28 +308,4 @@ def generate_launch_description(): output='both', parameters=[robot_description] ), - - Node( - package='moveit_ros_move_group', - executable='move_group', - # prefix='xterm -fs 10 -e gdb --ex run --args', - output='both', - parameters=[ - robot_description, - robot_description_semantic, - robot_description_planning, - ompl_planning_pipeline_config, - robot_description_kinematics, - planning, - trajectory_execution, - moveit_controllers, - planning_scene_monitor_parameters, - # octomap_config, - # octomap_updater_config - ] - ), - spawn_robot, - rviz, - - ]) diff --git a/src/ign_world/launch/new_launch.py b/src/ign_world/launch/new_launch.py new file mode 100644 index 0000000..2d26641 --- /dev/null +++ b/src/ign_world/launch/new_launch.py @@ -0,0 +1,55 @@ +import os +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch_ros.actions import Node, SetParameter +from launch.launch_description_sources import PythonLaunchDescriptionSource +from moveit_configs_utils import MoveItConfigsBuilder +from xacro import process as xacro_process + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs() + + return LaunchDescription([ + SetParameter(name="use_sim_time", value=True), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')), + launch_arguments={'gz_args': '-v 4 -r /home/ros/workspace/src/ign_world/world/gaz_new_test.sdf'}.items(), + ), + + Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"], + output='both', + ), + + #Node( + # package='ros_gz_bridge', + # executable='parameter_bridge', + # arguments=["/world/default/dynamic_pose/info@tf2_msgs/msg/TFMessage]ignition.msgs.Pose_V"], + # output='both', + # remappings=[ + # ('/world/default/dynamic_pose/info', '/tf'), + # ] + #), + + Node( + package='ros_actor_action_server', + executable='ros_actor_action_server' + ), + + #Node( + # package='ros_gz_sim', + # executable='create', + # arguments=['-world', 'default', '-string', xacro_process(os.path.join(get_package_share_directory('iisy_config_2'), 'config', 'iisy_xacro.urdf'))], + # output='screen' + #), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + str(moveit_config.package_path / "launch/demo.launch.py") + ), + ), + ]) \ No newline at end of file diff --git a/src/iisy_config/config/iisy_controllers.yml b/src/iisy_config/config/iisy_controllers.yml index 84195fa..00686b1 100644 --- a/src/iisy_config/config/iisy_controllers.yml +++ b/src/iisy_config/config/iisy_controllers.yml @@ -1,19 +1,16 @@ # see controllers: http://control.ros.org/ros2_controllers/doc/controllers_index.html controller_manager: ros__parameters: - update_rate: 500 # Hz + update_rate: 1000 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster # if left empty, all states are published https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp#L41 - position_trajectory_controller: + joint_trajectory_controller: # find declaration here https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_trajectory_controller/joint_trajectory_plugin.xml#L2 type: joint_trajectory_controller/JointTrajectoryController - forward_position_controller: - type: position_controllers/JointGroupPositionController - -position_trajectory_controller: +joint_trajectory_controller: ros__parameters: # find required parameters here https://github.com/ros-controls/ros2_controllers/blob/master/joint_trajectory_controller/src/joint_trajectory_controller.cpp joints: @@ -27,16 +24,4 @@ position_trajectory_controller: - position state_interfaces: - position - state_publish_rate: 50.0 - action_monitor_rate: 20.0 - -forward_position_controller: - ros__parameters: - joints: - - base_rot - - base_link1_joint - - link1_link2_joint - - link2_rot - - link2_link3_joint - - link3_rot - interface_name: "position" + - velocity diff --git a/src/iisy_config/config/iisy_gazebo_controllers.yml b/src/iisy_config/config/iisy_gazebo_controllers.yml index fe24c21..b1e0ad9 100644 --- a/src/iisy_config/config/iisy_gazebo_controllers.yml +++ b/src/iisy_config/config/iisy_gazebo_controllers.yml @@ -1,7 +1,7 @@ # see controllers: http://control.ros.org/ros2_controllers/doc/controllers_index.html controller_manager: ros__parameters: - update_rate: 500 # Hz + update_rate: 1000 # Hz joint_state_controller: type: joint_state_controller/JointStateController # if left empty, all states are published https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp#L41 \ No newline at end of file diff --git a/src/iisy_config/config/iisy_moveit_controllers.yml b/src/iisy_config/config/iisy_moveit_controllers.yml index 7e8cc98..6f45afe 100644 --- a/src/iisy_config/config/iisy_moveit_controllers.yml +++ b/src/iisy_config/config/iisy_moveit_controllers.yml @@ -1,9 +1,8 @@ controller_names: - - position_trajectory_controller + - joint_trajectory_controller # http://control.ros.org/ros2_controllers/joint_trajectory_controller/doc/userdoc.html#ros2-interface-of-the-controller -position_trajectory_controller: - action_ns: follow_joint_trajectory +joint_trajectory_controller: type: FollowJointTrajectory default: true joints: diff --git a/src/iisy_config/urdf/iisy.urdf b/src/iisy_config/urdf/iisy.urdf index b853a23..4f1ea1d 100644 --- a/src/iisy_config/urdf/iisy.urdf +++ b/src/iisy_config/urdf/iisy.urdf @@ -290,97 +290,50 @@ - - - -1 - 1 - - - -1 - 1 - + + - - - -1 - 1 - - - -1 - 1 - + + - - - -1 - 1 - - - -1 - 1 - + + - - - -1 - 1 - - - -1 - 1 - + + - - - -1 - 1 - - - -1 - 1 - + + - - - -1 - 1 - - - -1 - 1 - + + - - - - - - + $(find iisy_config)/config/iisy_controllers.yml - + lsl 0.2 0.2 Gazebo/Orange diff --git a/src/iisy_config/urdf/iisy_xacro.urdf b/src/iisy_config/urdf/iisy_xacro.urdf new file mode 100644 index 0000000..64b5809 --- /dev/null +++ b/src/iisy_config/urdf/iisy_xacro.urdf @@ -0,0 +1,397 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + ign_ros2_control/IgnitionSystem + + + + + + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + -1 + 1 + + + -1 + 1 + + + + + + + + + /home/ros/workspace/install/iisy_config/share/iisy_config/config/iisy_controllers.yml + + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Grey + + diff --git a/src/iisy_config_2/.setup_assistant b/src/iisy_config_2/.setup_assistant new file mode 100644 index 0000000..4092879 --- /dev/null +++ b/src/iisy_config_2/.setup_assistant @@ -0,0 +1,25 @@ +moveit_setup_assistant_config: + urdf: + package: iisy_config + relative_path: urdf/iisy_xacro.urdf + srdf: + relative_path: config/iisy.srdf + package_settings: + author_name: Bastian Hofmann + author_email: bhogm4@gmail.com + generated_timestamp: 1676632718 + control_xacro: + command: + - position + state: + - position + - velocity + modified_urdf: + xacros: + - control_xacro + control_xacro: + command: + - position + state: + - position + - velocity \ No newline at end of file diff --git a/src/iisy_config_2/CMakeLists.txt b/src/iisy_config_2/CMakeLists.txt new file mode 100644 index 0000000..733207e --- /dev/null +++ b/src/iisy_config_2/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.22) +project(iisy_config_2) + +find_package(ament_cmake REQUIRED) + +ament_package() + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) diff --git a/src/iisy_config_2/config/iisy.ros2_control.xacro b/src/iisy_config_2/config/iisy.ros2_control.xacro new file mode 100644 index 0000000..b18f164 --- /dev/null +++ b/src/iisy_config_2/config/iisy.ros2_control.xacro @@ -0,0 +1,62 @@ + + + + + + + + $(find iisy_config_2)/config/ros2_controllers.yaml + + + + + + + ign_ros2_control/IgnitionSystem + + + + + ${initial_positions['base_rot']} + + + + + + + ${initial_positions['base_link1_joint']} + + + + + + + ${initial_positions['link1_link2_joint']} + + + + + + + ${initial_positions['link2_rot']} + + + + + + + ${initial_positions['link2_link3_joint']} + + + + + + + ${initial_positions['link3_rot']} + + + + + + + diff --git a/src/iisy_config_2/config/iisy.srdf b/src/iisy_config_2/config/iisy.srdf new file mode 100644 index 0000000..2e5792f --- /dev/null +++ b/src/iisy_config_2/config/iisy.srdf @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/iisy_config_2/config/iisy.urdf.xacro b/src/iisy_config_2/config/iisy.urdf.xacro new file mode 100644 index 0000000..b876e91 --- /dev/null +++ b/src/iisy_config_2/config/iisy.urdf.xacro @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/src/iisy_config_2/config/iisy_xacro.urdf b/src/iisy_config_2/config/iisy_xacro.urdf new file mode 100644 index 0000000..feb1072 --- /dev/null +++ b/src/iisy_config_2/config/iisy_xacro.urdf @@ -0,0 +1,300 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Orange + + + 0.2 + 0.2 + Gazebo/Grey + + diff --git a/src/iisy_config_2/config/initial_positions.yaml b/src/iisy_config_2/config/initial_positions.yaml new file mode 100644 index 0000000..a527899 --- /dev/null +++ b/src/iisy_config_2/config/initial_positions.yaml @@ -0,0 +1,9 @@ +# Default initial positions for iisy's ros2_control fake system + +initial_positions: + base_link1_joint: 0 + base_rot: 0 + link1_link2_joint: 0 + link2_link3_joint: 0 + link2_rot: 0 + link3_rot: 0 \ No newline at end of file diff --git a/src/iisy_config_2/config/joint_limits.yaml b/src/iisy_config_2/config/joint_limits.yaml new file mode 100644 index 0000000..64ec7c1 --- /dev/null +++ b/src/iisy_config_2/config/joint_limits.yaml @@ -0,0 +1,40 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + base_link1_joint: + has_velocity_limits: true + max_velocity: 1.7453292519943 + has_acceleration_limits: false + max_acceleration: 0 + base_rot: + has_velocity_limits: true + max_velocity: 1.7453292519943 + has_acceleration_limits: false + max_acceleration: 0 + link1_link2_joint: + has_velocity_limits: true + max_velocity: 1.7453292519943 + has_acceleration_limits: false + max_acceleration: 0 + link2_link3_joint: + has_velocity_limits: true + max_velocity: 1.7453292519943 + has_acceleration_limits: false + max_acceleration: 0 + link2_rot: + has_velocity_limits: true + max_velocity: 1.7453292519943 + has_acceleration_limits: false + max_acceleration: 0 + link3_rot: + has_velocity_limits: true + max_velocity: 1.7453292519943 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/src/iisy_config_2/config/kinematics.yaml b/src/iisy_config_2/config/kinematics.yaml new file mode 100644 index 0000000..9e26dfe --- /dev/null +++ b/src/iisy_config_2/config/kinematics.yaml @@ -0,0 +1 @@ +{} \ No newline at end of file diff --git a/src/iisy_config_2/config/moveit.rviz b/src/iisy_config_2/config/moveit.rviz new file mode 100644 index 0000000..c8705e2 --- /dev/null +++ b/src/iisy_config_2/config/moveit.rviz @@ -0,0 +1,281 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 70 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - /InteractiveMarkers1 + Splitter Ratio: 0.5 + Tree Height: 163 + - Class: rviz_common/Help + Name: Help + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_bottom: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1_full: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2_bottom: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3_bottom: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + table: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: true + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_bottom: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1_full: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2_bottom: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3_bottom: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + table: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.9999999999999999 + - Class: rviz_default_plugins/InteractiveMarkers + Enable Transparency: true + Enabled: true + Interactive Markers Namespace: "" + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 4.384417533874512 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.10000000149011612 + Y: 0.25 + Z: 0.30000001192092896 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4799998998641968 + Target Frame: world + Value: Orbit (rviz_default_plugins) + Yaw: 3.7451796531677246 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 975 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002b400000379fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb000000100044006900730070006c006100790073010000003b00000124000000c700fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000165000001930000016a00fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fe000000b6000000a000ffffff000001f60000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1200 + X: 1360 + Y: 388 diff --git a/src/iisy_config_2/config/moveit_controllers.yaml b/src/iisy_config_2/config/moveit_controllers.yaml new file mode 100644 index 0000000..9c62b9f --- /dev/null +++ b/src/iisy_config_2/config/moveit_controllers.yaml @@ -0,0 +1,19 @@ +# MoveIt uses this configuration for controller management + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - arm_controller + + arm_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - base_rot + - base_link1_joint + - link1_link2_joint + - link2_rot + - link2_link3_joint + - link3_rot \ No newline at end of file diff --git a/src/iisy_config_2/config/pilz_cartesian_limits.yaml b/src/iisy_config_2/config/pilz_cartesian_limits.yaml new file mode 100644 index 0000000..b2997ca --- /dev/null +++ b/src/iisy_config_2/config/pilz_cartesian_limits.yaml @@ -0,0 +1,6 @@ +# Limits for the Pilz planner +cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 diff --git a/src/iisy_config_2/config/ros2_controllers.yaml b/src/iisy_config_2/config/ros2_controllers.yaml new file mode 100644 index 0000000..f919fac --- /dev/null +++ b/src/iisy_config_2/config/ros2_controllers.yaml @@ -0,0 +1,26 @@ +# This config file is used by ros2_control +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +arm_controller: + ros__parameters: + joints: + - base_rot + - base_link1_joint + - link1_link2_joint + - link2_rot + - link2_link3_joint + - link3_rot + command_interfaces: + - position + state_interfaces: + - position + - velocity \ No newline at end of file diff --git a/src/iisy_config_2/launch/demo.launch.py b/src/iisy_config_2/launch/demo.launch.py new file mode 100644 index 0000000..ef97431 --- /dev/null +++ b/src/iisy_config_2/launch/demo.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_demo_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs() + return generate_demo_launch(moveit_config) diff --git a/src/iisy_config_2/launch/move_group.launch.py b/src/iisy_config_2/launch/move_group.launch.py new file mode 100644 index 0000000..6b257c5 --- /dev/null +++ b/src/iisy_config_2/launch/move_group.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_move_group_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs() + return generate_move_group_launch(moveit_config) diff --git a/src/iisy_config_2/launch/moveit_rviz.launch.py b/src/iisy_config_2/launch/moveit_rviz.launch.py new file mode 100644 index 0000000..872ed17 --- /dev/null +++ b/src/iisy_config_2/launch/moveit_rviz.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_moveit_rviz_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs() + return generate_moveit_rviz_launch(moveit_config) diff --git a/src/iisy_config_2/launch/rsp.launch.py b/src/iisy_config_2/launch/rsp.launch.py new file mode 100644 index 0000000..9f63bae --- /dev/null +++ b/src/iisy_config_2/launch/rsp.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_rsp_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs() + return generate_rsp_launch(moveit_config) diff --git a/src/iisy_config_2/launch/setup_assistant.launch.py b/src/iisy_config_2/launch/setup_assistant.launch.py new file mode 100644 index 0000000..25bac44 --- /dev/null +++ b/src/iisy_config_2/launch/setup_assistant.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs() + return generate_setup_assistant_launch(moveit_config) diff --git a/src/iisy_config_2/launch/spawn_controllers.launch.py b/src/iisy_config_2/launch/spawn_controllers.launch.py new file mode 100644 index 0000000..613edbc --- /dev/null +++ b/src/iisy_config_2/launch/spawn_controllers.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_spawn_controllers_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs() + return generate_spawn_controllers_launch(moveit_config) diff --git a/src/iisy_config_2/launch/static_virtual_joint_tfs.launch.py b/src/iisy_config_2/launch/static_virtual_joint_tfs.launch.py new file mode 100644 index 0000000..b7198de --- /dev/null +++ b/src/iisy_config_2/launch/static_virtual_joint_tfs.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs() + return generate_static_virtual_joint_tfs_launch(moveit_config) diff --git a/src/iisy_config_2/launch/warehouse_db.launch.py b/src/iisy_config_2/launch/warehouse_db.launch.py new file mode 100644 index 0000000..9189152 --- /dev/null +++ b/src/iisy_config_2/launch/warehouse_db.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_warehouse_db_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config_2").to_moveit_configs() + return generate_warehouse_db_launch(moveit_config) diff --git a/src/iisy_config_2/package.xml b/src/iisy_config_2/package.xml new file mode 100644 index 0000000..84e7ffd --- /dev/null +++ b/src/iisy_config_2/package.xml @@ -0,0 +1,52 @@ + + + + iisy_config_2 + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the iisy with the MoveIt Motion Planning Framework + + Bastian Hofmann + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit2/issues + https://github.com/ros-planning/moveit2 + + Bastian Hofmann + + ament_cmake + + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + + + + controller_manager + iisy_config + moveit_configs_utils + moveit_ros_move_group + moveit_ros_visualization + moveit_ros_warehouse + moveit_setup_assistant + robot_state_publisher + rviz2 + rviz_common + rviz_default_plugins + tf2_ros + warehouse_ros_mongo + xacro + + + + ament_cmake + + diff --git a/src/moveit_test/CMakeLists.txt b/src/moveit_test/CMakeLists.txt new file mode 100644 index 0000000..482ba6b --- /dev/null +++ b/src/moveit_test/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) +project(moveit_test) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(geometry_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(move src/test.cpp) +set_property(TARGET move PROPERTY CXX_STANDARD 17) +ament_target_dependencies(move rclcpp moveit_ros_planning_interface geometry_msgs) + +install(TARGETS + move + DESTINATION lib/${PROJECT_NAME}) + +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/src/moveit_test/package.xml b/src/moveit_test/package.xml new file mode 100644 index 0000000..5ef1325 --- /dev/null +++ b/src/moveit_test/package.xml @@ -0,0 +1,23 @@ + + + + moveit_test + 0.1.0 + + Test for Moveit on IISY + + bastian + bastian + TODO: License declaration + + ament_cmake + + rclcpp + geometry_msgs + moveit_ros_planning_interface + + + ament_cmake + + + diff --git a/src/moveit_test/src/test.cpp b/src/moveit_test/src/test.cpp new file mode 100644 index 0000000..966dd58 --- /dev/null +++ b/src/moveit_test/src/test.cpp @@ -0,0 +1,55 @@ +#include +#include +#include + +int main(int argc, char * argv[]) +{ + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + auto const node = std::make_shared( + "hello_moveit", + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true) + ); + + // Create a ROS logger + auto const logger = rclcpp::get_logger("hello_moveit"); + + // Create the MoveIt MoveGroup Interface + using moveit::planning_interface::MoveGroupInterface; + auto move_group_interface = MoveGroupInterface(node, "iisy"); + + // Set a target Pose + auto const target_pose = []{ + geometry_msgs::msg::Pose msg; + msg.orientation.w = 1.0; + msg.position.x = 0.25; + msg.position.y = 0.25; + msg.position.z = 1.0; + return msg; + }(); + //move_group_interface.setPoseTarget(target_pose); + + std::map states = { + {"base_rot",10.0} + }; + + move_group_interface.setJointValueTarget(states); + + // Create a plan to that target pose + auto const [success, plan] = [&move_group_interface]{ + moveit::planning_interface::MoveGroupInterface::Plan msg; + auto const ok = static_cast(move_group_interface.plan(msg)); + return std::make_pair(ok, msg); + }(); + + // Execute the plan + if(success) { + move_group_interface.execute(plan); + } else { + RCLCPP_ERROR(logger, "Planing failed!"); + } + + // Shutdown ROS + rclcpp::shutdown(); + return 0; +} diff --git a/src/moveit_visual_tools b/src/moveit_visual_tools deleted file mode 160000 index ef16670..0000000 --- a/src/moveit_visual_tools +++ /dev/null @@ -1 +0,0 @@ -Subproject commit ef16670fab41acb5a9f265ce2de6d7875c6358eb