Drive installation and simple application of Ros under Kinect2

Catalogue

Directory Kinect2 ROS Drive node configuration libfreenect2 iai-kinect2 Simple use save picture Save picture Sequence Click Point Cloud to get coordinates

Kinect2

I believe that the students interested in this topic, KINECT2 should also be very familiar with it. This equipment is not expensive now, some east of about 2000 will be able to buy, and can also be equipped with a tripod. The effect of Kinect2 is indeed much better than 1 generations, whether it is bone point or image quality, and so on. More detailed introduction, interested students can self-Google, MS official on-line view. ROS

Do robot-related work of the classmate, at the same time on the part is not unfamiliar with it. The robot Operating system (ROS) is widely used and has many open source libraries that can be used by packages. At the same time, mainstream sensors are also supported in Ros. Of course, Kinect2 is also able to support it. Ros is installed on Ubuntu and I use the version Ubuntu14.04 + Ros Indigo. For the installation of Ros, you can check the relevant instructions on the website. Very simple step. Enter the following command in turn:

sudo sh-c ' echo ' Deb Http://packages.ros.org/ros/ubuntu $ (LSB_RELEASE-SC) main ">/etc/apt/sources.list.d/ Ros-latest.list '

sudo apt-key adv--keyserver hkp://ha.pool.sks-keyservers.net--recv-key 0xB01FA116

sudo Apt-get Update

sudo apt-get install ros-indigo-desktop-full

sudo rosdep init

rosdep update

echo " Source/opt/ros/indigo/setup.bash ">> ~/.bashrc

source ~/.BASHRC

sudo apt-get install Python-rosinstall

Once the above instructions have been executed, the ROS will be ready for installation. Of course, there is a need to create a workspace. Execute the following code:

Mkdir-p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace
cd
. Catkin_make

Drive Node Configuration

The use of KINECT2 in the ROS environment relies mainly on the IAI-KINECT2 package. GitHub Address: Https://github.com/code-iai/iai_kinect2.

First, you need to install its libfreenect2, the following steps (default to ubuntu14.04 or newer version is the system version, other systems, see the original website description): libfreenect2 download Libfreenect2 source code

git clone https://github.com/OpenKinect/libfreenect2.git
CD Libfreenect2

Download upgrade Deb File

CD depends;./download_debs_trusty.sh

Installing the Build Tool

sudo apt-get install build-essential cmake pkg-config

Install LIBUSB. Version Requirements >= 1.0.20.

sudo dpkg-i debs/libusb*deb

Installing Turbojpeg

sudo apt-get install libturbojpeg Libjpeg-turbo8-dev

Installing OpenGL

sudo dpkg-i debs/libglfw3*deb
sudo apt-get install-f
sudo apt-get install Libgl1-mesa-dri-lts-vivid

