Attached the Color Sensor Plugin

ca_description/urdf/color_sensor.xacro

@@ -25,6 +25,58 @@
       </visual>
   </link>
 
+    <gazebo reference="${prefix}_color_link">
+     <sensor name='${prefix}_camera' type='camera'>
+       <always_on>true</always_on>
+       <update_rate>60.0</update_rate>
+       <camera name='${prefix}_cam'>
+         <horizontal_fov>1.047</horizontal_fov>
+         <image>
+           <width>20</width>
+           <height>40</height>
+         </image>
+         <clip>
+           <near>0.1</near>
+           <far>10</far>
+         </clip>
+       </camera>
+       <plugin name="color_sensor_plugin" filename="libcolor_sensor_plugin.so">
+         <cameraName>${prefix}_cam</cameraName>
+          <alwaysOn>true</alwaysOn>
+          <updateRate>60.0</updateRate>
+         <always_on>true</always_on>
+         <update_rate>60.0</update_rate>
+<!--         <imageTopicName>${prefix}_rgb/image_raw</imageTopicName>-->
+         <frameName>${prefix}_camera_optical_frame</frameName>
+         <xacro:if value="${color == 'yellow'}">
+          <rvalue_max>255</rvalue_max>
+          <rvalue_min>240</rvalue_min>
+          <gvalue_max>255</gvalue_max>
+          <gvalue_min>240</gvalue_min>
+          <bvalue_max>140</bvalue_max>
+          <bvalue_min>10</bvalue_min>
+         </xacro:if>
+         <xacro:if value="${color == 'white'}">
+          <rvalue_max>255</rvalue_max>
+          <rvalue_min>240</rvalue_min>
+          <gvalue_max>255</gvalue_max>
+          <gvalue_min>240</gvalue_min>
+          <bvalue_max>255</bvalue_max>
+          <bvalue_min>240</bvalue_min>
+         </xacro:if>
+         <PublisherName>${prefix}_sensor</PublisherName>
+         <ColorPercentage>0.070</ColorPercentage>
+         <baseline>0.1</baseline>
+         <distortion_k1>0.0</distortion_k1>
+         <distortion_k2>0.0</distortion_k2>
+         <distortion_k3>0.0</distortion_k3>
+         <distortion_t1>0.0</distortion_t1>
+         <distortion_t2>0.0</distortion_t2>
+         <pointCloudCutoff>0.4</pointCloudCutoff>
+         <robotNamespace>/</robotNamespace>
+       </plugin>
+     </sensor>
+    </gazebo>
 </xacro:macro>  
 
-</robot>
+</robot>

ca_gazebo/CMakeLists.txt

@@ -24,17 +24,24 @@ else()
 			ca_msgs
 	)
 
