diff --git a/rois_description/robot/robot1.urdf.xacro b/rois_description/robot/robot1.urdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..29c6347ea67bc06a4ee9eeb8d0a2d7d261941df2
--- /dev/null
+++ b/rois_description/robot/robot1.urdf.xacro
@@ -0,0 +1,234 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/VelocityJointInterface
+
+
+ hardware_interface/VelocityJointInterface
+ 15
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/VelocityJointInterface
+
+
+ hardware_interface/VelocityJointInterface
+ 15
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Gazebo/Blue
+ false
+
+ 0.2 0 0 0 0 0
+ true
+ 40
+
+
+
+ 500
+ 1
+ -${M_PI*3/4}
+ ${M_PI*3/4}
+
+
+
+ 0.20
+ 131
+ 0.01
+
+
+
+ /scan
+ lrf_link
+ 0.9
+ 130
+
+
+
+
+
+
+
+ robot1
+
+
+
+
+ true
+ Gazebo/White
+
+
+
+
+
+ true
+ Gazebo/Green
+
+
+
+
+
+ true
+ Gazebo/Red
+
+
+
+
+
+ true
+ Gazebo/Black
+
+
+
+
+
diff --git a/rois_description/robot/roisbot.urdf.xacro b/rois_description/robot/roisbot.urdf.xacro
index b201f413dceede53369518817832a06dacef9c7e..01f4b7a3ee37cf8a9ab327b6ef7ba457bcf58005 100644
--- a/rois_description/robot/roisbot.urdf.xacro
+++ b/rois_description/robot/roisbot.urdf.xacro
@@ -1,12 +1,12 @@
-
-
-
-
-
-
-
+
+
+
+
+
+
+
diff --git a/rois_gazebo/config/controller.yaml b/rois_gazebo/config/controller.yaml
index d9ae17569223d3b43dbd0401a78a5e9910490d94..0c663c230062e03bf630c8cda99928540747af4c 100644
--- a/rois_gazebo/config/controller.yaml
+++ b/rois_gazebo/config/controller.yaml
@@ -1,46 +1,45 @@
-roisbot:
- joint_state_controller:
- type: joint_state_controller/JointStateController
- publish_rate: 50
+joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 50
- diff_drive_controller:
- type : "diff_drive_controller/DiffDriveController"
- left_wheel : 'left_wheel_joint'
- right_wheel : 'right_wheel_joint'
- pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
- twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
-
- # Wheel separation and diameter. These are both optional.
- # diff_drive_controller will attempt to read either one or both from the
- # URDF if not specified as a parameter
- wheel_separation : 0.3
- wheel_radius : 0.1
-
- # Wheel separation and radius multipliers
- wheel_separation_multiplier: 1.0 # default: 1.0
- wheel_radius_multiplier : 1.0 # default: 1.0
-
- # Velocity commands timeout [s], default 0.5
- cmd_vel_timeout: 1.0
-
- # Base frame_id
- base_frame_id: base_footprint #default: base_link
-
- # Velocity and acceleration limits
- # Whenever a min_* is unspecified, default to -max_*
- linear:
- x:
- has_velocity_limits : true
- max_velocity : 2.0 # m/s
- min_velocity : -2.0 # m/s
- has_acceleration_limits: true
- max_acceleration : 2.0 # m/s^2
- min_acceleration : -2.0 # m/s^2
- angular:
- z:
- has_velocity_limits : true
- max_velocity : 1.0 # rad/s
- min_velocity : -1.0
- has_acceleration_limits: true
- max_acceleration : 1.0 # rad/s^2
- min_acceleration : -1.0
+diff_drive_controller:
+ type : "diff_drive_controller/DiffDriveController"
+ left_wheel : 'left_wheel_joint'
+ right_wheel : 'right_wheel_joint'
+ pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+ twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
+
+ # Wheel separation and diameter. These are both optional.
+ # diff_drive_controller will attempt to read either one or both from the
+ # URDF if not specified as a parameter
+ wheel_separation : 0.3
+ wheel_radius : 0.1
+
+ # Wheel separation and radius multipliers
+ wheel_separation_multiplier: 1.0 # default: 1.0
+ wheel_radius_multiplier : 1.0 # default: 1.0
+
+ # Velocity commands timeout [s], default 0.5
+ cmd_vel_timeout: 1.0
+
+ # Base frame_id
+ base_frame_id: base_footprint #default: base_link
+
+ # Velocity and acceleration limits
+ # Whenever a min_* is unspecified, default to -max_*
+ linear:
+ x:
+ has_velocity_limits : true
+ max_velocity : 2.0 # m/s
+ min_velocity : -2.0 # m/s
+ has_acceleration_limits: true
+ max_acceleration : 2.0 # m/s^2
+ min_acceleration : -2.0 # m/s^2
+ angular:
+ z:
+ has_velocity_limits : true
+ max_velocity : 1.0 # rad/s
+ min_velocity : -1.0
+ has_acceleration_limits: true
+ max_acceleration : 1.0 # rad/s^2
+ min_acceleration : -1.0
diff --git a/rois_gazebo/launch/setup_gazebo.launch b/rois_gazebo/launch/setup_gazebo.launch
index 8b2b02db26257ad9692b964c1fb3ef9e6e4e646f..e47afbfb576b009f7c15969850530c1e207ae7a9 100644
--- a/rois_gazebo/launch/setup_gazebo.launch
+++ b/rois_gazebo/launch/setup_gazebo.launch
@@ -1,7 +1,10 @@
+
+ default="$(find rois_description)/robot/$(arg robot_name).urdf.xacro" />
@@ -15,33 +18,20 @@
value="$(arg world_name)" />
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/rois_gazebo/launch/teleop_keyboard.launch b/rois_gazebo/launch/teleop_keyboard.launch
index ad051e88cb750f9f58e8a519cc74e384f3616ac6..467546c366bba6f5eabbe2ed51c1fe8d04b22f11 100644
--- a/rois_gazebo/launch/teleop_keyboard.launch
+++ b/rois_gazebo/launch/teleop_keyboard.launch
@@ -1,5 +1,6 @@
+
+ to="/$(arg robot_name)/diff_drive_controller/cmd_vel" />