Skip to content
Permalink
Browse files

Repalce custom service with dynamic reconfigure, minor cleanups

  • Loading branch information...
luca-della-vedova committed Sep 9, 2019
1 parent a4f6d0b commit e112ea495f229d821e76c2580b4930c1d0e8ddf9
@@ -5,17 +5,16 @@ add_compile_options(-std=c++14 -Wall -Wpedantic)
# For debugging
# add_compile_options(-std=c++14 -fsanitize=address)

find_package(catkin REQUIRED COMPONENTS roscpp image_transport message_generation camera_info_manager)
find_package(catkin REQUIRED COMPONENTS roscpp image_transport message_generation camera_info_manager dynamic_reconfigure)

add_message_files(
FILES
ImageCorner.msg
Metadata.msg
)

add_service_files(
FILES
ConfigureFAST.srv
generate_dynamic_reconfigure_options(
cfg/Ovc3.cfg
)

generate_messages(
@@ -46,6 +45,7 @@ add_library(${PROJECT_NAME}
add_dependencies(${PROJECT_NAME} ovc_embedded_driver_generate_messages_cpp)

add_executable(${PROJECT_NAME}_node src/ovc_embedded_driver_node.cpp)
add_dependencies(${PROJECT_NAME}_node ${PROJECT_NAME}_gencfg)
add_dependencies(${PROJECT_NAME}_node ovc_embedded_driver_generate_messages_cpp)
add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME}
@@ -0,0 +1,11 @@
#!/usr/bin/env python
PACKAGE = "ovc_embedded_driver"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("fast_enable", bool_t, 0, "Enable FAST corner detection", True)
gen.add("fast_threshold", int_t, 0, "Threshold for FAST corner detection", 50, 0, 255)

exit(gen.generate(PACKAGE, "ovc_embedded_driver", "Ovc3"))
@@ -6,7 +6,8 @@
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>

#include <ovc_embedded_driver/ConfigureFAST.h>
#include <dynamic_reconfigure/server.h>
#include <ovc_embedded_driver/Ovc3Config.h>
#include <ovc_embedded_driver/sensor_constants.h>
#include <ovc_embedded_driver/utilities.h>
#include <ovc_embedded_driver/image_publisher.h>
@@ -96,6 +97,7 @@ void publish_vnav(ros::NodeHandle nh, std::shared_ptr<AtomicRosTime> time_ptr)
VNAVDriver spi(VECTORNAV_SPI_DEVICE, VECTORNAV_FSYNC_GPIO);
ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("vectornav", 10);
sensor_msgs::Imu imu_msg;
imu_msg.header.frame_id = "ovc_vnav_link";
while (ros::ok())
{
// Vectornav is not synchronised to anything, just use system time to stamp it
@@ -115,10 +117,7 @@ void publish_vnav(ros::NodeHandle nh, std::shared_ptr<AtomicRosTime> time_ptr)
imu_msg.linear_acceleration.x = imu.a_x;
imu_msg.linear_acceleration.y = imu.a_y;
imu_msg.linear_acceleration.z = imu.a_z;
// Sample synchronised with frame trigger
imu_pub.publish(imu_msg);
//if (imu.num_sample == 0)
// time_ptr->update();
}
}

@@ -130,10 +129,10 @@ void configureFAST(bool enable, int thresh)
uio.writeRegister(0, write_val);
}

bool configureFAST_cb(ConfigureFAST::Request &req, ConfigureFAST::Response &res)
void reconfigure_callback(Ovc3Config &conf, uint32_t level)
{
configureFAST(req.enable, req.threshold);
return true;
// TODO add more reconfigurability
configureFAST(conf.fast_enable, conf.fast_threshold);
}

int main(int argc, char **argv)
@@ -144,11 +143,12 @@ int main(int argc, char **argv)

std::shared_ptr<AtomicRosTime> curr_time_ptr = std::make_shared<AtomicRosTime>();
std::shared_ptr<AtomicRosTime> frame_time_ptr = std::make_shared<AtomicRosTime>();
ros::ServiceServer fast_serv = nh.advertiseService("configure_fast", configureFAST_cb);
dynamic_reconfigure::Server<Ovc3Config> server;
dynamic_reconfigure::Server<Ovc3Config>::CallbackType c;
c = boost::bind(&reconfigure_callback, _1, _2);
server.setCallback(c);
ros::AsyncSpinner spinner(1);

configureFAST(1, 60);

// Init threads
std::unique_ptr<std::thread> threads[NUM_CAMERAS + 3]; // one each for IMU and frame_time_ptr update threads
std::unique_ptr<ImagePublisher> image_publisher[NUM_CAMERAS];
@@ -159,7 +159,7 @@ int main(int argc, char **argv)
// Time object update thread
threads[NUM_CAMERAS + 1] = std::make_unique<std::thread>(update_time_ptr, nh, frame_time_ptr, curr_time_ptr);

threads[NUM_CAMERAS+2] = std::make_unique<std::thread>(publish_vnav, nh, curr_time_ptr);
threads[NUM_CAMERAS + 2] = std::make_unique<std::thread>(publish_vnav, nh, curr_time_ptr);

// Image Publisher threads
for (size_t i=0; i<NUM_CAMERAS; ++i)
@@ -175,8 +175,8 @@ int main(int argc, char **argv)

spinner.start();

// TODO refactor thread vector
// Wait for each thread to end
threads[NUM_CAMERAS]->join();

//publish_vnav(nh, time_ptr);
threads[NUM_CAMERAS+1]->join();
return 0;
}
@@ -160,7 +160,7 @@ void VNAVDriver::WriteRegister(uint8_t reg_addr, uint8_t* payload, size_t len)
tx_buf[1] = reg_addr;
tx_buf[2] = 0;
tx_buf[3] = 0;
for (int i=0; i<len; ++i)
for (size_t i=0; i<len; ++i)
tx_buf[4 + i] = payload[i];
Transmit(4 + len, 0);
}
@@ -176,6 +176,7 @@ void VNAVDriver::SetupRead(int reg_addr)

void VNAVDriver::waitNewSample()
{
// Poll falling edge interrupt on GPIO
struct pollfd pfd;
int tmp;
pfd.fd = gpio_fd;

This file was deleted.

0 comments on commit e112ea4

Please sign in to comment.
You can’t perform that action at this time.