Scenic Gazebo

src/scenic/simulators/Gazebo/README.md

@@ -0,0 +1,78 @@
+# Scenic-Gazebo Interface
+This repo contains the code to the Scenic-Gazebo interface.
+## Requirements & Installation
+
+ - Scenic 3 is required, as well as Python 3.8+, ROS Noetic, and Ubuntu 20.04.
+ - Before you install, it might be good to consider installing Scenic outside any virtual environment for Gazebo/ROS purposes since Gazebo/ROS utilizes some Ubuntu native packages that is difficult to get from conda/pip
+ - When installing Scenic, please do the "Repository" install outlined in this page: https://scenic-lang.readthedocs.io/en/latest/quickstart.html#installation 
+ - In terms of using ROS and Gazebo, it should be fine to control your robot without ROS. However, Gazebo needs to be started with ROS.
+
+## Instructions for Use
+
+ 1. Before running Scenic, start the Gazebo simulator with ROS, launch and bringup any ROS stack you need, and unpause the
+    simulation. 
+ 2. Open a separate terminal, run `scenic <YOUR PROGRAM>.scenic --simulate`(see https://scenic-lang.readthedocs.io/en/latest/options.html for more
+    command-line options). Make sure you sourced the corresponding `.bash` or `.zsh` needed to run your simulaton in this terminal, too!.
+ 3. In general, any launch files should be run before starting Scenic.
+ 4. Any new models (e.g. furniture, small objects) should be added in `model.scenic`. See below for more notes on adding models. 
+
+## Important Notes
+
+ - The Scenic coordinate system should match the `map` frame of the simulator which may or may not be the Gazebo `world` frame depending on the developer's choice, except that `yaw=0` is defined to be the +y axis rather than the +x as it is in the simulator.
+ - When adding new models, for objects whose reference frame used by Gazebo to determine its location is on the bottom, please specify a `positionOffset` field in the class definition. This field is the vector pointing from the center of the object to the position of the reference frame. When placing objects whose frame is at the bottom, please use the `on` syntax and never the `at` syntax.
+ - It is necessary to specify dimensions or shapes of new models you add. If this is not done, Scenic defaults to treating each object as boxes with side-length 1m, causing `RejectionExceptions` and other errors in generating scenes.
+ - When creating new actions, `applyTo` should return a function that takes no argument. If the user wishes to do other wise, please reflect the change in the `step` function in `simulator.py`.
+ - Before running the scene, it is helpful to run scenic `<YOUR PROGRAM>.scenic` with out the `--simulate` flag to see how Scenic is seeing the world as a sanity check. Note you need to run Scenic from a gui/desktop to do this rather than on an ssh terminal.
+
+## Model Library
+-   Robot
+	-   String name: name used by Gazebo to identify the robot
+	-   String object_type: the kind of object (table, chair, bottle etc.); used to access the .sdf/urdf file, default ‘robot’
+	-  String domain_name: the domain name of the robot in ROS
+	- Vector positionOffset: vector pointing from the center of the robot to the ref frame used by Gazebo to identify the robot's position
+	-   Vector Position: position in the Scenic coordinate
+	-  MeshShape shape: the shape of the robot as perceived by Scenic
+	-   float roll: roll in Scenic coordinates
+	-   float pitch: pitch in Scenic coordinates
+	-   float yaw: yaw in Scenic coordinates
+	-   Methods:
+		-   distanceToClosest
+			-   Inputs: type object_class
+			-   Returns the distance to the closest object of type object_class. Returns infinity if no object of type exists.
+		-   getClosest
+			-   Inputs: type object_class
+			-   Returns the closest instance of an object of object_class. Returns None if no such object exists
+		-   UniformHeadAngles:
+			-   Inputs: None
+			-   Returns a tuple of two lists. The first is a list of random positions to which the head joint can move, the second is the velocity at which they move.
+		-   UniformArmAngles:
+			-   Inputs: None
+			-   Returns a tuple of two lists. The first is a list of random positions to which each arm joint can move, the second is the velocity at which they move.
+
+-   General Objects, including KitchenTable, SmallBottle, BottleWithMarker, HighTable, and Chair
+	-   String name: name used by Gazebo to identify the object
+	-   String object_type: the kind of object (table, chair, bed); used to access the .sdf file
+	-  String description_file: the file that  is used to spawn the object
+	-  String description_file_type: the file type for the description file
+	- Vector positionOffset: vector pointing from the center of the robot to the ref frame used by Gazebo to identify the object's position
+	-   Vector Position: position in the robot  map coordinate
+	-  MeshShape shape: the shape of the robot as perceived by Scenic
+	-   float roll: the roll as used by Scenic
+	-   float pitch: the pitch as used by Scenic
+	-   float yaw: the yaw as used by Scenic
+	-   String marker_id (if the object is has a marker): identifies the tf frame created by the attached marker if there is one
+	-   bool collision: whether this object will be part of the collision world
+	-   String mesh (if available): uri to the mesh files for collision avoidance
+	-   shape: set if a mesh for the object exist  
+	-   float width (if no mesh)
+	-   float height (if no mesh)
+	-   float length (if no mesh) 
+	-   String rough_collision_shape: the shape (box, sphere, cylinder) that is used to approximated the object’s collision
+## On Implementing Actions and Behaviors:
+- One important thing when adapting your robot to the Gazebo interface is to separate out any code that handles waiting-until-finish for each action when implementing actions in `actions.py` . This is because it is important to not have any code that blocks execution of Scenic in the background. 
+- Instead, implement your action, and write the wait code as a behavior in `behavior.scenic`, and chain the `take` clause for the action and the `do` clause for the wait behavior inside another behavior, and call this new behavior to perform the action in your Scenic program. See the Sawyer implementation as an example.
+- In general, any kind of complex action/function from the robot's API that will block code execution should be written as a `behavior` rather than an `Action`.
+-
+-
+## Other notes: 
+- The `gazebo_models` contains object models that can be added to the simulation. This folder is a clone of this repo: https://github.com/osrf/gazebo_models