-	link_directories(${GAZEBO_LIBRARY_DIRS})
-  include_directories(include
+link_directories(${GAZEBO_LIBRARY_DIRS})
+
+include_directories(include
     ${catkin_INCLUDE_DIRS}
-		${GAZEBO_INCLUDE_DIRS}
+	${GAZEBO_INCLUDE_DIRS}
+	${Boost_INCLUDE_DIR}
   )
 
 	add_library(create_bumper_plugin src/create_bumper_plugin.cpp)
 	add_dependencies(create_bumper_plugin ${catkin_EXPORTED_TARGETS})
 	target_link_libraries(create_bumper_plugin
 				${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
 
+	add_library(color_sensor_plugin src/color_sensor_plugin.cpp)
+	add_dependencies(color_sensor_plugin ${catkin_EXPORTED_TARGETS})
+	target_link_libraries(color_sensor_plugin
+				${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+
   install(DIRECTORY launch worlds models
     DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 

ca_gazebo/include/ca_gazebo/color_sensor_plugin.h

@@ -0,0 +1,53 @@
+    #ifndef GAZEBO_ROS_COLOR_SENSOR_HH
+    #define GAZEBO_ROS_COLOR_SENSOR_HH
+
+    #include <string>
+    #include <vector>
+    // library for processing camera data for gazebo / ros conversions
+    #include <gazebo/plugins/CameraPlugin.hh>
+
+
+    #include <gazebo_plugins/gazebo_ros_camera_utils.h>
+
+
+    namespace gazebo
+    {
+      class GazeboRosColor : public CameraPlugin, GazeboRosCameraUtils
+      {
+        public:
+        ///Constructor
+        GazeboRosColor();    
+        ///Destructor
+        ~GazeboRosColor();
+        ///Load the plugin
+        void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); 
+
+        // Put the RGB limits from the SDF to the rgbmax_ and rgbmin_ vectors
+        void LoadRGBLimits(sdf::ElementPtr _sdf);
+
+        // Check if the data received from the sensor is in range with the parameters
+        bool InRange(std::vector<int> RGB);
+
+        /// \brief Update the controller
+        protected: virtual void OnNewFrame(const unsigned char *_image,
+        unsigned int _width, unsigned int _height,
+        unsigned int _depth, const std::string &_format);
+
+        private:
+        // vector rgbmax_: used to store the maximun values of the RGB range
+        std::vector<int> rgbmax_ {0, 0, 0};
+        // vector rgbmin_: used to store the minimun values of the RGB range
+        std::vector<int> rgbmin_ {0, 0, 0};  
+        //param publisher_name : Name of the publisher topic, received from SDF file
+        std::string publisher_name_;
+        //param publisher_name : Name of the publisher topic, received from SDF file
+        float color_percentage_;
+
+        /// Initialize ROS variables
+        ros::NodeHandle nh_;
+        ros::Publisher sensor_publisher_;
+
+      }; // GazeboRosColor
+    } // gazebo
+    #endif // GAZEBO_ROS_COLOR_SENSOR_HH
+

ca_gazebo/src/color_sensor_plugin.cpp

@@ -0,0 +1,126 @@
+#include "ca_gazebo/color_sensor_plugin.h"
+
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/sensors/Sensor.hh>
+#include <gazebo/sensors/CameraSensor.hh>
+#include <gazebo/sensors/SensorTypes.hh>
+#include <ros/ros.h>
+
+#include <string>
+#include <vector>
+
+#include "std_msgs/Bool.h"
+#include "std_msgs/Float64.h"
+#include "gazebo_plugins/gazebo_ros_camera.h"
+
+namespace gazebo
+{
+  // Register this plugin with the simulator
+  GZ_REGISTER_SENSOR_PLUGIN(GazeboRosColor)
+
+  ////////////////////////////////////////////////////////////////////////////////
+  // Constructor
+  GazeboRosColor::GazeboRosColor():
+    nh_("color_sensor_plugin")
+{
+
+}
+  ////////////////////////////////////////////////////////////////////////////////
+  // Destructor
+  GazeboRosColor::~GazeboRosColor()
+  {
+    ROS_DEBUG_STREAM_NAMED("camera","Unloaded");
+  }
+
+  void GazeboRosColor::Load(sensors::SensorPtr _parent, 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;
+    }
+
+    CameraPlugin::Load(_parent, _sdf);
+    GazeboRosCameraUtils::Load(_parent, _sdf);
+    // copying from CameraPlugin into GazeboRosCameraUtils
+    this->parentSensor_ = this->parentSensor;
+    this->width_ = this->width;
+    this->height_ = this->height;
+    this->depth_ = this->depth;
+    this->format_ = this->format;
+    this->camera_ = this->camera;
+    this->LoadRGBLimits(_sdf);
+    this->publisher_name_ = _sdf->Get<std::string>("PublisherName");
+    this->sensor_publisher_ = nh_.advertise<std_msgs::Bool>(this->publisher_name_, 1);
+    this->color_percentage_=_sdf->Get<double>("ColorPercentage");
+    this->parentSensor->SetActive(true);
+
+  }
+
+    void GazeboRosColor::LoadRGBLimits(sdf::ElementPtr _sdf)
+    {
+    this->rgbmax_[0] = _sdf->Get<double>("rvalue_max");
+    this->rgbmax_[1] = _sdf->Get<double>("gvalue_max");
+    this->rgbmax_[2] = _sdf->Get<double>("bvalue_max");
+
+    this->rgbmin_[0] =  _sdf->Get<double>("rvalue_min");   
+    this->rgbmin_[1] = _sdf->Get<double>("gvalue_min");
+    this->rgbmin_[2] = _sdf->Get<double>("bvalue_min");
+    }
+
+    bool GazeboRosColor::InRange(std::vector<int> RGB)
+    {
+      bool R_ok_,G_ok_, B_ok_;
+      R_ok_ = RGB[0] <= this->rgbmax_[0] and RGB[0] >= this->rgbmin_[0];
+      G_ok_ = RGB[1] <= this-> rgbmax_[1] and RGB[1] >= this->rgbmin_[1];
+      B_ok_ = RGB[2] <= this-> rgbmax_[2] and RGB[2] >= this->rgbmin_[2];
+
+      return R_ok_ and G_ok_ and B_ok_;
+    }
+
+
+  ////////////////////////////////////////////////////////////////////////////////
+  // Update the controller
+  void GazeboRosColor::OnNewFrame(const unsigned char *_image,
+    unsigned int _width, unsigned int _height, unsigned int _depth,
+    const std::string &_format)
+  {
+
+    double goal_color_=0, pixel_quant_=0;
+
+    this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
+
+    std_msgs::BoolPtr publ(new std_msgs::Bool);
+    const common::Time cur_time = this->world_->SimTime();
+    if (cur_time - this->last_update_time_ >= this->update_period_)
+    {
+      this->PutCameraData(_image);
+      this->PublishCameraInfo();
+      this->last_update_time_ = cur_time;
+      std::vector <int> RGB {0 , 0, 0};
+      bool color_detected_;
+
+      for (int i=0; i < _width * _height * 3 ; i = i + 3)
+      {
+       RGB[0] = _image[i];
+       RGB[1] = _image[i + 1];
+       RGB[2] = _image[i + 2];
+
+        pixel_quant_=_width*_height;
+
+        if (this->InRange(RGB))
+        {
+          goal_color_++;
+        }
+      }
+
+      color_detected_ = goal_color_/pixel_quant_ > this->color_percentage_;
+      publ->data = color_detected_;
+      this->sensor_publisher_.publish(publ);  
+
+      goal_color_ = 0;
+      }
+    }
+  }

ca_tools/CMakeLists.txt

@@ -1,13 +1,28 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(ca_tools)
 
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED
+  actionlib_msgs
+  message_generation
+  )
+
+add_action_files(
+  FILES
+  FollowLine.action
+)
+
+generate_messages(
+  DEPENDENCIES
+  actionlib_msgs
+)
 
 catkin_package(
   CATKIN_DEPENDS
     joy_teleop
     geometry_msgs
     rospy
+    actionlib_msgs
+    message_runtime
 )
 
 install(DIRECTORY config launch

ca_tools/action/FollowLine.action

@@ -0,0 +1,10 @@
+# Goal
+int32 amount
+
+---
+# Result
+string[] final_result
+
+---
+# Feedback
+int32 number_of_minutes

ca_tools/package.xml

@@ -12,10 +12,14 @@
 
   <author email="jperron@sfu.ca">Jacob Perron</author>
 
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>geometry_msgs</depend>
   <depend>rospy</depend>
+  <depend>actionlib_msgs</depend>
+
 
   <exec_depend>joy</exec_depend>
   <exec_depend>joy_teleop</exec_depend>