(If the last command conflicts and other packages, don ' t do it.) As suggested in the original text, when I installed, the third instruction did appear wrong, directly ignore the third instruction.

To install OpenCL (optional)

Intel GPU:

sudo apt-add-repository ppa:floe/beignet
sudo apt-get update
sudo apt-get install beignet-dev
sudo dpkg-i Debs/ocl-icd*deb

AMD Gpu:apt-get Install opencl-headers verification Installation: Clinfo

Installation CUDA (optional, nvidia only): (Ubuntu 14.04 only) Download cuda-repo-ubuntu1404...*.deb ("Deb") from Nvidia website , follow their installation instructions, including Apt-get install Cuda which installs Nvidia graphics driver. (Jetson TK1) It is preloaded. (Nvidia/intel dual GPUs) After Apt-get install Cuda, use sudo prime-select intel to use Intel GPUs for desktop. (Other) Follow Nvidia website ' s instructions. Install Vaapi (optional, Intel only)
sudo dpkg-i debs/{libva,i965}*deb; sudo apt-get install OpenNI2 (optional)

sudo apt-add-repository ppa:deb-rob/ros-trusty && sudo apt-get update
sudo apt-get install Libopenni2-dev

Build

CD..
mkdir build && CD build
cmake:-dcmake_install_prefix= $HOME/freenect2-denable_cxx11=on make
Make install

For the description of the CMake command above, the first parameter is specifically specified for the location of the installation, you can also specify other places where you feel happy, but the general standard path is the above example path or/usr/local. The second parameter is to increase support for c++11. Set Udev Rules:sudo CP. /platform/linux/udev/90-kinect2.rules/etc/udev/rules.d/and Reseat the Kinect2. All done, now try to run the demo program:./bin/protonect, no surprises, you should be able to see the following effects:


Iai-kinect2

Download the project source from GitHub on the command line into the SRC folder within the workspace:

CD ~/catkin_ws/src/
git clone https://github.com/code-iai/iai_kinect2.git
CD iai_kinect2
ROSDEP Install -R--from-paths.
CD ~/catkin_ws
catkin_make-dcmake_build_type= "Release"

For the last line of instructions in the above command, it should be noted that if the previous LIBFREENECT2 you installed location is not a standard two path, you need to provide parameters to specify the path of Libfreenect2:

Catkin_make-dfreenect2_dir=path_to_freenect2/lib/cmake/freenect2-dcmake_build_type= "Release"

Compile the end, everything OK, you will see the following prompt:



Finally, it's an exciting time to get Kinect2 data in Ros.
PS: Many students run subordinate commands, often encounter problems that can not be executed, most of the time there is no source corresponding directory. The source ~/catkin_ws/devel/setup.bash should be executed first, if corresponding to the ~/.BASHRC file has been written to the classmate, can be ignored.

Roslaunch Kinect2_bridge Kinect2_bridge.launch

Using Roslaunch to launch KINECT2 related nodes, you can see the following results, on the other command line, enter Rostopic list, you can view the topic published by the node, you can also enter Rosrun kinect2_viewer kinect2 _viewer SD Cloud, to open a viewer to view data. The result is shown in the following figure:

Simple Application

For a long time did not pay attention to this blog, the above content, is a work before the need to use Kinect2, so try to get a bit, to organize it down to form this blog.

Today, suddenly a classmate in the station to give me a private message, asked me this blog content. It's really nice to share something that helps others. On the question asked by the classmate, in fact, in the previous work, I have also achieved. Try to sort out the following. Save Picture

In fact, the purpose is one, the KINECT2 RGB map saved. In the previous narrative, enter Rosrun kinect2_viewer kinect2_viewer SD Cloud to see the display. The essence of this command is to run the Kinect2_viewer node in the Kinect2_viewer package, given two parameters SD and cloud. Enter the path to this package and it is possible to see the source of this node. The main function excerpt from the source code is as follows:

int main (int argc, char **argv) {
  ...//omitted some code
  Topiccolor = "/" + ns + Topiccolor;
  Topicdepth = "/" + ns + topicdepth;
  Out_info ("topic Color:" Fg_cyan << topiccolor << no_color);
  Out_info ("topic depth:" Fg_cyan << topicdepth << no_color);
  Receiver is a class that is also defined in the file. Useexact (True), usecompressed (false)
  receiver receiver (Topiccolor, topicdepth, Useexact, usecompressed);

  Out_info ("Starting receiver ...");
  When the runtime gives the parameter "cloud", the mode = Receiver::cloud
  //runtime gives the parameter "image", then mode = Receiver::image
  //runtime gives the parameter "both", then mode = R Eceiver::both
  receiver.run (mode);

  Ros::shutdown ();
  return 0;
}

Receiver class implementation is not too complicated. The main loop that is used for the display is in Imageviewer () or Cloudviewer (). Depending on the parameters given, different functions will be called. The corresponding relationship is as follows:

Switch (mode) {case
CLOUD:
  cloudviewer ();
  break;
Case IMAGE:
  imageviewer ();
  break;
Case BOTH:
  imageviewerthread = Std::thread (&receiver::imageviewer, this);
  Cloudviewer ();
  break;
}

Both option, two windows will appear to display the image. The above two functions have already implemented the function of picture saving. The code excerpt is as follows, two functions are similar, so it just extracts the contents of Imageviewer ():

int key = Cv::waitkey (1);
Switch (key & 0xFF) {case
:
' Q ':
  running = false;
  break;
Case ": Case
' s ':
  if (mode = = IMAGE) {
    createcloud (depth, color, cloud);
    Savecloudandimages (cloud, color, depth, depthdisp);
  } else {
    save = true;
  }
  break;
}

The key function Savecloudandimages () is implemented as follows:

void Savecloudandimages (const PCL::P OINTCLOUD<PCL::P ointxyzrgba>::constptr Cloud, const Cv::mat &color,
  Const Cv::mat &depth, const Cv::mat &depthcolored) {oss.str ("");
  OSS << "./" << Std::setfill (' 0 ') << std::setw (4) << frame;
  All files are saved in the current path under const std::string baseName = Oss.str ();
  Const std::string Cloudname = baseName + "_CLOUD.PCD";
  Const std::string colorname = baseName + "_color.jpg";
  Const std::string Depthname = baseName + "_depth.png";

  Const std::string Depthcoloredname = baseName + "_depth_colored.png";
  Out_info ("Saving Cloud:" << cloudname);
  Writer is the PCL of this class: the member variable of the:P cdwriter type writer.writebinary (Cloudname, *cloud);
  Out_info ("Saving color:" << colorname);
  Cv::imwrite (colorname, color, params);
  Out_info ("Saving depth:" << depthname);
  Cv::imwrite (depthname, depth, params);
  Out_info ("Saving depth:" << depthcoloredname); Cv::imwrite (Depthcoloredname, depthcolored, pArams);
  Out_info ("Saving complete!");
++frame; }

As can be seen from the above code, if you want to save the picture, just check the window showing the picture, and then enter the click the keyboard s key or the space bar is OK, the saved picture is in the current directory.

If there are some special needs, the above source code on the basis of implementation, will be more effective. Here is a small example. save a picture sequence

If you want to save a sequence of pictures, just control begins to end, for example, key B (Begin) is saved, and key E (end) is saved.

Complete such a function, the source code based on the appropriate changes to meet the requirements. First, you need to judge the keys B and E each time, you need to add to the above excerpt of the switch (Key & 0xFF) block. If you need a continuous save, simply set a flag bit.

First, copy the Vewer.cpp file, named Save_seq.cpp. Modify the Save_seq.cpp file to add a new member variable under the BOOL save variable in the receiver class, bool Save_seq. Initializes a save_seq (false) for the variable in the initialization list of the class's constructor. Locate the void Imageviewer () function and modify the corresponding switch (key & 0xFF) block as follows:

int key = Cv::waitkey (1);
Switch (key & 0xFF) {case
:
' Q ':
  running = false;
  break;
Case ' B ': Save_seq = true; Save = true; break;
Case ' e ': Save_seq = false; Save = false; break;
Case ": Case
' s ':"
  if (save_seq) break;
  if (mode = = IMAGE) {
    createcloud (depth, color, cloud);
    Savecloudandimages (cloud, color, depth, depthdisp);
  } else {
    save = true;
  }
  break;
}
if (save_seq) {
  createcloud (depth, color, cloud);
  Savecloudandimages (cloud, color, depth, depthdisp);
}

Navigate to the void Cloudviewer () function and modify the corresponding if (save) block as follows:

if (save | | save_seq) {
  save = false;
  Cv::mat Depthdisp;
  Dispdepth (Depth, depthdisp, 12000.0f);
  Savecloudandimages (cloud, color, depth, depthdisp);
}

Locate the void KeyboardEvent (const pcl::visualization::keyboardevent &event, void *) function and modify the source code as follows:

if (Event.keyup ()) {
  switch (Event.getkeycode ()) {case
  :
  ' Q ':
    running = false;
    break;
  Case ": Case
  ' s ':
    save = true;
    break;
  Case ' B ':
    save_seq = true;
    break;
  Case ' E ':
    save_seq = false;
    break;
  }
}

At the end of CMakeList.txt, add the following command:

Add_executable (save_seq src/save_seq.cpp)
target_link_libraries (save_seq
  ${catkin_libraries}
  ${ Opencv_libraries}
  ${pcl_libraries}
  ${kinect2_bridge_libraries}
)

Return to Catkin home directory, run catkin_make, compile, link no problem, you can see the effect. Of course, the first is the need to start kinect_bride. Run the following command in turn:

Roslaunch Kinect2_bridge kinect2_bridge.launch
rosrun kinect2_viewer save_seq HD Cloud

Select the popup window, press B to start, press E to end the save. Terminal output results are as follows:

Click Point Cloud to get coordinates

The main idea is that when the mouse moves in a point cloud, it displays the three-dimensional coordinates of the point currently pointed to by the mouse, and when clicked, gets the coordinates sent out.

In this case, the first need to have a callback function on the mouse, when the mouse state changes, make corresponding changes. Mouse changes can be reduced to three cases: mouse move the left mouse button click the right mouse button click

Because it involves a callback function, and it's just a small feature, the code implementation is simple. Using three global variables directly represents these three states (the code needs to support C++11, you can change the type to a volatile int if you don't want to be so troublesome), and a global normal function:

Std::atomic_int MouseX;
Std::atomic_int Mousey;
Std::atomic_int Mousebtntype;

void Onmouse (int event, int x, int y, int flags, void* USTC) {
    //Std::cout << "Onmouse:" << x << " "<< y << Std::endl;
    MouseX  = x;
    Mousey  = y;
    Mousebtntype = event;
}

Add two Ros in the initialization::P ublisher, corresponding to the left and right mouse button pressure should be published data to reach the topic. As shown below:

Ros::P ublisher leftbtnpointpub =
    nh.advertise<geometry_msgs::P ointstamped> ("/kinect2/click_point/left ", 1);
Ros::P ublisher rightbtnpointpub =
    nh.advertise<geometry_msgs::P ointstamped> ("/kinect2/click_point/ Right ", 1);

wherein the message format is geometry_msgs/pointstamped, the Ros comes with the message, in the Source Wharf department needs to contain the header file, #include <geometry_msgs/pointstamped.h>

Std_msgs/header Header
  uint32 seq Time
  stamp
  string frame_id
geometry_msgs/point point
  float64 x
  float64 y
  float64 Z

Then rewrite the Cloudviewer () function as follows:

void Cloudviewer () {Cv::mat color, depth;
  Std::chrono::time_point<std::chrono::high_resolution_clock> start, now;
  Double fps = 0;
  size_t framecount = 0;
  Std::ostringstream OSS; Std::ostringstream ossxyz;
  Added a string-stream const CV::P oint POS (5, 15);
  Const Cv::scalar Colortext = CV_RGB (255, 0, 0);
  Const double Sizetext = 0.5;
  const int linetext = 1;
  const int font = CV::FONT_HERSHEY_SIMPLEX;
  Gets the current mouse coordinate int img_x = MouseX from a global variable;
  int img_y = Mousey;
  GEOMETRY_MSGS::P ointstamped ptmsg;

  ptMsg.header.frame_id = "Kinect_link";
  Lock.lock ();
  color = this->color;
  depth = this->depth;
  Updatecloud = false;

  Lock.unlock ();
  Const std::string window_name = "color viewer";
  Cv::namedwindow (Window_name);
  Register the mouse callback function, the third parameter is the keyword in c++11, if not support c++11, replace with null cv::setmousecallback (Window_name, Onmouse, nullptr);

  Createcloud (depth, color, cloud); for (; running && Ros::ok ();)
      {if (Updatecloud) {lock.lock (); color = This-> color;
      depth = this->depth;
      Updatecloud = false;

      Lock.unlock ();
      Createcloud (depth, color, cloud);
      img_x = MouseX;
      img_y = Mousey;
      Const PCL::P ointxyzrgba& pt = cloud->points[img_y * depth.cols + img_x];
      Ptmsg.point.x = Pt.x;
      Ptmsg.point.y = Pt.y;

      Ptmsg.point.z = pt.z; According to the left mouse button press or right-click, respectively publish three-dimensional coordinates to different topics up switch (mousebtntype) {case Cv::event_lbuttonup:ptmsg.header.st
          AMP = Ros::time::now ();
          Leftbtnpointpub.publish (PTMSG);
          Ros::spinonce ();
      Break
          Case cv::event_rbuttonup:ptmsg.header.stamp = Ros::time::now ();
          Rightbtnpointpub.publish (PTMSG);
          Ros::spinonce ();
      Break
      Default:break;

      } mousebtntype = Cv::event_mousemove;
      ++framecount;
      now = Std::chrono::high_resolution_clock::now (); Double elapsed = Std::chrono::d uration_cast<std::chrono::milliseconds> (nOw-start). Count ()/1000.0;
        if (elapsed >= 1.0) {fps = framecount/elapsed;
        Oss.str ("");
        OSS << "fps:" << fps << "(" << elapsed/framecount * 1000.0 << "ms)";
        start = now;
      Framecount = 0;
      } CV::p uttext (color, Oss.str (), POS, Font, Sizetext, Colortext, Linetext, CV_AA);
      Ossxyz.str (""); OSSXYZ << "(" << ptmsg.point.x << "," << ptmsg.point.y <&lt ;
      "," << ptmsg.point.z << ")";
      CV::p uttext (color, OSSXYZ.STR (), CV::P oint (img_x, img_y), font, 1, Colortext, 3, CV_AA);
      Cv::circle (color, CV::P oint (MouseX, Mousey), 5, cv::scalar (0, 0, 255), 1);
      Cv::imshow (window_name, color);
      Cv::imshow (window_name, depth);
    Cv::waitkey (1);
  }} CV::d estroyallwindows ();
Cv::waitkey (100); }