src/scenic/simulators/Gazebo/actions.py

@@ -0,0 +1,35 @@
+import math
+import sys
+
+import actionlib
+from actionlib_msgs.msg import GoalStatus
+import controller_manager_msgs.srv
+from geometry_msgs.msg import (
+    Point,
+    Pose,
+    PoseStamped,
+    Quaternion,
+    TransformStamped,
+    Twist,
+)
+import numpy as np
+import rospy
+import tf2_ros
+
+# import tf.transformations as T
+from tf.transformations import euler_from_quaternion, quaternion_from_euler
+import trajectory_msgs.msg
+from visualization_msgs.msg import Marker, MarkerArray
+
+from scenic.core.simulators import *
+from scenic.simulators.Gazebo.utils.state_utils import ApplyROSTransform
+
+"""
+READ:
+This is the library of all Actions. For each any actions you would like to perform,
+check if a corresponding behavior exists in behavior.scenic. It is recommended to use
+the behaviors since those take care of waiting for controllers and waiting until finish
+
+If you would like to add a new Action, make sure the applyTo() function returns a function
+that takes no arguments
+"""

src/scenic/simulators/Gazebo/behaviors.scenic

@@ -0,0 +1,22 @@
+import rospy
+import time
+import numpy as np
+
+behavior WaitForTime(wait_time):
+    t0 = time.time()
+    t1 = t0
+    while t1 - t0 < wait_time:
+        wait
+        t1 = time.time()
+
+behavior WaitForTimeROS(wait_time):
+    """
+    Wait for time in terms of ros time
+    """
+    t0 = rospy.get_rostime()
+    t1 = rospy.get_rostime()
+    while (t1.secs + t1.nsecs/(10**9) - t0.secs - t0.nsecs/(10**9)) < wait_time:
+        wait
+        t1 = rospy.get_rostime()
+        # print((t1.secs + t1.nsecs/(10**9) - t0.secs - t0.nsecs/(10**9)))
+

src/scenic/simulators/Gazebo/gazebo_models/.gitignore

@@ -0,0 +1,6 @@
+build
+build_*
+Ogre.log
+model.tar.gz
+*.swp
+.DS_Store

