diff --git a/nextage_description/urdf/nxo_gz.xacro b/nextage_description/urdf/nxo_gz.xacro
new file mode 100644
index 00000000..a98230c8
--- /dev/null
+++ b/nextage_description/urdf/nxo_gz.xacro
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<robot name="NextageOpenGz"
+       xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find nextage_description)/urdf/NextageOpen.urdf" />
+  <xacro:include filename="$(find nextage_description)/urdf/nxo_gz_camera.xacro" />
+ 
+  <xacro:property name="M_PI" value="3.1415926535897931" />
+  <xacro:property name="jnt_head_camera_displacement_x" value="0.0175" />
+  <xacro:property name="jnt_head_camera_displacement_y" value="0.0725" />
+  <xacro:property name="jnt_head_camera_displacement_z" value="0.105" />
+  <xacro:property name="jnt_head_camera_rot_y" value="0.25" />
+  <xacro:property name="link_head_camera_rot_z" value="0.05" />
+
+  <!-- NOTE: The poses of headmount cameras are NOT precisely adjusted to
+             the 3D model nor the actual hardware config. These configs
+             are meant for a rapid prototyping only. If you need precise
+             configuration, please go ahead modify the following, then
+             (if possible) send a patch to https://github.com/tork-a/rtmros_nextage/pulls
+             so that other developers and users can use (and possible improve).
+  -->
+  <xacro:camera_joint_link_gzplugin
+         camera_name="L_HEADMOUNT_CAMERA" link_parent="HEAD_JOINT1_Link"
+         jnt_displacement_x="${jnt_head_camera_displacement_x}" jnt_displacement_y="${jnt_head_camera_displacement_y}" jnt_displacement_z="${jnt_head_camera_displacement_z}"
+         jnt_rotation_x="0" jnt_rotation_y="${jnt_head_camera_rot_y}" jnt_rotation_z="0"
+         link_displacement_x="0.0325" link_displacement_y="0" link_displacement_z="0"
+         link_rotation_x="0" link_rotation_y="${M_PI/2}" link_rotation_z="-${link_head_camera_rot_z}" />
+
+  <xacro:camera_joint_link_gzplugin
+         camera_name="R_HEADMOUNT_CAMERA" link_parent="HEAD_JOINT1_Link"
+         jnt_displacement_x="${jnt_head_camera_displacement_x}" jnt_displacement_y="-${jnt_head_camera_displacement_y}" jnt_displacement_z="${jnt_head_camera_displacement_z}"
+         jnt_rotation_x="0" jnt_rotation_y="${jnt_head_camera_rot_y}" jnt_rotation_z="0"
+         link_displacement_x="0.0325" link_displacement_y="0" link_displacement_z="0"
+         link_rotation_x="0" link_rotation_y="${M_PI/2}" link_rotation_z="${link_head_camera_rot_z}" />
+
+  <!-- TODO hand cameras can be added here with the very similar code as
+            headmount cameras. -->
+</robot>
diff --git a/nextage_description/urdf/nxo_gz_camera.xacro b/nextage_description/urdf/nxo_gz_camera.xacro
new file mode 100644
index 00000000..92cf760e
--- /dev/null
+++ b/nextage_description/urdf/nxo_gz_camera.xacro
@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+	<xacro:property name="M_PI" value="3.1415926535897931" />
+
+	<!-- Immitate two head-mount cameras, each of which are not stereo camera 
+		by themselves but are made to function as stereo by launch file https://github.com/tork-a/rtmros_nextage/blob/indigo-devel/nextage_ros_bridge/launch/nextage_ueye_stereo.launch 
+		Values in Gazebo plugin are originally copied from http://gazebosim.org/tutorials?tut=ros_gzplugins#camera 
+		and only partially tailored as of May 2017 -->
+
+	<!-- Adding camera for simulation -->
+	<xacro:macro name="camera_joint_link_gzplugin"
+		params="camera_name link_parent jnt_displacement_x jnt_displacement_y jnt_displacement_z jnt_rotation_x jnt_rotation_y jnt_rotation_z link_displacement_x link_displacement_y link_displacement_z link_rotation_x link_rotation_y link_rotation_z">
+		<joint name="${camera_name}" type="fixed">
+			<origin
+				xyz="${jnt_displacement_x} ${jnt_displacement_y} ${jnt_displacement_z}"
+				rpy="${jnt_rotation_x} ${jnt_rotation_y} ${jnt_rotation_z}" />
+			<parent link="${link_parent}" />
+			<child link="${camera_name}_Link" />
+		</joint>
+		<link name="${camera_name}_Link">
+			<visual>
+				<origin
+					xyz="${link_displacement_x} ${link_displacement_y} ${link_displacement_z}"
+					rpy="${link_rotation_x} ${link_rotation_y} ${link_rotation_z}" />
+				<geometry>
+					<cylinder length="0.01" radius="0.015" />
+				</geometry>
+				<material name="blue" />
+			</visual>
+			<inertial>
+				<origin xyz="0 0 0" rpy="0 0 0" />
+				<mass value="1" />
+				<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0"
+					izz="1.0" />
+			</inertial>
+		</link>
+		<gazebo reference="${camera_name}_Link">
+			<sensor type="camera" name="${camera_name}">
+				<update_rate>30.0</update_rate>
+				<camera name="${camera_name}">
+					<horizontal_fov>1.3962634</horizontal_fov>
+					<image>
+						<width>800</width>
+						<height>800</height>
+						<format>R8G8B8</format>
+					</image>
+					<clip>
+						<near>0.1</near>
+						<far>30</far>
+					</clip>
+					<noise>
+						<type>gaussian</type>
+						<!-- Noise is sampled independently per pixel on each frame. That pixel's 
+							noise value is added to each of its color channels, which at that point lie 
+							in the range [0,1]. -->
+						<mean>0.0</mean>
+						<stddev>0.007</stddev>
+					</noise>
+				</camera>
+				<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
+					<alwaysOn>true</alwaysOn>
+					<updateRate>0.0</updateRate>
+					<cameraName>${camera_name}</cameraName>
+					<imageTopicName>image_raw</imageTopicName>
+					<cameraInfoTopicName>camera_info</cameraInfoTopicName>
+					<frameName>${camera_name}_Link_frame</frameName>
+					<hackBaseline>0.07</hackBaseline>
+					<distortionK1>0.0</distortionK1>
+					<distortionK2>0.0</distortionK2>
+					<distortionK3>0.0</distortionK3>
+					<distortionT1>0.0</distortionT1>
+					<distortionT2>0.0</distortionT2>
+				</plugin>
+			</sensor>
+		</gazebo>
+	</xacro:macro>
+</robot>
diff --git a/nextage_gazebo/CMakeLists.txt b/nextage_gazebo/CMakeLists.txt
index 4720a651..434ba630 100644
--- a/nextage_gazebo/CMakeLists.txt
+++ b/nextage_gazebo/CMakeLists.txt
@@ -1,50 +1,12 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(nextage_gazebo)
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
   gazebo_ros
   gazebo_ros_control
   nextage_description
   rostest
 )
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if you package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES nextage_gazebo
@@ -52,82 +14,22 @@ catkin_package(
 #  DEPENDS system_lib
 )
 
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-# include_directories(include)
 include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-## Declare a C++ library
