├── .gitattributes
├── README.md
└── camshift
    ├── CMakeLists.txt
    ├── include
        └── camshift
        │   └── init.h
    ├── package.xml
    ├── src
        ├── listener.cpp
        └── publish.cpp
    └── start.launch


/.gitattributes:
--------------------------------------------------------------------------------
1 | # Auto detect text files and perform LF normalization
2 | * text=auto
3 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # objectTracking
 2 | 
 3 | ## **publish.cpp**
 4 | - 从摄像头获取 ROS Image，然后将其转换为 OpenCV Image;
 5 | 
 6 | - 鼠标选定目标，然后使用 Camshift 算法跟踪目标;
 7 | 
 8 | - 将目标的位置偏差值发送到 `Topic` --- “ chatter ” 上;
 9 | 
10 | ## **listener.cpp**
11 | - 订阅 `Topic` --- “ chatter ” ;
12 | 
13 | - 将订阅信息通过串口传输到机器人控制器(如DSP)，从而控制机器人运行;
14 | 
15 | 


--------------------------------------------------------------------------------
/camshift/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(camshift)
 3 | 
 4 | find_package(catkin REQUIRED COMPONENTS
 5 |   cv_bridge
 6 |   genmsg
 7 |   image_transport
 8 |   roscpp
 9 |   rospy
10 |   sensor_msgs
11 |   std_msgs
12 | )
13 | 
14 | generate_messages(DEPENDENCIES std_msgs)
15 | catkin_package()
16 | 
17 | include_directories(${catkin_INCLUDE_DIRS})
18 | include_directories(include ${catkin_INCLUDE_DIRS})
19 | 
20 | 
21 | add_executable(publish src/publish.cpp)
22 | target_link_libraries(publish ${catkin_LIBRARIES})
23 | add_dependencies(publish ${catkin_EXPORTED_TARGETS} )
24 | 
25 | add_executable(listener src/listener.cpp)
26 | target_link_libraries(listener ${catkin_LIBRARIES})
27 | ##add_dependencies(listener ${catkin_EXPORTED_TARGETS})
28 | add_dependencies(listener topic_generate_messages_cpp)
29 | 


--------------------------------------------------------------------------------
/camshift/include/camshift/init.h:
--------------------------------------------------------------------------------
 1 | #ifndef CAMSHIFT_H
 2 | #define CAMSHIFT_H
 3 | 
 4 | #include "opencv2/video/tracking.hpp"
 5 | #include "opencv2/imgproc/imgproc.hpp"
 6 | #include "opencv2/highgui/highgui.hpp"
 7 | #include <iostream>
 8 | 
 9 | using namespace cv;
10 | using namespace std;
11 | 
12 | Mat image;
13 | bool backprojMode = false; 
14 | bool selectObject = false;
15 | int trackObject = 0; 
16 | bool showHist = true;
17 | Point origin;
18 | Rect selection;
19 | 
20 | int vmin = 10, vmax = 256, smin = 30;
21 | int diff;
22 | int data=4;
23 | 
24 | Rect trackWindow;
25 | RotatedRect trackBox;
26 | 
27 | int hsize = 60;
28 | float hranges[] = {0,180};
29 | const float* phranges = hranges;
30 | Mat frame, hsv, hue, mask, hist, histimg = Mat::zeros(200, 320, CV_8UC3), backproj,bkpj;
31 | 
32 | #endif
33 | 


--------------------------------------------------------------------------------
/camshift/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>camshift</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The camshift package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="aicrobo@todo.todo">aicrobo</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/camshift</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, mutiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintianers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *_depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use build_depend for packages you need at compile time: -->
35 |   <!--   <build_depend>message_generation</build_depend> -->
36 |   <!-- Use buildtool_depend for build tool packages: -->
37 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
38 |   <!-- Use run_depend for packages you need at runtime: -->
39 |   <!--   <run_depend>message_runtime</run_depend> -->
40 |   <!-- Use test_depend for packages you need only for testing: -->
41 |   <!--   <test_depend>gtest</test_depend> -->
42 |   <buildtool_depend>catkin</buildtool_depend>
43 |   <buildtool_depend>genmsg</buildtool_depend>
44 |   <build_depend>cv_bridge</build_depend>
45 |   <build_depend>image_transport</build_depend>
46 |   <build_depend>roscpp</build_depend>
47 |   <build_depend>rospy</build_depend>
48 |   <build_depend>sensor_msgs</build_depend>
49 |   <build_depend>std_msgs</build_depend>
50 |   <run_depend>cv_bridge</run_depend>
51 |   <run_depend>image_transport</run_depend>
52 |   <run_depend>roscpp</run_depend>
53 |   <run_depend>rospy</run_depend>
54 |   <run_depend>sensor_msgs</run_depend>
55 |   <run_depend>std_msgs</run_depend>
56 | 
57 | 
58 |   <!-- The export tag contains other, unspecified, tags -->
59 |   <export>
60 |     <!-- Other tools can request additional information be placed here -->
61 | 
62 |   </export>
63 | </package>