src/scenic/simulators/Gazebo/gazebo_models/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+if(COMMAND CMAKE_POLICY)
+  CMAKE_POLICY(SET CMP0003 NEW)
+  CMAKE_POLICY(SET CMP0004 NEW)
+endif(COMMAND CMAKE_POLICY)
+
+set (CMAKE_INSTALL_PREFIX /usr/share CACHE STRING "Install path prefix")
+
+project (gazebo_models)
+string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
+
+exec_program(date ARGS -u +%Y-%m-%-dT%H:%M:%S OUTPUT_VARIABLE CURRENT_DATE)
+
+set (MODEL_URI_LIST "" CACHE INTERNAL "Model URI List" FORCE)
+
+# Convert install prefix to an absolute path, to support the common case of
+# doing this (which otherwise will fail during the install):`
+#   cmake -DCMAKE_INSTALL_PREFIX=~/.gazebo ..
+get_filename_component(CMAKE_INSTALL_PREFIX_ABSOLUTE ${CMAKE_INSTALL_PREFIX}
+  ABSOLUTE)
+
+file(GLOB_RECURSE MODEL_CFGS "model.config")
+foreach (MODEL_CFG ${MODEL_CFGS})
+  get_filename_component(dir ${MODEL_CFG} DIRECTORY) # get dir of model.config
+  get_filename_component(dir ${dir} NAME) # get dirname only, not full path
+  add_custom_target(${dir} ALL COMMAND mkdir -p ${PROJECT_BINARY_DIR}/${dir}
+    COMMAND tar czvf ${PROJECT_BINARY_DIR}/${dir}/model.tar.gz ../${dir})
+
+  install (DIRECTORY ${dir} DESTINATION ${CMAKE_INSTALL_PREFIX_ABSOLUTE}/models)
+  install (FILES ${PROJECT_BINARY_DIR}/${dir}/model.tar.gz DESTINATION
+    ${CMAKE_INSTALL_PREFIX_ABSOLUTE}/models/${dir})
+  set(MODEL_URI_LIST "${MODEL_URI_LIST}<uri>file://${dir}</uri>\n")
+endforeach ()
+
+configure_file("${CMAKE_SOURCE_DIR}/manifest.xml.in"
+               "${PROJECT_BINARY_DIR}/manifest.xml")
+
+install (FILES "${PROJECT_BINARY_DIR}/manifest.xml" DESTINATION
+         ${CMAKE_INSTALL_PREFIX_ABSOLUTE}/models/)
+
+configure_file("${CMAKE_SOURCE_DIR}/database.config.in"
+               "${PROJECT_BINARY_DIR}/database.config")
+
+install (FILES "${PROJECT_BINARY_DIR}/database.config" DESTINATION
+         ${CMAKE_INSTALL_PREFIX_ABSOLUTE}/models/)
+
+message (STATUS "Install path: ${CMAKE_INSTALL_PREFIX_ABSOLUTE}/models")
+
+# This must always be last!
+include(CPack)

src/scenic/simulators/Gazebo/gazebo_models/LICENSE

@@ -0,0 +1,15 @@
+Software License Agreement (Creative Commons Attribution 3.0 Unported)
+
+Copyright 2012 Nathan Koenig
+
+Licensed under the Creative Commons Attribution 3.0 Unported License 
+(the "License"); you may not use this file except in compliance with the
+License.  You may obtain a copy of the License at
+
+    http://creativecommons.org/licenses/by/3.0/
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.

src/scenic/simulators/Gazebo/gazebo_models/README.md

@@ -0,0 +1,5 @@
+This repository holds the [Gazebo](http://gazebosim.org) model database.
+
+Learn more about the database [here](http://gazebosim.org/tutorials?tut=model_structure&cat=build_robot).
+
+Learn how to contribute models [here](http://gazebosim.org/tutorials?tut=model_contrib&cat=build_robot).

src/scenic/simulators/Gazebo/gazebo_models/ambulance/meshes/ambulance.mtl

@@ -0,0 +1,30 @@
+# 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware
+# File Created: 27.10.2016 19:35:26
+
+newmtl Ambulance
+	Ns 10.0000
+	Ni 1.5000
+	d 1.0000
+	Tr 0.0000
+	Tf 1.0000 1.0000 1.0000 
+	illum 2
+	Ka 0.5882 0.5882 0.5882
+	Kd 0.5882 0.5882 0.5882
+	Ks 0.0000 0.0000 0.0000
+	Ke 0.0000 0.0000 0.0000
+	map_Ka ambulance.png
+	map_Kd ambulance.png
+
+newmtl Firetruck_Wheels
+	Ns 10.0000
+	Ni 1.5000
+	d 1.0000
+	Tr 0.0000
+	Tf 1.0000 1.0000 1.0000 
+	illum 2
+	Ka 0.5882 0.5882 0.5882
+	Kd 0.5882 0.5882 0.5882
+	Ks 0.0000 0.0000 0.0000
+	Ke 0.0000 0.0000 0.0000
+	map_Ka model://suv/materials/textures/wheels_01.png
+	map_Kd model://suv/materials/textures/wheels_01.png