Skip to content

A
apollo

Pinoxchio / apollo
与 Fork 源项目一致

从无法访问的项目Fork

通知 2
Star 0
Fork 0
A
apollo

体验新版 GitCode，发现更多精彩内容 >>

提交 ec22443b 编写于 作者: D dengchengliang 提交者： fengqikai1414
浏览文件
操作

update usb_cam driver to cybertron version

上级 9bd16323
隐藏空白更改
内联 并排
Showing with 1581 addition and 1658 deletion
modules/drivers/usb_cam/BUILD 0 → 100644
浏览文件 @ ec22443b
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
cc_binary(
name = "libusb_cam_component.so",
deps = [":usb_cam_component_lib"],
linkopts = [
"-shared",
"-lswscale",
"-lavformat",
"-lavcodec",
],
linkstatic = False,
)
cc_library(
name = "usb_cam_component_lib",
srcs = ["usb_cam_component.cc"],
hdrs = ["usb_cam_component.h"],
deps = [
":usb_cam",
"//modules/drivers/proto:sensor_proto",
"//framework:cybertron",
],
copts = ['-DMODULE_NAME=\\"usb_cam\\"']
)
cc_library(
name = "usb_cam",
srcs = [
"usb_cam.cc",
],
hdrs = [
"usb_cam.h",
],
deps = [
"//modules/drivers/usb_cam/proto:usb_cam_proto",
"//framework:cybertron",
"@adv_plat//:adv_plat",
],
)
cpplint()
modules/drivers/usb_cam/conf/usb_cam_front_conf.pb.txt 0 → 100644
浏览文件 @ ec22443b
camera_dev: "/dev/camera/lanemark"
frame_id: "camera_lanemark"
pixel_format: "yuyv"
io_method: IO_METHOD_MMAP
width: 1920
height: 1080
frame_rate: 30
monochrome: false
brightness: -1
contrast: -1
saturation: -1
sharpness: -1
gain: -1
auto_focus: false
focus: -1
auto_exposure: true
exposure: 100
auto_white_balance: true
white_balance: 4000
bytes_per_pixel: 2
trigger_internal: 0
trigger_fps: 20
channel_name: "/apollo/sensor/camera/obstacle/front_6mm"
device_wait: 2.0
spin_rate: 0.005
modules/drivers/usb_cam/conf/usb_cam_long_conf.pb.txt 0 → 100644
浏览文件 @ ec22443b
camera_dev: "/dev/camera/trafficlights"
frame_id: "camera_long"
pixel_format: "yuyv"
io_method: IO_METHOD_MMAP
width: 1920
height: 1080
frame_rate: 30
monochrome: false
brightness: -1
contrast: -1
saturation: -1
sharpness: -1
gain: -1
auto_focus: false
focus: -1
auto_exposure: true
exposure: 100
auto_white_balance: true
white_balance: 4000
bytes_per_pixel: 2
trigger_internal: 0
trigger_fps: 20
channel_name: "/apollo/sensor/camera/traffic/image_long"
device_wait: 2.0
spin_rate: 0.005
modules/drivers/usb_cam/conf/usb_cam_short_conf.pb.txt 0 → 100644
浏览文件 @ ec22443b
camera_dev: "/dev/camera/obstacle"
frame_id: "camera_short"
pixel_format: "yuyv"
io_method: IO_METHOD_MMAP
width: 1920
height: 1080
frame_rate: 30
monochrome: false
brightness: -1
contrast: -1
saturation: -1
sharpness: -1
gain: -1
auto_focus: false
focus: -1
auto_exposure: true
exposure: 100
auto_white_balance: true
white_balance: 4000
bytes_per_pixel: 2
trigger_internal: 0
trigger_fps: 20
channel_name: "/apollo/sensor/camera/traffic/image_short"
device_wait: 2.0
spin_rate: 0.005
modules/drivers/usb_cam/dag/dag_streaming_usb_cam.dag 0 → 100644
浏览文件 @ ec22443b
# Define all coms in DAG streaming.
module_config {
module_library : "/apollo/bazel-bin/modules/drivers/usb_cam/libusb_cam_component.so"
components {
class_name : "UsbCamComponent"
config {
name : "usb_cam_driver_short"
config_file_path : "/apollo/modules/drivers/usb_cam/conf/usb_cam_short_conf.pb.txt"
readers {channel: ""}
}
}
components {
class_name : "UsbCamComponent"
config {
name : "usb_cam_driver_long"
config_file_path : "/apollo/modules/drivers/usb_cam/conf/usb_cam_long_conf.pb.txt"
readers {channel: ""}
}
}
components {
class_name : "UsbCamComponent"
config {
name : "usb_cam_driver_front"
config_file_path : "/apollo/modules/drivers/usb_cam/conf/usb_cam_front_conf.pb.txt"
readers {channel: ""}
}
}
}
modules/drivers/usb_cam/launch/start_leopard.launch 已删除 100644 → 0
浏览文件 @ 9bd16323
<launch>
<arg name="traffic_short_topic_name" default="/apollo/sensor/camera/traffic/image_short" />
<arg name="traffic_long_topic_name" default="/apollo/sensor/camera/traffic/image_long" />
<arg name="perception_obstacle" default="/apollo/perception/obstacle/camera" />
<arg name="frame_rate" default="30"/>
<arg name="trigger_internal" default="0" />
<arg name="trigger_fps" default="20" />
<group ns="camera_long">
<node pkg="nodelet" type="nodelet" name="camera_nodelet_manager" args="manager" respawn="true" />
<node pkg="nodelet" type="nodelet" name="sensor_camera_traffic_long" output="screen"
args="load usb_cam/UsbCamNodelet camera_nodelet_manager" respawn="true" >
<param name="video_device" value="/dev/camera/trafficlights" />
<param name="image_width" value="1920" />
<param name="image_height" value="1080" />
<param name="pixel_format" value="yuyv" />
<param name="frame_rate" value="$(arg frame_rate)" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
<param name="camera_timeout" value="2000" />
<param name="error_code" value="11" />
<param name="camera_info_url" type="string" value="file://$(find usb_cam)/params/onsemi_traffic_intrinsics.yaml" />
<param name="trigger_internal" value="$(arg trigger_internal)"/>
<param name="trigger_fps" value="$(arg trigger_fps)" />
<remap from="/camera_long/image_raw0" to="$(arg traffic_long_topic_name)" />
<remap from="/apollo/sensor/camera/traffic/camera_info" to="$(arg traffic_long_topic_name)/camera_info" />
</node>
</group>
<group ns="camera_short">
<node pkg="nodelet" type="nodelet" name="camera_nodelet_manager" args="manager" respawn="true" />
<node pkg="nodelet" type="nodelet" name="sensor_camera_traffic_short" output="screen"
args="load usb_cam/UsbCamNodelet camera_nodelet_manager" respawn="true" >
<param name="video_device" value="/dev/camera/obstacle" />
<param name="image_width" value="1920" />
<param name="image_height" value="1080" />
<param name="pixel_format" value="yuyv" />
<param name="frame_rate" value="$(arg frame_rate)" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
<param name="camera_timeout" value="2000" />
<param name="error_code" value="14" />
<param name="camera_info_url" type="string" value="file://$(find usb_cam)/params/onsemi_obstacle_intrinsics.yaml" />
<param name="trigger_internal" value="$(arg trigger_internal)"/>
<param name="trigger_fps" value="$(arg trigger_fps)" />
<remap from="/camera_short/image_raw0" to="$(arg traffic_short_topic_name)" />
<remap from="/apollo/sensor/camera/traffic/camera_info" to="$(arg traffic_short_topic_name)/camera_info" />
</node>
</group>
<group ns="camera_obstacle">
<node pkg="nodelet" type="nodelet" name="camera_nodelet_manager" args="manager" respawn="true" />
<node pkg="nodelet" type="nodelet" name="sensor_camera_obstacle" output="screen"
args="load usb_cam/UsbCamNodelet camera_nodelet_manager" respawn="true" >
<param name="video_device" value="/dev/camera/obstacle" />
<param name="image_width" value="1920" />
<param name="image_height" value="1080" />
<param name="pixel_format" value="yuyv" />
<param name="frame_rate" value="$(arg frame_rate)" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
<param name="camera_timeout" value="2000" />
<param name="error_code" value="14" />
<param name="trigger_internal" value="$(arg trigger_internal)"/>
<param name="trigger_fps" value="$(arg trigger_fps)" />
<param name="camera_info_url" type="string" value="file://$(find usb_cam)/params/onsemi_obstacle_intrinsics.yaml" />
<remap from="/camera_short/image_raw0" to="$(arg perception_obstacle)" />
<remap from="/apollo/sensor/camera/traffic/camera_info" to="$(arg perception_obstacle)/camera_info" />
</node>
</group>
</launch>
modules/drivers/usb_cam/launch/start_obstacle_camera.launch 已删除 100644 → 0
浏览文件 @ 9bd16323
<launch>
<arg name="front_obstacle_6mm_topic_name" default="/apollo/sensor/camera/obstacle/front_6mm" />
<arg name="frame_rate" default="30"/>
<group ns="camera_obstacle">
<node pkg="nodelet" type="nodelet" name="camera_nodelet_manager" args="manager" respawn="true" />
<node pkg="nodelet" type="nodelet" name="sensor_camera_obstacle" output="screen"
args="load usb_cam/UsbCamNodelet camera_nodelet_manager" respawn="true" >
<param name="video_device" value="/dev/camera/lanemark" />
<param name="image_width" value="1920" />
<param name="image_height" value="1080" />
<param name="pixel_format" value="yuyv" />
<param name="frame_rate" value="$(arg frame_rate)" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
<param name="camera_timeout" value="2000" />
<param name="error_code" value="14" />
<param name="camera_info_url" type="string" value="file://$(find usb_cam)/params/onsemi_obstacle_intrinsics.yaml" />
<remap from="/camera_obstacle/image_raw0" to="$(arg front_obstacle_6mm_topic_name)" />
<remap from="/apollo/sensor/camera/traffic/camera_info" to="$(arg front_obstacle_6mm_topic_name)/camera_info" />
</node>
</group>
</launch>
modules/drivers/usb_cam/launch/start_one_leopard.launch 已删除 100644 → 0
浏览文件 @ 9bd16323
<launch>
<arg name="traffic_short_topic_name" default="/apollo/sensor/camera/traffic/image_short" />
<arg name="traffic_long_topic_name" default="/apollo/sensor/camera/traffic/image_long" />
<arg name="frame_rate" default="30"/>
<group ns="camera_long">
<node pkg="nodelet" type="nodelet" name="camera_nodelet_manager" args="manager" respawn="true" />
<node pkg="nodelet" type="nodelet" name="sensor_camera_traffic_long" output="screen"
args="load usb_cam/UsbCamNodelet camera_nodelet_manager" respawn="true" >
<param name="video_device" value="/dev/video0" />
<param name="image_width" value="1920" />
<param name="image_height" value="1080" />
<param name="pixel_format" value="yuyv" />
<param name="frame_rate" value="$(arg frame_rate)" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
<param name="camera_timeout" value="2000" />
<param name="camera_info_url" type="string" value="file://$(find usb_cam)/params/onsemi_traffic_intrinsics.yaml" />
<remap from="/camera_long/image_raw0" to="$(arg traffic_long_topic_name)" />
<remap from="/apollo/sensor/camera/traffic/camera_info" to="$(arg traffic_long_topic_name)/camera_info" />
</node>
</group>
</launch>
modules/drivers/usb_cam/launch/usb_cam-test.launch 已删除 100755 → 0
浏览文件 @ 9bd16323
<launch>
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
<param name="topic_name" value="image_raw0" />
<param name="video_device" value="/dev/video0" />
<param name="image_width" value="1920" />
<param name="image_height" value="1080" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
</node>
</launch>
modules/drivers/usb_cam/launch/usb_cam.launch 0 → 100644
浏览文件 @ ec22443b
<cybertron>
<module>
<name>usb_cam</name>
<dag_conf>/apollo/modules/drivers/usb_cam/dag/dag_streaming_usb_cam.dag</dag_conf>
<process_name>usb_cam</process_name>
</module>
</cybertron>
modules/drivers/usb_cam/nodelets.xml 已删除 100755 → 0
浏览文件 @ 9bd16323
<library path="lib/libusb_cam_nodelet">
<class name="usb_cam/UsbCamNodelet"
type="usb_cam::UsbCamNodelet"
base_class_type="nodelet::Nodelet">
<description>
usb camera nodelet
</description>
</class>
</library>
modules/drivers/usb_cam/nodes/usb_cam_node.cpp 已删除 100644 → 0
浏览文件 @ 9bd16323
/*********************************************************************
*
* 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.
*
*********************************************************************/
#include <ros/ros.h>
#include "usb_cam_wrapper.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "usb_cam");
ros::NodeHandle node;
ros::NodeHandle priv_nh("~");
usb_cam::UsbCamWrapper usbCam(node, priv_nh);
usbCam.spin();
return EXIT_SUCCESS;
}
modules/drivers/usb_cam/nodes/usb_cam_nodelet.cpp 已删除 100644 → 0
浏览文件 @ 9bd16323
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include "usb_cam_wrapper.h"
namespace usb_cam {
class UsbCamNodelet: public nodelet::Nodelet
{
public:
UsbCamNodelet() {}
~UsbCamNodelet() {
ROS_INFO("shutting down driver thread");
if (device_thread_ != nullptr && device_thread_->joinable()) {
device_thread_->join();
}
ROS_INFO("driver thread stopped");
};
private:
virtual void onInit();
boost::shared_ptr<UsbCamWrapper> usb_cam_ = nullptr;
boost::shared_ptr<boost::thread> device_thread_ = nullptr;
};
void UsbCamNodelet::onInit()
{
ROS_INFO("Usb cam nodelet init");
usb_cam_.reset(new UsbCamWrapper(getNodeHandle(), getPrivateNodeHandle()));
// spawn device poll thread
device_thread_ = boost::shared_ptr<boost::thread>
(new boost::thread(boost::bind(&UsbCamWrapper::spin, usb_cam_)));
}
}
// Register this plugin with pluginlib. Names must match nodelets.xml.
//
// parameters: package, class name, class type, base class type
PLUGINLIB_DECLARE_CLASS(usb_cam, UsbCamNodelet,
usb_cam::UsbCamNodelet, nodelet::Nodelet);
modules/drivers/usb_cam/nodes/usb_cam_wrapper.cpp 已删除 100644 → 0
浏览文件 @ 9bd16323
#include "usb_cam_wrapper.h"
#include <time.h> /* for clock_gettime */
#include "image_transport/camera_common.h"
#include "image_transport/publisher_plugin.h"
namespace usb_cam {
UsbCamWrapper::UsbCamWrapper(ros::NodeHandle node, ros::NodeHandle private_nh) :
node_(node), priv_node_(private_nh), last_stamp_(0)
{
// grab the parameters
priv_node_.param("topic_name", topic_name_, std::string("image_raw0"));
priv_node_.param("video_device", video_device_name_, std::string("/dev/video0"));
priv_node_.param("brightness", brightness_, -1); //0-255, -1 "leave alone"
priv_node_.param("contrast", contrast_, -1); //0-255, -1 "leave alone"
priv_node_.param("saturation", saturation_, -1); //0-255, -1 "leave alone"
priv_node_.param("sharpness", sharpness_, -1); //0-255, -1 "leave alone"
// possible values: mmap, read, userptr
priv_node_.param("io_method", io_method_name_, std::string("mmap"));
priv_node_.param("image_width", image_width_, 640);
priv_node_.param("image_height", image_height_, 480);
priv_node_.param("frame_rate", framerate_, 30);
priv_node_.param("trigger_internal", trigger_internal_, 0); //default gps pps
priv_node_.param("trigger_fps", trigger_fps_, 30);
// possible values: yuyv, uyvy, mjpeg, yuvmono10, rgb24
priv_node_.param("pixel_format", pixel_format_name_, std::string("mjpeg"));
// enable/disable autofocus
priv_node_.param("autofocus", autofocus_, false);
priv_node_.param("focus", focus_, -1); //0-255, -1 "leave alone"
// enable/disable autoexposure
priv_node_.param("autoexposure", autoexposure_, true);
priv_node_.param("exposure", exposure_, 100);
priv_node_.param("gain", gain_, -1); //0-100?, -1 "leave alone"
// enable/disable auto white balance temperature
priv_node_.param("auto_white_balance", auto_white_balance_, true);
priv_node_.param("white_balance", white_balance_, 4000);
// load the camera info
priv_node_.param("camera_frame_id", img_.header.frame_id, std::string("head_camera"));
priv_node_.param("camera_name", camera_name_, std::string("head_camera"));
priv_node_.param("camera_info_url", camera_info_url_, std::string(""));
cinfo_.reset(new camera_info_manager::CameraInfoManager(node_, camera_name_, camera_info_url_));
// default 3000 ms
priv_node_.param("camera_timeout", cam_timeout_, 1000);
priv_node_.param("spin_interval", spin_interval_, 0.005f);
// Warning when diff with last > 1.5* interval
frame_warning_interval_ = 1.5 / framerate_;
// now max fps 30, we use a appox time 0.9 to drop image.
frame_drop_interval_ = 0.9 / framerate_;
// advertise the main image topic
//image_transport::ImageTransport it(node_);
//image_pub_ = it.advertiseCamera(topic_name_, 1);
// Load transport publish plugin
std::string image_topic = node_.resolveName(topic_name_);
pub_loader_ = boost::make_shared<image_transport::PubLoader>("image_transport", "image_transport::PublisherPlugin");
std::string lookup_name = image_transport::PublisherPlugin::getLookupName(std::string("raw"));
image_pub_plugin_ = pub_loader_->createInstance(lookup_name);
if (image_pub_plugin_ != nullptr)
{
image_pub_plugin_->advertise(node_, image_topic, 1, image_transport::SubscriberStatusCallback(),
image_transport::SubscriberStatusCallback(), ros::VoidPtr(), false);
}
else
{
ROS_ERROR("create image publish plugin error. lookup_name: '%s'", lookup_name.c_str());
node_.shutdown();
return;
}
// camera info publish
std::string cam_info_topic = image_transport::getCameraInfoTopic(image_topic);
cam_info_pub_ = node_.advertise<sensor_msgs::CameraInfo>(cam_info_topic, 1, ros::SubscriberStatusCallback(),
ros::SubscriberStatusCallback(), ros::VoidPtr(), false);
// create Services
service_start_ = node_.advertiseService("start_capture", &UsbCamWrapper::service_start_cap, this);
service_stop_ = node_.advertiseService("stop_capture", &UsbCamWrapper::service_stop_cap, this);
// check for default camera info
if (!cinfo_->isCalibrated())
{
cinfo_->setCameraName(video_device_name_);
sensor_msgs::CameraInfo camera_info;
camera_info.header.frame_id = img_.header.frame_id;
camera_info.width = image_width_;
camera_info.height = image_height_;
cinfo_->setCameraInfo(camera_info);
}
//get the camera basical infomation
cam_info_.reset(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
ROS_INFO("Starting '%s' (%s) at %dx%d via %s (%s) at %i FPS", camera_name_.c_str(),
video_device_name_.c_str(),
image_width_, image_height_, io_method_name_.c_str(), pixel_format_name_.c_str(), framerate_);
// set the IO method
UsbCam::io_method io_method = UsbCam::io_method_from_string(io_method_name_);
if (io_method == UsbCam::IO_METHOD_UNKNOWN)
{
ROS_FATAL("Unknown IO method '%s'", io_method_name_.c_str());
node_.shutdown();
return;
}
// set the pixel format
UsbCam::pixel_format pixel_format = UsbCam::pixel_format_from_string(pixel_format_name_);
if (pixel_format == UsbCam::PIXEL_FORMAT_UNKNOWN)
{
ROS_FATAL("Unknown pixel format '%s'", pixel_format_name_.c_str());
node_.shutdown();
return;
}
// start the camera
cam_.start(video_device_name_.c_str(), io_method, pixel_format, image_width_, image_height_,
framerate_);
// set camera parameters
if (brightness_ >= 0)
cam_.set_v4l_parameter("brightness", brightness_);
if (contrast_ >= 0)
cam_.set_v4l_parameter("contrast", contrast_);
if (saturation_ >= 0)
cam_.set_v4l_parameter("saturation", saturation_);
if (sharpness_ >= 0)
cam_.set_v4l_parameter("sharpness", sharpness_);
if (gain_ >= 0)
cam_.set_v4l_parameter("gain", gain_);
// check auto white balance
if (auto_white_balance_)
{
cam_.set_v4l_parameter("white_balance_temperature_auto", 1);
}
else
{
cam_.set_v4l_parameter("white_balance_temperature_auto", 0);
cam_.set_v4l_parameter("white_balance_temperature", white_balance_);
}
// check auto exposure
if (!autoexposure_)
{
// turn down exposure control (from max of 3)
cam_.set_v4l_parameter("exposure_auto", 1);
// change the exposure level
cam_.set_v4l_parameter("exposure_absolute", exposure_);
}
// check auto focus
if (autofocus_)
{
cam_.set_auto_focus(1);
cam_.set_v4l_parameter("focus_auto", 1);
}
else
{
cam_.set_v4l_parameter("focus_auto", 0);
if (focus_ >= 0)
cam_.set_v4l_parameter("focus_absolute", focus_);
}
//trigger enable
int trigger_ret = cam_.trigger_enable(trigger_fps_, trigger_internal_);
if (0 != trigger_ret)
{
ROS_WARN("Camera trigger Fail ret '%d'", trigger_ret);
// node_.shutdown();
// return;
}
}
UsbCamWrapper::~UsbCamWrapper()
{
cam_.shutdown();
}
bool UsbCamWrapper::service_start_cap(std_srvs::Empty::Request& req,
std_srvs::Empty::Response& res)
{
cam_.start_capturing();
return true;
}
bool UsbCamWrapper::service_stop_cap(std_srvs::Empty::Request& req,
std_srvs::Empty::Response& res)
{
cam_.stop_capturing();
return true;
}
bool UsbCamWrapper::take_and_send_image()
{
// grab the image
bool get_new_image = cam_.grab_image(&img_, cam_timeout_);
if (!get_new_image)
return false;
// grab the camera info
//cam_info_ = sensor_msgs::CameraInfo(cinfo_->getCameraInfo());
cam_info_->header.frame_id = img_.header.frame_id;
cam_info_->header.stamp = img_.header.stamp;
if (last_stamp_ == ros::Time(0))
{
last_stamp_ = img_.header.stamp;
}
else
{
auto diff = (img_.header.stamp - last_stamp_).toSec();
// drop image by frame_rate
if (diff < frame_drop_interval_) {
ROS_INFO_STREAM("drop image:" << img_.header.stamp);
return true;
}
if (frame_warning_interval_ < diff)
ROS_WARN_STREAM("stamp jump.last stamp:" << last_stamp_
<< " current stamp:" << img_.header.stamp);
last_stamp_ = img_.header.stamp;
}
// publish the image
image_pub_plugin_->publish(img_);
cam_info_pub_.publish(cam_info_);
return true;
}
bool UsbCamWrapper::spin()
{
// spin loop rate should be in accord with the trigger frequence
ros::Duration loop_interval(this->spin_interval_);
while (node_.ok())
{
if (cam_.is_capturing())
if (!take_and_send_image())
ROS_ERROR("USB camera did not respond in time.");
//ros::spinOnce();
loop_interval.sleep();
}
return true;
}
}
modules/drivers/usb_cam/nodes/usb_cam_wrapper.h 已删除 100644 → 0
浏览文件 @ 9bd16323
#ifndef USB_CAM_INCLUDE_USB_CAM_USBCAMWRAPPER_H_
#define USB_CAM_INCLUDE_USB_CAM_USBCAMWRAPPER_H_
#include <ros/ros.h>
#include <usb_cam/usb_cam.h>
#include <image_transport/image_transport.h>
#include <camera_info_manager/camera_info_manager.h>
#include <pluginlib/class_loader.h>
#include <sstream>
#include <std_srvs/Empty.h>
namespace usb_cam {
enum TriggerFrequence {
FPS_10HZ = 10,
FPS_15HZ = 15,
FPS_20HZ = 20,
DEFAULT_FPS = 30
};
class UsbCamWrapper {
public:
UsbCamWrapper(ros::NodeHandle node, ros::NodeHandle private_nh);
virtual ~UsbCamWrapper();
bool service_start_cap(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
bool service_stop_cap(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
bool take_and_send_image();
bool spin();
private:
// shared image message
sensor_msgs::Image img_;
sensor_msgs::CameraInfoPtr cam_info_ = nullptr;
//image_transport::CameraPublisher image_pub_;
image_transport::PubLoaderPtr pub_loader_;
boost::shared_ptr<image_transport::PublisherPlugin> image_pub_plugin_;
ros::Publisher cam_info_pub_;
// parameters
std::string topic_name_;
std::string video_device_name_;
std::string io_method_name_;
std::string pixel_format_name_;
std::string camera_name_;
std::string camera_info_url_;
//std::string start_service_name_, start_service_name_;
//bool streaming_status_;
int image_width_;
int image_height_;
int framerate_;
int exposure_;
int brightness_;
int contrast_;
int saturation_;
int sharpness_;
int focus_;
int white_balance_;
int gain_;
int trigger_internal_;
int trigger_fps_;
bool autofocus_;
bool autoexposure_;
bool auto_white_balance_;
// usb will be reset when camera timeout
int cam_timeout_;
UsbCam cam_;
boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_;
ros::ServiceServer service_start_;
ros::ServiceServer service_stop_;
// private ROS node handle
ros::NodeHandle node_;
ros::NodeHandle priv_node_;
ros::Time last_stamp_;
float frame_warning_interval_;
float frame_drop_interval_;
float spin_interval_;
int error_code_;
};
}
#endif /* USB_CAM_INCLUDE_USB_CAM_USBCAMWRAPPER_H_ */
modules/drivers/usb_cam/package.xml 已删除 100644 → 0
浏览文件 @ 9bd16323
<package>
<name>usb_cam</name>
<version>0.3.4</version>
<description>A ROS Driver for V4L USB Cameras</description>
<maintainer email="rctoris@wpi.edu">Russell Toris</maintainer>
<author email="benjamin.pitzer@bosch.com">Benjamin Pitzer</author>
<license>BSD</license>
<url type="website">http://wiki.ros.org/usb_cam</url>
<url type="bugtracker">https://github.com/bosch-ros-pkg/usb_cam/issues</url>
<url type="repository">https://github.com/bosch-ros-pkg/usb_cam</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>image_transport</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>ffmpeg</build_depend>
<build_depend>camera_info_manager</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>pluginlib</build_depend>
<run_depend>image_transport</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>std_srvs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>ffmpeg</run_depend>
<run_depend>camera_info_manager</run_depend>
<run_depend>v4l-utils</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>pluginlib</run_depend>
<export>
<nodelet plugin="${prefix}/nodelets.xml"/>
</export>
</package>
modules/drivers/usb_cam/params/onsemi_obstacle_intrinsics.yaml 已删除 100644 → 0
浏览文件 @ 9bd16323
image_width: 1080
image_height: 1920
camera_name: usb_cam
camera_matrix:
rows: 3
cols: 3
data: [4827.94, 0, 1223.5, 0, 4835.62, 1024.5, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.41527, 0.31874, -0.00197, 0.00071, 0]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [4827.94, 0, 1223.5, 0, 0, 4835.62, 1024.5, 0, 0, 0, 1, 0]
modules/drivers/usb_cam/params/onsemi_traffic_intrinsics.yaml 已删除 100644 → 0
浏览文件 @ 9bd16323
image_width: 1080
image_height: 1920
camera_name: usb_cam
camera_matrix:
rows: 3
cols: 3
data: [4827.94, 0, 1223.5, 0, 4835.62, 1024.5, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.41527, 0.31874, -0.00197, 0.00071, 0]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [4827.94, 0, 1223.5, 0, 0, 4835.62, 1024.5, 0, 0, 0, 1, 0]
modules/drivers/usb_cam/proto/BUILD 0 → 100644
浏览文件 @ ec22443b
load("//tools:cpplint.bzl", "cpplint")
package(default_visibility = ["//visibility:public"])
cc_library(
name = "usb_cam_proto",
deps = [
":sensor_usb_cam_proto",
],
)
cc_proto_library(
name = "sensor_usb_cam_proto",
deps = [
":usb_cam_proto_lib",
],
)
proto_library(
name = "usb_cam_proto_lib",
srcs = [
"config.proto",
],
deps = [
"//modules/common/proto:header_proto_lib",
],
)
cpplint()
modules/drivers/usb_cam/proto/config.proto 0 → 100644
浏览文件 @ ec22443b
syntax = "proto2";
package apollo.drivers.usb_cam.config;
enum IOMethod {
IO_METHOD_UNKNOWN = 0;
IO_METHOD_READ = 1;
IO_METHOD_MMAP = 2;
IO_METHOD_USERPTR = 3;
}
message Config {
required string camera_dev = 1;
required string frame_id = 2;
//v4l pixel format
required string pixel_format = 3 [default="yuyv"];
//mmap, userptr, read
required IOMethod io_method = 4;
required uint32 width = 5;
required uint32 height = 6;
required uint32 frame_rate = 7;
required bool monochrome = 8 [default=false];
required int32 brightness = 9 [default=-1];
required int32 contrast = 10 [default=-1];
required int32 saturation = 11 [default=-1];
required int32 sharpness = 12 [default=-1];
required int32 gain = 13 [default=-1];
required bool auto_focus = 14 [default=false];
required int32 focus = 15 [default=-1];
required bool auto_exposure = 16 [default=true];
required int32 exposure = 17 [default=100];
required bool auto_white_balance = 18 [default=true];
required int32 white_balance = 19 [default=4000];
required uint32 bytes_per_pixel = 20 [default=3];
required uint32 trigger_internal = 21 [default=0];
required uint32 trigger_fps = 22 [default=30];
optional string channel_name = 23 [ default="" ];
// wait time when camera select timeout
optional float device_wait = 24 [ default=2.0];
// camera select spin time
optional float spin_rate = 25 [ default=0.005];
}
modules/drivers/usb_cam/src/LICENSE 已删除 100755 → 0
浏览文件 @ 9bd16323
Video for Linux Two API Specification
Revision 0.24
Michael H Schimek <mschimek@gmx.at>
Bill Dirks
Hans Verkuil
Martin Rubli
Copyright © 1999, 2000, 2001, 2002, 2003, 2004, 2005, 2006, 2007, 2008 Bill Dirks, Michael H. Schimek, Hans Verkuil, Martin Rubli
This document is copyrighted © 1999-2008 by Bill Dirks, Michael H. Schimek, Hans Verkuil and Martin Rubli.
Permission is granted to copy, distribute and/or modify this document under the terms of the GNU Free Documentation License, Version 1.1 or any later version published by the Free Software Foundation; with no Invariant Sections, with no Front-Cover Texts, and with no Back-Cover Texts. A copy of the license is included in the appendix entitled "GNU Free Documentation License".
Programming examples can be used and distributed without restrictions.
modules/drivers/usb_cam/src/usb_cam.cpp 已删除 100644 → 0
浏览文件 @ 9bd16323
/*********************************************************************
*
* 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.
*
*********************************************************************/
#define __STDC_CONSTANT_MACROS
#include <assert.h>
#include <errno.h>
#include <fcntl.h> /* low-level i/o */
#include <malloc.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <iostream>
#include <sstream>
#include <ros/ros.h>
#include <sensor_msgs/fill_image.h>
#include <boost/lexical_cast.hpp>
#include <usb_cam/usb_cam.h>
#include "include/adv_trigger_ctl.h"
#define CLEAR(x) memset(&(x), 0, sizeof(x))
namespace usb_cam {
static void errno_exit(const char *s) {
ROS_ERROR("%s error %d, %s", s, errno, strerror(errno));
exit(EXIT_FAILURE);
}
static int xioctl(int fd, int request, void *arg) {
int r;
do
r = ioctl(fd, request, arg);
while (-1 == r && EINTR == errno);
return r;
}
const unsigned char uchar_clipping_table[] = {
0, 0, 0, 0, 0, 0, 0,
0, // -128 - -121
0, 0, 0, 0, 0, 0, 0,
0, // -120 - -113
0, 0, 0, 0, 0, 0, 0,
0, // -112 - -105
0, 0, 0, 0, 0, 0, 0,
0, // -104 - -97
0, 0, 0, 0, 0, 0, 0,
0, // -96 - -89
0, 0, 0, 0, 0, 0, 0,
0, // -88 - -81
0, 0, 0, 0, 0, 0, 0,
0, // -80 - -73
0, 0, 0, 0, 0, 0, 0,
0, // -72 - -65
0, 0, 0, 0, 0, 0, 0,
0, // -64 - -57
0, 0, 0, 0, 0, 0, 0,
0, // -56 - -49
0, 0, 0, 0, 0, 0, 0,
0, // -48 - -41
0, 0, 0, 0, 0, 0, 0,
0, // -40 - -33
0, 0, 0, 0, 0, 0, 0,
0, // -32 - -25
0, 0, 0, 0, 0, 0, 0,
0, // -24 - -17
0, 0, 0, 0, 0, 0, 0,
0, // -16 - -9
0, 0, 0, 0, 0, 0, 0,
0, // -8 - -1
0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29,
30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44,
45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59,
60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74,
75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89,
90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104,
105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119,
120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134,
135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149,
150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164,
165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179,
180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194,
195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209,
210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224,
225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239,
240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254,
255, 255, 255, 255, 255, 255, 255, 255, 255, // 256-263
255, 255, 255, 255, 255, 255, 255, 255, // 264-271
255, 255, 255, 255, 255, 255, 255, 255, // 272-279
255, 255, 255, 255, 255, 255, 255, 255, // 280-287
255, 255, 255, 255, 255, 255, 255, 255, // 288-295
255, 255, 255, 255, 255, 255, 255, 255, // 296-303
255, 255, 255, 255, 255, 255, 255, 255, // 304-311
255, 255, 255, 255, 255, 255, 255, 255, // 312-319
255, 255, 255, 255, 255, 255, 255, 255, // 320-327
255, 255, 255, 255, 255, 255, 255, 255, // 328-335
255, 255, 255, 255, 255, 255, 255, 255, // 336-343
255, 255, 255, 255, 255, 255, 255, 255, // 344-351
255, 255, 255, 255, 255, 255, 255, 255, // 352-359
255, 255, 255, 255, 255, 255, 255, 255, // 360-367
255, 255, 255, 255, 255, 255, 255, 255, // 368-375
255, 255, 255, 255, 255, 255, 255, 255, // 376-383
};
const int clipping_table_offset = 128;
/** Clip a value to the range 0<val<255. For speed this is done using an
* array, so can only cope with numbers in the range -128<val<383.
*/
static unsigned char CLIPVALUE(int val) {
// Old method (if)
/* val = val < 0 ? 0 : val; */
/* return val > 255 ? 255 : val; */
// New method (array)
return uchar_clipping_table[val + clipping_table_offset];
}
/**
* Conversion from YUV to RGB.
* The normal conversion matrix is due to Julien (surname unknown):
*
* [ R ] [ 1.0 0.0 1.403 ] [ Y ]
* [ G ] = [ 1.0 -0.344 -0.714 ] [ U ]
* [ B ] [ 1.0 1.770 0.0 ] [ V ]
*
* and the firewire one is similar:
*
* [ R ] [ 1.0 0.0 0.700 ] [ Y ]
* [ G ] = [ 1.0 -0.198 -0.291 ] [ U ]
* [ B ] [ 1.0 1.015 0.0 ] [ V ]
*
* Corrected by BJT (coriander's transforms RGB->YUV and YUV->RGB
* do not get you back to the same RGB!)
* [ R ] [ 1.0 0.0 1.136 ] [ Y ]
* [ G ] = [ 1.0 -0.396 -0.578 ] [ U ]
* [ B ] [ 1.0 2.041 0.002 ] [ V ]
*
*/
static void YUV2RGB(const unsigned char y, const unsigned char u,
const unsigned char v, unsigned char *r, unsigned char *g,
unsigned char *b) {
const int y2 = (int)y;
const int u2 = (int)u - 128;
const int v2 = (int)v - 128;
// std::cerr << "YUV=("<<y2<<","<<u2<<","<<v2<<")"<<std::endl;
// This is the normal YUV conversion, but
// appears to be incorrect for the firewire cameras
// int r2 = y2 + ( (v2*91947) >> 16);
// int g2 = y2 - ( ((u2*22544) + (v2*46793)) >> 16 );
// int b2 = y2 + ( (u2*115999) >> 16);
// This is an adjusted version (UV spread out a bit)
int r2 = y2 + ((v2 * 37221) >> 15);
int g2 = y2 - (((u2 * 12975) + (v2 * 18949)) >> 15);
int b2 = y2 + ((u2 * 66883) >> 15);
// std::cerr << " RGB=("<<r2<<","<<g2<<","<<b2<<")"<<std::endl;
// Cap the values.
*r = CLIPVALUE(r2);
*g = CLIPVALUE(g2);
*b = CLIPVALUE(b2);
}
void uyvy2rgb(char *YUV, char *RGB, int NumPixels) {
int i, j;
unsigned char y0, y1, u, v;
unsigned char r, g, b;
for (i = 0, j = 0; i < (NumPixels << 1); i += 4, j += 6) {
u = (unsigned char)YUV[i + 0];
y0 = (unsigned char)YUV[i + 1];
v = (unsigned char)YUV[i + 2];
y1 = (unsigned char)YUV[i + 3];
YUV2RGB(y0, u, v, &r, &g, &b);
RGB[j + 0] = r;
RGB[j + 1] = g;
RGB[j + 2] = b;
YUV2RGB(y1, u, v, &r, &g, &b);
RGB[j + 3] = r;
RGB[j + 4] = g;
RGB[j + 5] = b;
}
}
static void mono102mono8(char *RAW, char *MONO, int NumPixels) {
int i, j;
for (i = 0, j = 0; i < (NumPixels << 1); i += 2, j += 1) {
// first byte is low byte, second byte is high byte; smash together and
// convert to 8-bit
MONO[j] = (unsigned char)(((RAW[i + 0] >> 2) & 0x3F) |
((RAW[i + 1] << 6) & 0xC0));
}
}
static void yuyv2rgb(char *YUV, char *RGB, int NumPixels) {
int i, j;
unsigned char y0, y1, u, v;
unsigned char r, g, b;
for (i = 0, j = 0; i < (NumPixels << 1); i += 4, j += 6) {
y0 = (unsigned char)YUV[i + 0];
u = (unsigned char)YUV[i + 1];
y1 = (unsigned char)YUV[i + 2];
v = (unsigned char)YUV[i + 3];
YUV2RGB(y0, u, v, &r, &g, &b);
RGB[j + 0] = r;
RGB[j + 1] = g;
RGB[j + 2] = b;
YUV2RGB(y1, u, v, &r, &g, &b);
RGB[j + 3] = r;
RGB[j + 4] = g;
RGB[j + 5] = b;
}
}
void rgb242rgb(char *YUV, char *RGB, int NumPixels) {
memcpy(RGB, YUV, NumPixels * 3);
}
UsbCam::UsbCam()
: io_(IO_METHOD_MMAP),
fd_(-1),
n_buffers_(0),
avframe_camera_(NULL),
avframe_rgb_(NULL),
avcodec_(NULL),
avoptions_(NULL),
avcodec_context_(NULL),
avframe_camera_size_(0),
avframe_rgb_size_(0),
video_sws_(NULL),
image_(NULL),
is_capturing_(false) {}
UsbCam::~UsbCam() { shutdown(); }
int UsbCam::init_mjpeg_decoder(int image_width, int image_height) {
avcodec_register_all();
avcodec_ = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
if (!avcodec_) {
ROS_ERROR("Could not find MJPEG decoder");
return 0;
}
avcodec_context_ = avcodec_alloc_context3(avcodec_);
avframe_camera_ = avcodec_alloc_frame();
avframe_rgb_ = avcodec_alloc_frame();
avpicture_alloc((AVPicture *)avframe_rgb_, PIX_FMT_RGB24, image_width,
image_height);
avcodec_context_->codec_id = AV_CODEC_ID_MJPEG;
avcodec_context_->width = image_width;
avcodec_context_->height = image_height;
#if LIBAVCODEC_VERSION_MAJOR > 52
avcodec_context_->pix_fmt = PIX_FMT_YUV422P;
avcodec_context_->codec_type = AVMEDIA_TYPE_VIDEO;
#endif
avframe_camera_size_ =
avpicture_get_size(PIX_FMT_YUV422P, image_width, image_height);
avframe_rgb_size_ =
avpicture_get_size(PIX_FMT_RGB24, image_width, image_height);
/* open it */
if (avcodec_open2(avcodec_context_, avcodec_, &avoptions_) < 0) {
ROS_ERROR("Could not open MJPEG Decoder");
return 0;
}
return 1;
}
void UsbCam::mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels) {
int got_picture;
memset(RGB, 0, avframe_rgb_size_);
#if LIBAVCODEC_VERSION_MAJOR > 52
int decoded_len;
AVPacket avpkt;
av_init_packet(&avpkt);
avpkt.size = len;
avpkt.data = (unsigned char *)MJPEG;
decoded_len = avcodec_decode_video2(avcodec_context_, avframe_camera_,
&got_picture, &avpkt);
if (decoded_len < 0) {
ROS_ERROR("Error while decoding frame.");
return;
}
#else
avcodec_decode_video(avcodec_context_, avframe_camera_, &got_picture,
(uint8_t *)MJPEG, len);
#endif
if (!got_picture) {
ROS_ERROR("Webcam: expected picture but didn't get it...");
return;
}
int xsize = avcodec_context_->width;
int ysize = avcodec_context_->height;
int pic_size = avpicture_get_size(avcodec_context_->pix_fmt, xsize, ysize);
if (pic_size != avframe_camera_size_) {
ROS_ERROR("outbuf size mismatch. pic_size: %d bufsize: %d", pic_size,
avframe_camera_size_);
return;
}
video_sws_ =
sws_getContext(xsize, ysize, avcodec_context_->pix_fmt, xsize, ysize,
PIX_FMT_RGB24, SWS_BILINEAR, NULL, NULL, NULL);
sws_scale(video_sws_, avframe_camera_->data, avframe_camera_->linesize, 0,
ysize, avframe_rgb_->data, avframe_rgb_->linesize);
sws_freeContext(video_sws_);
int size = avpicture_layout((AVPicture *)avframe_rgb_, PIX_FMT_RGB24, xsize,
ysize, (uint8_t *)RGB, avframe_rgb_size_);
if (size != avframe_rgb_size_) {
ROS_ERROR("webcam: avpicture_layout error: %d", size);
return;
}
}
bool UsbCam::process_image(const void *src, int len,
boost::shared_ptr<CameraImage> dest) {
if (src == NULL || dest == NULL) {
ROS_ERROR("process image error. src or dest is null");
return false;
}
if (pixelformat_ == V4L2_PIX_FMT_YUYV || pixelformat_ == V4L2_PIX_FMT_UYVY) {
memcpy(dest->image, src, dest->width * dest->height * 2);
} else {
ROS_ERROR("unsupported pixel format: %d", pixelformat_);
return false;
}
return true;
}
int UsbCam::read_frame() {
struct v4l2_buffer v4l_buf;
unsigned int i;
int len;
bool result = false;
switch (io_) {
case IO_METHOD_MMAP:
CLEAR(v4l_buf);
v4l_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l_buf.memory = V4L2_MEMORY_MMAP;
if (-1 == xioctl(fd_, VIDIOC_DQBUF, &v4l_buf)) {
switch (errno) {
case EAGAIN:
return 0;
case EIO:
// Could ignore EIO, see spec.
// fall through
default:
errno_exit("VIDIOC_DQBUF");
}
}
assert(v4l_buf.index < n_buffers_);
len = v4l_buf.bytesused;
image_->tv_sec = v4l_buf.timestamp.tv_sec;
image_->tv_usec = v4l_buf.timestamp.tv_usec;
ROS_DEBUG("new image timestamp: %d.%d", image_->tv_sec, image_->tv_usec);
result = process_image(buffers_[v4l_buf.index].start, len, image_);
if (!result) {
return 0;
}
if (-1 == xioctl(fd_, VIDIOC_QBUF, &v4l_buf)) errno_exit("VIDIOC_QBUF");
break;
}
return 1;
}
bool UsbCam::is_capturing() { return is_capturing_; }
void UsbCam::stop_capturing(void) {
if (!is_capturing_) {
return;
}
is_capturing_ = false;
enum v4l2_buf_type v4l_type;
switch (io_) {
case IO_METHOD_MMAP:
v4l_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (-1 == xioctl(fd_, VIDIOC_STREAMOFF, &v4l_type))
errno_exit("VIDIOC_STREAMOFF");
break;
}
}
void UsbCam::start_capturing(void) {
if (is_capturing_) {
return;
}
unsigned int i = 0;
enum v4l2_buf_type v4l_type;
switch (io_) {
case IO_METHOD_MMAP:
for (i = 0; i < n_buffers_; ++i) {
struct v4l2_buffer tmp_buf;
CLEAR(tmp_buf);
tmp_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
tmp_buf.memory = V4L2_MEMORY_MMAP;
tmp_buf.index = i;
if (-1 == xioctl(fd_, VIDIOC_QBUF, &tmp_buf)) errno_exit("VIDIOC_QBUF");
}
v4l_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (-1 == xioctl(fd_, VIDIOC_STREAMON, &v4l_type))
errno_exit("VIDIOC_STREAMON");
break;
}
is_capturing_ = true;
}
void UsbCam::uninit_device(void) {
unsigned int i;
switch (io_) {
case IO_METHOD_MMAP:
for (i = 0; i < n_buffers_; ++i)
if (-1 == munmap(buffers_[i].start, buffers_[i].length))
errno_exit("munmap");
break;
}
}
void UsbCam::init_read(unsigned int buffer_size) {
// return;
}
void UsbCam::init_mmap(void) {
struct v4l2_requestbuffers v4l_req;
CLEAR(v4l_req);
v4l_req.count = 4;
v4l_req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l_req.memory = V4L2_MEMORY_MMAP;
if (-1 == xioctl(fd_, VIDIOC_REQBUFS, &v4l_req)) {
if (EINVAL == errno) {
ROS_ERROR_STREAM(camera_dev_ << " don't support mmap");
exit(EXIT_FAILURE);
} else {
errno_exit("VIDIOC_REQBUFS");
}
}
buffers_.resize(v4l_req.count);
for (n_buffers_ = 0; n_buffers_ < v4l_req.count; ++n_buffers_) {
struct v4l2_buffer tmp_buf;
CLEAR(tmp_buf);
tmp_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
tmp_buf.memory = V4L2_MEMORY_MMAP;
tmp_buf.index = n_buffers_;
if (-1 == xioctl(fd_, VIDIOC_QUERYBUF, &tmp_buf))
errno_exit("VIDIOC_QUERYBUF");
buffers_[n_buffers_].length = tmp_buf.length;
buffers_[n_buffers_].start =
mmap(NULL, tmp_buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd_,
tmp_buf.m.offset);
if (MAP_FAILED == buffers_[n_buffers_].start) errno_exit("mmap");
}
}
void UsbCam::init_userp(unsigned int buffer_size) {
// not support
}
void UsbCam::init_device(int image_width, int image_height, int framerate) {
struct v4l2_capability v4l_cap;
struct v4l2_cropcap v4l_cropcap;
struct v4l2_crop v4l_crop;
struct v4l2_format v4l_fmt;
unsigned int min;
if (-1 == xioctl(fd_, VIDIOC_QUERYCAP, &v4l_cap)) {
if (EINVAL == errno) {
ROS_ERROR_STREAM(camera_dev_ << " is no V4L2 device");
exit(EXIT_FAILURE);
} else {
errno_exit("VIDIOC_QUERYCAP");
}
}
if (!(v4l_cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
ROS_ERROR_STREAM(camera_dev_ << " is no video capture device");
exit(EXIT_FAILURE);
}
switch (io_) {
case IO_METHOD_MMAP:
if (!(v4l_cap.capabilities & V4L2_CAP_STREAMING)) {
ROS_ERROR_STREAM(camera_dev_ << " does not support streaming i/o");
exit(EXIT_FAILURE);
}
break;
}
CLEAR(v4l_cropcap);
v4l_cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (0 == xioctl(fd_, VIDIOC_CROPCAP, &v4l_cropcap)) {
v4l_crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l_crop.c = v4l_cropcap.defrect; /* reset to default */
if (-1 == xioctl(fd_, VIDIOC_S_CROP, &v4l_crop)) {
switch (errno) {
case EINVAL:
// Cropping not supported.
break;
default:
// Errors ignored
break;
}
}
} else {
/* ignored. */
}
CLEAR(v4l_fmt);
v4l_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v4l_fmt.fmt.pix.width = image_width;
v4l_fmt.fmt.pix.height = image_height;
v4l_fmt.fmt.pix.pixelformat = pixelformat_;
v4l_fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
if (-1 == xioctl(fd_, VIDIOC_S_FMT, &v4l_fmt)) errno_exit("VIDIOC_S_FMT");
min = v4l_fmt.fmt.pix.width * 2;
if (v4l_fmt.fmt.pix.bytesperline < min) v4l_fmt.fmt.pix.bytesperline = min;
min = v4l_fmt.fmt.pix.bytesperline * v4l_fmt.fmt.pix.height;
if (v4l_fmt.fmt.pix.sizeimage < min) v4l_fmt.fmt.pix.sizeimage = min;
image_width = v4l_fmt.fmt.pix.width;
image_height = v4l_fmt.fmt.pix.height;
struct v4l2_streamparm stream_params;
memset(&stream_params, 0, sizeof(stream_params));
stream_params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
// if (xioctl(fd_, VIDIOC_G_PARM, &stream_params) < 0)
// errno_exit("Couldn't query v4l fps!");
ROS_DEBUG("Capability flag: 0x%x", stream_params.parm.capture.capability);
stream_params.parm.capture.timeperframe.numerator = 1;
stream_params.parm.capture.timeperframe.denominator = framerate;
if (xioctl(fd_, VIDIOC_S_PARM, &stream_params) < 0)
ROS_WARN("Couldn't set camera framerate");
else
ROS_DEBUG("Set framerate to be %i", framerate);
switch (io_) {
case IO_METHOD_MMAP:
init_mmap();
break;
}
}
void UsbCam::close_device(void) {
if (-1 == close(fd_)) errno_exit("close");
fd_ = -1;
}
void UsbCam::open_device(void) {
struct stat st;
if (-1 == stat(camera_dev_.c_str(), &st)) {
ROS_ERROR_STREAM("Cannot identify '" << camera_dev_ << "': " << errno
<< ", " << strerror(errno));
exit(EXIT_FAILURE);
}
if (!S_ISCHR(st.st_mode)) {
ROS_ERROR_STREAM(camera_dev_ << " is no device");
exit(EXIT_FAILURE);
}
fd_ = open(camera_dev_.c_str(), O_RDWR | O_NONBLOCK, 0);
if (-1 == fd_) {
ROS_ERROR_STREAM("Cannot open '" << camera_dev_ << "': " << errno << ", "
<< strerror(errno));
exit(EXIT_FAILURE);
}
}
void UsbCam::start(const std::string &dev, io_method io_method,
pixel_format pixel_format, int image_width, int image_height,
int framerate) {
camera_dev_ = dev;
io_ = io_method;
monochrome_ = false;
if (pixel_format == PIXEL_FORMAT_YUYV)
pixelformat_ = V4L2_PIX_FMT_YUYV;
else if (pixel_format == PIXEL_FORMAT_UYVY)
pixelformat_ = V4L2_PIX_FMT_UYVY;
else if (pixel_format == PIXEL_FORMAT_MJPEG) {
pixelformat_ = V4L2_PIX_FMT_MJPEG;
init_mjpeg_decoder(image_width, image_height);
} else if (pixel_format == PIXEL_FORMAT_YUVMONO10) {
// actually format V4L2_PIX_FMT_Y16 (10-bit mono expresed as 16-bit pixels),
// but we need to use the advertised type (yuyv)
pixelformat_ = V4L2_PIX_FMT_YUYV;
monochrome_ = true;
} else if (pixel_format == PIXEL_FORMAT_RGB24) {
pixelformat_ = V4L2_PIX_FMT_RGB24;
} else {
ROS_ERROR("Unknown pixel format.");
exit(EXIT_FAILURE);
}
open_device();
init_device(image_width, image_height, framerate);
start_capturing();
// instead of malloc with smart pointer
image_ = boost::make_shared<CameraImage>();
image_->width = image_width;
image_->height = image_height;
image_->bytes_per_pixel = 2; // corrected 11/10/15 (BYTES not BITS per pixel)
image_->image_size = image_->width * image_->height * image_->bytes_per_pixel;
image_->is_new = 0;
image_->image = new char[image_->image_size]();
if (!image_->image) {
ROS_ERROR("Outof memory!");
exit(EXIT_FAILURE);
}
}
void UsbCam::shutdown(void) {
stop_capturing();
uninit_device();
close_device();
if (avcodec_context_) {
avcodec_close(avcodec_context_);
av_free(avcodec_context_);
avcodec_context_ = NULL;
}
if (avframe_camera_) av_free(avframe_camera_);
avframe_camera_ = NULL;
if (avframe_rgb_) av_free(avframe_rgb_);
avframe_rgb_ = NULL;
}
bool UsbCam::grab_image(sensor_msgs::Image *msg, int timeout) {
// grab the image
bool get_new_image = grab_image(timeout);
if (!get_new_image) {
return false;
}
// stamp the image
msg->header.stamp.sec = image_->tv_sec;
msg->header.stamp.nsec = 1000 * image_->tv_usec;
// fill the info
if (monochrome_) {
fillImage(*msg, "mono8", image_->height, image_->width, image_->width,
image_->image);
} else {
// fillImage(*msg, "rgb8", image_->height, image_->width, 3 * image_->width,
// image_->image);
msg->encoding = "yuyv";
msg->height = image_->height;
msg->width = image_->width;
msg->step = 2 * image_->width;
size_t len = image_->width * image_->height * 2;
msg->data.resize(len);
memcpy(&msg->data[0], image_->image, len);
msg->is_bigendian = 0;
}
return true;
}
bool UsbCam::grab_image(int timeout) {
fd_set fds;
struct timeval time_val;
int r = 0;
FD_ZERO(&fds);
FD_SET(fd_, &fds);
// Timeout.
time_val.tv_sec = timeout / 1000;
time_val.tv_usec = 0;
r = select(fd_ + 1, &fds, NULL, NULL, &time_val);
if (-1 == r) {
if (EINTR == errno) {
ROS_ERROR("select error EINTR.");
return false;
}
errno_exit("select error.");
}
if (0 == r) {
ROS_WARN_STREAM("camera is offline:" << camera_dev_);
// reset usb when camera timeout
// reset_device();
exit(EXIT_FAILURE);
}
int get_new_image = read_frame();
if (!get_new_image) {
ROS_ERROR("read frame error.");
return false;
}
image_->is_new = 1;
return true;
}
// enables/disables auto focus
void UsbCam::set_auto_focus(int value) {
struct v4l2_queryctrl queryctrl;
struct v4l2_ext_control control;
memset(&queryctrl, 0, sizeof(queryctrl));
queryctrl.id = V4L2_CID_FOCUS_AUTO;
if (-1 == xioctl(fd_, VIDIOC_QUERYCTRL, &queryctrl)) {
if (errno != EINVAL) {
perror("VIDIOC_QUERYCTRL");
return;
} else {
ROS_INFO("V4L2_CID_FOCUS_AUTO is not supported");
return;
}
} else if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
ROS_INFO("V4L2_CID_FOCUS_AUTO is not supported");
return;
} else {
memset(&control, 0, sizeof(control));
control.id = V4L2_CID_FOCUS_AUTO;
control.value = value;
if (-1 == xioctl(fd_, VIDIOC_S_CTRL, &control)) {
perror("VIDIOC_S_CTRL");
return;
}
}
}
/**
* Set video device parameter via call to v4l-utils.
*
* @param param The name of the parameter to set
* @param param The value to assign
*/
void UsbCam::set_v4l_parameter(const std::string &param, int value) {
set_v4l_parameter(param, boost::lexical_cast<std::string>(value));
}
/**
* Set video device parameter via call to v4l-utils.
*
* @param param The name of the parameter to set
* @param param The value to assign
*/
void UsbCam::set_v4l_parameter(const std::string &param,
const std::string &value) {
// build the command
std::stringstream ss;
ss << "v4l2-ctl --device=" << camera_dev_ << " -c " << param << "=" << value
<< " 2>&1";
std::string cmd = ss.str();
// capture the output
std::string output;
int buffer_size = 256;
char buffer[buffer_size];
FILE *stream = popen(cmd.c_str(), "r");
if (stream) {
while (!feof(stream))
if (fgets(buffer, buffer_size, stream) != NULL) output.append(buffer);
pclose(stream);
// any output should be an error
if (output.length() > 0) ROS_WARN("%s", output.c_str());
} else
ROS_WARN("usb_cam_node could not run '%s'", cmd.c_str());
}
UsbCam::io_method UsbCam::io_method_from_string(const std::string &str) {
if (str == "mmap")
return IO_METHOD_MMAP;
else
return IO_METHOD_UNKNOWN;
}
UsbCam::pixel_format UsbCam::pixel_format_from_string(const std::string &str) {
if (str == "yuyv")
return PIXEL_FORMAT_YUYV;
else if (str == "uyvy")
return PIXEL_FORMAT_UYVY;
else if (str == "mjpeg")
return PIXEL_FORMAT_MJPEG;
else if (str == "yuvmono10")
return PIXEL_FORMAT_YUVMONO10;
else if (str == "rgb24")
return PIXEL_FORMAT_RGB24;
else
return PIXEL_FORMAT_UNKNOWN;
}
int UsbCam::trigger_enable(unsigned char fps, unsigned char internal) {
ROS_INFO("Trigger enable, dev:%s, fps:%d, internal:%d", camera_dev_.c_str(),
fps, internal);
return adv_trigger_enable(camera_dev_.c_str(), fps, internal);
}
int UsbCam::trigger_disable() {
return adv_trigger_disable(camera_dev_.c_str());
}
} // namespace usb_cam
modules/drivers/usb_cam/usb_cam.cc 0 → 100644
浏览文件 @ ec22443b
/*********************************************************************
*
* 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.
*
*********************************************************************/
#include <cmath>
#include <string>
#include "adv_plat/include/adv_trigger.h"
#include "modules/drivers/usb_cam/usb_cam.h"
#define __STDC_CONSTANT_MACROS
#define CLEAR(x) memset(&(x), 0, sizeof(x))
namespace apollo {
namespace drivers {
namespace usb_cam {
UsbCam::UsbCam()
: fd_(-1),
buffers_(NULL),
n_buffers_(0),
is_capturing_(false),
image_seq_(0),
device_wait_sec_(2),
last_nsec_(0),
frame_drop_interval_(0.0) {}
UsbCam::~UsbCam() {
stop_capturing();
uninit_device();
close_device();
}
bool UsbCam::init(
std::shared_ptr<Config>& cameraconfig_) {
config_ = cameraconfig_;
if (config_->pixel_format() == "yuyv") {
pixel_format_ = V4L2_PIX_FMT_YUYV;
} else if (config_->pixel_format() == "uyvy") {
pixel_format_ = V4L2_PIX_FMT_UYVY;
} else if (config_->pixel_format() == "mjpeg") {
pixel_format_ = V4L2_PIX_FMT_MJPEG;
} else if (config_->pixel_format() == "yuvmono10") {
pixel_format_ = V4L2_PIX_FMT_YUYV;
config_->set_monochrome(true);
} else if (config_->pixel_format() == "rgb24") {
pixel_format_ = V4L2_PIX_FMT_RGB24;
} else {
pixel_format_ = V4L2_PIX_FMT_YUYV;
AERROR << "Wrong pixel fromat:" << config_->pixel_format()
<< ",must be yuyv | uyvy | mjpeg | yuvmono10 | rgb24";
return false;
}
if (pixel_format_ == V4L2_PIX_FMT_MJPEG) {
init_mjpeg_decoder(config_->width(), config_->height());
}
// Warning when diff with last > 1.5* interval
frame_warning_interval_ = 1.5 / config_->frame_rate();
// now max fps 30, we use a appox time 0.9 to drop image.
frame_drop_interval_ = 0.9 / config_->frame_rate();
return true;
}
int UsbCam::init_mjpeg_decoder(int image_width, int image_height) {
avcodec_register_all();
avcodec_ = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
if (!avcodec_) {
AERROR << "Could not find MJPEG decoder";
return 0;
}
avcodec_context_ = avcodec_alloc_context3(avcodec_);
avframe_camera_ = avcodec_alloc_frame();
avframe_rgb_ = avcodec_alloc_frame();
avpicture_alloc((AVPicture*)avframe_rgb_, PIX_FMT_RGB24, image_width,
image_height);
avcodec_context_->codec_id = AV_CODEC_ID_MJPEG;
avcodec_context_->width = image_width;
avcodec_context_->height = image_height;
#if LIBAVCODEC_VERSION_MAJOR > 52
avcodec_context_->pix_fmt = PIX_FMT_YUV422P;
avcodec_context_->codec_type = AVMEDIA_TYPE_VIDEO;
#endif
avframe_camera_size_ =
avpicture_get_size(PIX_FMT_YUV422P, image_width, image_height);
avframe_rgb_size_ =
avpicture_get_size(PIX_FMT_RGB24, image_width, image_height);
/* open it */
if (avcodec_open2(avcodec_context_, avcodec_, &avoptions_) < 0) {
AERROR << "Could not open MJPEG Decoder";
return 0;
}
return 1;
}
void UsbCam::mjpeg2rgb(char* mjpeg_buffer, int len, char* rgb_buffer,
int NumPixels) {
(void)NumPixels;
int got_picture = 0;
memset(rgb_buffer, 0, avframe_rgb_size_);
#if LIBAVCODEC_VERSION_MAJOR > 52
int decoded_len;
AVPacket avpkt;
av_init_packet(&avpkt);
avpkt.size = len;
avpkt.data = (unsigned char*)mjpeg_buffer;
decoded_len = avcodec_decode_video2(avcodec_context_, avframe_camera_,
&got_picture, &avpkt);
if (decoded_len < 0) {
AERROR << "Error while decoding frame.";
return;
}
#else
avcodec_decode_video(avcodec_context_, avframe_camera_, &got_picture,
(uint8_t *)mjpeg_buffer, len);
#endif
if (!got_picture) {
AERROR << "Webcam: expected picture but didn't get it...";
return;
}
int xsize = avcodec_context_->width;
int ysize = avcodec_context_->height;
int pic_size = avpicture_get_size(avcodec_context_->pix_fmt, xsize, ysize);
if (pic_size != avframe_camera_size_) {
AERROR << "outbuf size mismatch. pic_size:" << pic_size
<< ",buffer_size:" << avframe_camera_size_;
return;
}
video_sws_ =
sws_getContext(xsize, ysize, avcodec_context_->pix_fmt, xsize, ysize,
PIX_FMT_RGB24, SWS_BILINEAR, NULL, NULL, NULL);
sws_scale(video_sws_, avframe_camera_->data, avframe_camera_->linesize, 0,
ysize, avframe_rgb_->data, avframe_rgb_->linesize);
sws_freeContext(video_sws_);
int size = avpicture_layout((AVPicture*)avframe_rgb_, PIX_FMT_RGB24, xsize,
ysize, (uint8_t*)rgb_buffer, avframe_rgb_size_);
if (size != avframe_rgb_size_) {
AERROR << "webcam: avpicture_layout error: " << size;
return;
}
}
bool UsbCam::poll(const CameraImagePtr& raw_image) {
//raw_image->width = config_->width();
//raw_image->height = config_->height();
//raw_image->bytes_per_pixel = config_->bytes_per_pixel();
//raw_image->image_size =
// raw_image->width * raw_image->height * raw_image->bytes_per_pixel;
raw_image->is_new = 0;
// free memory in this struct desturctor
//raw_image->image = (char*)calloc(raw_image->image_size, sizeof(char));
memset(raw_image->image, 0, raw_image->image_size * sizeof(char));
fd_set fds;
struct timeval tv;
int r = 0;
FD_ZERO(&fds);
FD_SET(fd_, &fds);
/* Timeout. */
tv.tv_sec = 2;
tv.tv_usec = 0;
r = select(fd_ + 1, &fds, NULL, NULL, &tv);
if (-1 == r) {
if (EINTR == errno) {
return false;
}
// errno_exit("select");
reconnect();
}
if (0 == r) {
AERROR << "select timeout";
reconnect();
}
int get_new_image = read_frame(raw_image);
if (!get_new_image) {
return false;
}
raw_image->is_new = 1;
return true;
}
bool UsbCam::open_device(void) {
struct stat st;
if (-1 == stat(config_->camera_dev().c_str(), &st)) {
AERROR << "Cannot identify '" << config_->camera_dev() << "': " << errno
<< ", " << strerror(errno);
return false;
}
if (!S_ISCHR(st.st_mode)) {
AERROR << config_->camera_dev() << " is no device";
return false;
}
fd_ = open(config_->camera_dev().c_str(), O_RDWR /* required */ | O_NONBLOCK,
0);
if (-1 == fd_) {
AERROR << "Cannot open '" << config_->camera_dev() << "': " << errno
<< ", " << strerror(errno);
return false;
}
return true;
}
bool UsbCam::init_device(void) {
struct v4l2_capability cap;
struct v4l2_cropcap cropcap;
struct v4l2_crop crop;
struct v4l2_format fmt;
unsigned int min = 0;
if (-1 == xioctl(fd_, VIDIOC_QUERYCAP, &cap)) {
if (EINVAL == errno) {
AERROR << config_->camera_dev() << " is no V4L2 device";
// exit(EXIT_FAILURE);
// reconnect();
return false;
}
// errno_exit("VIDIOC_QUERYCAP");
AERROR << "VIDIOC_QUERYCAP";
// reconnect();
return false;
}
if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
AERROR << config_->camera_dev() << " is no video capture device";
// exit(EXIT_FAILURE);
// reconnect();
return false;
}
switch (config_->io_method()) {
case IO_METHOD_READ:
if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
AERROR << config_->camera_dev() << " does not support read i/o";
// exit(EXIT_FAILURE);
// reconnect();
return false;
}
break;
case IO_METHOD_MMAP:
case IO_METHOD_USERPTR:
if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
AERROR << config_->camera_dev()
<< " does not support streaming i/o";
// exit(EXIT_FAILURE);
// reconnect();
return false;
}
break;
case IO_METHOD_UNKNOWN:
AERROR << config_->camera_dev() << " does not support unknown i/o";
// exit(EXIT_FAILURE);
// reconnect();
return false;
break;
}
/* Select video input, video standard and tune here. */
CLEAR(cropcap);
cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (0 == xioctl(fd_, VIDIOC_CROPCAP, &cropcap)) {
crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
crop.c = cropcap.defrect; /* reset to default */
if (-1 == xioctl(fd_, VIDIOC_S_CROP, &crop)) {
switch (errno) {
case EINVAL:
/* Cropping not supported. */
break;
}
}
}
CLEAR(fmt);
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
fmt.fmt.pix.width = config_->width();
fmt.fmt.pix.height = config_->height();
fmt.fmt.pix.pixelformat = pixel_format_;
fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
if (-1 == xioctl(fd_, VIDIOC_S_FMT, &fmt)) {
// errno_exit("VIDIOC_S_FMT");
AERROR << "VIDIOC_S_FMT";
return false;
// reconnect();
}
/* Note VIDIOC_S_FMT may change width and height. */
/* Buggy driver paranoia. */
min = fmt.fmt.pix.width * 2;
if (fmt.fmt.pix.bytesperline < min) {
fmt.fmt.pix.bytesperline = min;
}
min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
if (fmt.fmt.pix.sizeimage < min) {
fmt.fmt.pix.sizeimage = min;
}
config_->set_width(fmt.fmt.pix.width);
config_->set_height(fmt.fmt.pix.height);
struct v4l2_streamparm stream_params;
memset(&stream_params, 0, sizeof(stream_params));
stream_params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
// if (xioctl(fd_, VIDIOC_G_PARM, &stream_params) < 0) {
// // errno_exit("Couldn't query v4l fps!");
// AERROR << "Couldn't query v4l fps!";
// // reconnect();
// return false;
// }
AINFO << "Capability flag: 0x" << stream_params.parm.capture.capability;
stream_params.parm.capture.timeperframe.numerator = 1;
stream_params.parm.capture.timeperframe.denominator = config_->frame_rate();
if (xioctl(fd_, VIDIOC_S_PARM, &stream_params) < 0) {
AINFO << "Couldn't set camera framerate";
} else {
AINFO << "Set framerate to be " << config_->frame_rate();
}
switch (config_->io_method()) {
case IO_METHOD_READ:
init_read(fmt.fmt.pix.sizeimage);
break;
case IO_METHOD_MMAP:
init_mmap();
break;
case IO_METHOD_USERPTR:
init_userp(fmt.fmt.pix.sizeimage);
break;
case IO_METHOD_UNKNOWN:
AERROR << " does not support unknown i/o";
break;
}
// TODO: clearly return value, now exit when error
return true;
}
bool UsbCam::set_adv_trigger() {
AINFO << "Trigger enable, dev:" << config_->camera_dev() << ", fps:"
<< config_->trigger_fps() << ", internal:" << config_->trigger_internal();
int ret = adv_trigger_enable(config_->camera_dev().c_str(),
config_->trigger_fps(),
config_->trigger_internal());
if(ret != 0) {
AERROR << "trigger failed:" << ret;
return false;
}
return true;
}
int UsbCam::xioctl(int fd, int request, void * arg) {
int r;
do
r = ioctl(fd, request, arg);
while (-1 == r && EINTR == errno);
return r;
}
bool UsbCam::init_read(unsigned int buffer_size) {
buffers_ = (buffer*)calloc(1, sizeof(*buffers_));
if (!buffers_) {
AERROR << "Out of memory";
// exit(EXIT_FAILURE);
// reconnect();
return false;
}
buffers_[0].length = buffer_size;
buffers_[0].start = malloc(buffer_size);
if (!buffers_[0].start) {
AERROR << "Out of memory";
// exit(EXIT_FAILURE);
// reconnect();
return false;
}
return true;
}
bool UsbCam::init_mmap(void) {
struct v4l2_requestbuffers req;
CLEAR(req);
req.count = 1;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_MMAP;
if (-1 == xioctl(fd_, VIDIOC_REQBUFS, &req)) {
if (EINVAL == errno) {
AERROR << config_->camera_dev() << " does not support memory mapping";
// exit(EXIT_FAILURE);
// reconnect();
return false;
}
// errno_exit("VIDIOC_REQBUFS");
// reconnect();
return false;
}
//if (req.count < 2) {
// AERROR << "Insufficient buffer memory on " << config_->camera_dev();
// exit(EXIT_FAILURE);
//}
buffers_ = (buffer*)calloc(req.count, sizeof(*buffers_));
if (!buffers_) {
AERROR << "Out of memory";
// exit(EXIT_FAILURE);
// reconnect();
return false;
}
for (n_buffers_ = 0; n_buffers_ < req.count; ++n_buffers_) {
struct v4l2_buffer buf;
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = n_buffers_;
if (-1 == xioctl(fd_, VIDIOC_QUERYBUF, &buf)) {
// errno_exit("VIDIOC_QUERYBUF");
// reconnect();
AERROR << "VIDIOC_QUERYBUF";
return false;
}
buffers_[n_buffers_].length = buf.length;
buffers_[n_buffers_].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE,
MAP_SHARED, fd_, buf.m.offset);
if (MAP_FAILED == buffers_[n_buffers_].start) {
// errno_exit("mmap");
// reconnect();
AERROR << "mmap";
return false;
}
}
return true;
}
bool UsbCam::init_userp(unsigned int buffer_size) {
struct v4l2_requestbuffers req;
unsigned int page_size = 0;
page_size = getpagesize();
buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
CLEAR(req);
req.count = 4;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_USERPTR;
if (-1 == xioctl(fd_, VIDIOC_REQBUFS, &req)) {
if (EINVAL == errno) {
AERROR << config_->camera_dev() << " does not support "
"user pointer i/o";
// exit(EXIT_FAILURE);
// reconnect();
return false;
}
// errno_exit("VIDIOC_REQBUFS");
// reconnect();
AERROR << "VIDIOC_REQBUFS";
return false;
}
buffers_ = (buffer*)calloc(4, sizeof(*buffers_));
if (!buffers_) {
AERROR << "Out of memory";
// exit(EXIT_FAILURE);
// reconnect();
return false;
}
for (n_buffers_ = 0; n_buffers_ < 4; ++n_buffers_) {
buffers_[n_buffers_].length = buffer_size;
buffers_[n_buffers_].start =
memalign(/* boundary */ page_size, buffer_size);
if (!buffers_[n_buffers_].start) {
AERROR << "Out of memory";
// exit(EXIT_FAILURE);
// reconnect();
return false;
}
}
return true;
}
bool UsbCam::start_capturing(void) {
if (is_capturing_) {
return true;
}
unsigned int i = 0;
enum v4l2_buf_type type;
switch (config_->io_method()) {
case IO_METHOD_READ:
/* Nothing to do. */
break;
case IO_METHOD_MMAP:
for (i = 0; i < n_buffers_; ++i) {
struct v4l2_buffer buf;
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) {
// errno_exit("VIDIOC_QBUF");
// reconnect();
AERROR << "VIDIOC_QBUF";
return false;
}
}
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type)) {
// errno_exit("VIDIOC_STREAMON");
// reconnect();
AERROR << "VIDIOC_STREAMON";
return false;
}
break;
case IO_METHOD_USERPTR:
for (i = 0; i < n_buffers_; ++i) {
struct v4l2_buffer buf;
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_USERPTR;
buf.index = i;
buf.m.userptr = (unsigned long)buffers_[i].start;
buf.length = buffers_[i].length;
if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) {
// errno_exit("VIDIOC_QBUF");
// reconnect();
AERROR << "VIDIOC_QBUF";
return false;
}
}
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (-1 == xioctl(fd_, VIDIOC_STREAMON, &type)) {
// errno_exit("VIDIOC_STREAMON");
// reconnect();
AERROR << "VIDIOC_STREAMON";
return false;
}
break;
case IO_METHOD_UNKNOWN:
// errno_exit("unknown IO");
// reconnect();
AERROR << "unknown IO";
return false;
break;
}
is_capturing_ = true;
return true;
}
void UsbCam::set_device_config() {
if (config_->brightness() >= 0) {
set_v4l_parameter("brightness", config_->brightness());
}
if (config_->contrast() >= 0) {
set_v4l_parameter("contrast", config_->contrast());
}
if (config_->saturation() >= 0) {
set_v4l_parameter("saturation", config_->saturation());
}
if (config_->sharpness() >= 0) {
set_v4l_parameter("sharpness", config_->sharpness());
}
if (config_->gain() >= 0) {
set_v4l_parameter("gain", config_->gain());
}
// check auto white balance
if (config_->auto_white_balance()) {
set_v4l_parameter("white_balance_temperature_auto", 1);
} else {
set_v4l_parameter("white_balance_temperature_auto", 0);
set_v4l_parameter("white_balance_temperature", config_->white_balance());
}
// check auto exposure
if (!config_->auto_exposure()) {
// turn down exposure control (from max of 3)
set_v4l_parameter("auto_exposure", 1);
// change the exposure level
set_v4l_parameter("exposure_absolute", config_->exposure());
}
// check auto focus
if (config_->auto_focus()) {
set_auto_focus(1);
set_v4l_parameter("focus_auto", 1);
} else {
set_v4l_parameter("focus_auto", 0);
if (config_->focus() >= 0) {
set_v4l_parameter("focus_absolute", config_->focus());
}
}
}
bool UsbCam::uninit_device(void) {
unsigned int i = 0;
switch (config_->io_method()) {
case IO_METHOD_READ:
free(buffers_[0].start);
break;
case IO_METHOD_MMAP:
for (i = 0; i < n_buffers_; ++i) {
if (-1 == munmap(buffers_[i].start, buffers_[i].length)) {
// errno_exit("munmap");
AERROR << "munmap";
return false;
}
}
break;
case IO_METHOD_USERPTR:
for (i = 0; i < n_buffers_; ++i) {
free(buffers_[i].start);
}
break;
case IO_METHOD_UNKNOWN:
AERROR << "unknown IO";
break;
}
return true;
}
bool UsbCam::close_device(void) {
if (-1 == close(fd_)) {
// errno_exit("close");
// reconnect();
AERROR << "close";
return false;
}
fd_ = -1;
return true;
}
bool UsbCam::stop_capturing(void) {
if (!is_capturing_) {
return true;
}
is_capturing_ = false;
enum v4l2_buf_type type;
switch (config_->io_method()) {
case IO_METHOD_READ:
/* Nothing to do. */
break;
case IO_METHOD_MMAP:
case IO_METHOD_USERPTR:
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (-1 == xioctl(fd_, VIDIOC_STREAMOFF, &type)) {
// errno_exit("VIDIOC_STREAMOFF");
// reconnect();
AERROR << "VIDIOC_STREAMOFF";
return false;
}
break;
case IO_METHOD_UNKNOWN:
AERROR << "unknown IO";
return false;
break;
}
return true;
}
bool UsbCam::read_frame(CameraImagePtr raw_image) {
struct v4l2_buffer buf;
unsigned int i = 0;
int len = 0;
switch (config_->io_method()) {
case IO_METHOD_READ:
len = read(fd_, buffers_[0].start, buffers_[0].length);
if (len == -1) {
switch (errno) {
case EAGAIN:
AINFO << "EAGAIN";
return false;
case EIO:
/* Could ignore EIO, see spec. */
/* fall through */
default:
// errno_exit("read");
// reconnect();
AERROR << "read";
return false;
}
}
process_image(buffers_[0].start, len, raw_image);
break;
case IO_METHOD_MMAP:
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
if (-1 == xioctl(fd_, VIDIOC_DQBUF, &buf)) {
switch (errno) {
case EAGAIN:
return false;
case EIO:
/* Could ignore EIO, see spec. */
/* fall through */
default:
// errno_exit("VIDIOC_DQBUF");
// reconnect();
AERROR << "VIDIOC_DQBUF";
reconnect();
return false;
}
}
assert(buf.index < n_buffers_);
len = buf.bytesused;
raw_image->tv_sec = buf.timestamp.tv_sec;
raw_image->tv_usec = buf.timestamp.tv_usec;
{
cybertron::Time image_time(raw_image->tv_sec, 1000 * raw_image->tv_usec);
uint64_t camera_timestamp = image_time.ToNanosecond();
if (last_nsec_ == 0) {
last_nsec_ = camera_timestamp;
} else {
double diff = (camera_timestamp - last_nsec_) / 1e9;
// drop image by frame_rate
if (diff < frame_drop_interval_) {
AINFO << "drop image:" << camera_timestamp;
if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) {
AERROR << "VIDIOC_QBUF ERROR";
}
return false;
}
if (frame_warning_interval_ < diff) {
LOG_WARN << "stamp jump.last stamp:" << last_nsec_
<< " current stamp:" << camera_timestamp;
}
last_nsec_ = camera_timestamp;
}
double now_s = cybertron::Time::Now().ToSecond();
double image_s = camera_timestamp / 1e9;
double diff = now_s - image_s;
if (diff > 0.5 || diff < 0) {
std::stringstream warning_stream;
std::string warning_str;
warning_stream << "camera time diff exception,diff:"
<< diff << ";dev:" << config_->camera_dev();
warning_stream >> warning_str;
LOG_WARN << warning_str;
}
}
if (len < raw_image->width * raw_image->height) {
AERROR << "Wrong Buffer Len: " << len << ", dev: " << config_->camera_dev();
} else {
process_image(buffers_[buf.index].start, len, raw_image);
}
if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) {
// errno_exit("VIDIOC_QBUF");
// reconnect();
AERROR << "VIDIOC_QBUF";
return false;
}
break;
case IO_METHOD_USERPTR:
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_USERPTR;
if (-1 == xioctl(fd_, VIDIOC_DQBUF, &buf)) {
switch (errno) {
case EAGAIN:
return false;
case EIO:
/* Could ignore EIO, see spec. */
/* fall through */
default:
// errno_exit("VIDIOC_DQBUF");
// reconnect();
AERROR << "VIDIOC_DQBUF";
return false;
}
}
for (i = 0; i < n_buffers_; ++i) {
if (buf.m.userptr == (unsigned long)buffers_[i].start &&
buf.length == buffers_[i].length) {
break;
}
}
assert(i < n_buffers_);
len = buf.bytesused;
process_image((void*)buf.m.userptr, len, raw_image);
if (-1 == xioctl(fd_, VIDIOC_QBUF, &buf)) {
// errno_exit("VIDIOC_QBUF");
// reconnect();
AERROR << "VIDIOC_QBUF";
return false;
}
break;
case IO_METHOD_UNKNOWN:
AERROR << "unknown IO";
return false;
break;
}
return true;
}
bool UsbCam::process_image(const void* src, int len,
CameraImagePtr dest) {
if (src == NULL || dest == NULL) {
AERROR << "process image error. src or dest is null";
return false;
}
if (pixel_format_ == V4L2_PIX_FMT_YUYV || pixel_format_ == V4L2_PIX_FMT_UYVY) {
memcpy(dest->image, src, dest->width * dest->height * 2);
} else {
AERROR << "unsupported pixel format:" << pixel_format_;
return false;
}
return true;
}
bool UsbCam::is_capturing() { return is_capturing_; }
// enables/disables auto focus
void UsbCam::set_auto_focus(int value) {
struct v4l2_queryctrl queryctrl;
struct v4l2_ext_control control;
memset(&queryctrl, 0, sizeof(queryctrl));
queryctrl.id = V4L2_CID_FOCUS_AUTO;
if (-1 == xioctl(fd_, VIDIOC_QUERYCTRL, &queryctrl)) {
if (errno != EINVAL) {
perror("VIDIOC_QUERYCTRL");
return;
} else {
AINFO << "V4L2_CID_FOCUS_AUTO is not supported";
return;
}
} else if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
AINFO << "V4L2_CID_FOCUS_AUTO is not supported";
return;
} else {
memset(&control, 0, sizeof(control));
control.id = V4L2_CID_FOCUS_AUTO;
control.value = value;
if (-1 == xioctl(fd_, VIDIOC_S_CTRL, &control)) {
perror("VIDIOC_S_CTRL");
return;
}
}
}
/**
* Set video device parameter via call to v4l-utils.
*
* @param param The name of the parameter to set
* @param param The value to assign
*/
void UsbCam::set_v4l_parameter(const std::string& param, int value) {
set_v4l_parameter(param, std::to_string(value));
}
/**
* Set video device parameter via call to v4l-utils.
*
* @param param The name of the parameter to set
* @param param The value to assign
*/
void UsbCam::set_v4l_parameter(const std::string& param,
const std::string& value) {
// build the command
std::stringstream ss;
ss << "v4l2-ctl --device=" << config_->camera_dev() << " -c " << param << "="
<< value << " 2>&1";
std::string cmd = ss.str();
// capture the output
std::string output;
int buffer_size = 256;
char buffer[buffer_size];
FILE* stream = popen(cmd.c_str(), "r");
if (stream) {
while (!feof(stream)) {
if (fgets(buffer, buffer_size, stream) != NULL) {
output.append(buffer);
}
}
pclose(stream);
// any output should be an error
if (output.length() > 0) {
AERROR << output.c_str();
}
} else {
AERROR << "usb_cam_node could not run " << cmd.c_str();
}
}
bool UsbCam::wait_for_device() {
if (is_capturing_) {
AINFO << "is capturing";
return true;
}
if (!open_device()) {
return false;
}
if (!init_device()) {
close_device();
return false;
}
if (!start_capturing()) {
uninit_device();
close_device();
return false;
}
if (!set_adv_trigger()) {
stop_capturing();
uninit_device();
close_device();
return false;
}
return true;
}
void UsbCam::reconnect() {
stop_capturing();
uninit_device();
close_device();
}
} // namespace du_driver
} // namespace caros
} // namespace adu
modules/drivers/usb_cam/include/usb_cam/usb_cam.h → modules/drivers/usb_cam/usb_cam.h
浏览文件 @ ec22443b
......@@ -33,130 +33,140 @@
* POSSIBILITY OF SUCH DAMAGE.
*
*********************************************************************/
#ifndef USB_CAM_USB_CAM_H
#define USB_CAM_USB_CAM_H
#ifndef MODULES_DRIVERS_USB_CAM_USB_CAM_H_
#define MODULES_DRIVERS_USB_CAM_USB_CAM_H_
#include <asm/types.h> /* for videodev2.h */
#include <memory>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <malloc.h>
#include <immintrin.h>
#include <x86intrin.h>
extern "C" {
#include <linux/videodev2.h>
#include <libavcodec/avcodec.h>
#include <libavutil/mem.h>
#include <libswscale/swscale.h>
#include <linux/usb/video.h>
#include <linux/uvcvideo.h>
#include <linux/videodev2.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 <sstream>
#include <string>
#include <sstream>
#include <sensor_msgs/Image.h>
namespace usb_cam {
class UsbCam {
public:
typedef enum {
IO_METHOD_MMAP,
IO_METHOD_UNKNOWN,
} io_method;
typedef enum {
PIXEL_FORMAT_YUYV,
PIXEL_FORMAT_UYVY,
PIXEL_FORMAT_MJPEG,
PIXEL_FORMAT_YUVMONO10,
PIXEL_FORMAT_RGB24,
PIXEL_FORMAT_UNKNOWN
} pixel_format;
#include "cybertron/cybertron.h"
UsbCam();
~UsbCam();
#include "modules/drivers/usb_cam/proto/config.pb.h"
// 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);
namespace apollo {
namespace drivers {
namespace usb_cam {
// grabs a new image from the camera
bool grab_image(sensor_msgs::Image *image, int timeout);
using apollo::drivers::usb_cam::config::Config;
using apollo::drivers::usb_cam::config::IO_METHOD_READ;
using apollo::drivers::usb_cam::config::IO_METHOD_MMAP;
using apollo::drivers::usb_cam::config::IO_METHOD_USERPTR;
using apollo::drivers::usb_cam::config::IO_METHOD_UNKNOWN;
// camera raw image struct
struct CameraImage {
int width;
int height;
int bytes_per_pixel;
int image_size;
int is_new;
int tv_sec;
int tv_usec;
char* image;
~CameraImage() {
if (image != nullptr) {
free((void*)image);
image = nullptr;
}
}
};
// enables/disable auto focus
void set_auto_focus(int value);
typedef std::shared_ptr<CameraImage> CameraImagePtr;
// 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);
struct buffer {
void* start;
size_t length;
};
// enable video triggers //see libadv_trigger_ctl
int trigger_enable(unsigned char fps, unsigned char internal);
int trigger_disable();
class UsbCam {
public:
UsbCam();
virtual ~UsbCam();
static io_method io_method_from_string(const std::string &str);
static pixel_format pixel_format_from_string(const std::string &str);
virtual bool init(
std::shared_ptr<Config>& camera_config);
// user use this function to get camera frame data
virtual bool poll(const CameraImagePtr& raw_image);
void stop_capturing(void);
void start_capturing(void);
bool is_capturing();
bool wait_for_device(void);
private:
struct CameraImage {
int width;
int height;
int bytes_per_pixel;
int image_size;
char *image;
int is_new;
int tv_sec;
int tv_usec;
};
struct Buffer {
void *start;
size_t length;
};
int xioctl(int fd, int request, void * arg);
bool init_device(void);
bool uninit_device(void);
int init_mjpeg_decoder(int image_width, int image_height);
void mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels);
bool process_image(const void *src, int len,
boost::shared_ptr<CameraImage> 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);
// TODO
// void reset_device(void);
bool grab_image(int timeout);
void set_device_config();
// 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);
bool is_capturing_;
std::string camera_dev_;
unsigned int pixelformat_;
bool monochrome_;
io_method io_;
int init_mjpeg_decoder(int image_width, int image_height);
void mjpeg2rgb(char* mjepg_buffer, int len, char* rgb_buffer, int pixels);
bool init_read(unsigned int buffer_size);
bool init_mmap(void);
bool init_userp(unsigned int buffer_size);
bool set_adv_trigger(void);
bool close_device(void);
bool open_device(void);
bool read_frame(CameraImagePtr raw_image);
bool process_image(const void* src, int len, CameraImagePtr dest);
bool start_capturing(void);
bool stop_capturing(void);
void reconnect();
void reset_device();
std::shared_ptr<Config> config_;
int pixel_format_;
int fd_;
std::vector<Buffer> buffers_;
buffer* buffers_;
unsigned int n_buffers_;
AVFrame *avframe_camera_;
AVFrame *avframe_rgb_;
AVCodec *avcodec_;
AVDictionary *avoptions_;
AVCodecContext *avcodec_context_;
bool is_capturing_;
long image_seq_;
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_;
boost::shared_ptr<CameraImage> image_;
struct SwsContext* video_sws_;
float frame_warning_interval_;
float device_wait_sec_;
uint64_t last_nsec_;
float frame_drop_interval_;
};
}
}
}
#endif
#endif // ONBOARD_DRIVERS_USB_CAMERA_INCLUDE_USB_CAMERA_CAMERA_DEVICE_H
modules/drivers/usb_cam/usb_cam_component.cc 0 → 100644
浏览文件 @ ec22443b
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/drivers/usb_cam/usb_cam_component.h"
namespace apollo {
namespace drivers {
namespace usb_cam {
bool UsbCamComponent::Init() {
camera_config_ = std::make_shared<Config>();
if(!apollo::cybertron::common::GetProtoFromFile(config_file_path_, camera_config_.get())){
return false;
}
AINFO << "UsbCam config: " << camera_config_->DebugString();
camera_device_.reset(new UsbCam());
camera_device_->init(camera_config_);
raw_image_.reset(new CameraImage);
raw_image_->width = camera_config_->width();
raw_image_->height = camera_config_->height();
raw_image_->bytes_per_pixel = camera_config_->bytes_per_pixel();
device_wait_ = camera_config_->device_wait();
spin_rate_ = camera_config_->spin_rate();
raw_image_->image_size =
raw_image_->width * raw_image_->height * raw_image_->bytes_per_pixel;
raw_image_->is_new = 0;
// free memory in this struct desturctor
raw_image_->image = (char*)calloc(raw_image_->image_size, sizeof(char));
pb_image_.reset(new Image);
pb_image_->mutable_header()->set_frame_id(camera_config_->frame_id());
pb_image_->set_encoding("yuyv");
pb_image_->set_width(raw_image_->width);
pb_image_->set_height(raw_image_->height);
pb_image_->set_step(2 * raw_image_->width);
pb_image_->mutable_data()->reserve(raw_image_->image_size);
writer_ = node_->CreateWriter<Image>(camera_config_->channel_name());
device_thread_ = std::shared_ptr<std::thread>(
new std::thread(std::bind(&UsbCamComponent::run, this)));
device_thread_->detach();
return true;
}
void UsbCamComponent::run() {
while (!cybertron::IsShutdown()) {
if (!camera_device_->wait_for_device()) {
//sleep 2s for next check
sleep(device_wait_);
continue;
}
if (!camera_device_->poll(raw_image_)) {
LOG_ERROR << "camera device poll failed";
continue;
}
cybertron::Time image_time(raw_image_->tv_sec, 1000 * raw_image_->tv_usec);
pb_image_->mutable_header()->set_timestamp_sec(cybertron::Time::Now().ToSecond());
pb_image_->set_measurement_time(image_time.ToSecond());
pb_image_->set_data(raw_image_->image, raw_image_->image_size);
writer_->Write(pb_image_);
sleep(spin_rate_);
}
}
}
}
} // namespace cybertron
modules/drivers/usb_cam/usb_cam_component.h 0 → 100644
浏览文件 @ ec22443b
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#ifndef MODULES_DRIVERS_USB_CAM_USB_CAM_COMPONENT_H
#define MODULES_DRIVERS_USB_CAM_USB_CAM_COMPONENT_H
#include <memory>
#include "cybertron/cybertron.h"
#include "modules/drivers/usb_cam/proto/config.pb.h"
#include "modules/drivers/proto/sensor_image.pb.h"
#include "modules/drivers/usb_cam/usb_cam.h"
namespace apollo {
namespace drivers {
namespace usb_cam {
using apollo::drivers::usb_cam::config::Config;
using apollo::drivers::Image;
using apollo::cybertron::Component;
using apollo::cybertron::Writer;
using apollo::cybertron::Reader;
class UsbCamComponent : public Component<> {
public:
bool Init() override;
private:
void run();
std::shared_ptr<Writer<Image>> writer_ = nullptr;
std::unique_ptr<UsbCam> camera_device_;
std::shared_ptr<Config> camera_config_;
CameraImagePtr raw_image_ = nullptr;
std::shared_ptr<Image> pb_image_ = nullptr;
std::shared_ptr<std::thread> device_thread_;
float spin_rate_ = 0.005;
float device_wait_ = 2.0;
};
CYBERTRON_REGISTER_COMPONENT(UsbCamComponent)
}
}
} // namespace cybertron
#endif // ONBOARD_DRIVERS_USB_CAMERA_INCLUDE_USB_CAMERA_COMPONENT_H
third_party/adv_plat.BUILD
浏览文件 @ ec22443b
......@@ -5,8 +5,8 @@ licenses(["notice"])
cc_library(
name = "adv_plat",
srcs = [
"lib/libadv_bcan.a",
"lib/libadv_trigger.a",
"lib/libadv_bcan.so.1",
"lib/libadv_trigger.so.1",
],
hdrs = [
"include/adv_trigger.h",
......@@ -18,3 +18,4 @@ cc_library(
linkopts = [
],
)
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或