--------------------------------------------------------------------------------
/camshift/src/listener.cpp:
--------------------------------------------------------------------------------
  1 | #include <stdio.h>
  2 | #include <string.h>
  3 | #include <sys/types.h>
  4 | #include <errno.h>
  5 | #include <sys/stat.h>
  6 | #include <fcntl.h>
  7 | #include <unistd.h>
  8 | #include <termios.h>
  9 | #include <stdlib.h>
 10 | #include "ros/ros.h"
 11 | #include "std_msgs/String.h"
 12 | #include <sstream>
 13 | 
 14 | char *data = new char[1000];
 15 | int fd;
 16 | 
 17 | /*********************serial*******************************/
 18 | int set_opt(int fd,int nSpeed, int nBits, char nEvent, int nStop)
 19 | {
 20 |     struct termios newtio,oldtio;
 21 |     if  ( tcgetattr( fd,&oldtio)  !=  0) 
 22 |     { 
 23 |         perror("SetupSerial 1");
 24 |         return -1;
 25 |     }
 26 | 
 27 |     bzero( &newtio, sizeof( newtio ) );
 28 |     newtio.c_cflag  |=  CLOCAL | CREAD; 
 29 |     newtio.c_cflag &= ~CSIZE; 
 30 | 
 31 |     switch( nBits )
 32 |     {
 33 |     case 7:
 34 |         newtio.c_cflag |= CS7;
 35 |         break;
 36 |     case 8:
 37 |         newtio.c_cflag |= CS8;
 38 |         break;
 39 |     }
 40 | 
 41 |     switch( nEvent )
 42 |     {
 43 |     case 'O':                     //ÆæУÑé
 44 |         newtio.c_cflag |= PARENB;
 45 |         newtio.c_cflag |= PARODD;
 46 |         newtio.c_iflag |= (INPCK | ISTRIP);
 47 |         break;
 48 |     case 'E':                     //ŌУÑé
 49 |         newtio.c_iflag |= (INPCK | ISTRIP);
 50 |         newtio.c_cflag |= PARENB;
 51 |         newtio.c_cflag &= ~PARODD;
 52 |         break;
 53 |     case 'N':                    //ÎÞУÑé
 54 |         newtio.c_cflag &= ~PARENB;
 55 |         break;
 56 |     }
 57 | 
 58 | switch( nSpeed )
 59 |     {
 60 |     case 2400:
 61 |         cfsetispeed(&newtio, B2400);
 62 |         cfsetospeed(&newtio, B2400);
 63 |         break;
 64 |     case 4800:
 65 |         cfsetispeed(&newtio, B4800);
 66 |         cfsetospeed(&newtio, B4800);
 67 |         break;
 68 |     case 9600:
 69 |         cfsetispeed(&newtio, B9600);
 70 |         cfsetospeed(&newtio, B9600);
 71 |         break;
 72 |     case 115200:
 73 |         cfsetispeed(&newtio, B115200);
 74 |         cfsetospeed(&newtio, B115200);
 75 |         break;
 76 |     default:
 77 |         cfsetispeed(&newtio, B9600);
 78 |         cfsetospeed(&newtio, B9600);
 79 |         break;
 80 |     }
 81 | 
 82 |     if( nStop == 1 )
 83 |     {
 84 |         newtio.c_cflag &=  ~CSTOPB;
 85 |     }
 86 |     else if ( nStop == 2 )
 87 |     {
 88 |         newtio.c_cflag |=  CSTOPB;
 89 |     }
 90 |     newtio.c_cc[VTIME]  = 0;
 91 |     newtio.c_cc[VMIN] = 1;
 92 |     tcflush(fd,TCIFLUSH);
 93 |     if((tcsetattr(fd,TCSANOW,&newtio))!=0)
 94 |     {
 95 |         perror("com set error");
 96 |         return -1;
 97 |     }
 98 |     printf("set done!\n");
 99 |     return 0;
100 | }
101 | 
102 | /******************************open port***************************/
103 | 
104 | int open_port(int fd, int comport)
105 | {
106 | 	const char *dev[] = { "/dev/ttyUSB0","/dev/ttyS1","/dev/ttyS2" };
107 | 	long  vdisable;
108 | 
109 | 	if (comport == 1)
110 | 	{
111 | 		fd = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NDELAY);
112 | 		if (-1 == fd)
113 | 		{
114 | 			perror("Can't Open Serial Port");
115 | 			return(-1);
116 | 		}
117 | 		else
118 | 		{
119 | 			printf("open ttyUSB0 .....\n");
120 | 		}
121 | 	}
122 | 	else if (comport == 2)
123 | 	{
124 | 		fd = open("/dev/ttyS1", O_RDWR | O_NOCTTY | O_NDELAY);
125 | 		if (-1 == fd)
126 | 		{
127 | 			perror("Can't Open Serial Port");
128 | 			return(-1);
129 | 		}
130 | 		else
131 | 		{
132 | 			printf("open ttyS1 .....\n");
133 | 		}
134 | 	}
135 | 	else if (comport == 3)
136 | 	{
137 | 		fd = open("/dev/ttyS2", O_RDWR | O_NOCTTY | O_NDELAY);
138 | 		if (-1 == fd)
139 | 		{
140 | 			perror("Can't Open Serial Port");
141 | 			return(-1);
142 | 		}
143 | 		else
144 | 		{
145 | 			printf("open ttyS2 .....\n");
146 | 		}
147 | 	}
148 | 	if (fcntl(fd, F_SETFL, 0) < 0)
149 | 	{
150 | 		printf("fcntl failed!\n");
151 | 	}
152 | 	else
153 | 	{
154 | 		printf("fcntl=%d\n", fcntl(fd, F_SETFL, 0));
155 | 	}
156 | 	if (isatty(STDIN_FILENO) == 0)
157 | 	{
158 | 		printf("standard input is not a terminal device\n");
159 | 	}
160 | 	else
161 | 	{
162 | 		printf("isatty success!\n");
163 | 	}
164 | 	printf("fd-open=%d\n", fd);
165 | 
166 | 	return fd;
167 | }
168 | 
169 | /*-----------------------CallBack method----------------------------*/
170 | void chatterCallback(const std_msgs::String::ConstPtr& msg)
171 | {
172 | 	//char *data = new char[1000];
173 | 	int nwrite;
174 | 	strcpy(data, msg->data.c_str());
175 | 	nwrite = write(fd, data, 1);
176 | 	printf("nwrite=%s\n", data);
177 | 	//ROS_INFO("I heard: %s",msg->data.c_str());
178 | 	//send(fd,data,sizeof(data));
179 | 	// const char* dataformain = msg->data.c_str();
180 | 	// ROS_INFO("main: %s",dataformain);
181 | 	//dataformain = data;
182 | 	// ROS_INFO("I heard: %s", msg->data.c_str());
183 | }
184 | 
185 | /********************************main*****************************/
186 | int main(int ac, char **av)
187 | {
188 | 	ros::init(ac, av, "listener");
189 | 	ros::NodeHandle n;
190 | 
191 | 	ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
192 | 	//int fd;
193 | 	int nread, nwrite, i;
194 | 	char buff[] = "0";
195 | 	// char buff1[]=data;
196 | 	// const char *s = new char[1000];
197 | 	// s = data;
198 | 
199 | 	if ((fd = open_port(fd, 1)) < 0)
200 | 	{
201 | 		perror("open_port error");
202 | 		return 0;
203 | 	}
204 | 	if ((i = set_opt(fd, 9600, 8, 'N', 1)) < 0)
205 | 	{
206 | 		perror("set_opt error");
207 | 		return 0;
208 | 	}
209 | 	printf("fd=%d\n", fd);
210 | 
211 | 	while (ros::ok()) {
212 | 		//nwrite = write(fd, data, 1);
213 | 		//printf("nwrite=%s\n", data);
214 | 		//if (nwrite) {
215 | 		//	nread = read(fd, buff, 1);
216 | 		//	printf("nread=%s\n", buff);
217 | 		//}
218 | 
219 | 		ros::spin();
220 | 		close(fd);
221 | 
222 | 		return 0;
223 | 	}
224 | }
225 | 


