Skip to content

Commit

Permalink
fixing lint errors
Browse files Browse the repository at this point in the history
  • Loading branch information
flynneva committed Mar 24, 2021
1 parent 56b7a35 commit 50bc2fd
Show file tree
Hide file tree
Showing 4 changed files with 194 additions and 189 deletions.
35 changes: 24 additions & 11 deletions include/usb_cam/usb_cam.h
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,8 @@
* POSSIBILITY OF SUCH DAMAGE.
*
*********************************************************************/
#ifndef USB_CAM_USB_CAM_H
#define USB_CAM_USB_CAM_H
#ifndef USB_CAM__USB_CAM_H_
#define USB_CAM__USB_CAM_H_

#include <asm/types.h> /* for videodev2.h */

Expand All @@ -53,9 +53,11 @@ extern "C"
#endif

#include <builtin_interfaces/msg/time.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/time.hpp>
// #include <sensor_msgs/msg/image.h>
#include <string>
#include <vector>
#include <sstream>

#define ROS_INFO(msg, ...) printf(msg, ##__VA_ARGS__)
Expand All @@ -65,9 +67,12 @@ extern "C"
// #define ROS_DEBUG(msg, ...) // ##__VA_ARGS__
#define __FILENAME__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__)

#define INFO(msg) std::cout << "I [" << __FILENAME__ << ":" << __LINE__ << " " << __FUNCTION__ << "] " << msg << "\n"
#define WARN(msg) std::cout << "W [" << __FILENAME__ << ":" << __LINE__ << " " << __FUNCTION__ << "] " << msg << "\n"
#define ERROR(msg) std::cerr << "E [" << __FILENAME__ << ":" << __LINE__ << " " << __FUNCTION__ << "] " << msg << std::endl
#define INFO(msg) std::cout << "I [" << __FILENAME__ << ":" << __LINE__ \
<< " " << __FUNCTION__ << "] " << msg << "\n"
#define WARN(msg) std::cout << "W [" << __FILENAME__ << ":" << __LINE__ \
<< " " << __FUNCTION__ << "] " << msg << "\n"
#define ERROR(msg) std::cerr << "E [" << __FILENAME__ << ":" << __LINE__ \
<< " " << __FUNCTION__ << "] " << msg << std::endl
#define ROS_ERROR_STREAM(msg) ERROR(msg)

namespace usb_cam {
Expand All @@ -76,20 +81,29 @@ class UsbCam {
public:
typedef enum
{
IO_METHOD_READ, IO_METHOD_MMAP, IO_METHOD_USERPTR, IO_METHOD_UNKNOWN,
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_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
bool start(const std::string& dev, io_method io, pixel_format pf,
int image_width, int image_height, int framerate);
int image_width, int image_height, int framerate);
// shutdown camera
bool shutdown(void);

Expand Down Expand Up @@ -167,10 +181,9 @@ class UsbCam {
int avframe_rgb_size_;
struct SwsContext *video_sws_;
camera_image_t *image_;

};

}
} // namespace usb_cam

#endif
#endif // USB_CAM__USB_CAM_H_

30 changes: 14 additions & 16 deletions nodes/usb_cam_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,6 @@
* POSSIBILITY OF SUCH DAMAGE.
*
*********************************************************************/

#include <rclcpp/rclcpp.hpp>
#include <usb_cam/usb_cam.h>
// #include <image_transport/image_transport.h>
Expand All @@ -42,6 +41,9 @@
#include <sstream>
// #include <std_srvs/srv/Empty.h>

#include <string>
#include <memory>

std::ostream& operator<<(std::ostream& ostr, const rclcpp::Time& tm) {
ostr << tm.nanoseconds();
return ostr;
Expand Down Expand Up @@ -132,17 +134,17 @@ class UsbCamNode : public rclcpp::Node
get_parameter_or("image_height", image_height_, image_height_);

#if 0
node_.param("brightness", brightness_, -1); //0-255, -1 "leave alone"
node_.param("contrast", contrast_, -1); //0-255, -1 "leave alone"
node_.param("saturation", saturation_, -1); //0-255, -1 "leave alone"
node_.param("sharpness", sharpness_, -1); //0-255, -1 "leave alone"
node_.param("brightness", brightness_, -1); // 0-255, -1 "leave alone"
node_.param("contrast", contrast_, -1); // 0-255, -1 "leave alone"
node_.param("saturation", saturation_, -1); // 0-255, -1 "leave alone"
node_.param("sharpness", sharpness_, -1); // 0-255, -1 "leave alone"
// enable/disable autofocus
node_.param("autofocus", autofocus_, false);
node_.param("focus", focus_, -1); //0-255, -1 "leave alone"
node_.param("focus", focus_, -1); // 0-255, -1 "leave alone"
// enable/disable autoexposure
node_.param("autoexposure", autoexposure_, true);
node_.param("exposure", exposure_, 100);
node_.param("gain", gain_, -1); //0-100?, -1 "leave alone"
node_.param("gain", gain_, -1); // 0-100?, -1 "leave alone"
// enable/disable auto white balance temperature
node_.param("auto_white_balance", auto_white_balance_, true);
node_.param("white_balance", white_balance_, 4000);
Expand Down Expand Up @@ -197,7 +199,7 @@ class UsbCamNode : public rclcpp::Node

// start the camera
cam_.start(video_device_name_.c_str(), io_method, pixel_format, image_width_,
image_height_, framerate_);
image_height_, framerate_);

cam_.get_formats();

Expand Down Expand Up @@ -232,9 +234,7 @@ class UsbCamNode : public rclcpp::Node
if (auto_white_balance_)
{
cam_.set_v4l_parameter("white_balance_temperature_auto", 1);
}
else
{
} else {
cam_.set_v4l_parameter("white_balance_temperature_auto", 0);
cam_.set_v4l_parameter("white_balance_temperature", white_balance_);
}
Expand All @@ -253,9 +253,7 @@ class UsbCamNode : public rclcpp::Node
{
cam_.set_auto_focus(1);
cam_.set_v4l_parameter("focus_auto", 1);
}
else
{
} else {
cam_.set_v4l_parameter("focus_auto", 0);
if (focus_ >= 0)
{
Expand All @@ -268,7 +266,7 @@ class UsbCamNode : public rclcpp::Node
// TODO(lucasw) how to do small than ms, or fractional ms- std::chrono::nanoseconds?
const int period_ms = 1000.0 / framerate_;
timer_ = this->create_wall_timer(
std::chrono::milliseconds(static_cast<long int>(period_ms)),
std::chrono::milliseconds(static_cast<int64_t>(period_ms)),
std::bind(&UsbCamNode::update, this));
INFO("starting timer " << period_ms);
}
Expand Down Expand Up @@ -318,7 +316,7 @@ class UsbCamNode : public rclcpp::Node
}
};

}
} // namespace usb_cam

int main(int argc, char **argv)
{
Expand Down
4 changes: 4 additions & 0 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,10 @@

<buildtool_depend>ament_cmake</buildtool_depend>

<depend>ffmpeg</depend>
<depend>libboost-dev</depend>
<depend>libboost-lexical-cast-dev</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

Expand Down
Loading

0 comments on commit 50bc2fd

Please sign in to comment.