Import example code and basic prototype
This commit is contained in:
57
CMakeLists.txt
Normal file
57
CMakeLists.txt
Normal file
@@ -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()
|
||||
47
Dockerfile
Normal file
47
Dockerfile
Normal file
@@ -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
|
||||
71
examples/cpp/ex_follow_target.cpp
Normal file
71
examples/cpp/ex_follow_target.cpp
Normal file
@@ -0,0 +1,71 @@
|
||||
/// Example that uses MoveIt 2 to follow a target inside Ignition Gazebo
|
||||
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <moveit/move_group_interface/move_group_interface.h>
|
||||
#include <rclcpp/qos.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
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<geometry_msgs::msg::PoseStamped>::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<rclcpp::Node>(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<geometry_msgs::msg::PoseStamped>("/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<MoveItFollowTarget>();
|
||||
|
||||
rclcpp::executors::SingleThreadedExecutor executor;
|
||||
executor.add_node(target_follower);
|
||||
executor.spin();
|
||||
|
||||
rclcpp::shutdown();
|
||||
return EXIT_SUCCESS;
|
||||
}
|
||||
83
examples/py/ex_follow_target.py
Executable file
83
examples/py/ex_follow_target.py
Executable file
@@ -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()
|
||||
151
examples/py/ex_throw_object.py
Executable file
151
examples/py/ex_throw_object.py
Executable file
@@ -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()
|
||||
21
ign_moveit2_examples.repos
Normal file
21
ign_moveit2_examples.repos
Normal file
@@ -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
|
||||
150
launch/default.launch.py
Executable file
150
launch/default.launch.py
Executable file
@@ -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.",
|
||||
),
|
||||
]
|
||||
187
launch/ex_cpp_follow_target.launch.py
Executable file
187
launch/ex_cpp_follow_target.launch.py
Executable file
@@ -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.",
|
||||
),
|
||||
]
|
||||
105
launch/ex_py_follow_target.launch.py
Executable file
105
launch/ex_py_follow_target.launch.py
Executable file
@@ -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.",
|
||||
),
|
||||
]
|
||||
105
launch/ex_py_throw_object.launch.py
Executable file
105
launch/ex_py_throw_object.launch.py
Executable file
@@ -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.",
|
||||
),
|
||||
]
|
||||
61
launch/robots/robot_panda.launch.py
Executable file
61
launch/robots/robot_panda.launch.py
Executable file
@@ -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.",
|
||||
),
|
||||
]
|
||||
91
launch/worlds/world_default.launch.py
Executable file
91
launch/worlds/world_default.launch.py
Executable file
@@ -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.",
|
||||
),
|
||||
]
|
||||
113
launch/worlds/world_follow_target.launch.py
Executable file
113
launch/worlds/world_follow_target.launch.py
Executable file
@@ -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.",
|
||||
),
|
||||
]
|
||||
97
launch/worlds/world_throw_object.launch.py
Executable file
97
launch/worlds/world_throw_object.launch.py
Executable file
@@ -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.",
|
||||
),
|
||||
]
|
||||
33
package.xml
Normal file
33
package.xml
Normal file
@@ -0,0 +1,33 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>ign_moveit2_examples</name>
|
||||
<version>1.0.0</version>
|
||||
<description>C++ and Python examples of using MoveIt 2 for planning motions that are executed inside Ignition Gazebo simulation environment</description>
|
||||
<maintainer email="orsula.andrej@gmail.com">Andrej Orsula</maintainer>
|
||||
<author>Andrej Orsula</author>
|
||||
<license>BSD</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>moveit_ros_planning_interface</depend>
|
||||
|
||||
<exec_depend>ign_ros2_control</exec_depend>
|
||||
<exec_depend>moveit</exec_depend>
|
||||
<exec_depend>pymoveit2</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>ros_ign_bridge</exec_depend>
|
||||
<exec_depend>ros_ign_gazebo</exec_depend>
|
||||
<exec_depend>rviz2</exec_depend>
|
||||
|
||||
<exec_depend>panda_description</exec_depend>
|
||||
<exec_depend>panda_moveit_config</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
337
rviz/ign_moveit2_examples.rviz
Normal file
337
rviz/ign_moveit2_examples.rviz
Normal file
@@ -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: <Fixed 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
|
||||
50
src/draw_svg/CMakeLists.txt
Normal file
50
src/draw_svg/CMakeLists.txt
Normal file
@@ -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
|
||||
# $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
# $<INSTALL_INTERFACE:include>)
|
||||
#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()
|
||||
150
src/draw_svg/launch/default.launch.py
Executable file
150
src/draw_svg/launch/default.launch.py
Executable file
@@ -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.",
|
||||
),
|
||||
]
|
||||
112
src/draw_svg/launch/draw_svg.launch.py
Executable file
112
src/draw_svg/launch/draw_svg.launch.py
Executable file
@@ -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.",
|
||||
),
|
||||
]
|
||||
61
src/draw_svg/launch/robots/robot_panda.launch.py
Executable file
61
src/draw_svg/launch/robots/robot_panda.launch.py
Executable file
@@ -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.",
|
||||
),
|
||||
]
|
||||
91
src/draw_svg/launch/worlds/world_default.launch.py
Executable file
91
src/draw_svg/launch/worlds/world_default.launch.py
Executable file
@@ -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.",
|
||||
),
|
||||
]
|
||||
113
src/draw_svg/launch/worlds/world_draw_svg.launch.py
Executable file
113
src/draw_svg/launch/worlds/world_draw_svg.launch.py
Executable file
@@ -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.",
|
||||
),
|
||||
]
|
||||
24
src/draw_svg/package.xml
Normal file
24
src/draw_svg/package.xml
Normal file
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>draw_svg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<!-- <depend>ign_moveit2_examples</depend> -->
|
||||
<depend>rclcpp</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<!-- <depend>moveit_ros_planning_interface</depend> -->
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
3
src/draw_svg/rebuild.sh
Normal file
3
src/draw_svg/rebuild.sh
Normal file
@@ -0,0 +1,3 @@
|
||||
rm -r build/ install/ log/
|
||||
colcon build
|
||||
#ros2 launch draw_svg draw_svg.launch.py
|
||||
337
src/draw_svg/rviz/ign_moveit2_examples.rviz
Normal file
337
src/draw_svg/rviz/ign_moveit2_examples.rviz
Normal file
@@ -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: <Fixed 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
|
||||
10
src/draw_svg/src/cpp/draw_svg.cpp
Normal file
10
src/draw_svg/src/cpp/draw_svg.cpp
Normal file
@@ -0,0 +1,10 @@
|
||||
#include <cstdio>
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
(void) argc;
|
||||
(void) argv;
|
||||
|
||||
printf("hello world draw_svg package\n");
|
||||
return 0;
|
||||
}
|
||||
84
src/draw_svg/src/py/draw_svg.py
Executable file
84
src/draw_svg/src/py/draw_svg.py
Executable file
@@ -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()
|
||||
87
src/draw_svg/src/py/follow.py
Normal file
87
src/draw_svg/src/py/follow.py
Normal file
@@ -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()
|
||||
1
src/draw_svg/test.sh
Normal file
1
src/draw_svg/test.sh
Normal file
@@ -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: {}}}'
|
||||
140
src/draw_svg/tf.txt
Normal file
140
src/draw_svg/tf.txt
Normal file
@@ -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
|
||||
109
src/draw_svg/worlds/follow_target.sdf
Normal file
109
src/draw_svg/worlds/follow_target.sdf
Normal file
@@ -0,0 +1,109 @@
|
||||
<?xml version="1.0"?>
|
||||
<sdf version="1.9">
|
||||
<world name="draw_svg_world">
|
||||
|
||||
<!-- Physics -->
|
||||
<plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
|
||||
<engine>
|
||||
<filename>ignition-physics-dartsim-plugin</filename>
|
||||
</engine>
|
||||
<dart>
|
||||
<collision_detector>bullet</collision_detector>
|
||||
</dart>
|
||||
</plugin>
|
||||
<physics name="physics_config" type="dart">
|
||||
<max_step_size>0.005</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
</physics>
|
||||
|
||||
<!-- Scene -->
|
||||
<plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster">
|
||||
</plugin>
|
||||
<scene>
|
||||
<ambient>0.8 0.8 0.8</ambient>
|
||||
<grid>false</grid>
|
||||
</scene>
|
||||
|
||||
<!-- User Commands (transform control) -->
|
||||
<plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
|
||||
</plugin>
|
||||
|
||||
|
||||
<!-- -->
|
||||
<!-- Illumination -->
|
||||
<!-- -->
|
||||
<light type="directional" name="sun">
|
||||
<cast_shadows>true</cast_shadows>
|
||||
<pose>0 0 10 0 0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
<attenuation>
|
||||
<range>1000</range>
|
||||
<constant>0.9</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<direction>-0.3 0.3 -0.9</direction>
|
||||
</light>
|
||||
|
||||
|
||||
<!-- -->
|
||||
<!-- Models -->
|
||||
<!-- -->
|
||||
<!-- Ground -->
|
||||
<model name="ground_plane">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<static>true</static>
|
||||
<link name="ground_plane_link">
|
||||
<collision name="ground_plane_collision">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
</plane>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="ground_plane_visual">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>4 4</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.8 0.8 0.8 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<!-- Static target -->
|
||||
<!-- <model name="target">
|
||||
<static>true</static>
|
||||
<pose>0.5 -0.25 0.5 3.1415927 0 0</pose>
|
||||
<link name="target_link">
|
||||
<visual name="target_visual">
|
||||
<cast_shadows>false</cast_shadows>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.04 0.04 0.04</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.1 0.1 0.1 1</diffuse>
|
||||
<specular>0.4 0.4 0.4 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
-->
|
||||
<!-- Pose publisher plugin, used to determine the target pose to reach -->
|
||||
<!-- <plugin filename="ignition-gazebo-pose-publisher-system" name="ignition::gazebo::systems::PosePublisher">
|
||||
<publish_nested_model_pose>true</publish_nested_model_pose>
|
||||
<publish_link_pose>false</publish_link_pose>
|
||||
<publish_collision_pose>false</publish_collision_pose>
|
||||
<publish_visual_pose>false</publish_visual_pose>
|
||||
</plugin>
|
||||
</model> -->
|
||||
|
||||
</world>
|
||||
</sdf>
|
||||
108
worlds/follow_target.sdf
Normal file
108
worlds/follow_target.sdf
Normal file
@@ -0,0 +1,108 @@
|
||||
<?xml version="1.0"?>
|
||||
<sdf version="1.9">
|
||||
<world name="ign_moveit2_follow_target_world">
|
||||
|
||||
<!-- Physics -->
|
||||
<plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
|
||||
<engine>
|
||||
<filename>ignition-physics-dartsim-plugin</filename>
|
||||
</engine>
|
||||
<dart>
|
||||
<collision_detector>bullet</collision_detector>
|
||||
</dart>
|
||||
</plugin>
|
||||
<physics name="physics_config" type="dart">
|
||||
<max_step_size>0.005</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
</physics>
|
||||
|
||||
<!-- Scene -->
|
||||
<plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster">
|
||||
</plugin>
|
||||
<scene>
|
||||
<ambient>0.8 0.8 0.8</ambient>
|
||||
<grid>false</grid>
|
||||
</scene>
|
||||
|
||||
<!-- User Commands (transform control) -->
|
||||
<plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
|
||||
</plugin>
|
||||
|
||||
|
||||
<!-- -->
|
||||
<!-- Illumination -->
|
||||
<!-- -->
|
||||
<light type="directional" name="sun">
|
||||
<cast_shadows>true</cast_shadows>
|
||||
<pose>0 0 10 0 0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
<attenuation>
|
||||
<range>1000</range>
|
||||
<constant>0.9</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<direction>-0.3 0.3 -0.9</direction>
|
||||
</light>
|
||||
|
||||
|
||||
<!-- -->
|
||||
<!-- Models -->
|
||||
<!-- -->
|
||||
<!-- Ground -->
|
||||
<model name="ground_plane">
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<static>true</static>
|
||||
<link name="ground_plane_link">
|
||||
<collision name="ground_plane_collision">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
</plane>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="ground_plane_visual">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>4 4</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.8 0.8 0.8 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<!-- Static target -->
|
||||
<model name="target">
|
||||
<static>true</static>
|
||||
<pose>0.5 -0.25 0.5 3.1415927 0 0</pose>
|
||||
<link name="target_link">
|
||||
<visual name="target_visual">
|
||||
<cast_shadows>false</cast_shadows>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.04 0.04 0.04</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.1 0.1 0.1 1</diffuse>
|
||||
<specular>0.4 0.4 0.4 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<!-- Pose publisher plugin, used to determine the target pose to reach -->
|
||||
<plugin filename="ignition-gazebo-pose-publisher-system" name="ignition::gazebo::systems::PosePublisher">
|
||||
<publish_nested_model_pose>true</publish_nested_model_pose>
|
||||
<publish_link_pose>false</publish_link_pose>
|
||||
<publish_collision_pose>false</publish_collision_pose>
|
||||
<publish_visual_pose>false</publish_visual_pose>
|
||||
</plugin>
|
||||
</model>
|
||||
|
||||
</world>
|
||||
</sdf>
|
||||
693
worlds/throw_object.sdf
Normal file
693
worlds/throw_object.sdf
Normal file
@@ -0,0 +1,693 @@
|
||||
<?xml version="1.0"?>
|
||||
<sdf version="1.9">
|
||||
<world name="ign_moveit2_follow_target_world">
|
||||
|
||||
<!-- Physics -->
|
||||
<plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
|
||||
<engine>
|
||||
<filename>ignition-physics-dartsim-plugin</filename>
|
||||
</engine>
|
||||
<dart>
|
||||
<collision_detector>bullet</collision_detector>
|
||||
</dart>
|
||||
</plugin>
|
||||
<physics name="physics_config" type="dart">
|
||||
<max_step_size>0.002</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
</physics>
|
||||
|
||||
<!-- Scene -->
|
||||
<plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster">
|
||||
</plugin>
|
||||
<scene>
|
||||
<ambient>0.8 0.8 0.8</ambient>
|
||||
<grid>false</grid>
|
||||
</scene>
|
||||
|
||||
<!-- User Commands (transform control) -->
|
||||
<plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
|
||||
</plugin>
|
||||
|
||||
|
||||
<!-- -->
|
||||
<!-- Illumination -->
|
||||
<!-- -->
|
||||
<light type="directional" name="sun">
|
||||
<cast_shadows>true</cast_shadows>
|
||||
<pose>0 0 10 0 0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
<attenuation>
|
||||
<range>1000</range>
|
||||
<constant>0.9</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<direction>-0.3 0.3 -0.9</direction>
|
||||
</light>
|
||||
|
||||
|
||||
<!-- -->
|
||||
<!-- Models -->
|
||||
<!-- -->
|
||||
<!-- Ground -->
|
||||
<model name="ground_plane">
|
||||
<pose>0.5 0 0 0 0 0</pose>
|
||||
<static>true</static>
|
||||
<link name="ground_plane_link">
|
||||
<collision name="ground_plane_collision">
|
||||
<pose>0 0 -0.05 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>5 2 0.1</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="ground_plane_visual">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>5 2</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.8 0.8 0.8 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
<model name="underground_plane">
|
||||
<pose>0.5 0 -10.0 0 0 0</pose>
|
||||
<static>true</static>
|
||||
<link name="underground_plane_link">
|
||||
<collision name="underground_plane_collision">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
</plane>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<!-- Blockade -->
|
||||
<model name="blockade">
|
||||
<pose>2.95 0 0.5 0 0 0</pose>
|
||||
<static>true</static>
|
||||
<link name="blockade_link">
|
||||
<collision name="blockade_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.1 2.0 1.0</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="blockade_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.1 2.0 1.0</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.8 0.8 0.8 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<!-- Object to throw -->
|
||||
<model name="throwing_object">
|
||||
<pose>0.5 0 0.025 1.5707963 0 0</pose>
|
||||
<link name="throwing_object_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00025</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00025</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00025</izz>
|
||||
</inertia>
|
||||
<mass>0.6</mass>
|
||||
</inertial>
|
||||
<collision name="throwing_object_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>2.0</mu>
|
||||
<mu2>2.0</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name="throwing_object_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.1 0.1 0.1 1</diffuse>
|
||||
<specular>0.4 0.4 0.4 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
|
||||
<!-- Target pyramid -->
|
||||
<!-- Re-enable boxes if you want a larger pyramid -->
|
||||
<!-- <model name="pyramid_box0">
|
||||
<pose>1.5 -0.15 0.025 0 0 0</pose>
|
||||
<link name="pyramid_box0_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box0_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box0_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model> -->
|
||||
|
||||
<model name="pyramid_box1">
|
||||
<pose>1.5 -0.075 0.025 0 0 0</pose>
|
||||
<link name="pyramid_box1_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box1_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box1_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<model name="pyramid_box2">
|
||||
<pose>1.5 0 0.025 0 0 0</pose>
|
||||
<link name="pyramid_box2_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box2_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box2_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<model name="pyramid_box3">
|
||||
<pose>1.5 0.075 0.025 0 0 0</pose>
|
||||
<link name="pyramid_box3_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box3_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box3_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<!-- <model name="pyramid_box4">
|
||||
<pose>1.5 0.15 0.025 0 0 0</pose>
|
||||
<link name="pyramid_box4_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box4_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box4_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model> -->
|
||||
|
||||
<!-- <model name="pyramid_box5">
|
||||
<pose>1.5 -0.1125 0.075 0 0 0</pose>
|
||||
<link name="pyramid_box5_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box5_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box5_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model> -->
|
||||
|
||||
<model name="pyramid_box6">
|
||||
<pose>1.5 -0.0375 0.075 0 0 0</pose>
|
||||
<link name="pyramid_box6_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box6_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box6_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<model name="pyramid_box7">
|
||||
<pose>1.5 0.0375 0.075 0 0 0</pose>
|
||||
<link name="pyramid_box7_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box7_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box7_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<!-- <model name="pyramid_box8">
|
||||
<pose>1.5 0.1125 0.075 0 0 0</pose>
|
||||
<link name="pyramid_box8_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box8_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box8_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model> -->
|
||||
|
||||
<!-- <model name="pyramid_box9">
|
||||
<pose>1.5 -0.075 0.125 0 0 0</pose>
|
||||
<link name="pyramid_box9_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box9_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box9_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model> -->
|
||||
|
||||
<model name="pyramid_box10">
|
||||
<pose>1.5 0 0.125 0 0 0</pose>
|
||||
<link name="pyramid_box10_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box10_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box10_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
<!-- <model name="pyramid_box11">
|
||||
<pose>1.5 0.075 0.125 0 0 0</pose>
|
||||
<link name="pyramid_box11_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box11_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box11_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model> -->
|
||||
|
||||
<!-- <model name="pyramid_box12">
|
||||
<pose>1.5 -0.0375 0.175 0 0 0</pose>
|
||||
<link name="pyramid_box12_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box12_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box12_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model> -->
|
||||
|
||||
<!-- <model name="pyramid_box13">
|
||||
<pose>1.5 0.0375 0.175 0 0 0</pose>
|
||||
<link name="pyramid_box13_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box13_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box13_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model> -->
|
||||
|
||||
<!-- <model name="pyramid_box14">
|
||||
<pose>1.5 0 0.225 0 0 0</pose>
|
||||
<link name="pyramid_box14_link">
|
||||
<inertial>
|
||||
<inertia>
|
||||
<ixx>0.00005</ixx>
|
||||
<ixy>0.0</ixy>
|
||||
<ixz>0.0</ixz>
|
||||
<iyy>0.00005</iyy>
|
||||
<iyz>0.0</iyz>
|
||||
<izz>0.00005</izz>
|
||||
</inertia>
|
||||
<mass>0.12</mass>
|
||||
</inertial>
|
||||
<collision name="pyramid_box14_collision">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="pyramid_box14_visual">
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0.6 0.6 0.6 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model> -->
|
||||
|
||||
</world>
|
||||
</sdf>
|
||||
Reference in New Issue
Block a user