--------------------------------------------------------------------------------
/camshift/src/publish.cpp:
--------------------------------------------------------------------------------
  1 | #include <ros/ros.h>
  2 | #include <image_transport/image_transport.h>
  3 | #include <cv_bridge/cv_bridge.h>
  4 | #include <sensor_msgs/image_encodings.h>
  5 | #include "../include/camshift/init.h"
  6 | #include "std_msgs/String.h"
  7 | #include <sstream>
  8 | #include <iostream>
  9 | #include <string>
 10 | 
 11 | static const std::string OPENCV_WINDOW = "Camshift Demo";
 12 | cv_bridge::CvImagePtr cv_ptr;
 13 | 
 14 | 
 15 | static void onMouse( int event, int x, int y, int, void* )
 16 | {
 17 |   if( selectObject )
 18 |     {
 19 | 	  selection.x = MIN(x, origin.x);
 20 | 	  selection.y = MIN(y, origin.y);
 21 | 	  selection.width = std::abs(x - origin.x);
 22 | 	  selection.height = std::abs(y - origin.y);
 23 | 	  selection &= Rect(0, 0, image.cols, image.rows);
 24 |     } 
 25 |   switch( event )
 26 |     {
 27 | 	  case CV_EVENT_LBUTTONDOWN:
 28 | 	       origin = Point(x,y);
 29 | 	       selection = Rect(x,y,0,0);
 30 | 	       selectObject = true;
 31 | 	  break;
 32 | 	  case CV_EVENT_LBUTTONUP:
 33 | 	       selectObject = false;
 34 | 		   if( selection.width > 0 && selection.height > 0 )
 35 | 		   trackObject = -1;
 36 | 	  break;
 37 |     }
 38 | }
 39 | 
 40 | 
 41 | int camshift()
 42 | { 
 43 |    cvtColor(image, hsv, CV_BGR2HSV);    
 44 |    if (trackObject)
 45 |    {
 46 | 	   int _vmin = vmin, _vmax = vmax;
 47 | 	   inRange(hsv, Scalar(0, smin, MIN(_vmin, _vmax)), Scalar(180, 256, MAX(_vmin, _vmax)), mask);
 48 | 	   int ch[] = { 0, 0 };
 49 | 	   hue.create(hsv.size(), hsv.depth());
 50 | 	   mixChannels(&hsv, 1, &hue, 1, ch, 1);
 51 | 	   if (trackObject < 0)
 52 | 	   {
 53 | 		   Mat roi(hue, selection), maskroi(mask, selection);
 54 | 		   calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges);
 55 | 		   normalize(hist, hist, 0, 255, CV_MINMAX);
 56 | 		   trackWindow = selection;
 57 | 		   trackObject = 1;
 58 | 		   histimg = Scalar::all(0);
 59 | 		   int binW = histimg.cols / hsize;
 60 | 		   Mat buf(1, hsize, CV_8UC3);
 61 | 		   for (int i = 0; i < hsize; i++)
 62 | 			   buf.at<Vec3b>(i) = Vec3b(saturate_cast<uchar>(i*180. / hsize), 255, 255);
 63 | 		   cvtColor(buf, buf, CV_HSV2BGR);
 64 | 	   }
 65 | 	   calcBackProject(&hue, 1, 0, hist, backproj, &phranges);
 66 | 	   backproj &= mask;
 67 | 	   RotatedRect trackBox = CamShift(backproj, trackWindow, TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1));
 68 | 	   if (trackWindow.area() <= 1)
 69 | 	   {
 70 | 		   int cols = backproj.cols, rows = backproj.rows, r = (MIN(cols, rows) + 5) / 6;
 71 | 		   trackWindow = Rect(trackWindow.x - r, trackWindow.y - r, trackWindow.x + r, trackWindow.y + r) &Rect(0, 0, cols, rows);
 72 | 	   }
 73 | 
 74 | 	   if (backprojMode)
 75 | 		   cvtColor(backproj, image, CV_GRAY2BGR);
 76 | 	   ellipse(image, trackBox, Scalar(255, 255, 255), 3, CV_AA);
 77 | 	   int width = image.cols / 2;
 78 | 	   diff = width - trackBox.center.x;
 79 | 	   if ((-300 < diff) && (diff < -240))
 80 | 		   data = 0;
 81 | 	   else if ((-240 < diff) && (diff < -180))
 82 | 		   data = 1;
 83 | 	   else if ((-180 < diff) && (diff < -120))
 84 | 		   data = 2;
 85 | 	   else if ((-120 < diff) && (diff < -60))
 86 | 		   data = 3;
 87 | 	   else if ((-60 < diff) && (diff < 0))
 88 | 		   data = 4;
 89 | 	   else if ((0 < diff) && (diff < 60))
 90 | 		   data = 5;
 91 | 	   else if ((60 < diff) && (diff < 120))
 92 | 		   data = 6;
 93 | 	   else if ((120 < diff) && (diff < 180))
 94 | 		   data = 7;
 95 | 	   else if ((180 < diff) && (diff < 240))
 96 | 		   data = 8;
 97 | 	   //else if((240<diff)&&(diff<300))
 98 | 		   //data=9;
 99 | 