Finally, a little rewrite of the main function is OK, the entire main function excerpt is as follows, which removed the redundant parameter parsing, so that the use of more fixed, simple.

int main (int argc, char **argv) {#if extended_output rosconsole_autoinit;
    if (!getenv ("Rosconsole_format")) {ros::console::g_formatter.tokens_.clear ();
  Ros::console::g_formatter.init ("[${severity}] ${message}");

  } #endif ros::init (argc, argv, "Kinect2_viewer", ros::init_options::anonymousname);
  if (!ros::ok ()) {return 0;
  } std::string ns = K2_default_ns;
  std::string topiccolor = K2_topic_hd K2_topic_image_color k2_topic_image_rect;
  std::string topicdepth = K2_topic_hd k2_topic_image_depth k2_topic_image_rect;
  bool Useexact = true;
  BOOL usecompressed = false;

  Receiver::mode Mode = Receiver::cloud;
  Topiccolor = "/" + ns + Topiccolor;
  Topicdepth = "/" + ns + topicdepth;
  Out_info ("topic Color:" Fg_cyan << topiccolor << no_color);

  Out_info ("topic depth:" Fg_cyan << topicdepth << no_color);

  Receiver receiver (Topiccolor, topicdepth, Useexact, usecompressed);
  Out_info ("Starting receiver ..."); Out_info ("pleaseClick Mouse in Color Viewer ... ");

  Receiver.run (mode);
  Ros::shutdown ();
return 0; }

Add the following two paragraphs to the CMakeList.txt and make is OK.

Add_executable (Click_rgb src/click_rgb.cpp)
target_link_libraries (Click_rgb
  ${catkin_libraries}
  ${ Opencv_libraries}
  ${pcl_libraries}
  ${kinect2_bridge_libraries}
)

install (TARGETS Click_rgb