Main Robot Module

<?xml version="1.0" encoding="UTF-8"?>

<launch>
<arg name="world_file" default="$(find
turtlecosta_gazebo)/worlds/costa_coffee_mod_clean.world"/>

<arg name="base" value="barista"/>

<!-- arg name="battery" value="$(optenv TURTLEBOT_BATTERY
/proc/acpi/battery/BAT0)"/ --> <!-- /proc/acpi/battery/BAT0 -->
<arg name="stacks" value="hexagons"/> <!-- circles, hexagons -->
<arg name="3d_sensor" value="asus_xtion_pro"/> <!-- kinect,
asus_xtion_pro -->

<arg name="paused" default="true"/>

<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="true"/>

<include file="$(find gazebo_ros)/launch/empty_world.launch">

<arg name="world_name" value="$(arg world_file)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="headless" value="$(arg headless)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
</include>

<include file="$(find turtlecosta_gazebo)/launch/includes/$(arg

base).launch.xml">
<arg name="base" value="$(arg base)"/>
<arg name="stacks" value="$(arg stacks)"/>
<arg name="3d_sensor" value="$(arg 3d_sensor)"/>
</include>

<node pkg="robot_state_publisher" type="robot_state_publisher"

name="robot_state_publisher">
<param name="publish_frequency" type="double" value="30.0" />
</node>

<!-- Fake laser -->

<!-- node pkg="nodelet" type="nodelet"
name="laserscan_nodelet_manager" args="manager"/>
<node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan"
args="load
depthimage_to_laserscan/DepthImageToLaserScanNodelet
laserscan_nodelet_manager">
<param name="scan_height" value="10"/>
<param name="output_frame_id" value="/camera_depth_frame"/>
 <param name="range_min" value="0.45"/>
<remap from="image" to="/camera/depth/image_raw"/>
<remap from="scan" to="/scan"/>
</node -->
</launch>

Simulated Load Sensor Driver

#!/usr/bin/env python

import rospy
from std_msgs.msg import Float32
from gazebo_msgs.msg import ContactsState
import math

"""
rosmsg show gazebo_msgs/ContactState
string info
string collision1_name
string collision2_name
geometry_msgs/Wrench[] wrenches
geometry_msgs/Vector3 force
float64 x
float64 y
float64 z
geometry_msgs/Vector3 torque
float64 x
float64 y
float64 z
geometry_msgs/Wrench total_wrench
geometry_msgs/Vector3 force
float64 x
float64 y
float64 z
geometry_msgs/Vector3 torque
float64 x
float64 y
float64 z
geometry_msgs/Vector3[] contact_positions
float64 x
float64 y
float64 z
geometry_msgs/Vector3[] contact_normals
float64 x
float64 y
float64 z
float64[] depths
"""

class LoadSensorInfo(object):
 def __init__(self, gravity=9.81):

self.min_pressure = 0.0
self.max_pressure = 2.0

self._gravity = gravity

self.load_msg = Float32()
self.load_msg.data = 0.0

rospy.Subscriber("/loadsensor_link_contactsensor_state",
ContactsState, self.loadsensor_callback)
self._pub = rospy.Publisher('/load_sensor', Float32,
queue_size=10)
rospy.loginfo("Simulated LOAD Sensor Ready")

def loadsensor_callback(self, data):

"""
Converts Contact Sensor Data into Weight (mass in grams) of
all the object detected
:param data:
:return:
"""
if len(data.states) >= 1:
total_force = 0
objects_contact_list = []
rospy.logdebug("##### 1 #######")
for contact_state in data.states:
collision1 = contact_state.collision1_name
# standard_apple::link::collision
object_name = collision1.split("::")[0]

if object_name not in objects_contact_list:

objects_contact_list.append(object_name)
rospy.logdebug("New Model model_name=" +
str(object_name))
Fx = contact_state.total_wrench.force.x
Fy = contact_state.total_wrench.force.y
Fz = contact_state.total_wrench.force.z
force_magnitude = math.sqrt(pow(Fx, 2) + pow(Fy,
2) + pow(Fz, 2))

total_force += force_magnitude
mass = force_magnitude / self._gravity
rospy.logdebug("ForceMagnitude =" +
str(force_magnitude))
rospy.logdebug("mass =" + str(mass))
rospy.logdebug("[F=" + str(force_magnitude) +
",M=" + str(mass) + "]")

else:
# We have already added that weight
 pass

total_mass = total_force / self._gravity

rospy.logdebug("TotalForce =" + str(total_force))
rospy.logdebug("TotalMass =" + str(total_mass))

# We publish the mass in grams

self.load_msg.data = total_mass * 1000
rospy.logdebug("##### 0 #######")

else:
rospy.logdebug("NO object on top detected...")
self.load_msg.data = 0.0

self._pub.publish(self.load_msg)

if __name__ == '__main__':
rospy.init_node('loadsensor_node', anonymous=True,
log_level=rospy.INFO)
loadsensor_object = LoadSensorInfo()
rospy.spin()

Gmapping Module
<launch>

<!-- Gmapping -->

<arg name="custom_gmapping_launch_file" default="$(find
costa_robot)/launch/includes/gmapping.launch.xml"/>
<include file="$(arg custom_gmapping_launch_file)"/>
<node pkg="laser_filters" type="scan_to_scan_filter_chain"
name="laser_filter">
<rosparam command="load" file="$(find
costa_robot)/param/laserscan_filter.yaml" />
</node>

<!--
<include file="$(find
barista_description)/launch/rviz_gmapping.launch"/>
-->
</launch>

Teleop Module

<launch>
<arg name="cmd_vel_topic" default="cmd_vel_mux/input/teleop"/>
<!-- turtlebot_teleop_key already has its own built in velocity
smoother -->
 <node pkg="turtlebot_teleop" type="turtlebot_teleop_key"
name="turtlebot_teleop_keyboard" output="screen">
<param name="scale_linear" value="0.5" type="double"/>
<param name="scale_angular" value="1.5" type="double"/>
<!--<remap from="turtlebot_teleop_keyboard/cmd_vel"
to="cmd_vel_mux/input/teleop"/>-->
<remap from="turtlebot_teleop_keyboard/cmd_vel" to="$(arg
cmd_vel_topic)"/>
</node>
</launch>