100 |    }
101 |    if(selectObject)
102 |      rectangle(image,selection,Scalar(255,255,255),3,8,0);
103 |   
104 |      imshow( OPENCV_WINDOW,image );   
105 |      waitKey(10);
106 |      
107 |      return 0;
108 | }
109 | 
110 | 
111 | class ImageConverter
112 | {
113 | 	ros::NodeHandle nh_;
114 | 	image_transport::ImageTransport it_;
115 | 	image_transport::Subscriber image_sub_;
116 | 	image_transport::Publisher image_pub_;
117 | public:
118 | 	ImageConverter()
119 | 		:it_(nh_)
120 | 	{
121 | 		// Subscrive to input video feed and publish output video feed
122 | 		image_sub_ = it_.subscribe("/camera/rgb/image_raw", 1, &ImageConverter::imageCb, this);
123 | 
124 | 		image_pub_ = it_.advertise("/image_converter/output_video", 1);
125 | 		namedWindow(OPENCV_WINDOW);
126 | 
127 | 		setMouseCallback(OPENCV_WINDOW, onMouse, 0);
128 | 
129 | 	}
130 | 	~ImageConverter()
131 | 	{
132 | 		cv::destroyWindow(OPENCV_WINDOW);
133 | 	}
134 | 
135 | 	void imageCb(const sensor_msgs::ImageConstPtr& msg)
136 | 	{
137 | 		// cv_bridge::CvImagePtr cv_ptr;
138 | 		try
139 | 		{
140 | 			cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
141 | 			//cv::setMouseCallback(OPENCV_WINDOW, onMouse,0);
142 | 
143 | 		}
144 | 		catch (cv_bridge::Exception& e)
145 | 		{
146 | 			ROS_ERROR("cv_bridge exception: %s", e.what());
147 | 			return;
148 | 		}
149 | 
150 | 		// Update GUI Window
151 | 		// cv::imshow(OPENCV_WINDOW, cv_ptr->image);
152 | 		image = cv_ptr->image;
153 | 		camshift();
154 | 
155 | 		// cv::waitKey(3);     
156 | 		// Output modified video stream
157 | 		image_pub_.publish(cv_ptr->toImageMsg());
158 | 	}
159 | };
160 | 
161 | 
162 | int main(int argc, char** argv)
163 | {
164 | 	ros::init(argc, argv, "image_converter");
165 | 	ros::NodeHandle n;
166 | 	ImageConverter ic;
167 | 
168 | 	ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
169 | 	ros::Rate loop_rate(100);
170 | 
171 | 	while (ros::ok())
172 | 	{
173 | 		std_msgs::String msg;
174 | 		std::stringstream ss;
175 | 		ss << data;
176 | 
177 | 		msg.data = ss.str();
178 | 		chatter_pub.publish(msg);
179 | 		ROS_INFO("%s", msg.data.c_str());
180 | 
181 | 		loop_rate.sleep();
182 | 		ros::spinOnce();
183 | 	}
184 | 
185 | 	return 0;
186 | }
187 | 
188 | 
189 | 
190 | 
191 | 


