Skip to content
Snippets Groups Projects
Commit 063a9825 authored by Paul Beuchat's avatar Paul Beuchat
Browse files

Added template for an encoder counter ROS node.

parent 3bd787ae
Branches
No related tags found
1 merge request!15Added template for an encoder counter ROS node.
Hide whitespace changes
Inline Side-by-side
catkin_ws/src/asclinic_pkg/CMakeLists.txt
+ 30
− 26
View file @ 063a9825
......@@ -58,8 +58,9 @@ find_package(catkin REQUIRED COMPONENTS
add_message_files(
FILES
TemplateMessage.msg
ServoPulseWidth.msg
TemplateMessage.msg
LeftAndRightInt.msg
)
## Generate services in the 'srv' folder
......@@ -159,19 +160,20 @@ include_directories(
# add_executable(${PROJECT_NAME}_node src/asclinic_node.cpp)
# TEMPLATE CPP NODE:
add_executable(template_cpp_node src/nodes/template_cpp_node.cpp)
add_executable(template_cpp_node_minimal src/nodes/template_cpp_node_minimal.cpp)
add_executable(template_gpio_event_triggered src/nodes/template_gpio_event_triggered.cpp)
add_executable(template_gpio_polling src/nodes/template_gpio_polling.cpp)
add_executable(template_i2c_internal src/nodes/template_i2c_internal.cpp
src/drivers/src/i2c_driver/i2c_driver.cpp
src/drivers/src/pololu_smc_g2/pololu_smc_g2.cpp
src/drivers/src/pca9685/pca9685.cpp)
add_executable(template_i2c_external src/nodes/template_i2c_external.cpp
src/drivers/src/i2c_driver/i2c_driver.cpp
src/drivers/src/vl53l1x/vl53l1x.cpp
src/drivers/src/vl53l1x/core/VL53L1X_api.c
src/drivers/src/vl53l1x/platform/vl53l1_platform.c)
add_executable(template_cpp_node src/nodes/template_cpp_node.cpp)
add_executable(template_cpp_node_minimal src/nodes/template_cpp_node_minimal.cpp)
add_executable(template_gpio_event_triggered src/nodes/template_gpio_event_triggered.cpp)
add_executable(template_gpio_polling src/nodes/template_gpio_polling.cpp)
add_executable(template_encoder_read_multi_threaded src/nodes/template_encoder_read_multi_threaded.cpp)
add_executable(template_i2c_internal src/nodes/template_i2c_internal.cpp
src/drivers/src/i2c_driver/i2c_driver.cpp
src/drivers/src/pololu_smc_g2/pololu_smc_g2.cpp
src/drivers/src/pca9685/pca9685.cpp)
add_executable(template_i2c_external src/nodes/template_i2c_external.cpp
src/drivers/src/i2c_driver/i2c_driver.cpp
src/drivers/src/vl53l1x/vl53l1x.cpp
src/drivers/src/vl53l1x/core/VL53L1X_api.c
src/drivers/src/vl53l1x/platform/vl53l1_platform.c)
## Rename C++ executable without prefix
......@@ -185,12 +187,13 @@ add_executable(template_i2c_external src/nodes/template_i2c_external.cp
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
# TEMPLATE CPP NODE:
add_dependencies(template_cpp_node asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(template_cpp_node_minimal asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(template_gpio_event_triggered asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(template_gpio_polling asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(template_i2c_internal asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(template_i2c_external asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(template_cpp_node asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(template_cpp_node_minimal asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(template_gpio_event_triggered asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(template_gpio_polling asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(template_encoder_read_multi_threaded asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(template_i2c_internal asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(template_i2c_external asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
......@@ -199,12 +202,13 @@ add_dependencies(template_i2c_external asclinic_pkg_generate_messages_c
# )
# TEMPLATE CPP NODE
target_link_libraries(template_cpp_node ${catkin_LIBRARIES})
target_link_libraries(template_cpp_node_minimal ${catkin_LIBRARIES})
target_link_libraries(template_gpio_event_triggered ${catkin_LIBRARIES} -lgpiod)
target_link_libraries(template_gpio_polling ${catkin_LIBRARIES} -lgpiod)
target_link_libraries(template_i2c_internal ${catkin_LIBRARIES})
target_link_libraries(template_i2c_external ${catkin_LIBRARIES})
target_link_libraries(template_cpp_node ${catkin_LIBRARIES})
target_link_libraries(template_cpp_node_minimal ${catkin_LIBRARIES})
target_link_libraries(template_gpio_event_triggered ${catkin_LIBRARIES} -lgpiod)
target_link_libraries(template_gpio_polling ${catkin_LIBRARIES} -lgpiod)
target_link_libraries(template_encoder_read_multi_threaded ${catkin_LIBRARIES} -lgpiod)
target_link_libraries(template_i2c_internal ${catkin_LIBRARIES})
target_link_libraries(template_i2c_external ${catkin_LIBRARIES})
#############
......
catkin_ws/src/asclinic_pkg/launch/template_encoder.launch 0 → 100644
+ 49
− 0
View file @ 063a9825
<launch>
<!-- NOTE -->
<!-- Two nodes canNOT access the same GPIO line -->
<!-- The first node to open a line blocks other processes from accessing it -->
<!-- START A GROUP WITH NAMESPACE "template" -->
<group ns="ns_for_group">
<!-- LAUNCH A "Template Encoder Read Multi Threaded" NODE -->
<node
pkg = "asclinic_pkg"
name = "template_encoder_read_multi_threaded"
output = "screen"
type = "template_encoder_read_multi_threaded"
>
<param
name = "line_number_for_motor_left_channel_a"
value = "133"
/>
<param
name = "line_number_for_motor_right_channel_a"
value = "105"
/>
<param
name = "delta_t_for_publishing_counts"
value = "0.1"
/>
<!-- NOTE: The following parameters are purely for the convenience of testing. -->
<param
name = "time_in_seconds_to_drive_motors"
value = "10.0"
/>
<param
name = "drive_motor_target_speed"
value = "30"
/>
</node>
<!-- LAUNCH A "Template I2C Internal" NODE -->
<node
pkg = "asclinic_pkg"
name = "template_i2c_internal"
output = "screen"
type = "template_i2c_internal"
/>
</group>
</launch>
catkin_ws/src/asclinic_pkg/msg/LeftAndRightInt.msg 0 → 100644
+ 2
− 0
View file @ 063a9825
int32 left
int32 right
catkin_ws/src/asclinic_pkg/src/drivers/test/test_cpp_conventions.cpp 0 → 100644
+ 32
− 0
View file @ 063a9825
#include <stdint.h>
#include <stdio.h>
#include <unistd.h>
#include <cmath>
int main()
{
printf("\nStatic int cast\n");
float a_float = 0.7;
int a_int_cast = static_cast<int>(a_float);
int a_int_cast_round = static_cast<int>(std::round(a_float));
float b_float = -0.2;
int b_int_cast = static_cast<int>(b_float);
int b_int_cast_round = static_cast<int>(std::round(b_float));
float c_float = -0.7;
int c_int_cast = static_cast<int>(c_float);
int c_int_cast_round = static_cast<int>(std::round(c_float));
printf("a_float = %f, a_int_cast = %d, a_int_cast_round = %d\n", a_float, a_int_cast, a_int_cast_round);
printf("b_float = %f, b_int_cast = %d, b_int_cast_round = %d\n", b_float, b_int_cast, b_int_cast_round);
printf("c_float = %f, c_int_cast = %d, c_int_cast_round = %d\n", c_float, c_int_cast, c_int_cast_round);
// Return
return 0;
}
catkin_ws/src/asclinic_pkg/src/nodes/template_encoder_read_multi_threaded.cpp 0 → 100644
+ 344
− 0
View file @ 063a9825
// Copyright (C) 2021, The University of Melbourne, Department of Electrical and Electronic Engineering (EEE)
//
// This file is part of ASClinic-System.
//
// See the root of the repository for license details.
//
// ----------------------------------------------------------------------------
// _ ____ ____ _ _ _ ____ _
// / \ / ___| / ___| (_)____ (_) ___ / ___| _ _ ___| |_ ___ ________
// / _ \ \___ \| | | | | _ \| |/ __|___\___ \| | | / __| __/ _ \ _ _ \
// / ___ \ ___) | |___| | | | | | | (_|_____|__) | |_| \__ \ || __/ | | | | |
// /_/ \_\____/ \____|_|_|_| |_|_|\___| |____/ \__, |___/\__\___|_| |_| |_|
// |___/
//
// DESCRIPTION:
// Template node for mointoring edge events on a GPIO pin
//
// ----------------------------------------------------------------------------
#include "ros/ros.h"
#include <ros/package.h>
#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"
#include <gpiod.h>
// Includes required for threading
// > Mutex for handling shared variable
#include <mutex>
// > Include only one of the following two options:
#include <thread>
//#include <boost/thread/thread.hpp>
// Include the asclinic message types
#include "asclinic_pkg/LeftAndRightInt.h"
// Namespacing the package
using namespace asclinic_pkg;
// MEMBER VARIABLES FOR THIS NODE:
// > Publisher and timer for the current
// encoder counts
ros::Publisher m_encoder_counts_publisher;
ros::Timer m_timer_for_publishing;
// > For sharing the encoder counts between nodes
int m_encoder_counts_for_motor_left = 0;
int m_encoder_counts_for_motor_right = 0;
// > Mutex for preventing multiple-access of shared variables
std::mutex m_counting_mutex;
// > The line numbers to read
int m_line_number_for_motor_left_channel_a = 133;
int m_line_number_for_motor_right_channel_a = 105;
// > The "delta t" used for the frequency of publishing encoder counts
float m_delta_t_for_publishing_counts = 0.1;
// > NOTE: The following variables are purely for
// the convenience of testing.
float m_time_in_seconds_to_drive_motors = 1.0;
ros::Publisher m_motor_pwm_publisher;
float m_drive_motor_target_speed = 20.0;
// Respond to timer callback
void timerCallbackForPublishing(const ros::TimerEvent&)
{
// Get the current counts into a local variable
// > And reset the shared counts variable to zero
int counts_motor_left_local_copy;
int counts_motor_right_local_copy;
m_counting_mutex.lock();
counts_motor_left_local_copy = m_encoder_counts_for_motor_left;
counts_motor_right_local_copy = m_encoder_counts_for_motor_right;
m_encoder_counts_for_motor_left = 0;
m_encoder_counts_for_motor_right = 0;
m_counting_mutex.unlock();
// Publish a message
LeftAndRightInt msg;
msg.left = counts_motor_left_local_copy;
msg.right = counts_motor_right_local_copy;
m_encoder_counts_publisher.publish(msg);
// NOTE: the remainder of this function is
// purely for the convenience of testing.
static bool did_start_motors = false;
static bool did_finish_test = false;
static bool did_display_cum_sum = false;
static float elapsed_time_in_seconds = 0.0;
static int cum_sum_left = 0;
static int cum_sum_right = 0;
// Add the counts
cum_sum_left += counts_motor_left_local_copy;
cum_sum_right += counts_motor_right_local_copy;// Increment the time
// Increment the time
elapsed_time_in_seconds += m_delta_t_for_publishing_counts;
// Start the motors after a few seconds
if ( !(did_start_motors) && (elapsed_time_in_seconds>=2.0) )
{
// Publish message to start the motors
std_msgs::Float32 target_speed_msg;
target_speed_msg.data = m_drive_motor_target_speed;
m_motor_pwm_publisher.publish(target_speed_msg);
// Update the flag
did_start_motors = true;
}
// Stop the motors after "m_time_in_seconds_to_drive_motors"
if ( !(did_finish_test) && (elapsed_time_in_seconds>=(2.0+m_time_in_seconds_to_drive_motors)) )
{
// Publish message to stop the motors
std_msgs::Float32 target_speed_msg;
target_speed_msg.data = 0.0;
m_motor_pwm_publisher.publish(target_speed_msg);
// Update the flag
did_finish_test = true;
}
// Display the cumulative cum
if ( !(did_display_cum_sum) && (elapsed_time_in_seconds>=(2.0+2.0+m_time_in_seconds_to_drive_motors)) )
{
ROS_INFO_STREAM("[TEMPLATE ENCODER READ MULTI THREADED] cumulative sum left = " << cum_sum_left << ", right = " << cum_sum_right);
// Update the flag
did_display_cum_sum = true;
}
}
void encoderCountingThreadMain()
{
// Specify the chip name of the GPIO interface
// > Note: for the 40-pin header of the Jetson SBCs, this
// is "/dev/gpiochip0"
const char * gpio_chip_name = "/dev/gpiochip0";
// Make a local copy of the line number member variables
int line_number_left_a = m_line_number_for_motor_left_channel_a;
int line_number_right_a = m_line_number_for_motor_right_channel_a;
// Initialise a GPIO chip, line, and event objects
struct gpiod_chip *chip;
struct gpiod_line *line_left_a;
struct gpiod_line *line_right_a;
struct gpiod_line_bulk line_bulk;
struct gpiod_line_event event;
struct gpiod_line_bulk event_bulk;
// Specify the timeout specifications
// > The first entry is seconds
// > The second entry is nano-seconds
struct timespec timeout_spec = { 0, 10000000 };
// Intialise a variable for the flags returned
// by GPIO calls
int returned_wait_flag;
int returned_read_flag;
// Get and print the value of the GPIO line
// > Note: the third argument to "gpiod_ctxless_get_value"
// is an "active_low" boolean input argument.
// If true, this indicate to the function that active state
// of this line is low.
int value;
value = gpiod_ctxless_get_value(gpio_chip_name, line_number_left_a, false, "foobar");
ROS_INFO_STREAM("[TEMPLATE ENCODER READ MULTI THREADED] On startup of node, chip " << gpio_chip_name << " line " << line_number_left_a << " returned value = " << value);
value = gpiod_ctxless_get_value(gpio_chip_name, line_number_right_a, false, "foobar");
ROS_INFO_STREAM("[TEMPLATE ENCODER READ MULTI THREADED] On startup of node, chip " << gpio_chip_name << " line " << line_number_right_a << " returned value = " << value);
// Open the GPIO chip
chip = gpiod_chip_open(gpio_chip_name);
// Retrieve the GPIO lines
line_left_a = gpiod_chip_get_line(chip,line_number_left_a);
line_right_a = gpiod_chip_get_line(chip,line_number_right_a);
// Initialise the line bulk
gpiod_line_bulk_init(&line_bulk);
// Add the lines to the line bulk
gpiod_line_bulk_add(&line_bulk, line_left_a);
gpiod_line_bulk_add(&line_bulk, line_right_a);
// Display the status
ROS_INFO_STREAM("[TEMPLATE ENCODER READ MULTI THREADED] Chip " << gpio_chip_name << " opened and lines " << line_number_left_a << " and " << line_number_right_a << " retrieved");
// Request the line events to be mointored
// > Note: only one of these should be uncommented
//gpiod_line_request_bulk_rising_edge_events(&line_bulk, "foobar");
gpiod_line_request_bulk_falling_edge_events(&line_bulk, "foobar");
//gpiod_line_request_bulk_both_edges_events(&line_bulk, "foobar");
// Display the line event values for rising and falling
ROS_INFO_STREAM("[TEMPLATE ENCODER READ MULTI THREADED] The constants defined for distinguishing line events are:, GPIOD_LINE_EVENT_RISING_EDGE = " << GPIOD_LINE_EVENT_RISING_EDGE << ", and GPIOD_LINE_EVENT_FALLING_EDGE = " << GPIOD_LINE_EVENT_FALLING_EDGE);
// Enter a loop that endlessly monitors the encoders
while (true)
{
// Monitor for the requested events on the GPIO line bulk
// > Note: the function "gpiod_line_event_wait" returns:
// 0 if wait timed out
// -1 if an error occurred
// 1 if an event occurred.
returned_wait_flag = gpiod_line_event_wait_bulk(&line_bulk, &timeout_spec, &event_bulk);
// Respond based on the the return flag
if (returned_wait_flag == 1)
{
// Get the number of events that occurred
int num_events_during_wait = gpiod_line_bulk_num_lines(&event_bulk);
// Lock the mutex while before counting the events
m_counting_mutex.lock();
// Iterate over the event
for (int i_event = 0; i_event < num_events_during_wait; i_event++)
{
// Get the line handle for this event
struct gpiod_line *line_handle = gpiod_line_bulk_get_line(&event_bulk, i_event);
// Get the number of this line
unsigned int this_line_number = gpiod_line_offset(line_handle);
// Read the event on the GPIO line
// > Note: the function "gpiod_line_event_read" returns:
// 0 if the event was read correctly
// -1 if an error occurred
returned_read_flag = gpiod_line_event_read(line_handle,&event);
// Respond based on the the return flag
if (returned_read_flag == 0)
{
// Increment the respective count
if (this_line_number == line_number_left_a)
m_encoder_counts_for_motor_left++;
if (this_line_number == line_number_right_a)
m_encoder_counts_for_motor_right++;
} // END OF: "if (returned_read_flag == 0)"
} // END OF: "for (int i_event = 0; i_event < num_events_during_wait; i_event++)"
// Unlock the mutex
m_counting_mutex.unlock();
} // END OF: "if (returned_wait_flag == 1)"
} // END OF: "while (true)"
// Close the GPIO chip
gpiod_chip_close(chip);
}
int main(int argc, char* argv[])
{
// Initialise the node
ros::init(argc, argv, "template_encoder_read_multi_threaded");
ros::NodeHandle nodeHandle("~");
// Get the GPIO line number to monitor
// Notes:
// > If you look at the "template_encoder.launch" file located in
// the "launch" folder, you see the following lines of code:
// <param
// name = "line_number_for_motor_left_channel_a"
// value = 133
// />
// > These lines of code add a parameter named to this node
// with the parameter name: "line_number_for_motor_left_channel_a"
// > Thus, to access this parameter, we first get a handle to
// this node within the namespace that it was launched.
//
// Get the line number parameters:
// > For channel A of the left side motor
if ( !nodeHandle.getParam("line_number_for_motor_left_channel_a", m_line_number_for_motor_left_channel_a) )
{
// Display an error message
ROS_INFO("[TEMPLATE ENCODER READ MULTI THREADED] FAILED to get \"line_number_for_motor_left_channel_a\" parameter. Using default value instead.");
}
// > For channel A of the right side motor
if ( !nodeHandle.getParam("line_number_for_motor_right_channel_a", m_line_number_for_motor_right_channel_a) )
{
// Display an error message
ROS_INFO("[TEMPLATE ENCODER READ MULTI THREADED] FAILED to get \"line_number_for_motor_right_channel_a\" parameter. Using default value instead.");
}
// > Display the line numbers being monitored
ROS_INFO_STREAM("[TEMPLATE ENCODER READ MULTI THREADED] Will monitor line_numbers = " << m_line_number_for_motor_left_channel_a << " and " << m_line_number_for_motor_right_channel_a);
// Get the "detla t" parameter for the publishing frequency
if ( !nodeHandle.getParam("delta_t_for_publishing_counts", m_delta_t_for_publishing_counts) )
{
// Display an error message
ROS_INFO("[TEMPLATE ENCODER READ MULTI THREADED] FAILED to get \"delta_t_for_publishing_counts\" parameter. Using default value instead.");
}
// Initialise a publisher for the encoder counts
// > Note, the second is the size of our publishing queue. We choose to
// buffer encoder counts messages because we want to avoid losing counts.
m_encoder_counts_publisher = nodeHandle.advertise<LeftAndRightInt>("encoder_counts", 10, false);
// Initialise a timer
m_timer_for_publishing = nodeHandle.createTimer(ros::Duration(m_delta_t_for_publishing_counts), timerCallbackForPublishing, false);
// Get the parameter for how long to drive the motors
// NOTE: this parameter is purely for the convenience of testing.
if ( !nodeHandle.getParam("time_in_seconds_to_drive_motors", m_time_in_seconds_to_drive_motors) )
ROS_INFO("[TEMPLATE ENCODER READ MULTI THREADED] FAILED to get \"time_in_seconds_to_drive_motors\" parameter. Using default value instead.");
// Initialise a publisher for commanding the motors
// NOTE: this publisher and what it is used for is
// purely for the convenience of testing.
std::string temp_namespace = ros::this_node::getNamespace();
std::string temp_namespace_to_i2c_internal = temp_namespace + "/template_i2c_internal";
ros::NodeHandle nodeHandle_to_i2c_internal(temp_namespace_to_i2c_internal);
m_motor_pwm_publisher = nodeHandle_to_i2c_internal.advertise<std_msgs::Float32>("set_motor_duty_cycle", 10, false);
// Get the parameter for target speed when driving the motors
// NOTE: this parameter is purely for the convenience of testing.
if ( !nodeHandle.getParam("drive_motor_target_speed", m_drive_motor_target_speed) )
ROS_INFO("[TEMPLATE ENCODER READ MULTI THREADED] FAILED to get \"drive_motor_target_speed\" parameter. Using default value instead.");
// Create thread for counting the encoder events
std::thread encoder_counting_thread (encoderCountingThreadMain);
//boost::thread encoder_counting_thread(encoderCountingThreadMain);
// Spin the node
ros::spin();
// Join back the encoder counting thread
encoder_counting_thread.join();
return 0;
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment