initial

README.md

@@ -0,0 +1 @@
+Move path folder to ~/.gazebo/models/

igv_gazebo/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(igv_gazebo)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED COMPONENTS 
+  roscpp 
+  gazebo_ros 
+  geometry_msgs
+)
+
+# Depend on system install of Gazebo
+find_package(gazebo REQUIRED)
+
+link_directories(${GAZEBO_LIBRARY_DIRS})
+include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+
+add_library(${PROJECT_NAME} src/alfred_plugin.cc)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+

igv_gazebo/launch/basicArena.launch

@@ -0,0 +1,5 @@
+<launch>
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find igv_gazebo)/worlds/basicWorld.world"/>
+  </include>
+</launch>

igv_gazebo/package.xml

@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+<package>
+  <name>igv_gazebo</name>
+  <version>0.0.0</version>
+  <description>The igv_gazebo package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="codestation@todo.todo">codestation</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/igv_gazebo</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>gazebo_ros</build_depend>
+  <build_depend>roscpp</build_depend>
+  <run_depend>gazebo_ros</run_depend>
+  <run_depend>roscpp</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+  </export>
+</package>

igv_gazebo/scripts/lane.py

@@ -0,0 +1,172 @@
+#!/usr/bin/env python
+
+import cv2
+import numpy as np
+import math
+import rospy
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+
+frame = None
+bridge = None
+def nothing(x):
+    pass
+
+def trackbars():
+    cv2.namedWindow('Tune')
+    cv2.moveWindow('Tune',500,400)
+    cv2.createTrackbar('HL','Tune',0,255,nothing)
+    cv2.createTrackbar('HH','Tune',0,255,nothing)
+    cv2.createTrackbar('SL','Tune',0,255,nothing)
+    cv2.createTrackbar('SH','Tune',0,255,nothing)
+    cv2.createTrackbar('VL','Tune',0,255,nothing)
+    cv2.createTrackbar('VH','Tune',0,255,nothing)
+    cv2.setTrackbarPos('HL','Tune',0)
+    cv2.setTrackbarPos('HH','Tune',180)
+    cv2.setTrackbarPos('SL','Tune',0)
+    cv2.setTrackbarPos('SH','Tune',28)
+    cv2.setTrackbarPos('VL','Tune',136)
+    cv2.setTrackbarPos('VH','Tune',255)
+
+def captureNextFrame(data):
+    global frame,bridge
+    try:
+        frame = bridge.imgmsg_to_cv2(data, "bgr8")
+    except CvBridgeError as e:
+        print(e)
+    print "callback"
+    #frame=cv2.cvtColor(cv_image,cv2.COLOR_BGR2RGB)
+
+def findDirection(roi,lines,side):
+    if lines == None:
+        if side == 0:
+            return 10000, 0
+        else:
+            return 1,-10000
+    avgDistX = avgDistY = avgSlope = 0
+    for x in range(len(lines)):
+        for x1,y1,x2,y2 in lines[x]:
+            avgDistX += ((x1+x2)/2)
+            avgDistY += ((y1+y2)/2)
+            if(x2-x1):
+                avgSlope += (y2-y1)*1.0/(x2-x1)
+            else:
+                avgSlope += (y2-y1)*1.0/0.0001
+    avgDistX = avgDistX/len(lines)
+    if side:
+        avgDistX += roi.shape[1]*0.5
+    avgDistY = avgDistY/len(lines)
+    avgSlope = avgSlope/len(lines)
+    print avgSlope
+    if avgSlope == 0:
+        avgSlope = 0.001
+    if side == 0:
+        cv2.circle(roi,(int(avgDistX),int(avgDistY)),5,(0,0,255))
+        cv2.line(roi,(int(-avgDistY/avgSlope + avgDistX),0),(int((roi.shape[0] - avgDistY)/avgSlope + avgDistX),roi.shape[0]),(255,0,255),2)
+    else:
+        cv2.line(roi,(int(-avgDistY/avgSlope + avgDistX),0),(int((roi.shape[0] - avgDistY)/avgSlope + avgDistX),roi.shape[0]),(255,255,0),2)
+        # cv2.line(roi,(int(roi.shape[1]*0.5)+int(-avgDistY/avgSlope + avgDistX),0),(int(roi.shape[1]*0.5)+int(roi.shape[0] - avgDistY/avgSlope + avgDistX),roi.shape[0]),(0,255,0),2)
+    return avgSlope,-avgDistX/avgSlope + avgDistY
+
+def findLines(roi,img,side):
+    edges = cv2.Canny(img,200,200,apertureSize = 3)
+    # cv2.imshow('Edges',edges)
+    # cv2.moveWindow('Edges',100,400)
+
+    # lines = cv2.HoughLines(edges,1,np.pi/180,50)
+    # print lines
+    # if lines!=None:
+    #     for x in range(len(lines)):
+    #         for rho,theta in lines[x]:
+    #             a = np.cos(theta)
+    #             b = np.sin(theta)
+    #             x0 = a*rho
+    #             y0 = b*rho
+    #             x1 = int(x0 + 1000*(-b))
+    #             y1 = int(y0 + 1000*(a))
+    #             x2 = int(x0 - 1000*(-b))
+    #             y2 = int(y0 - 1000*(a))
+    #
+    #             if side==0:
+    #                 cv2.line(roi,(x1,y1),(x2,y2),(0,255,0),2)
+    #             else:
+    #                 cv2.line(roi,(int(roi.shape[1]*0.5)+x1,y1),(int(roi.shape[1]*0.5)+x2,y2),(0,0,255),2)
+
+    minLineLength = 10
+    maxLineGap = 10
+    lines = cv2.HoughLinesP(edges,1,np.pi/180,1,minLineLength,maxLineGap)
+    if lines!=None:
+        for x in range(len(lines)):
+            for x1,y1,x2,y2 in lines[x]:
+                if side==0:
+                    cv2.line(roi,(x1,y1),(x2,y2),(0,255,0),2)
+                else:
+                    cv2.line(roi,(int(roi.shape[1]*0.5)+x1,y1),(int(roi.shape[1]*0.5)+x2,y2),(0,0,255),2)
+    return lines
+    # lowRanges = [cv2.getTrackbarPos('HL','Tune'), cv2.getTrackbarPos('SL','Tune'), cv2.getTrackbarPos('VL','Tune') ]
+    # highRanges = [cv2.getTrackbarPos('HH','Tune'), cv2.getTrackbarPos('SH','Tune'), cv2.getTrackbarPos('VH','Tune') ]
+    # grayROI = cv2.cvtColor(roi,cv2.COLOR_BGR2GRAY)
+    # ret2,binary = cv2.threshold(grayROI,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU)
+    # roiHSV = cv2.cvtColor(roi,cv2.COLOR_BGR2HSV)
+    # binary = cv2.inRange(roiHSV,np.array(lowRanges),np.array(highRanges))
+    # cv2.imshow('Binary',edges)
+    # cv2.moveWindow('Binary',100,400)
+
+def main():
+    global frame, bridge
+    pause = False
+    rospy.init_node("MotorControlPanel")
+    velocityPub = rospy.Publisher("/cmd_vel",Twist,queue_size=1)
+    rospy.Subscriber("/rrbot/camera1/image_raw",Image, captureNextFrame)
+    twistMsg = Twist()
+    bridge = CvBridge()
+    trackbars()
+    #vid = cv2.VideoCapture('1.avi')
+    while(True):
+        #if not pause:
+        #    ret, frame = vid.read()
+        if frame==None:
+            continue
+        frameTemp = frame[int(0.5*frame.shape[0]):frame.shape[0],0:frame.shape[1]]
+        cv2.imshow('Feed',frameTemp)
+        cv2.moveWindow('Feed',100,100)
+        roi = frameTemp.copy()
+        roi = roi[int(0.5*roi.shape[0]):int(roi.shape[0]*0.8),0:roi.shape[1]]
+        leftImage = roi[0:roi.shape[0],0:roi.shape[1]*0.5]
+        rightImage =  roi[0:roi.shape[0],roi.shape[1]*0.5:roi.shape[1]]
+        cv2.imshow('Left',leftImage)
+        linesLeft = findLines(roi,leftImage,0)
+        linesRight = findLines(roi,rightImage,1)
+        m1,c1 = findDirection(roi,linesLeft,0)
+        m2,c2 = findDirection(roi,linesRight,1)
+        print m1,c1,m2,c2
+        if(m1!=m2):
+            x = (c2-c1)/(m1-m2)
+        if x > roi.shape[1]*0.40 and x < roi.shape[1]*0.60:
+            twistMsg.linear.x = 0.2
+            twistMsg.angular.z = 0
+            print "Forward",  x/roi.shape[1]
+        elif x <= roi.shape[1]*0.40:
+            twistMsg.linear.x = 0.2
+            twistMsg.angular.z = 0.5
+            print "Left", x/roi.shape[1]
+        else:
+            twistMsg.linear.x = 0.2
+            twistMsg.angular.z = -0.5
+            print "Right", x/roi.shape[1]
+
+        velocityPub.publish(twistMsg)
+
+        cv2.imshow('ROI',roi)
+        cv2.moveWindow('ROI',100,500)
+        c = cv2.waitKey(10)
+        if c == 27:
+            break
+        elif c == 112:
+            pause = not pause
+
+
+
+if __name__=='__main__':
+    main()

igv_gazebo/scripts/lane0.py

@@ -0,0 +1,151 @@
+#!/usr/bin/env python
+
+import cv2
+import numpy as np
+import math
+import rospy
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+
+frame = None
+bridge = None
+def nothing(x):
+    pass
+
+def trackbars():
+    cv2.namedWindow('Tune')
+    cv2.moveWindow('Tune',500,400)
+    cv2.createTrackbar('HL','Tune',0,255,nothing)
+    cv2.createTrackbar('HH','Tune',0,255,nothing)
+    cv2.createTrackbar('SL','Tune',0,255,nothing)
+    cv2.createTrackbar('SH','Tune',0,255,nothing)
+    cv2.createTrackbar('VL','Tune',0,255,nothing)
+    cv2.createTrackbar('VH','Tune',0,255,nothing)
+    cv2.setTrackbarPos('HL','Tune',0)
+    cv2.setTrackbarPos('HH','Tune',180)
+    cv2.setTrackbarPos('SL','Tune',0)
+    cv2.setTrackbarPos('SH','Tune',28)
+    cv2.setTrackbarPos('VL','Tune',136)
+    cv2.setTrackbarPos('VH','Tune',255)
+
+def captureNextFrame(data):
+    global frame,bridge
+    try:
+        frame = bridge.imgmsg_to_cv2(data, "bgr8")
+    except CvBridgeError as e:
+        print(e)
+    print "callback"
+    #frame=cv2.cvtColor(cv_image,cv2.COLOR_BGR2RGB)
+
+def findDirection(roi,lines,side):
+    if lines == None:
+        if side == 0:
+            return 10000, 0
+        else:
+            return 1,-10000
+    avgDistX = avgDistY = avgSlope = 0
+    for x in range(len(lines)):
+        for x1,y1,x2,y2 in lines[x]:
+            avgDistX += ((x1+x2)/2)
+            avgDistY += ((y1+y2)/2)
+            if(x2-x1):
+                avgSlope += (y2-y1)*1.0/(x2-x1)
+            else:
+                avgSlope += (y2-y1)*1.0/0.0001
+    avgDistX = avgDistX/len(lines)
+    if side:
+        avgDistX += roi.shape[1]*0.5
+    avgDistY = avgDistY/len(lines)
+    avgSlope = avgSlope/len(lines)
+    print avgSlope
+    if avgSlope == 0:
+        avgSlope = 0.001
+    if side == 0:
+        cv2.circle(roi,(int(avgDistX),int(avgDistY)),5,(0,0,255))
+        cv2.line(roi,(int(-avgDistY/avgSlope + avgDistX),0),(int((roi.shape[0] - avgDistY)/avgSlope + avgDistX),roi.shape[0]),(255,0,255),2)
+    else:
+        cv2.line(roi,(int(-avgDistY/avgSlope + avgDistX),0),(int((roi.shape[0] - avgDistY)/avgSlope + avgDistX),roi.shape[0]),(255,255,0),2)
+        # cv2.line(roi,(int(roi.shape[1]*0.5)+int(-avgDistY/avgSlope + avgDistX),0),(int(roi.shape[1]*0.5)+int(roi.shape[0] - avgDistY/avgSlope + avgDistX),roi.shape[0]),(0,255,0),2)
+    return avgSlope,-avgDistX/avgSlope + avgDistY
+
+def findLines(roi,img,side):
+    edges = cv2.Canny(img,200,200,apertureSize = 3)
+
+    cv2.imshow('Edges',edges)
+    # cv2.moveWindow('Edges',100,400)
+
+    # lines = cv2.HoughLines(edges,1,np.pi/180,50)
+    # print lines
+    # if lines!=None:
+    #     for x in range(len(lines)):
+    #         for rho,theta in lines[x]:
+    #             a = np.cos(theta)
+    #             b = np.sin(theta)
+    #             x0 = a*rho
+    #             y0 = b*rho
+    #             x1 = int(x0 + 1000*(-b))
+    #             y1 = int(y0 + 1000*(a))
+    #             x2 = int(x0 - 1000*(-b))
+    #             y2 = int(y0 - 1000*(a))
+    #
+    #             if side==0:
+    #                 cv2.line(roi,(x1,y1),(x2,y2),(0,255,0),2)
+    #             else:
+    #                 cv2.line(roi,(int(roi.shape[1]*0.5)+x1,y1),(int(roi.shape[1]*0.5)+x2,y2),(0,0,255),2)
+
+    minLineLength = 10
+    maxLineGap = 10
+    lines = cv2.HoughLinesP(edges,1,np.pi/180,1,minLineLength,maxLineGap)
+    return lines
+    # lowRanges = [cv2.getTrackbarPos('HL','Tune'), cv2.getTrackbarPos('SL','Tune'), cv2.getTrackbarPos('VL','Tune') ]
+    # highRanges = [cv2.getTrackbarPos('HH','Tune'), cv2.getTrackbarPos('SH','Tune'), cv2.getTrackbarPos('VH','Tune') ]
+    # grayROI = cv2.cvtColor(roi,cv2.COLOR_BGR2GRAY)
+    # ret2,binary = cv2.threshold(grayROI,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU)
+    # roiHSV = cv2.cvtColor(roi,cv2.COLOR_BGR2HSV)
+    # binary = cv2.inRange(roiHSV,np.array(lowRanges),np.array(highRanges))
+    # cv2.imshow('Binary',edges)
+    # cv2.moveWindow('Binary',100,400)
+
+def main():
+    global frame, bridge
+    pause = False
+    rospy.init_node("MotorControlPanel")
+    velocityPub = rospy.Publisher("/cmd_vel",Twist,queue_size=1)
+    rospy.Subscriber("/rrbot/camera1/image_raw",Image, captureNextFrame)
+    twistMsg = Twist()
+    bridge = CvBridge()
+    trackbars()
+    vid = cv2.VideoCapture('1.avi')
+    while(True):
+        if not pause:
+            ret, frame = vid.read()
+        if frame==None:
+            continue
+        frameTemp = frame[int(0.5*frame.shape[0]):frame.shape[0],0:frame.shape[1]]
+        cv2.imshow('Feed',frameTemp)
+        #cv2.moveWindow('Feed',100,100)
+        roi = frameTemp.copy()
+        roi = roi#[int(0.5*roi.shape[0]):int(roi.shape[0]*0.8),0:roi.shape[1]]
+        leftImage = roi#[0:roi.shape[0],0:roi.shape[1]*0.5]
+        #rightImage =  roi#[0:roi.shape[0],roi.shape[1]*0.5:roi.shape[1]]
+        cv2.imshow('Left',leftImage)
+        lines = findLines(roi,leftImage,0)
+        size = roi.shape[0],roi.shape[1],3
+        black = np.zeros(size, dtype=np.uint8)
+        if lines!=None:
+            for x in range(len(lines)):
+                for x1,y1,x2,y2 in lines[x]:
+                    cv2.line(black,(x1,y1),(x2,y2),(255,255,255),2)
+        cv2.imshow('ROI',black)
+        #cv2.moveWindow('ROI',100,500)
+        c = cv2.waitKey(10)
+        if c == 27:
+            break
+        elif c == 112:
+            pause = not pause
+
+
+
+if __name__=='__main__':
+    main()