--------------------------------------------------------------------------------
/camshift/start.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |    <node
 4 | 	name="listener"
 5 | 	pkg="camshift"
 6 | 	type="listener"
 7 | 	required="true"
 8 | 	launch-prefix="xterm -e"
 9 | 	
10 |    />
11 | 
12 |    <node
13 | 	name="publish"
14 | 	pkg="camshift"
15 | 	type="publish"
16 | 	respawn="true"
17 | 	output="screen"
18 | 	
19 |    />
20 | 
21 |     <arg name="video_device" default="/dev/video1" />
22 | 
23 |     <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" clear_params="true" output="screen">
24 |     
25 |     	<remap from="usb_cam/image_raw" to="/camera/rgb/image_raw" />
26 |     	<remap from="usb_cam/camera_info" to="/camera/rgb/camera_info" />
27 | 
28 |         <param name="video_device" value="$(arg video_device)" />
29 |         <param name="image_width" value="640" />
30 |         <param name="image_height" value="480" />
31 |         <param name="framerate" value="30" />
32 |         <param name="pixel_format" value="yuyv" />
33 |         <param name="contrast" value="32" />
34 |         <param name="brightness" value="32" />
35 |         <param name="saturation" value="32" />
36 |         <param name="autofocus" value="true" />
37 |         <param name="camera_frame_id" value="camera_link" />
38 |         
39 |     </node>
40 | 
41 | 
42 | </launch>
43 | 


--------------------------------------------------------------------------------