diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..fbe10ff --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.5) +project(ign_moveit2_examples) + +# Default to C11 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 11) +endif() +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +# Compiler options +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Testing and linting +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) + +# Install C++ examples +set(EXAMPLES_CPP_DIR examples/cpp) +# Example 0 - Follow target +set(EXECUTABLE_0 ex_follow_target) +add_executable(${EXECUTABLE_0} ${EXAMPLES_CPP_DIR}/${EXECUTABLE_0}.cpp) +ament_target_dependencies(${EXECUTABLE_0} + rclcpp + geometry_msgs + moveit_ros_planning_interface +) +install(TARGETS + ${EXECUTABLE_0} + DESTINATION lib/${PROJECT_NAME} +) + +# Install Python examples +set(EXAMPLES_PY_DIR examples/py) +install(PROGRAMS + ${EXAMPLES_PY_DIR}/ex_follow_target.py + ${EXAMPLES_PY_DIR}/ex_throw_object.py + DESTINATION lib/${PROJECT_NAME} +) + +# Install directories +install(DIRECTORY launch rviz worlds DESTINATION share/${PROJECT_NAME}) + +# Setup the project +ament_package() diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..f1dacb9 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,47 @@ +ARG ROS_DISTRO=galactic +FROM ros:${ROS_DISTRO}-ros-base + +### Use bash by default +SHELL ["/bin/bash", "-c"] + +### Define working directory +ARG WS_DIR=/root/ws +ENV WS_DIR=${WS_DIR} +ENV WS_SRC_DIR=${WS_DIR}/src +ENV WS_INSTALL_DIR=${WS_DIR}/install +ENV WS_LOG_DIR=${WS_DIR}/log +WORKDIR ${WS_DIR} + +### Install Gazebo +ARG IGNITION_VERSION=fortress +ENV IGNITION_VERSION=${IGNITION_VERSION} +RUN apt-get update && \ + apt-get install -yq --no-install-recommends \ + ignition-${IGNITION_VERSION} && \ + rm -rf /var/lib/apt/lists/* + +### Import and install dependencies, then build these dependencies (not ign_moveit2_examples yet) +COPY ./ign_moveit2_examples.repos ${WS_SRC_DIR}/ign_moveit2_examples/ign_moveit2_examples.repos +RUN vcs import --shallow ${WS_SRC_DIR} < ${WS_SRC_DIR}/ign_moveit2_examples/ign_moveit2_examples.repos && \ + rosdep update && \ + apt-get update && \ + rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR} && \ + rm -rf /var/lib/apt/lists/* && \ + source "/opt/ros/${ROS_DISTRO}/setup.bash" && \ + colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \ + rm -rf ${WS_LOG_DIR} + +### Copy over the rest of ign_moveit2_examples, then install dependencies and build +COPY ./ ${WS_SRC_DIR}/ign_moveit2_examples/ +RUN rosdep update && \ + apt-get update && \ + rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR} && \ + rm -rf /var/lib/apt/lists/* && \ + source "/opt/ros/${ROS_DISTRO}/setup.bash" && \ + colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \ + rm -rf ${WS_LOG_DIR} + +### Add workspace to the ROS entrypoint +### Source ROS workspace inside `~/.bashrc` to enable autocompletion +RUN sed -i '$i source "${WS_INSTALL_DIR}/local_setup.bash" --' /ros_entrypoint.sh && \ + sed -i '$a source "/opt/ros/${ROS_DISTRO}/setup.bash"' ~/.bashrc diff --git a/examples/cpp/ex_follow_target.cpp b/examples/cpp/ex_follow_target.cpp new file mode 100644 index 0000000..1ee6e22 --- /dev/null +++ b/examples/cpp/ex_follow_target.cpp @@ -0,0 +1,71 @@ +/// Example that uses MoveIt 2 to follow a target inside Ignition Gazebo + +#include +#include +#include +#include + +const std::string MOVE_GROUP = "arm"; + +class MoveItFollowTarget : public rclcpp::Node +{ +public: + /// Constructor + MoveItFollowTarget(); + + /// Move group interface for the robot + moveit::planning_interface::MoveGroupInterface move_group_; + /// Subscriber for target pose + rclcpp::Subscription::SharedPtr target_pose_sub_; + /// Target pose that is used to detect changes + geometry_msgs::msg::Pose previous_target_pose_; + +private: + /// Callback that plans and executes trajectory each time the target pose is changed + void target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg); +}; + +MoveItFollowTarget::MoveItFollowTarget() : Node("ex_follow_target"), + move_group_(std::shared_ptr(std::move(this)), MOVE_GROUP) +{ + // Use upper joint velocity and acceleration limits + this->move_group_.setMaxAccelerationScalingFactor(1.0); + this->move_group_.setMaxVelocityScalingFactor(1.0); + + // Subscribe to target pose + target_pose_sub_ = this->create_subscription("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1)); + + RCLCPP_INFO(this->get_logger(), "Initialization successful."); +} + +void MoveItFollowTarget::target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) +{ + // Return if target pose is unchanged + if (msg->pose == previous_target_pose_) + { + return; + } + + RCLCPP_INFO(this->get_logger(), "Target pose has changed. Planning and executing..."); + + // Plan and execute motion + this->move_group_.setPoseTarget(msg->pose); + this->move_group_.move(); + + // Update for next callback + previous_target_pose_ = msg->pose; +} + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + + auto target_follower = std::make_shared(); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(target_follower); + executor.spin(); + + rclcpp::shutdown(); + return EXIT_SUCCESS; +} diff --git a/examples/py/ex_follow_target.py b/examples/py/ex_follow_target.py new file mode 100755 index 0000000..7fd21c2 --- /dev/null +++ b/examples/py/ex_follow_target.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +"""Example that uses MoveIt 2 to follow a target inside Ignition Gazebo""" + +import rclpy +from geometry_msgs.msg import Pose, PoseStamped +from pymoveit2 import MoveIt2 +from pymoveit2.robots import panda +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from rclpy.qos import QoSProfile + + +class MoveItFollowTarget(Node): + def __init__(self): + + super().__init__("ex_follow_target_py") + + # Create callback group that allows execution of callbacks in parallel without restrictions + self._callback_group = ReentrantCallbackGroup() + + # Create MoveIt 2 interface + self._moveit2 = MoveIt2( + node=self, + joint_names=panda.joint_names(), + base_link_name=panda.base_link_name(), + end_effector_name=panda.end_effector_name(), + group_name=panda.MOVE_GROUP_ARM, + execute_via_moveit=True, + callback_group=self._callback_group, + ) + # Use upper joint velocity and acceleration limits + self._moveit2.max_velocity = 1.0 + self._moveit2.max_acceleration = 1.0 + + # Create a subscriber for target pose + self.__previous_target_pose = Pose() + self.create_subscription( + msg_type=PoseStamped, + topic="/target_pose", + callback=self.target_pose_callback, + qos_profile=QoSProfile(depth=1), + callback_group=self._callback_group, + ) + + self.get_logger().info("Initialization successful.") + + def target_pose_callback(self, msg: PoseStamped): + """ + Plan and execute trajectory each time the target pose is changed + """ + + # Return if target pose is unchanged + if msg.pose == self.__previous_target_pose: + return + + self.get_logger().info("Target pose has changed. Planning and executing...") + + # Plan and execute motion + self._moveit2.move_to_pose( + position=msg.pose.position, + quat_xyzw=msg.pose.orientation, + ) + + # Update for next callback + self.__previous_target_pose = msg.pose + + +def main(args=None): + + rclpy.init(args=args) + + target_follower = MoveItFollowTarget() + + executor = rclpy.executors.MultiThreadedExecutor(2) + executor.add_node(target_follower) + executor.spin() + + rclpy.shutdown() + exit(0) + + +if __name__ == "__main__": + main() diff --git a/examples/py/ex_throw_object.py b/examples/py/ex_throw_object.py new file mode 100755 index 0000000..28b5aa0 --- /dev/null +++ b/examples/py/ex_throw_object.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python3 + +from copy import deepcopy +from threading import Thread + +import rclpy +from geometry_msgs.msg import Point, Quaternion +from pymoveit2 import MoveIt2, MoveIt2Gripper +from pymoveit2.robots import panda +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node + + +class MoveItThrowObject(Node): + def __init__(self): + + super().__init__("ex_throw_object") + + # Create callback group that allows execution of callbacks in parallel without restrictions + self._callback_group = ReentrantCallbackGroup() + + # Create MoveIt 2 interface + self._moveit2 = MoveIt2( + node=self, + joint_names=panda.joint_names(), + base_link_name=panda.base_link_name(), + end_effector_name=panda.end_effector_name(), + group_name=panda.MOVE_GROUP_ARM, + callback_group=self._callback_group, + ) + # Use upper joint velocity and acceleration limits + self._moveit2.max_velocity = 1.0 + self._moveit2.max_acceleration = 1.0 + + # Create MoveIt 2 interface for gripper + self._moveit2_gripper = MoveIt2Gripper( + node=self, + gripper_joint_names=panda.gripper_joint_names(), + open_gripper_joint_positions=panda.OPEN_GRIPPER_JOINT_POSITIONS, + closed_gripper_joint_positions=panda.CLOSED_GRIPPER_JOINT_POSITIONS, + gripper_group_name=panda.MOVE_GROUP_GRIPPER, + callback_group=self._callback_group, + ) + + self.get_logger().info("Initialization successful.") + + def throw(self): + """ + Plan and execute hard-coded trajectory with intention to throw an object + """ + + self.get_logger().info("Throwing... Wish me luck!") + + throwing_object_pos = Point(x=0.5, y=0.0, z=0.015) + default_quat = Quaternion(x=1.0, y=0.0, z=0.0, w=0.0) + + # Open gripper + self._moveit2_gripper.open() + self._moveit2_gripper.wait_until_executed() + + # Move above object + position_above_object = deepcopy(throwing_object_pos) + position_above_object.z += 0.15 + self._moveit2.move_to_pose( + position=position_above_object, + quat_xyzw=default_quat, + ) + self._moveit2.wait_until_executed() + + # Move to grasp position + self._moveit2.move_to_pose( + position=throwing_object_pos, + quat_xyzw=default_quat, + ) + self._moveit2.wait_until_executed() + + # Close gripper + self._moveit2_gripper.close() + self._moveit2_gripper.wait_until_executed() + + # Decrease speed + self._moveit2.max_velocity = 0.25 + self._moveit2.max_acceleration = 0.1 + + # Move above object (again) + self._moveit2.move_to_pose( + position=position_above_object, + quat_xyzw=default_quat, + ) + self._moveit2.wait_until_executed() + + # Move to pre-throw configuration + joint_configuration_pre_throw = [0.0, -1.75, 0.0, -0.1, 0.0, 3.6, 0.8] + self._moveit2.move_to_configuration(joint_configuration_pre_throw) + self._moveit2.wait_until_executed() + + # Increase speed + self._moveit2.max_velocity = 1.0 + self._moveit2.max_acceleration = 1.0 + + # Throw itself + joint_configuration_throw = [0.0, 1.0, 0.0, -1.1, 0.0, 1.9, 0.8] + self._moveit2.move_to_configuration(joint_configuration_throw) + + # Release object while executing motion + sleep_duration_s = 1.2 + if rclpy.ok(): + self.create_rate(1 / sleep_duration_s).sleep() + self._moveit2_gripper.open() + self._moveit2_gripper.wait_until_executed() + self._moveit2.wait_until_executed() + + # Return to default configuration + joint_configuration_default = [ + 0.0, + -0.7853981633974483, + 0.0, + -2.356194490192345, + 0.0, + 1.5707963267948966, + 0.7853981633974483, + ] + self._moveit2.move_to_configuration(joint_configuration_default) + self._moveit2.wait_until_executed() + + +def main(args=None): + + rclpy.init(args=args) + + object_thrower = MoveItThrowObject() + + # Spin the node in background thread(s) + executor = rclpy.executors.MultiThreadedExecutor(3) + executor.add_node(object_thrower) + executor_thread = Thread(target=executor.spin, daemon=True, args=()) + executor_thread.start() + + # Wait for everything to setup + sleep_duration_s = 2.0 + if rclpy.ok(): + object_thrower.create_rate(1 / sleep_duration_s).sleep() + + object_thrower.throw() + + rclpy.shutdown() + exit(0) + + +if __name__ == "__main__": + main() diff --git a/ign_moveit2_examples.repos b/ign_moveit2_examples.repos new file mode 100644 index 0000000..f22a360 --- /dev/null +++ b/ign_moveit2_examples.repos @@ -0,0 +1,21 @@ +repositories: + pymoveit2: + type: git + url: https://github.com/AndrejOrsula/pymoveit2.git + version: master + panda_ign_moveit2: + type: git + url: https://github.com/AndrejOrsula/panda_ign_moveit2.git + version: master + ros_ign: + type: git + url: https://github.com/ignitionrobotics/ros_ign.git + version: galactic + ign_ros2_control: + type: git + url: https://github.com/AndrejOrsula/ign_ros2_control.git + version: devel + ros2_controllers: + type: git + url: https://github.com/AndrejOrsula/ros2_controllers.git + version: jtc_effort diff --git a/launch/default.launch.py b/launch/default.launch.py new file mode 100755 index 0000000..9ccd57c --- /dev/null +++ b/launch/default.launch.py @@ -0,0 +1,150 @@ +#!/usr/bin/env -S ros2 launch +"""Launch default world with the default robot (configurable)""" + +from os import path +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + + +def generate_launch_description() -> LaunchDescription: + + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + world_type = LaunchConfiguration("world_type") + robot_type = LaunchConfiguration("robot_type") + rviz_config = LaunchConfiguration("rviz_config") + use_sim_time = LaunchConfiguration("use_sim_time") + ign_verbosity = LaunchConfiguration("ign_verbosity") + log_level = LaunchConfiguration("log_level") + + # Determine what world/robot combination to launch + declared_arguments.append( + DeclareLaunchArgument( + "__world_launch_basename", + default_value=["world_", world_type, ".launch.py"], + ), + ) + declared_arguments.append( + DeclareLaunchArgument( + "__robot_launch_basename", + default_value=["robot_", robot_type, ".launch.py"], + ), + ) + + # List of included launch descriptions + launch_descriptions = [ + # Launch Ignition Gazebo with the required ROS<->IGN bridges + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("ign_moveit2_examples"), + "launch", + "worlds", + LaunchConfiguration("__world_launch_basename"), + ] + ) + ), + launch_arguments=[ + ("use_sim_time", use_sim_time), + ("ign_verbosity", ign_verbosity), + ("log_level", log_level), + ], + ), + # Spawn robot + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("ign_moveit2_examples"), + "launch", + "robots", + LaunchConfiguration("__robot_launch_basename"), + ] + ) + ), + launch_arguments=[ + ("use_sim_time", use_sim_time), + ("ign_verbosity", ign_verbosity), + ("log_level", log_level), + ], + ), + # Launch move_group of MoveIt 2 + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare([robot_type, "_moveit_config"]), + "launch", + "move_group.launch.py", + ] + ) + ), + launch_arguments=[ + ("ros2_control_plugin", "ign"), + ("ros2_control_command_interface", "effort"), + # TODO: Re-enable colligion geometry for manipulator arm once spawning with specific joint configuration is enabled + ("collision_arm", "false"), + ("rviz_config", rviz_config), + ("use_sim_time", use_sim_time), + ("log_level", log_level), + ], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # World selection + DeclareLaunchArgument( + "world_type", + default_value="default", + description="Name of the world configuration to load.", + ), + # Robot selection + DeclareLaunchArgument( + "robot_type", + default_value="panda", + description="Name of the robot type to use.", + ), + # Miscellaneous + DeclareLaunchArgument( + "rviz_config", + default_value=path.join( + get_package_share_directory("ign_moveit2_examples"), + "rviz", + "ign_moveit2_examples.rviz", + ), + description="Path to configuration for RViz2.", + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "ign_verbosity", + default_value="2", + description="Verbosity level for Ignition Gazebo (0~4).", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/launch/ex_cpp_follow_target.launch.py b/launch/ex_cpp_follow_target.launch.py new file mode 100755 index 0000000..77ef796 --- /dev/null +++ b/launch/ex_cpp_follow_target.launch.py @@ -0,0 +1,187 @@ +#!/usr/bin/env -S ros2 launch +"""Launch C++ example for following a target""" + +from os import path +from typing import List + +import yaml +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) + + +def generate_launch_description() -> LaunchDescription: + + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + description_package = LaunchConfiguration("description_package") + description_filepath = LaunchConfiguration("description_filepath") + moveit_config_package = "panda_moveit_config" + robot_type = LaunchConfiguration("robot_type") + rviz_config = LaunchConfiguration("rviz_config") + use_sim_time = LaunchConfiguration("use_sim_time") + ign_verbosity = LaunchConfiguration("ign_verbosity") + log_level = LaunchConfiguration("log_level") + + # URDF + _robot_description_xml = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), description_filepath] + ), + " ", + "name:=", + robot_type, + ] + ) + robot_description = {"robot_description": _robot_description_xml} + + # SRDF + _robot_description_semantic_xml = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare(moveit_config_package), + "srdf", + "panda.srdf.xacro", + ] + ), + " ", + "name:=", + robot_type, + ] + ) + robot_description_semantic = { + "robot_description_semantic": _robot_description_semantic_xml + } + + # List of included launch descriptions + launch_descriptions = [ + # Launch world with robot (configured for this example) + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("ign_moveit2_examples"), + "launch", + "default.launch.py", + ] + ) + ), + launch_arguments=[ + ("world_type", "follow_target"), + ("robot_type", robot_type), + ("rviz_config", rviz_config), + ("use_sim_time", use_sim_time), + ("ign_verbosity", ign_verbosity), + ("log_level", log_level), + ], + ), + ] + + # List of nodes to be launched + nodes = [ + # Run the example node (C++) + Node( + package="ign_moveit2_examples", + executable="ex_follow_target", + output="log", + arguments=["--ros-args", "--log-level", log_level], + parameters=[ + robot_description, + robot_description_semantic, + {"use_sim_time": use_sim_time}, + ], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions + nodes) + + +def load_yaml(package_name: str, file_path: str): + """ + Load yaml configuration based on package name and file path relative to its share. + """ + + package_path = get_package_share_directory(package_name) + absolute_file_path = path.join(package_path, file_path) + return parse_yaml(absolute_file_path) + + +def parse_yaml(absolute_file_path: str): + """ + Parse yaml from file, given its absolute file path. + """ + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: + return None + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # Locations of robot resources + DeclareLaunchArgument( + "description_package", + default_value="panda_description", + description="Custom package with robot description.", + ), + DeclareLaunchArgument( + "description_filepath", + default_value=path.join("urdf", "panda.urdf.xacro"), + description="Path to xacro or URDF description of the robot, relative to share of `description_package`.", + ), + # Robot selection + DeclareLaunchArgument( + "robot_type", + default_value="panda", + description="Name of the robot to use.", + ), + # Miscellaneous + DeclareLaunchArgument( + "rviz_config", + default_value=path.join( + get_package_share_directory("ign_moveit2_examples"), + "rviz", + "ign_moveit2_examples.rviz", + ), + description="Path to configuration for RViz2.", + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "ign_verbosity", + default_value="2", + description="Verbosity level for Ignition Gazebo (0~4).", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/launch/ex_py_follow_target.launch.py b/launch/ex_py_follow_target.launch.py new file mode 100755 index 0000000..1268d47 --- /dev/null +++ b/launch/ex_py_follow_target.launch.py @@ -0,0 +1,105 @@ +#!/usr/bin/env -S ros2 launch +"""Launch Python example for following a target""" + +from os import path +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + + +def generate_launch_description() -> LaunchDescription: + + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + robot_type = LaunchConfiguration("robot_type") + rviz_config = LaunchConfiguration("rviz_config") + use_sim_time = LaunchConfiguration("use_sim_time") + ign_verbosity = LaunchConfiguration("ign_verbosity") + log_level = LaunchConfiguration("log_level") + + # List of included launch descriptions + launch_descriptions = [ + # Launch world with robot (configured for this example) + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("ign_moveit2_examples"), + "launch", + "default.launch.py", + ] + ) + ), + launch_arguments=[ + ("world_type", "follow_target"), + ("robot_type", robot_type), + ("rviz_config", rviz_config), + ("use_sim_time", use_sim_time), + ("ign_verbosity", ign_verbosity), + ("log_level", log_level), + ], + ), + ] + + # List of nodes to be launched + nodes = [ + # Run the example node (Python) + Node( + package="ign_moveit2_examples", + executable="ex_follow_target.py", + output="log", + arguments=["--ros-args", "--log-level", log_level], + parameters=[{"use_sim_time": use_sim_time}], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions + nodes) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # Robot selection + DeclareLaunchArgument( + "robot_type", + default_value="panda", + description="Name of the robot to use.", + ), + # Miscellaneous + DeclareLaunchArgument( + "rviz_config", + default_value=path.join( + get_package_share_directory("ign_moveit2_examples"), + "rviz", + "ign_moveit2_examples.rviz", + ), + description="Path to configuration for RViz2.", + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "ign_verbosity", + default_value="2", + description="Verbosity level for Ignition Gazebo (0~4).", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/launch/ex_py_throw_object.launch.py b/launch/ex_py_throw_object.launch.py new file mode 100755 index 0000000..9a20a07 --- /dev/null +++ b/launch/ex_py_throw_object.launch.py @@ -0,0 +1,105 @@ +#!/usr/bin/env -S ros2 launch +"""Launch Python example for throwing an object""" + +from os import path +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + + +def generate_launch_description() -> LaunchDescription: + + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + robot_type = LaunchConfiguration("robot_type") + rviz_config = LaunchConfiguration("rviz_config") + use_sim_time = LaunchConfiguration("use_sim_time") + ign_verbosity = LaunchConfiguration("ign_verbosity") + log_level = LaunchConfiguration("log_level") + + # List of included launch descriptions + launch_descriptions = [ + # Launch world with robot (configured for this example) + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("ign_moveit2_examples"), + "launch", + "default.launch.py", + ] + ) + ), + launch_arguments=[ + ("world_type", "throw_object"), + ("robot_type", robot_type), + ("rviz_config", rviz_config), + ("use_sim_time", use_sim_time), + ("ign_verbosity", ign_verbosity), + ("log_level", log_level), + ], + ), + ] + + # List of nodes to be launched + nodes = [ + # Run the example node (Python) + Node( + package="ign_moveit2_examples", + executable="ex_throw_object.py", + output="log", + arguments=["--ros-args", "--log-level", log_level], + parameters=[{"use_sim_time": use_sim_time}], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions + nodes) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # Robot selection + DeclareLaunchArgument( + "robot_type", + default_value="panda", + description="Name of the robot to use.", + ), + # Miscellaneous + DeclareLaunchArgument( + "rviz_config", + default_value=path.join( + get_package_share_directory("ign_moveit2_examples"), + "rviz", + "ign_moveit2_examples.rviz", + ), + description="Path to configuration for RViz2.", + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "ign_verbosity", + default_value="2", + description="Verbosity level for Ignition Gazebo (0~4).", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/launch/robots/robot_panda.launch.py b/launch/robots/robot_panda.launch.py new file mode 100755 index 0000000..4747b34 --- /dev/null +++ b/launch/robots/robot_panda.launch.py @@ -0,0 +1,61 @@ +#!/usr/bin/env -S ros2 launch +"""Launch script for spawning Franka Emika Panda into Ignition Gazebo world""" + +from typing import List + +from launch_ros.actions import Node + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description() -> LaunchDescription: + + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + model = LaunchConfiguration("model") + use_sim_time = LaunchConfiguration("use_sim_time") + log_level = LaunchConfiguration("log_level") + + # List of nodes to be launched + nodes = [ + # ros_ign_gazebo_create + Node( + package="ros_ign_gazebo", + executable="create", + output="log", + arguments=["-file", model, "--ros-args", "--log-level", log_level], + parameters=[{"use_sim_time": use_sim_time}], + ), + ] + + return LaunchDescription(declared_arguments + nodes) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # Model for Ignition Gazebo + DeclareLaunchArgument( + "model", + default_value="panda", + description="Name or filepath of model to load.", + ), + # Miscellaneous + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/launch/worlds/world_default.launch.py b/launch/worlds/world_default.launch.py new file mode 100755 index 0000000..750e2c3 --- /dev/null +++ b/launch/worlds/world_default.launch.py @@ -0,0 +1,91 @@ +#!/usr/bin/env -S ros2 launch +"""Launch default.sdf and the required ROS<->IGN bridges""" + +from typing import List + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + + +def generate_launch_description() -> LaunchDescription: + + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + world = LaunchConfiguration("world") + use_sim_time = LaunchConfiguration("use_sim_time") + ign_verbosity = LaunchConfiguration("ign_verbosity") + log_level = LaunchConfiguration("log_level") + + # List of included launch descriptions + launch_descriptions = [ + # Launch Ignition Gazebo + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("ros_ign_gazebo"), + "launch", + "ign_gazebo.launch.py", + ] + ) + ), + launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])], + ), + ] + + # List of nodes to be launched + nodes = [ + # ros_ign_bridge (clock -> ROS 2) + Node( + package="ros_ign_bridge", + executable="parameter_bridge", + output="log", + arguments=[ + "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock", + "--ros-args", + "--log-level", + log_level, + ], + parameters=[{"use_sim_time": use_sim_time}], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions + nodes) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # World for Ignition Gazebo + DeclareLaunchArgument( + "world", + default_value="default.sdf", + description="Name or filepath of world to load.", + ), + # Miscellaneous + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "ign_verbosity", + default_value="2", + description="Verbosity level for Ignition Gazebo (0~4).", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/launch/worlds/world_follow_target.launch.py b/launch/worlds/world_follow_target.launch.py new file mode 100755 index 0000000..2d81d5e --- /dev/null +++ b/launch/worlds/world_follow_target.launch.py @@ -0,0 +1,113 @@ +#!/usr/bin/env -S ros2 launch +"""Launch worlds/follow_target.sdf and the required ROS<->IGN bridges""" + +from os import path +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + + +def generate_launch_description() -> LaunchDescription: + + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + world = LaunchConfiguration("world") + use_sim_time = LaunchConfiguration("use_sim_time") + ign_verbosity = LaunchConfiguration("ign_verbosity") + log_level = LaunchConfiguration("log_level") + + # List of included launch descriptions + launch_descriptions = [ + # Launch Ignition Gazebo + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("ros_ign_gazebo"), + "launch", + "ign_gazebo.launch.py", + ] + ) + ), + launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])], + ), + ] + + # List of nodes to be launched + nodes = [ + # ros_ign_bridge (clock -> ROS 2) + Node( + package="ros_ign_bridge", + executable="parameter_bridge", + output="log", + arguments=[ + "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock", + "--ros-args", + "--log-level", + log_level, + ], + parameters=[{"use_sim_time": use_sim_time}], + ), + # ros_ign_bridge (target pose -> ROS 2) + Node( + package="ros_ign_bridge", + executable="parameter_bridge", + output="log", + arguments=[ + "/model/target/pose" + + "@" + + "geometry_msgs/msg/PoseStamped[ignition.msgs.Pose", + "--ros-args", + "--log-level", + log_level, + ], + parameters=[{"use_sim_time": use_sim_time}], + remappings=[("/model/target/pose", "/target_pose")], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions + nodes) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # World for Ignition Gazebo + DeclareLaunchArgument( + "world", + default_value=path.join( + get_package_share_directory("ign_moveit2_examples"), + "worlds", + "follow_target.sdf", + ), + description="Name or filepath of world to load.", + ), + # Miscellaneous + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "ign_verbosity", + default_value="2", + description="Verbosity level for Ignition Gazebo (0~4).", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/launch/worlds/world_throw_object.launch.py b/launch/worlds/world_throw_object.launch.py new file mode 100755 index 0000000..65ee17d --- /dev/null +++ b/launch/worlds/world_throw_object.launch.py @@ -0,0 +1,97 @@ +#!/usr/bin/env -S ros2 launch +"""Launch worlds/throw_object.sdf and the required ROS<->IGN bridges""" + +from os import path +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + + +def generate_launch_description() -> LaunchDescription: + + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + world = LaunchConfiguration("world") + use_sim_time = LaunchConfiguration("use_sim_time") + ign_verbosity = LaunchConfiguration("ign_verbosity") + log_level = LaunchConfiguration("log_level") + + # List of included launch descriptions + launch_descriptions = [ + # Launch Ignition Gazebo + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("ros_ign_gazebo"), + "launch", + "ign_gazebo.launch.py", + ] + ) + ), + launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])], + ), + ] + + # List of nodes to be launched + nodes = [ + # ros_ign_bridge (clock -> ROS 2) + Node( + package="ros_ign_bridge", + executable="parameter_bridge", + output="log", + arguments=[ + "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock", + "--ros-args", + "--log-level", + log_level, + ], + parameters=[{"use_sim_time": use_sim_time}], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions + nodes) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # World for Ignition Gazebo + DeclareLaunchArgument( + "world", + default_value=path.join( + get_package_share_directory("ign_moveit2_examples"), + "worlds", + "throw_object.sdf", + ), + description="Name or filepath of world to load.", + ), + # Miscellaneous + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "ign_verbosity", + default_value="2", + description="Verbosity level for Ignition Gazebo (0~4).", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..4af70cd --- /dev/null +++ b/package.xml @@ -0,0 +1,33 @@ + + + ign_moveit2_examples + 1.0.0 + C++ and Python examples of using MoveIt 2 for planning motions that are executed inside Ignition Gazebo simulation environment + Andrej Orsula + Andrej Orsula + BSD + + ament_cmake + + rclcpp + geometry_msgs + moveit_ros_planning_interface + + ign_ros2_control + moveit + pymoveit2 + rclpy + ros_ign_bridge + ros_ign_gazebo + rviz2 + + panda_description + panda_moveit_config + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rviz/ign_moveit2_examples.rviz b/rviz/ign_moveit2_examples.rviz new file mode 100644 index 0000000..07baebb --- /dev/null +++ b/rviz/ign_moveit2_examples.rviz @@ -0,0 +1,337 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /MotionPlanning1 + Splitter Ratio: 0.6278260946273804 + Tree Height: 412 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Acceleration_Scaling_Factor: 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_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + 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 + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + 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.25 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.1s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + 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: 0.5 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0.10000000149011612 + Joint Violation Color: 255; 0; 255 + Planning Group: arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 0.5 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + 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 + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: panda_link0 + 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 + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.5 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.3333333432674408 + Y: 0 + Z: 0.5 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.39269909262657166 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 720 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000247000003b0fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000046000001eb000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000005201000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000232000001c4000001b9010000030000000100000110000003b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000046000003b0000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000048fc0100000002fb0000000800540069006d0065000000000000000780000002d701000003fb0000000800540069006d0065010000000000000450000000000000000000000538000003b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1280 + X: 0 + Y: 0 diff --git a/src/draw_svg/CMakeLists.txt b/src/draw_svg/CMakeLists.txt new file mode 100644 index 0000000..0ca4ed3 --- /dev/null +++ b/src/draw_svg/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.8) +project(draw_svg) + +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(ign_moveit2_examples REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) + +#add_executable(draw_svg src/draw_svg.cpp) +#target_include_directories(draw_svg PUBLIC +# $ +# $) +#target_compile_features(draw_svg PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +#ament_target_dependencies( +# draw_svg +# "ign_moveit2_examples" +#) +#install(TARGETS draw_svg +# DESTINATION lib/${PROJECT_NAME}) + +# Install Python +set(SRC_PY_DIR src/py) +install(PROGRAMS + ${SRC_PY_DIR}/draw_svg.py + ${SRC_PY_DIR}/follow.py + 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() + +# Install directories +install(DIRECTORY launch rviz worlds DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/src/draw_svg/launch/default.launch.py b/src/draw_svg/launch/default.launch.py new file mode 100755 index 0000000..a66b44d --- /dev/null +++ b/src/draw_svg/launch/default.launch.py @@ -0,0 +1,150 @@ +#!/usr/bin/env -S ros2 launch +"""Launch default world with the default robot (configurable)""" + +from os import path +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + + +def generate_launch_description() -> LaunchDescription: + + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + world_type = LaunchConfiguration("world_type") + robot_type = LaunchConfiguration("robot_type") + rviz_config = LaunchConfiguration("rviz_config") + use_sim_time = LaunchConfiguration("use_sim_time") + ign_verbosity = LaunchConfiguration("ign_verbosity") + log_level = LaunchConfiguration("log_level") + + # Determine what world/robot combination to launch + declared_arguments.append( + DeclareLaunchArgument( + "__world_launch_basename", + default_value=["world_", world_type, ".launch.py"], + ), + ) + declared_arguments.append( + DeclareLaunchArgument( + "__robot_launch_basename", + default_value=["robot_", robot_type, ".launch.py"], + ), + ) + + # List of included launch descriptions + launch_descriptions = [ + # Launch Ignition Gazebo with the required ROS<->IGN bridges + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("draw_svg"), + "launch", + "worlds", + LaunchConfiguration("__world_launch_basename"), + ] + ) + ), + launch_arguments=[ + ("use_sim_time", use_sim_time), + ("ign_verbosity", ign_verbosity), + ("log_level", log_level), + ], + ), + # Spawn robot + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("draw_svg"), + "launch", + "robots", + LaunchConfiguration("__robot_launch_basename"), + ] + ) + ), + launch_arguments=[ + ("use_sim_time", use_sim_time), + ("ign_verbosity", ign_verbosity), + ("log_level", log_level), + ], + ), + # Launch move_group of MoveIt 2 + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare([robot_type, "_moveit_config"]), + "launch", + "move_group.launch.py", + ] + ) + ), + launch_arguments=[ + ("ros2_control_plugin", "ign"), + ("ros2_control_command_interface", "effort"), + # TODO: Re-enable colligion geometry for manipulator arm once spawning with specific joint configuration is enabled + ("collision_arm", "false"), + ("rviz_config", rviz_config), + ("use_sim_time", use_sim_time), + ("log_level", log_level), + ], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # World selection + DeclareLaunchArgument( + "world_type", + default_value="default", + description="Name of the world configuration to load.", + ), + # Robot selection + DeclareLaunchArgument( + "robot_type", + default_value="panda", + description="Name of the robot type to use.", + ), + # Miscellaneous + DeclareLaunchArgument( + "rviz_config", + default_value=path.join( + get_package_share_directory("draw_svg"), + "rviz", + "ign_moveit2_examples.rviz", + ), + description="Path to configuration for RViz2.", + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "ign_verbosity", + default_value="2", + description="Verbosity level for Ignition Gazebo (0~4).", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/src/draw_svg/launch/draw_svg.launch.py b/src/draw_svg/launch/draw_svg.launch.py new file mode 100755 index 0000000..bb5f38a --- /dev/null +++ b/src/draw_svg/launch/draw_svg.launch.py @@ -0,0 +1,112 @@ +#!/usr/bin/env -S ros2 launch +"""Launch Python example for following a target""" + +from os import path +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + + +def generate_launch_description() -> LaunchDescription: + + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + robot_type = LaunchConfiguration("robot_type") + rviz_config = LaunchConfiguration("rviz_config") + use_sim_time = LaunchConfiguration("use_sim_time") + ign_verbosity = LaunchConfiguration("ign_verbosity") + log_level = LaunchConfiguration("log_level") + + # List of included launch descriptions + launch_descriptions = [ + # Launch world with robot (configured for this example) + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("draw_svg"), + "launch", + "default.launch.py", + ] + ) + ), + launch_arguments=[ + ("world_type", "draw_svg"), + ("robot_type", robot_type), + ("rviz_config", rviz_config), + ("use_sim_time", use_sim_time), + ("ign_verbosity", ign_verbosity), + ("log_level", log_level), + ], + ), + ] + + # List of nodes to be launched + nodes = [ + # Run the example node (Python) + Node( + package="draw_svg", + executable="follow.py", + output="log", + arguments=["--ros-args", "--log-level", log_level], + parameters=[{"use_sim_time": use_sim_time}], + ), + Node( + package="draw_svg", + executable="draw_svg.py", + output="log", + arguments=["--ros-args", "--log-level", log_level], + parameters=[{"use_sim_time": use_sim_time}], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions + nodes) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # Robot selection + DeclareLaunchArgument( + "robot_type", + default_value="panda", + description="Name of the robot to use.", + ), + # Miscellaneous + DeclareLaunchArgument( + "rviz_config", + default_value=path.join( + get_package_share_directory("ign_moveit2_examples"), + "rviz", + "ign_moveit2_examples.rviz", + ), + description="Path to configuration for RViz2.", + ), + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "ign_verbosity", + default_value="2", + description="Verbosity level for Ignition Gazebo (0~4).", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/src/draw_svg/launch/robots/robot_panda.launch.py b/src/draw_svg/launch/robots/robot_panda.launch.py new file mode 100755 index 0000000..4747b34 --- /dev/null +++ b/src/draw_svg/launch/robots/robot_panda.launch.py @@ -0,0 +1,61 @@ +#!/usr/bin/env -S ros2 launch +"""Launch script for spawning Franka Emika Panda into Ignition Gazebo world""" + +from typing import List + +from launch_ros.actions import Node + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description() -> LaunchDescription: + + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + model = LaunchConfiguration("model") + use_sim_time = LaunchConfiguration("use_sim_time") + log_level = LaunchConfiguration("log_level") + + # List of nodes to be launched + nodes = [ + # ros_ign_gazebo_create + Node( + package="ros_ign_gazebo", + executable="create", + output="log", + arguments=["-file", model, "--ros-args", "--log-level", log_level], + parameters=[{"use_sim_time": use_sim_time}], + ), + ] + + return LaunchDescription(declared_arguments + nodes) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # Model for Ignition Gazebo + DeclareLaunchArgument( + "model", + default_value="panda", + description="Name or filepath of model to load.", + ), + # Miscellaneous + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/src/draw_svg/launch/worlds/world_default.launch.py b/src/draw_svg/launch/worlds/world_default.launch.py new file mode 100755 index 0000000..750e2c3 --- /dev/null +++ b/src/draw_svg/launch/worlds/world_default.launch.py @@ -0,0 +1,91 @@ +#!/usr/bin/env -S ros2 launch +"""Launch default.sdf and the required ROS<->IGN bridges""" + +from typing import List + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + + +def generate_launch_description() -> LaunchDescription: + + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + world = LaunchConfiguration("world") + use_sim_time = LaunchConfiguration("use_sim_time") + ign_verbosity = LaunchConfiguration("ign_verbosity") + log_level = LaunchConfiguration("log_level") + + # List of included launch descriptions + launch_descriptions = [ + # Launch Ignition Gazebo + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("ros_ign_gazebo"), + "launch", + "ign_gazebo.launch.py", + ] + ) + ), + launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])], + ), + ] + + # List of nodes to be launched + nodes = [ + # ros_ign_bridge (clock -> ROS 2) + Node( + package="ros_ign_bridge", + executable="parameter_bridge", + output="log", + arguments=[ + "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock", + "--ros-args", + "--log-level", + log_level, + ], + parameters=[{"use_sim_time": use_sim_time}], + ), + ] + + return LaunchDescription(declared_arguments + launch_descriptions + nodes) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # World for Ignition Gazebo + DeclareLaunchArgument( + "world", + default_value="default.sdf", + description="Name or filepath of world to load.", + ), + # Miscellaneous + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "ign_verbosity", + default_value="2", + description="Verbosity level for Ignition Gazebo (0~4).", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/src/draw_svg/launch/worlds/world_draw_svg.launch.py b/src/draw_svg/launch/worlds/world_draw_svg.launch.py new file mode 100755 index 0000000..2380cc5 --- /dev/null +++ b/src/draw_svg/launch/worlds/world_draw_svg.launch.py @@ -0,0 +1,113 @@ +#!/usr/bin/env -S ros2 launch +"""Launch worlds/follow_target.sdf and the required ROS<->IGN bridges""" + +from os import path +from typing import List + +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + + +def generate_launch_description() -> LaunchDescription: + + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + world = LaunchConfiguration("world") + use_sim_time = LaunchConfiguration("use_sim_time") + ign_verbosity = LaunchConfiguration("ign_verbosity") + log_level = LaunchConfiguration("log_level") + + # List of included launch descriptions + launch_descriptions = [ + # Launch Ignition Gazebo + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("ros_ign_gazebo"), + "launch", + "ign_gazebo.launch.py", + ] + ) + ), + launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])], + ), + ] + + # List of nodes to be launched + nodes = [ + # ros_ign_bridge (clock -> ROS 2) + Node( + package="ros_ign_bridge", + executable="parameter_bridge", + output="log", + arguments=[ + "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock", + "--ros-args", + "--log-level", + log_level, + ], + parameters=[{"use_sim_time": use_sim_time}], + ), + # ros_ign_bridge (target pose -> ROS 2) + #Node( + # package="ros_ign_bridge", + # executable="parameter_bridge", + # output="log", + # arguments=[ + # "/model/target/pose" + # + "@" + # + "geometry_msgs/msg/PoseStamped[ignition.msgs.Pose", + # "--ros-args", + # "--log-level", + # log_level, + # ], + # parameters=[{"use_sim_time": use_sim_time}], + # remappings=[("/model/target/pose", "/target_pose")], + #), + ] + + return LaunchDescription(declared_arguments + launch_descriptions + nodes) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # World for Ignition Gazebo + DeclareLaunchArgument( + "world", + default_value=path.join( + get_package_share_directory("draw_svg"), + "worlds", + "follow_target.sdf", + ), + description="Name or filepath of world to load.", + ), + # Miscellaneous + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "ign_verbosity", + default_value="2", + description="Verbosity level for Ignition Gazebo (0~4).", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/src/draw_svg/package.xml b/src/draw_svg/package.xml new file mode 100644 index 0000000..9a7f99a --- /dev/null +++ b/src/draw_svg/package.xml @@ -0,0 +1,24 @@ + + + + draw_svg + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + + rclcpp + geometry_msgs + tf2_ros + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/draw_svg/rebuild.sh b/src/draw_svg/rebuild.sh new file mode 100644 index 0000000..f0b2fc4 --- /dev/null +++ b/src/draw_svg/rebuild.sh @@ -0,0 +1,3 @@ +rm -r build/ install/ log/ +colcon build +#ros2 launch draw_svg draw_svg.launch.py diff --git a/src/draw_svg/rviz/ign_moveit2_examples.rviz b/src/draw_svg/rviz/ign_moveit2_examples.rviz new file mode 100644 index 0000000..07baebb --- /dev/null +++ b/src/draw_svg/rviz/ign_moveit2_examples.rviz @@ -0,0 +1,337 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /MotionPlanning1 + Splitter Ratio: 0.6278260946273804 + Tree Height: 412 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Acceleration_Scaling_Factor: 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_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + 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 + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + 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.25 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.1s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + 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: 0.5 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0.10000000149011612 + Joint Violation Color: 255; 0; 255 + Planning Group: arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 0.5 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + 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 + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_hand_tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: panda_link0 + 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 + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.5 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.3333333432674408 + Y: 0 + Z: 0.5 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.39269909262657166 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 720 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000247000003b0fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000046000001eb000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000005201000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000232000001c4000001b9010000030000000100000110000003b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000046000003b0000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000048fc0100000002fb0000000800540069006d0065000000000000000780000002d701000003fb0000000800540069006d0065010000000000000450000000000000000000000538000003b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1280 + X: 0 + Y: 0 diff --git a/src/draw_svg/src/cpp/draw_svg.cpp b/src/draw_svg/src/cpp/draw_svg.cpp new file mode 100644 index 0000000..d37dc92 --- /dev/null +++ b/src/draw_svg/src/cpp/draw_svg.cpp @@ -0,0 +1,10 @@ +#include + +int main(int argc, char ** argv) +{ + (void) argc; + (void) argv; + + printf("hello world draw_svg package\n"); + return 0; +} diff --git a/src/draw_svg/src/py/draw_svg.py b/src/draw_svg/src/py/draw_svg.py new file mode 100755 index 0000000..b226770 --- /dev/null +++ b/src/draw_svg/src/py/draw_svg.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +"""Example that uses MoveIt 2 to follow a target inside Ignition Gazebo""" + +import rclpy +from geometry_msgs.msg import Pose, PoseStamped +from pymoveit2 import MoveIt2 +from pymoveit2.robots import panda +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from rclpy.qos import QoSProfile +from random import uniform as rand +import math +#from tf2_ros.transformations import quaternion_from_euler + +def quaternion_from_euler(ai, aj, ak): + ai /= 2.0 + aj /= 2.0 + ak /= 2.0 + ci = math.cos(ai) + si = math.sin(ai) + cj = math.cos(aj) + sj = math.sin(aj) + ck = math.cos(ak) + sk = math.sin(ak) + cc = ci*ck + cs = ci*sk + sc = si*ck + ss = si*sk + + q = [0,0,0,0] + q[0] = cj*sc - sj*cs + q[1] = cj*ss + sj*cc + q[2] = cj*cs - sj*sc + q[3] = cj*cc + sj*ss + + return q + + +class PublishTarget(Node): + def __init__(self): + super().__init__('publisher') + self.publisher_ = self.create_publisher(PoseStamped, '/target_pose', 10) + timer_period = 3.0 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + #print(p) + #print(p.position) + #print(p.orientation) + + def timer_callback(self): + p = Pose() + p.position.x = rand(0.1,0.4) + p.position.y = rand(0.1,0.4) + p.position.z = 0.1 + q = quaternion_from_euler(0.0, math.pi, 0.0) + #p.orientation = q + p.orientation.x = q[0] + p.orientation.y = q[1] + p.orientation.z = q[2] + p.orientation.w = q[3] + ps = PoseStamped() + ps.pose = p + #print(ps) + self.publisher_.publish(ps) + self.get_logger().info('Publishing to /target_pose: "%s"' % p) + self.i += 1 + +def main(args=None): + rclpy.init(args=args) + + publisher = PublishTarget() + + rclpy.spin(publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/draw_svg/src/py/follow.py b/src/draw_svg/src/py/follow.py new file mode 100644 index 0000000..0e0828d --- /dev/null +++ b/src/draw_svg/src/py/follow.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 +"""Example that uses MoveIt 2 to follow a target inside Ignition Gazebo""" + +import rclpy +from geometry_msgs.msg import Pose, PoseStamped +from pymoveit2 import MoveIt2 +from pymoveit2.robots import panda +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from rclpy.qos import QoSProfile + + +class FollowTarget(Node): + def __init__(self): + + super().__init__("follow_py") + + # Create callback group that allows execution of callbacks in parallel without restrictions + self._callback_group = ReentrantCallbackGroup() + + # Create MoveIt 2 interface + self._moveit2 = MoveIt2( + node=self, + joint_names=panda.joint_names(), + base_link_name=panda.base_link_name(), + end_effector_name=panda.end_effector_name(), + group_name=panda.MOVE_GROUP_ARM, + execute_via_moveit=True, + callback_group=self._callback_group, + ) + # Use upper joint velocity and acceleration limits + self._moveit2.max_velocity = 1.0 + self._moveit2.max_acceleration = 1.0 + + # Create a subscriber for target pose + p = Pose() + p.position.x=0.5 + p.position.y=0.5 + self.__previous_target_pose = p + self.create_subscription( + msg_type=PoseStamped, + topic="/target_pose", + callback=self.target_pose_callback, + qos_profile=QoSProfile(depth=1), + callback_group=self._callback_group, + ) + + self.get_logger().info("Initialization successful.") + + def target_pose_callback(self, msg: PoseStamped): + """ + Plan and execute trajectory each time the target pose is changed + """ + + # Return if target pose is unchanged + if msg.pose == self.__previous_target_pose: + return + + self.get_logger().info("Target pose has changed. Planning and executing...") + + # Options https://github.com/AndrejOrsula/pymoveit2/blob/master/pymoveit2/moveit2.py + # Plan and execute motion + self._moveit2.move_to_pose( + position=msg.pose.position, + quat_xyzw=msg.pose.orientation, + cartesian=True, + ) + + # Update for next callback + self.__previous_target_pose = msg.pose + +def main(args=None): + + rclpy.init(args=args) + + target_follower = FollowTarget() + + executor = rclpy.executors.MultiThreadedExecutor(2) + executor.add_node(target_follower) + executor.spin() + + rclpy.shutdown() + exit(0) + + +if __name__ == "__main__": + main() diff --git a/src/draw_svg/test.sh b/src/draw_svg/test.sh new file mode 100644 index 0000000..fcd3028 --- /dev/null +++ b/src/draw_svg/test.sh @@ -0,0 +1 @@ +ros2 topic pub --once /target_pose geometry_msgs/msg/PoseStamped '{header:{}, pose:{position: {x: 0.0, y: 0.0, z: 0.0}, orientation: {}}}' diff --git a/src/draw_svg/tf.txt b/src/draw_svg/tf.txt new file mode 100644 index 0000000..b64df2c --- /dev/null +++ b/src/draw_svg/tf.txt @@ -0,0 +1,140 @@ +/apply_planning_scene +/check_state_validity +/clear_octomap +/compute_cartesian_path +/compute_fk +/compute_ik +/controller_manager/configure_and_start_controller +/controller_manager/configure_controller +/controller_manager/describe_parameters +/controller_manager/get_parameter_types +/controller_manager/get_parameters +/controller_manager/list_controller_types +/controller_manager/list_controllers +/controller_manager/list_hardware_interfaces +/controller_manager/list_parameters +/controller_manager/load_and_configure_controller +/controller_manager/load_and_start_controller +/controller_manager/load_controller +/controller_manager/reload_controller_libraries +/controller_manager/set_parameters +/controller_manager/set_parameters_atomically +/controller_manager/switch_controller +/controller_manager/unload_controller +/follow_py/describe_parameters +/follow_py/get_parameter_types +/follow_py/get_parameters +/follow_py/list_parameters +/follow_py/set_parameters +/follow_py/set_parameters_atomically +/get_planner_params +/get_planning_scene +/gripper_trajectory_controller/describe_parameters +/gripper_trajectory_controller/get_parameter_types +/gripper_trajectory_controller/get_parameters +/gripper_trajectory_controller/list_parameters +/gripper_trajectory_controller/set_parameters +/gripper_trajectory_controller/set_parameters_atomically +/ignition_ros_control/describe_parameters +/ignition_ros_control/get_parameter_types +/ignition_ros_control/get_parameters +/ignition_ros_control/list_parameters +/ignition_ros_control/set_parameters +/ignition_ros_control/set_parameters_atomically +/interactive_marker_display_94740324816224/describe_parameters +/interactive_marker_display_94740324816224/get_parameter_types +/interactive_marker_display_94740324816224/get_parameters +/interactive_marker_display_94740324816224/list_parameters +/interactive_marker_display_94740324816224/set_parameters +/interactive_marker_display_94740324816224/set_parameters_atomically +/joint_state_broadcaster/describe_parameters +/joint_state_broadcaster/get_parameter_types +/joint_state_broadcaster/get_parameters +/joint_state_broadcaster/list_parameters +/joint_state_broadcaster/set_parameters +/joint_state_broadcaster/set_parameters_atomically +/joint_trajectory_controller/describe_parameters +/joint_trajectory_controller/get_parameter_types +/joint_trajectory_controller/get_parameters +/joint_trajectory_controller/list_parameters +/joint_trajectory_controller/set_parameters +/joint_trajectory_controller/set_parameters_atomically +/load_map +/move_group/describe_parameters +/move_group/get_parameter_types +/move_group/get_parameters +/move_group/list_parameters +/move_group/set_parameters +/move_group/set_parameters_atomically +/move_group_interface_node/describe_parameters +/move_group_interface_node/get_parameter_types +/move_group_interface_node/get_parameters +/move_group_interface_node/list_parameters +/move_group_interface_node/set_parameters +/move_group_interface_node/set_parameters_atomically +/move_group_private_93885309918944/describe_parameters +/move_group_private_93885309918944/get_parameter_types +/move_group_private_93885309918944/get_parameters +/move_group_private_93885309918944/list_parameters +/move_group_private_93885309918944/set_parameters +/move_group_private_93885309918944/set_parameters_atomically +/moveit_simple_controller_manager/describe_parameters +/moveit_simple_controller_manager/get_parameter_types +/moveit_simple_controller_manager/get_parameters +/moveit_simple_controller_manager/list_parameters +/moveit_simple_controller_manager/set_parameters +/moveit_simple_controller_manager/set_parameters_atomically +/plan_kinematic_path +/publisher/describe_parameters +/publisher/get_parameter_types +/publisher/get_parameters +/publisher/list_parameters +/publisher/set_parameters +/publisher/set_parameters_atomically +/query_planner_interface +/robot_state_publisher/describe_parameters +/robot_state_publisher/get_parameter_types +/robot_state_publisher/get_parameters +/robot_state_publisher/list_parameters +/robot_state_publisher/set_parameters +/robot_state_publisher/set_parameters_atomically +/ros_ign_bridge/describe_parameters +/ros_ign_bridge/get_parameter_types +/ros_ign_bridge/get_parameters +/ros_ign_bridge/list_parameters +/ros_ign_bridge/set_parameters +/ros_ign_bridge/set_parameters_atomically +/rviz/describe_parameters +/rviz/get_parameter_types +/rviz/get_parameters +/rviz/list_parameters +/rviz/set_parameters +/rviz/set_parameters_atomically +/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/get_interactive_markers +/rviz_private_139912250992336/describe_parameters +/rviz_private_139912250992336/get_parameter_types +/rviz_private_139912250992336/get_parameters +/rviz_private_139912250992336/list_parameters +/rviz_private_139912250992336/set_parameters +/rviz_private_139912250992336/set_parameters_atomically +/save_map +/servo_node/change_control_dimensions +/servo_node/change_drift_dimensions +/servo_node/describe_parameters +/servo_node/get_parameter_types +/servo_node/get_parameters +/servo_node/list_parameters +/servo_node/pause_servo +/servo_node/reset_servo_status +/servo_node/set_parameters +/servo_node/set_parameters_atomically +/servo_node/start_servo +/servo_node/stop_servo +/servo_node/unpause_servo +/servo_node_private_94162402444928/describe_parameters +/servo_node_private_94162402444928/get_parameter_types +/servo_node_private_94162402444928/get_parameters +/servo_node_private_94162402444928/list_parameters +/servo_node_private_94162402444928/set_parameters +/servo_node_private_94162402444928/set_parameters_atomically +/set_planner_params diff --git a/src/draw_svg/worlds/follow_target.sdf b/src/draw_svg/worlds/follow_target.sdf new file mode 100644 index 0000000..cc8c6ab --- /dev/null +++ b/src/draw_svg/worlds/follow_target.sdf @@ -0,0 +1,109 @@ + + + + + + + + ignition-physics-dartsim-plugin + + + bullet + + + + 0.005 + 1.0 + + + + + + + 0.8 0.8 0.8 + false + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.3 0.3 -0.9 + + + + + + + + + 0 0 0 0 0 0 + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 4 4 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + + + + + diff --git a/worlds/follow_target.sdf b/worlds/follow_target.sdf new file mode 100644 index 0000000..b887c48 --- /dev/null +++ b/worlds/follow_target.sdf @@ -0,0 +1,108 @@ + + + + + + + + ignition-physics-dartsim-plugin + + + bullet + + + + 0.005 + 1.0 + + + + + + + 0.8 0.8 0.8 + false + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.3 0.3 -0.9 + + + + + + + + + 0 0 0 0 0 0 + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 4 4 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + true + 0.5 -0.25 0.5 3.1415927 0 0 + + + false + + + 0.04 0.04 0.04 + + + + 0.1 0.1 0.1 1 + 0.4 0.4 0.4 1 + + + + + + true + false + false + false + + + + + diff --git a/worlds/throw_object.sdf b/worlds/throw_object.sdf new file mode 100644 index 0000000..14ab3b3 --- /dev/null +++ b/worlds/throw_object.sdf @@ -0,0 +1,693 @@ + + + + + + + + ignition-physics-dartsim-plugin + + + bullet + + + + 0.002 + 1.0 + + + + + + + 0.8 0.8 0.8 + false + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.3 0.3 -0.9 + + + + + + + + + 0.5 0 0 0 0 0 + true + + + 0 0 -0.05 0 0 0 + + + 5 2 0.1 + + + + + + + 0 0 1 + 5 2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0.5 0 -10.0 0 0 0 + true + + + + + 0 0 1 + + + + + + + + + 2.95 0 0.5 0 0 0 + true + + + + + 0.1 2.0 1.0 + + + + + + + 0.1 2.0 1.0 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + 0.5 0 0.025 1.5707963 0 0 + + + + 0.00025 + 0.0 + 0.0 + 0.00025 + 0.0 + 0.00025 + + 0.6 + + + + + 0.05 0.05 0.05 + + + + + + 2.0 + 2.0 + + + + + + + + 0.05 0.05 0.05 + + + + 0.1 0.1 0.1 1 + 0.4 0.4 0.4 1 + + + + + + + + + + + + 1.5 -0.075 0.025 0 0 0 + + + + 0.00005 + 0.0 + 0.0 + 0.00005 + 0.0 + 0.00005 + + 0.12 + + + + + 0.05 0.05 0.05 + + + + + + + 0.05 0.05 0.05 + + + + 0.6 0.6 0.6 1 + 0.2 0.2 0.2 1 + + + + + + + 1.5 0 0.025 0 0 0 + + + + 0.00005 + 0.0 + 0.0 + 0.00005 + 0.0 + 0.00005 + + 0.12 + + + + + 0.05 0.05 0.05 + + + + + + + 0.05 0.05 0.05 + + + + 0.6 0.6 0.6 1 + 0.2 0.2 0.2 1 + + + + + + + 1.5 0.075 0.025 0 0 0 + + + + 0.00005 + 0.0 + 0.0 + 0.00005 + 0.0 + 0.00005 + + 0.12 + + + + + 0.05 0.05 0.05 + + + + + + + 0.05 0.05 0.05 + + + + 0.6 0.6 0.6 1 + 0.2 0.2 0.2 1 + + + + + + + + + + + 1.5 -0.0375 0.075 0 0 0 + + + + 0.00005 + 0.0 + 0.0 + 0.00005 + 0.0 + 0.00005 + + 0.12 + + + + + 0.05 0.05 0.05 + + + + + + + 0.05 0.05 0.05 + + + + 0.6 0.6 0.6 1 + 0.2 0.2 0.2 1 + + + + + + + 1.5 0.0375 0.075 0 0 0 + + + + 0.00005 + 0.0 + 0.0 + 0.00005 + 0.0 + 0.00005 + + 0.12 + + + + + 0.05 0.05 0.05 + + + + + + + 0.05 0.05 0.05 + + + + 0.6 0.6 0.6 1 + 0.2 0.2 0.2 1 + + + + + + + + + + + 1.5 0 0.125 0 0 0 + + + + 0.00005 + 0.0 + 0.0 + 0.00005 + 0.0 + 0.00005 + + 0.12 + + + + + 0.05 0.05 0.05 + + + + + + + 0.05 0.05 0.05 + + + + 0.6 0.6 0.6 1 + 0.2 0.2 0.2 1 + + + + + + + + + + + + + + +