initial import

Author: yosuke

.gitignore

@@ -0,0 +1,4 @@
+build
+dist
+*.egg-info
+*.pyc

README.md

@@ -0,0 +1,48 @@
+Utility command to dump/undump pose of objects in gazebo
+
+Install
+-------
+
+```
+pip install git+https://github.com/devrt/gzdump
+```
+
+Usage
+-----
+
+```
+gzdump [--base-frame world]
+```
+
+Dump all the objects in the simulation world in [name, x, y, z, r, p, y] format (in meters, degrees).
+
+```
+gzundump [--base-frame world] [--offset-x 0] [--offset-y 0] [--offset-z 0]
+```
+
+Undump objects pose from stdin in [name, x, y, z, r, p, y] format (in meters, degrees).
+
+Example
+-------
+
+```
+gzdump > poses.txt
+```
+
+Save current poses to the file.
+
+```
+gzdump | gzundump --offset-z -1
+```
+
+Add one meters offset along with Z axes to all the objects in the world.
+
+License
+-------
+
+MIT
+
+Author
+------
+
+Yosuke Matsusaka

gzdump/__init__.py

@@ -0,0 +1,2 @@
+from dump import dump
+from undump import undump

gzdump/dump.py

@@ -0,0 +1,41 @@
+# Utility command to dump/undump pose of objects in gazebo
+#  written by Yosuke Matsusaka
+#  distributed under MIT License
+import rospy
+import math
+import argparse
+from tf import transformations
+from gazebo_msgs.srv import GetWorldProperties, GetModelState
+
+def main():
+    parser = argparse.ArgumentParser(description='Dump object pose.')
+    parser.add_argument('--base-frame', dest='base_frame',
+                        type=str, default='world',
+                        help='base frame (default: world)')
+    args = parser.parse_args()
+    dump(args)
+
+def dump(opts):
+    rospy.wait_for_service('/gazebo/get_world_properties')
+    rospy.wait_for_service('/gazebo/get_model_state')
+    world_propertiies = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
+    model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
+
+    for name in world_propertiies().model_names:
+        pose = model_state(name, opts.base_frame).pose
+        quaternion = (
+            pose.orientation.x,
+            pose.orientation.y,
+            pose.orientation.z,
+            pose.orientation.w
+        )
+        euler = map(math.degrees, transformations.euler_from_quaternion(quaternion))
+        print(','.join(map(str,[
+            name,
+            pose.position.x,
+            pose.position.y,
+            pose.position.z,
+            euler[0],
+            euler[1],
+            euler[2]
+        ])))

gzdump/undump.py

@@ -0,0 +1,51 @@
+# Utility command to dump/undump pose of objects in gazebo
+#  written by Yosuke Matsusaka
+#  distributed under MIT License
+import sys
+import rospy
+import math
+import argparse
+from tf import transformations
+from gazebo_msgs.msg import ModelState
+from gazebo_msgs.srv import SetModelState
+
+def main():
+    parser = argparse.ArgumentParser(description='Undump object pose.')
+    parser.add_argument('--base-frame', dest='base_frame',
+                        type=str, default='world',
+                        help='base frame (default: world)')
+    parser.add_argument('--offset-x', dest='offset_x',
+                        type=float, default=0,
+                        help='offset along X axis (default: 0)')
+    parser.add_argument('--offset-y', dest='offset_y',
+                        type=float, default=0,
+                        help='offset along Y axis (default: 0)')
+    parser.add_argument('--offset-z', dest='offset_z',
+                        type=float, default=0,
+                        help='offset along Z axis (default: 0)')
+    args = parser.parse_args()
+    undump(args)
+
+def undump(opts):
+    rospy.wait_for_service('/gazebo/set_model_state')
+    set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
+
+    for line in sys.stdin:
+        (name, x, y, z, r, p, ay) = line.split(',')
+        (x, y, z) = map(float, [x, y, z])
+        (r, p, ay) = map(math.radians, map(float, [r, p, ay]))
+        x += opts.offset_x
+        y += opts.offset_y
+        z += opts.offset_z
+        quaternion = transformations.quaternion_from_euler(r, p, ay)
+        model_state = ModelState()
+        model_state.model_name = name
+        model_state.reference_frame = opts.base_frame
+        model_state.pose.position.x = x
+        model_state.pose.position.y = y
+        model_state.pose.position.z = z
+        model_state.pose.orientation.x = quaternion[0]
+        model_state.pose.orientation.y = quaternion[1]
+        model_state.pose.orientation.z = quaternion[2]
+        model_state.pose.orientation.w = quaternion[3]
+        set_model_state(model_state)

setup.py

@@ -0,0 +1,28 @@
+import setuptools
+
+with open("README.md", "r") as fh:
+    long_description = fh.read()
+
+if __name__ == "__main__":
+    setuptools.setup(
+        name='gzdump',
+        version='0.0.1',
+        author="Yosuke Matsusaka",
+        author_email="[email protected]",
+        description="Utility command to dump pose of objects in gazebo",
+        long_description=long_description,
+        long_description_content_type="text/markdown",
+        url="https://github.com/devrt/gzdump",
+        packages=setuptools.find_packages(),
+        classifiers=[
+            "Programming Language :: Python :: 2",
+            "License :: OSI Approved :: MIT License",
+            "Operating System :: OS Independent",
+        ],
+        entry_points={
+            'console_scripts': [
+                'gzdump = gzdump.dump:main',
+                'gzundump = gzdump.undump:main'
+            ]
+        }
+    )