update

CMakeLists.txt

@@ -0,0 +1,79 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ur3_hardware)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS image_transport roscpp std_msgs std_srvs sensor_msgs camera_info_manager gazebo_msgs geometry_msgs message_generation)
+
+#add_message files
+add_message_files(FILES blocks_poses.msg)
+add_message_files(FILES Tracker.msg)
+
+generate_messages(DEPENDENCIES std_msgs)
+
+## pkg-config libraries
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(avcodec libavcodec REQUIRED)
+pkg_check_modules(swscale libswscale REQUIRED)
+
+###################################################
+## Declare things to be passed to other projects ##
+###################################################
+
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS message_runtime
+)
+
+###########
+## Build ##
+###########
+
+include_directories(include
+  ${catkin_INCLUDE_DIRS}
+  ${avcodec_INCLUDE_DIRS}
+  ${swscale_INCLUDE_DIRS}
+)
+
+## Build the USB camera library
+add_library(${PROJECT_NAME} src/usb_cam.cpp)
+target_link_libraries(${PROJECT_NAME}
+  ${avcodec_LIBRARIES}
+  ${swscale_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME}_node nodes/usb_cam_node.cpp)
+target_link_libraries(${PROJECT_NAME}_node
+  ${PROJECT_NAME}
+  ${avcodec_LIBRARIES}
+  ${swscale_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+## Mark executables and/or libraries for installation
+install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+## Copy launch files
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+  FILES_MATCHING PATTERN "*.launch"
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+   FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
+)

include/usb_cam/usb_cam.h

@@ -0,0 +1,155 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2014, Robert Bosch LLC.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Robert Bosch nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *********************************************************************/
+#ifndef USB_CAM_USB_CAM_H
+#define USB_CAM_USB_CAM_H
+
+#include <asm/types.h>          /* for videodev2.h */
+
+extern "C"
+{
+#include <linux/videodev2.h>
+#include <libavcodec/avcodec.h>
+#include <libswscale/swscale.h>
+#include <libavutil/mem.h>
+}
+
+// legacy reasons
+#include <libavcodec/version.h>
+#if LIBAVCODEC_VERSION_MAJOR < 55
+#define AV_CODEC_ID_MJPEG CODEC_ID_MJPEG
+#endif
+
+#include <string>
+#include <sstream>
+
+#include <sensor_msgs/Image.h>
+
+namespace usb_cam {
+
+class UsbCam {
+ public:
+  typedef enum
+  {
+    IO_METHOD_READ, IO_METHOD_MMAP, IO_METHOD_USERPTR, IO_METHOD_UNKNOWN,
+  } io_method;
+
+  typedef enum
+  {
+    PIXEL_FORMAT_YUYV, PIXEL_FORMAT_UYVY, PIXEL_FORMAT_MJPEG, PIXEL_FORMAT_YUVMONO10, PIXEL_FORMAT_RGB24, PIXEL_FORMAT_GREY, PIXEL_FORMAT_UNKNOWN
+  } pixel_format;
+
+  UsbCam();
+  ~UsbCam();
+
+  // start camera
+  void start(const std::string& dev, io_method io, pixel_format pf,
+		    int image_width, int image_height, int framerate);
+  // shutdown camera
+  void shutdown(void);
+
+  // grabs a new image from the camera
+  void grab_image(sensor_msgs::Image* image);
+
+  // enables/disable auto focus
+  void set_auto_focus(int value);
+
+  // Set video device parameters
+  void set_v4l_parameter(const std::string& param, int value);
+  void set_v4l_parameter(const std::string& param, const std::string& value);
+
+  static io_method io_method_from_string(const std::string& str);
+  static pixel_format pixel_format_from_string(const std::string& str);
+
+  void stop_capturing(void);
+  void start_capturing(void);
+  bool is_capturing();
+
+ private:
+  typedef struct
+  {
+    int width;
+    int height;
+    int bytes_per_pixel;
+    int image_size;
+    char *image;
+    int is_new;
+  } camera_image_t;
+
+  struct buffer
+  {
+    void * start;
+    size_t length;
+  };
+
+
+  int init_mjpeg_decoder(int image_width, int image_height);
+  void mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels);
+  void process_image(const void * src, int len, camera_image_t *dest);
+  int read_frame();
+  void uninit_device(void);
+  void init_read(unsigned int buffer_size);
+  void init_mmap(void);
+  void init_userp(unsigned int buffer_size);
+  void init_device(int image_width, int image_height, int framerate);
+  void close_device(void);
+  void open_device(void);
+  void grab_image();
+  bool is_capturing_;
+
+
+  std::string camera_dev_;
+  unsigned int pixelformat_;
+  bool monochrome_;
+  io_method io_;
+  int fd_;
+  buffer * buffers_;
+  unsigned int n_buffers_;
+  AVFrame *avframe_camera_;
+  AVFrame *avframe_rgb_;
+  AVCodec *avcodec_;
+  AVDictionary *avoptions_;
+  AVCodecContext *avcodec_context_;
+  int avframe_camera_size_;
+  int avframe_rgb_size_;
+  struct SwsContext *video_sws_;
+  camera_image_t *image_;
+
+};
+
+}
+
+#endif
+

launch/intialize.launch

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<launch>
+  <include file="$(find ur_modern_driver)/launch/ur3_bringup.launch">
+    <arg name="robot_ip" default="192.168.1.102"/>
+  </include>
+  <!-- Launch moveit -->
+  <include file="$(find ur3_moveit_config)/launch/ur3_moveit_planning_execution.launch">
+    <arg name="limited" default="$(arg limited)"/>
+    <arg name="debug" default="$(arg debug)" />
+  </include>
+
+  <include file="$(find ur3_moveit_config)/launch/moveit_rviz.launch">
+    <arg name="config" default="true"/>
+  </include>
+
+  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
+    <param name="video_device" value="/dev/video0" />
+    <param name="image_width" value="640" />
+    <param name="image_height" value="480" />
+    <param name="pixel_format" value="yuyv" />
+    <param name="camera_frame_id" value="usb_cam" />
+    <param name="io_method" value="mmap"/>
+  </node>
+  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
+    <remap from="image" to="/usb_cam/image_raw"/>
+    <param name="autosize" value="true" />
+  </node>
+</launch>

msg/Tracker.msg

@@ -0,0 +1,12 @@
+# message type to describe the tracking information of the blocks
+# to be published as a topic
+
+float64 x  # x coordinate in the world
+float64 y  # y coordinate in the world
+float64 z  # z coordinate in the world
+float64 error_x
+float64 error_y
+float64 error_z
+bool flag1
+bool flag2
+bool flag3

msg/blocks_poses.msg

@@ -0,0 +1,7 @@
+# message type to describe 3-D position of the cylinder blocks
+# variable length array, length decided by topic /current_cylinder_blocks
+# to be published as a topic
+
+float64[] x  # x coordinate in the world
+float64[] y  # y coordinate in the world
+float64[] z  # z coordinate in the world