igv_gazebo/src/alfred_plugin.cc

@@ -0,0 +1,72 @@
+#ifndef _ALFRED_PLUGIN_HH
+#define _ALFRED_PLUGIN_HH
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <string>
+#include <thread>
+#include <ros/ros.h>
+#include <ros/callback_queue.h>
+#include <ros/subscribe_options.h>
+#include <geometry_msgs/Twist.h>
+
+namespace gazebo
+{
+	class AlfredPlugin : public ModelPlugin
+	{
+		public: AlfredPlugin() {}
+
+		public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
+		{
+			if(_model->GetJointCount()==0)
+			{
+				std::cerr<<"\nInvalid joint count, plugin not loaded.\n";
+				return;
+			}
+			this->model = _model;
+			this->leftJoint = this->model->GetJoint("left_wheel_joint");
+			this->rightJoint = this->model->GetJoint("right_wheel_joint");
+			this->pid = common::PID(10,2,0);
+			this->model->GetJointController()->SetVelocityPID(this->leftJoint->GetScopedName(),this->pid);
+			this->model->GetJointController()->SetVelocityPID(this->rightJoint->GetScopedName(),this->pid);
+			if (!ros::isInitialized())
+			{
+			  int argc = 0;
+			  char **argv = NULL;
+			  ros::init(argc, argv, "alfred_simulator", ros::init_options::NoSigintHandler);
+			}
+			this->rosNode.reset(new ros::NodeHandle("alfred_simulator"));
+			ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::Twist>("/cmd_vel",1,boost::bind(&AlfredPlugin::cmdVelCallback, this, _1),ros::VoidPtr(), &this->rosQueue);
+			this->cmdVelSub = this->rosNode->subscribe(so);
+			this->rosQueueThread = std::thread(std::bind(&AlfredPlugin::QueueThread, this));
+		}
+
+		public: void cmdVelCallback(const geometry_msgs::TwistConstPtr &_msg)
+		{
+			double leftVel = _msg->linear.x - _msg->angular.z*0.5*0.34;
+			double rightVel = _msg->linear.x + _msg->angular.z*0.5*0.34;
+			this->model->GetJointController()->SetVelocityTarget(this->leftJoint->GetScopedName(), leftVel/0.05);
+			this->model->GetJointController()->SetVelocityTarget(this->rightJoint->GetScopedName(), rightVel/0.05);
+		}
+
+		private: void QueueThread()
+		{
+		  static const double timeout = 0.01;
+		  while (this->rosNode->ok())
+		  {
+		    this->rosQueue.callAvailable(ros::WallDuration(timeout));
+		  }
+		}
+		private:
+			physics::ModelPtr model;
+			physics::JointPtr leftJoint,rightJoint;
+			common::PID pid;
+			std::unique_ptr<ros::NodeHandle> rosNode;
+			ros::Subscriber cmdVelSub;
+			ros::CallbackQueue rosQueue;
+			std::thread rosQueueThread;
+
+	};
+	GZ_REGISTER_MODEL_PLUGIN(AlfredPlugin)
+}
+#endif

