Merge pull request #135 from PickNikRobotics/scan-and-plan-world

New scan and plan config based on gazebo config with different world.

src/picknik_ur_gazebo_config/launch/sim/hardware_sim.launch.py

@@ -54,17 +54,18 @@ def path_pattern_change_for_gazebo(urdf_string):
 
 
 def generate_simulation_description(context, *args, **settings):
-    world_name = settings.get("gazebo_world_name", "space_station_blocks_world.sdf")
+    world_path = settings.get(
+        "gazebo_world_path",
+        "description/simulation_worlds/space_station_blocks_world.sdf",
+    )
     use_gui = settings.get("gazebo_gui", False)
     is_verbose = settings.get("gazebo_verbose", False)
     gz_renderer = os.environ.get("GAZEBO_RENDERER", "ogre")
 
     # Create a Gazebo world file that swaps out package:// paths with absolute path.
-    original_world_file = os.path.join(
-        get_package_share_directory("picknik_ur_gazebo_config"),
-        "description",
-        "simulation_worlds",
-        world_name,
+    original_world_file = get_ros_path(
+        settings.get("gazebo_world_package_name", "picknik_ur_gazebo_config"),
+        world_path,
     )
     modified_world_file = os.path.join(
         get_config_folder(), "auto_created", "gazebo_world.sdf"
@@ -75,7 +76,7 @@ def generate_simulation_description(context, *args, **settings):
         file.write(world_sdf)
 
     # Launch Gazebo.
-    print(f"Starting Gazebo with world {world_name}")
+    print(f"Starting Gazebo with world at {world_path}")
     print(f"GUI: {use_gui}, Verbose: {is_verbose}")
 
     sim_args = f"-r --render-engine {gz_renderer}"

src/picknik_ur_gazebo_scan_and_plan_config/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.22)
+project(picknik_ur_gazebo_scan_and_plan_config)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY
+    config
+    description
+  DESTINATION
+    share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/picknik_ur_gazebo_scan_and_plan_config/LICENSE

@@ -0,0 +1,25 @@
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+
+   * Neither the name of the copyright holder nor the names of its
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.

src/picknik_ur_gazebo_scan_and_plan_config/README.md

@@ -0,0 +1,5 @@
+# picknik_ur_gazebo_scan_and_plan_config
+
+A MoveIt Studio site configuration for a UR5e simulated in Gazebo to show "scan and plan" applications, based on the `picknik_ur_gazebo_config` package.
+
+For further documentation see: [MoveIt Studio Documentation](https://docs.picknik.ai/)

src/picknik_ur_gazebo_scan_and_plan_config/config/config.yaml

@@ -0,0 +1,11 @@
+#
+#  This contains information for a unique instance of a robot.
+#
+
+# Name of the package to specialize
+based_on_package: "picknik_ur_gazebo_config"
+
+# Optional parameters that can be read in your launch files for specific functionality
+optional_feature_params:
+  gazebo_world_package_name: "picknik_ur_gazebo_scan_and_plan_config"
+  gazebo_world_path: "description/simulation_worlds/scan_and_plan_world.sdf"

src/picknik_ur_gazebo_scan_and_plan_config/description/simulation_worlds/scan_and_plan_world.sdf

@@ -0,0 +1,201 @@
+<sdf version="1.6">
+  <world name="scan_and_plan_world">
+    <!-- Sim must be faster than the controller rate or there will be an error -->
+    <physics name="sim_time" type="ignored">
+      <max_step_size>0.005</max_step_size>
+      <real_time_update_rate>0.005</real_time_update_rate>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+            filename="ignition-gazebo-physics-system"
+            name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+            filename="ignition-gazebo-user-commands-system"
+            name="ignition::gazebo::systems::UserCommands">
+    </plugin>
+    <plugin
+            filename="ignition-gazebo-scene-broadcaster-system"
+            name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+            filename="ignition-gazebo-contact-system"
+            name="ignition::gazebo::systems::Contact">
+    </plugin>
+    <!-- FT Plugin -->
+    <plugin
+            filename="ignition-gazebo-forcetorque-system"
+            name="ignition::gazebo::systems::ForceTorque">
+    </plugin>
+
+    <scene>
+      <ambient>0.0 0.0 0.0 1.0</ambient>
+      <shadows>0</shadows>
+    </scene>
+
+    <light type="directional" name="right-light">
+      <cast_shadows>false</cast_shadows>
+      <pose>0 0 100 0 0 0</pose>
+      <diffuse>0.6 0.6 0.6 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.5 0.5 -1.0</direction>
+    </light>
+    <light type="directional" name="left-light">
+      <cast_shadows>false</cast_shadows>
+      <pose>0 0 100 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.5 -0.5 -1.0</direction>
+    </light>
+    <light type="directional" name="bottom-light">
+      <cast_shadows>false</cast_shadows>
+      <pose>0 0 -100 0 0 0</pose>
+      <diffuse>0.5 0.5 0.5 1</diffuse>
+      <specular>0.5 0.5 0.5 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>0.0 0.0 1.0</direction>
+    </light>
+    <light type="directional" name="front-right-light">
+      <cast_shadows>false</cast_shadows>
+      <pose>100 75 1.5 0 0 0</pose>
+      <diffuse>0.4 0.4 0.4 1</diffuse>
+      <specular>0.4 0.4 0.4 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-1.0 -0.75 0.0</direction>
+    </light>
+    <light type="directional" name="front-left-light">
+      <cast_shadows>false</cast_shadows>
+      <pose>100 -75 1.5 0 0 0</pose>
+      <diffuse>0.4 0.4 0.4</diffuse>
+      <specular>0.4 0.4 0.4 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-1.0 0.75 0.0</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- Table -->
+    <model name="table">
+      <static>true</static>
+      <pose>0 0 1 0 0 0</pose>
+      <link name="table_link">
+        <collision name="box_collision">
+          <geometry>
+            <box>
+              <size>1.5 1.0 0.05</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu1>0.6</mu1>
+                <mu2>0.6</mu2>
+              </ode>
+            </friction>
+            <contact>
+              <ode>
+                <kp>1e+5</kp>
+                <kd>1</kd>
+                <soft_cfm>0</soft_cfm>
+                <soft_erp>0.2</soft_erp>
+                <minDepth>0.001</minDepth>
+              </ode>
+            </contact>
+          </surface>
+        </collision>
+
+        <visual name="table_visual">
+          <geometry>
+            <box>
+              <size>1.5 1.0 0.05</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0.93 0.89 0.75 1.0</ambient>
+            <emissive>0.93 0.89 0.75 1.0</emissive>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- Hercules head on table -->
+    <model name="head">
+      <static>true</static>
+      <pose>-0.3 -0.1 1.05 0 0 1.57</pose>
+      <link name="head_link">
+        <collision name="head_collision">
+          <geometry>
+            <mesh>
+              <uri>package://picknik_accessories/descriptions/miscellaneous/farnese-hercules-head.stl</uri>
+            </mesh>
+          </geometry>
+        </collision>
+        <visual name="head_visual">
+          <geometry>
+            <mesh>
+              <uri>package://picknik_accessories/descriptions/miscellaneous/farnese-hercules-head.stl</uri>
+            </mesh>
+          </geometry>
+          <material>
+            <ambient>0 0 0 1</ambient>
+            <diffuse>0.5 0.5 0.5 1</diffuse>
+            <specular>0.2 0.2 0.2 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+  </world>
+</sdf>

src/picknik_ur_gazebo_scan_and_plan_config/package.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>picknik_ur_gazebo_scan_and_plan_config</name>
+  <version>2.11.0</version>
+
+  <description>Site configuration package for the UR5e simulated in Gazebo to show "scan and plan" applications.</description>
+
+  <maintainer email="[email protected]">MoveIt Studio Maintainer</maintainer>
+
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>picknik_ur_gazebo_config</depend>
+
+  <exec_depend>moveit_ros_perception</exec_depend>
+  <exec_depend>ros_gz</exec_depend>
+  <exec_depend>ign_ros2_control</exec_depend>
+  <exec_depend>picknik_accessories</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+
+  <test_depend>ament_clang_format</test_depend>
+  <test_depend>ament_clang_tidy</test_depend>
+  <test_depend>ament_cmake_copyright</test_depend>
+  <test_depend>ament_cmake_lint_cmake</test_depend>
+  <test_depend>picknik_ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>