-# add_library(nextage_gazebo
-#   src/${PROJECT_NAME}/nextage_gazebo.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(nextage_gazebo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-# add_executable(nextage_gazebo_node src/nextage_gazebo_node.cpp)
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(nextage_gazebo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(nextage_gazebo_node
-#   ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
 install(PROGRAMS
    scripts/go_initial.py
    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
-## Mark executables and/or libraries for installation
-# install(TARGETS nextage_gazebo nextage_gazebo_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
 ## Mark other files for installation (e.g. launch and bag files, etc.)
 install(DIRECTORY config launch model test
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
    USE_SOURCE_PERMISSIONS
 )
 
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_nextage_gazebo.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-add_rostest(test/gz.test) # important to have one gz.test files, becuase running gazebo parallely makes trouble on port settings
-
+if (CATKIN_ENABLE_TESTING)
+  find_package(catkin REQUIRED COMPONENTS rostest)
+  add_rostest(test/gz.test) # important to have one gz.test files, becuase running gazebo parallely makes trouble on port settings
+endif()
diff --git a/nextage_gazebo/launch/gz.rviz b/nextage_gazebo/launch/gz.rviz
new file mode 100644
index 00000000..15a07a1f
--- /dev/null
+++ b/nextage_gazebo/launch/gz.rviz
@@ -0,0 +1,340 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 630
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Image_head_L
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        CHEST_JOINT0_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        HEAD_JOINT0_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        HEAD_JOINT1_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LARM_JOINT0_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LARM_JOINT1_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LARM_JOINT2_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LARM_JOINT3_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LARM_JOINT4_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LARM_JOINT5_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        L_HEADMOUNT_CAMERA_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Link Tree Style: Links in Alphabetic Order
+        RARM_JOINT0_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RARM_JOINT1_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RARM_JOINT2_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RARM_JOINT3_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RARM_JOINT4_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RARM_JOINT5_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        R_HEADMOUNT_CAMERA_Link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        WAIST:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /L_HEADMOUNT_CAMERA/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image_head_L
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /hand_left/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image_hand_L
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /hand_right/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image_hand_R
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /R_HEADMOUNT_CAMERA/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image_head_R
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        CHEST_JOINT0_Link:
+          Value: true
+        HEAD_JOINT0_Link:
+          Value: true
+        HEAD_JOINT1_Link:
+          Value: true
+        LARM_JOINT0_Link:
+          Value: true
+        LARM_JOINT1_Link:
+          Value: true
+        LARM_JOINT2_Link:
+          Value: true
+        LARM_JOINT3_Link:
+          Value: true
+        LARM_JOINT4_Link:
+          Value: true
+        LARM_JOINT5_Link:
+          Value: true
+        L_HEADMOUNT_CAMERA_Link:
+          Value: true
+        RARM_JOINT0_Link:
+          Value: true
+        RARM_JOINT1_Link:
+          Value: true
+        RARM_JOINT2_Link:
+          Value: true
+        RARM_JOINT3_Link:
+          Value: true
+        RARM_JOINT4_Link:
+          Value: true
+        RARM_JOINT5_Link:
+          Value: true
+        R_HEADMOUNT_CAMERA_Link:
+          Value: true
+        WAIST:
+          Value: true
+      Marker Scale: 0.5
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        WAIST:
+          CHEST_JOINT0_Link:
+            HEAD_JOINT0_Link:
+              HEAD_JOINT1_Link:
+                L_HEADMOUNT_CAMERA_Link:
+                  {}
+                R_HEADMOUNT_CAMERA_Link:
+                  {}
+            LARM_JOINT0_Link:
+              LARM_JOINT1_Link:
+                LARM_JOINT2_Link:
+                  LARM_JOINT3_Link:
+                    LARM_JOINT4_Link:
+                      LARM_JOINT5_Link:
+                        {}
+            RARM_JOINT0_Link:
+              RARM_JOINT1_Link:
+                RARM_JOINT2_Link:
+                  RARM_JOINT3_Link:
+                    RARM_JOINT4_Link:
+                      RARM_JOINT5_Link:
+                        {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: WAIST
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.67016
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.140808
+        Y: -0.0369538
+        Z: 0.238255
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.650399
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.505404
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 833
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image_hand_L:
+    collapsed: false
+  Image_hand_R:
+    collapsed: false
+  Image_head_L:
+    collapsed: false
+  Image_head_R:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002b7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000041a00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002b7000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000123000002b7fc0200000008fb000000180049006d006100670065005f0068006500610064005f004c01000000280000009f0000001600fffffffb000000180049006d006100670065005f0068006500610064005f005201000000cd000000c90000001600fffffffc0000019c0000009f0000001600fffffffa000000000100000002fb000000180049006d006100670065005f00680061006e0064005f004c0100000000ffffffff0000009300fffffffb000000180049006d006100670065005f0068006500610064005f005200000003a10000010f0000000000000000fb000000180049006d006100670065005f00680061006e0064005f004c010000016a0000009b0000000000000000fb000000180049006d006100670065005f00680061006e0064005f005201000002410000009e0000001600fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005170000003efc0100000002fb0000000800540069006d0065010000000000000517000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000027e000002b700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1303
+  X: 437
+  Y: 1125
diff --git a/nextage_gazebo/launch/gz_viz.launch b/nextage_gazebo/launch/gz_viz.launch
new file mode 100644
index 00000000..c9e7c41e
--- /dev/null
+++ b/nextage_gazebo/launch/gz_viz.launch
@@ -0,0 +1,38 @@
+<!-- Took nextage_moveit_config/launch/moveit_rviz.launch as an example. -->
+<launch>
+  <arg name="debug" default="false" doc="Passing true to `debug` allows you to run this launch on gdb."/>
+  <arg name="extra_gazebo_args" default=""/>
+  <arg name="gui" default="true"/>
+  <arg name="gzpose" default="-x 0.01 -y 0.01 -z 1.0" />
+  <arg name="headless" default="false"/>
+  <arg name="paused" default="false"/>
+  <arg name="physics" default="ode"/>
+  <arg name="sec_idle" default="1.0" />
+  <arg name="use_sim_time" default="true"/>
+  <arg name="verbose" default="false"/>
+  <arg name="visualize_laser" default="false" />
+  <!--  <arg name="world_file" default="$(find turtlebot_gazebo)/worlds/playground.world" /> -->
+  <arg name="world_file" default="worlds/empty.world" />
+
+  <arg name="RVIZ_FIXEDFRAME" default="WAIST" />
+  <arg name="command_args" default="-d $(find nextage_gazebo)/launch/gz.rviz --fixed-frame $(arg RVIZ_FIXEDFRAME)" />
+  
+  <include file="$(find nextage_gazebo)/launch/nextage_world.launch">
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="extra_gazebo_args" value="$(arg extra_gazebo_args)" />
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="gzpose" value="$(arg gzpose)" />
+    <arg name="headless" value="$(arg headless)" />
+    <arg name="paused" value="$(arg paused)" />
+    <arg name="physics" value="$(arg physics)" />
+    <arg name="sec_idle" value="$(arg sec_idle)" />
+    <arg name="use_sim_time" value="$(arg use_sim_time)" />
+    <arg name="verbose" value="$(arg verbose)" />
+    <arg name="visualize_laser" value="$(arg visualize_laser)" />
+    <arg name="world_file" value="$(arg world_file)" />
+  </include>
+  <include file="$(find nextage_gazebo)/launch/rviz.launch">
+    <arg name="RVIZ_FIXEDFRAME" value="$(arg RVIZ_FIXEDFRAME)" />
+    <arg name="command_args" value="$(arg command_args)" />  
+  </include>
+</launch>
diff --git a/nextage_gazebo/launch/nextage_world.launch b/nextage_gazebo/launch/nextage_world.launch
index b07c4b22..d29263d9 100644
--- a/nextage_gazebo/launch/nextage_world.launch
+++ b/nextage_gazebo/launch/nextage_world.launch
@@ -52,7 +52,7 @@
 
 
   <!-- Load universal robotic description format (URDF) -->
-  <arg name="model" value="$(find nextage_description)/urdf/NextageOpen.urdf" />
+  <arg name="model" value="$(find nextage_description)/urdf/nxo_gz.xacro" />
   <param name="robot_description" command="$(find xacro)/xacro.py '$(arg model)'" />
   <node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen"
         args="$(arg gzpose) -J LARM_JOINT2 -2.0 -J RARM_JOINT2 -2.0 -urdf -model NextageOpen -param robot_description" />
diff --git a/nextage_gazebo/launch/rviz.launch b/nextage_gazebo/launch/rviz.launch
new file mode 100644
index 00000000..99cab84b
--- /dev/null
+++ b/nextage_gazebo/launch/rviz.launch
@@ -0,0 +1,8 @@
+<!-- Took nextage_moveit_config/launch/moveit_rviz.launch as an example. -->
+<launch> 
+  <arg name="RVIZ_FIXEDFRAME" default="WAIST" />
+  <arg name="command_args" default="-d $(find nextage_gazebo)/launch/gz.rviz --fixed-frame $(arg RVIZ_FIXEDFRAME)" />
+  <arg name="debug" default="false" doc="Passing true to `debug` allows you to run this launch on gdb."/>
+
+  <include file="$(find hironx_ros_bridge)/launch/rviz.launch" pass_all_args="true" />
+</launch>
diff --git a/nextage_gazebo/model/materials.urdf.xacro b/nextage_gazebo/model/materials.urdf.xacro
deleted file mode 100644
index ed36c74a..00000000
--- a/nextage_gazebo/model/materials.urdf.xacro
+++ /dev/null
@@ -1,36 +0,0 @@
-<?xml version="1.0"?>
-<robot>
-
-    <material name="Black">
-        <color rgba="0.0 0.0 0.0 1.0"/>
-    </material>
-
-    <material name="Blue">
-        <color rgba="0.0 0.0 0.8 1.0"/>
-    </material>
-
-    <material name="Green">
-        <color rgba="0.0 0.8 0.0 1.0"/>
-    </material>
-
-    <material name="LightGrey">
-        <color rgba="0.6 0.6 0.6 1.0"/>
-    </material>
-
-    <material name="Grey">
-        <color rgba="0.4 0.4 0.4 1.0"/>
-    </material>
-
-    <material name="DarkGrey">
-        <color rgba="0.2 0.2 0.2 1.0"/>
-    </material>
-
-    <material name="Red">
-        <color rgba="0.8 0.0 0.0 1.0"/>
-    </material>
-
-    <material name="White">
-        <color rgba="1.0 1.0 1.0 1.0"/>
-    </material>
-
-</robot>
diff --git a/nextage_gazebo/package.xml b/nextage_gazebo/package.xml
index 3168a4a8..ff49f1d5 100644
--- a/nextage_gazebo/package.xml
+++ b/nextage_gazebo/package.xml
@@ -11,11 +11,6 @@
   <url type="website">http://wiki.ros.org/nextage_gazebo</url>
 
   <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>gazebo_ros</build_depend>
-  <build_depend>gazebo_ros_control</build_depend>
-  <build_depend>nextage_description</build_depend>
-  <build_depend>nextage_moveit_config</build_depend>
-  <build_depend>ros_controllers</build_depend>
   <run_depend>gazebo_ros</run_depend>
   <run_depend>gazebo_ros_control</run_depend>
   <run_depend>nextage_description</run_depend>
diff --git a/nextage_gazebo/test/gz.test b/nextage_gazebo/test/gz.test
index 33026923..551bd04a 100644
--- a/nextage_gazebo/test/gz.test
+++ b/nextage_gazebo/test/gz.test
@@ -22,4 +22,15 @@
   <test pkg="nextage_moveit_config" type="test_moveit.py" test-name="nxo_moveit"
         time-limit="300" retry="2"
         args="" />
+
+  <!-- Check if cameras publish image topics. -->
+  <arg name='TESTNAME_CAMERA_TOPICS' value='test_camera_topics' />
+  <test pkg="rostest" type="hztest" test-name="$(arg TESTNAME_CAMERA_TOPICS)"
+        name="$(arg TESTNAME_CAMERA_TOPICS)" time-limit="10" retry="3" >
+    <param name="topic" value="/L_HEADMOUNT_CAMERA/image_raw" />
+    <param name="hz" value="30.0" />
+    <param name="hzerror" value="3.00" />
+    <param name="test_duration" value="5.0" />
+    <param name="wait_time" value="30.0" />
+  </test>  
 </launch>
diff --git a/nextage_ros_bridge/launch/nextage_ros_bridge_real_hrpsys315.2.7-.launch b/nextage_ros_bridge/launch/nextage_ros_bridge_real_-hrpsys315.2.7.launch
similarity index 80%
rename from nextage_ros_bridge/launch/nextage_ros_bridge_real_hrpsys315.2.7-.launch
rename to nextage_ros_bridge/launch/nextage_ros_bridge_real_-hrpsys315.2.7.launch
index a07be942..43f8d3d8 100644
--- a/nextage_ros_bridge/launch/nextage_ros_bridge_real_hrpsys315.2.7-.launch
+++ b/nextage_ros_bridge/launch/nextage_ros_bridge_real_-hrpsys315.2.7.launch
@@ -1,4 +1,6 @@
 <launch>
+  <!-- This file is targeted for the robot that runs on hrpsys older than the version 315.2.7.
+       For those on newer version use nextage_ros_bridge_real.launch. -->
   <arg name="COLLADA_FILE" default="$(find nextage_description)/models/main.dae" /> 
   <arg name="CONF_FILE_COLLISIONDETECT" default="$(find nextage_ros_bridge)/conf/realrobot_fixedpath.conf" />
   <arg name="corbaport" default="15005" />
diff --git a/nextage_ros_bridge/launch/rviz.launch b/nextage_ros_bridge/launch/rviz.launch
index 6f496d7c..57cf54f9 100644
--- a/nextage_ros_bridge/launch/rviz.launch
+++ b/nextage_ros_bridge/launch/rviz.launch
@@ -2,11 +2,6 @@
 <launch> 
   <arg name="RVIZ_FIXEDFRAME" default="WAIST" />
   <arg name="command_args" default="--fixed-frame $(arg RVIZ_FIXEDFRAME) -d $(find hironx_ros_bridge)/launch/hironxo.rviz" />
-  
-  <!-- Passing true to `debug` allows you to run this launch on gdb. -->
-  <arg name="debug" default="false" />
-  <arg unless="$(arg debug)" name="launch_prefix" value="" />
-  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
-
+  <arg name="debug" default="false" doc="Passing true to `debug` allows you to run this launch on gdb." />
   <include file="$(find hironx_ros_bridge)/launch/rviz.launch" pass_all_args="true" />
 </launch>