igv_gazebo/src/simple_world_plugin.cpp

@@ -0,0 +1,28 @@
+#include <gazebo/common/Plugin.hh>
+#include <ros/ros.h>
+
+namespace gazebo
+{
+class WorldPluginTutorial : public WorldPlugin
+{
+public:
+  WorldPluginTutorial() : WorldPlugin()
+  {
+  }
+
+  void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
+  {
+    // Make sure the ROS node for Gazebo has already been initialized                                                                                    
+    if (!ros::isInitialized())
+    {
+      ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
+        << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
+      return;
+    }
+
+    ROS_INFO("Hello World!");
+  }
+
+};
+GZ_REGISTER_WORLD_PLUGIN(WorldPluginTutorial)
+}

igv_gazebo/worlds/basicArena.world

@@ -0,0 +1,223 @@
+<sdf version='1.6'>
+  <world name='default'>
+    <model name='ground_plane'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+    </model>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose frame=''>0 0 10 0 -0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.1 0.1 0.1 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.5 -1</direction>
+    </light>
+    <plugin name='gazebo_tutorials' filename='libigv_gazebo.so'/>
+    <model name='my_mesh'>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <static>1</static>
+      <link name='body'>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <uri>model://basicArena.dae</uri>
+              <scale>1 1 1</scale>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <uri>model://basicArena.dae</uri>
+              <scale>1 1 1</scale>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <self_collide>0</self_collide>
+        <kinematic>0</kinematic>
+        <gravity>1</gravity>
+      </link>
+    </model>
+    <gravity>0 0 -9.8</gravity>
+    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
+    <atmosphere type='adiabatic'/>
+    <physics name='default_physics' default='0' type='ode'>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>1</shadows>
+    </scene>
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+      <latitude_deg>0</latitude_deg>
+      <longitude_deg>0</longitude_deg>
+      <elevation>0</elevation>
+      <heading_deg>0</heading_deg>
+    </spherical_coordinates>
+    <light name='user_point_light_0' type='point'>
+      <pose frame=''>-1.09284 -1.99664 1 0 -0 0</pose>
+      <diffuse>0.5 0.5 0.5 1</diffuse>
+      <specular>0.1 0.1 0.1 1</specular>
+      <attenuation>
+        <range>20</range>
+        <constant>0.5</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <cast_shadows>0</cast_shadows>
+      <direction>0 0 -1</direction>
+    </light>
+    <light name='user_point_light_1' type='point'>
+      <pose frame=''>-1.09243 2.01906 1 0 -0 0</pose>
+      <diffuse>0.5 0.5 0.5 1</diffuse>
+      <specular>0.1 0.1 0.1 1</specular>
+      <attenuation>
+        <range>20</range>
+        <constant>0.5</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <cast_shadows>0</cast_shadows>
+      <direction>0 0 -1</direction>
+    </light>
+    <light name='user_point_light_2' type='point'>
+      <pose frame=''>1.10541 2.0251 1 0 -0 0</pose>
+      <diffuse>0.5 0.5 0.5 1</diffuse>
+      <specular>0.1 0.1 0.1 1</specular>
+      <attenuation>
+        <range>20</range>
+        <constant>0.5</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <cast_shadows>0</cast_shadows>
+      <direction>0 0 -1</direction>
+    </light>
+    <light name='user_point_light_3' type='point'>
+      <pose frame=''>1.08305 -2.0142 1 0 -0 0</pose>
+      <diffuse>0.5 0.5 0.5 1</diffuse>
+      <specular>0.1 0.1 0.1 1</specular>
+      <attenuation>
+        <range>20</range>
+        <constant>0.5</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <cast_shadows>0</cast_shadows>
+      <direction>0 0 -1</direction>
+    </light>
+    <state world_name='default'>
+      <sim_time>756 850000000</sim_time>
+      <real_time>123 843586380</real_time>
+      <wall_time>1485941703 881016830</wall_time>
+      <iterations>123188</iterations>
+      <model name='ground_plane'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose frame=''>0 0 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='my_mesh'>
+        <pose frame=''>-0.530562 -1.22301 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='body'>
+          <pose frame=''>-0.530562 -1.22301 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <light name='sun'>
+        <pose frame=''>0 0 10 0 -0 0</pose>
+      </light>
+      <light name='user_point_light_0'>
+        <pose frame=''>-1.09284 -1.99664 1 0 -0 0</pose>
+      </light>
+      <light name='user_point_light_1'>
+        <pose frame=''>-1.09243 2.01906 1 0 -0 0</pose>
+      </light>
+      <light name='user_point_light_2'>
+        <pose frame=''>1.10541 2.0251 1 0 -0 0</pose>
+      </light>
+      <light name='user_point_light_3'>
+        <pose frame=''>1.08305 -2.0142 1 0 -0 0</pose>
+      </light>
+    </state>
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>-0.05099 0.02188 18.6627 -3.00594 1.57079 0.135653</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+  </world>
+</sdf>

path/materials/scripts/path.material

@@ -0,0 +1,16 @@
+    material path/Image
+    {
+      technique
+      {
+        pass
+        {
+          ambient 0.5 0.5 0.5 1.0
+          diffuse 1.0 1.0 1.0 1.0
+          specular 0.0 0.0 0.0 1.0 0.5
+          texture_unit
+          {
+            texture path.png
+          }
+        }
+      }
+    }

path/model.config

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+  <name>path</name>
+  <version>1.0</version>
+  <sdf version='1.4'>model.sdf</sdf>
+
+  <author>
+   <name>Yike Stone</name>
+   <email>me@my.email</email>
+  </author>
+
+  <description>
+    Line Follower Path
+  </description>
+</model>

path/model.sdf

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<sdf version="1.4">
+<model name="path">
+  <static>true</static>
+    <link name="link">
+      <collision name="collision">
+        <geometry>
+          <plane>
+            <normal>0 0 1</normal>
+            <size>4 4</size>
+          </plane>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>100</mu>
+              <mu2>50</mu2>
+            </ode>
+          </friction>
+        </surface>
+      </collision>
+      <visual name="visual">
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <plane>
+            <normal>0 0 1</normal>
+            <size>4 4</size>
+          </plane>
+        </geometry>
+        <material>
+          <script>
+            <uri>model://path/materials/scripts/</uri>
+            <uri>model://path/materials/textures</uri>
+            <name>path/Image</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+  </model>
+</sdf>