Commit d3d2bb8a authored by Sebastian Olsson's avatar Sebastian Olsson
Browse files

159: WIP added files

parent c821bd59
Pipeline #84457 failed with stage
in 2 minutes and 59 seconds
#include "lhw_qi/noice_cancellation.hpp"
#define _USE_MATH_DEFINES
/*
For reasoning behind some of the design choices in this file, please see the qi wiki page
*/
namespace lhw_qi
{
NoiceCancellation::NoiceCancellation(const rclcpp::NodeOptions & options)
: Node("noice_cancellation", options)
{
mic_buff_sub_ = this->create_subscription<lhw_interfaces::msg::Audio>("microphone_buffer", 1000);
//pub_ = this->create_publisher<lhw_interfaces::msg::Audio>("microphone_buffer", 1000);
}
void NoiceCancellation::initiate(void)
{
int front_mic_channel_id = 3;
listener_.call<qi::AnyValue>("setClientPreferences",
"sMicrophone",
16000,
front_mic_channel_id,
0);
listener_.call<qi::AnyValue>("subscribe", "sMicrophone");
// Publishes all the data in the deque on the microphone_buffer topic periodically
int seconds = 1;
int pub_nano_period = int(seconds * pow(10, 9));
pub_timer_ = rclcpp::create_timer(
this,
this->get_clock(),
rclcpp::Duration(std::chrono::nanoseconds(pub_nano_period)),
std::bind(&Microphone::publish_buffer_cb, this)
);
}
void Microphone::initiate(qi::SessionPtr session, boost::shared_ptr<Microphone> microphone_ptr)
{
session_ = session;
session_->waitForService("ALAudioDevice");
listener_ = session_->service("ALAudioDevice");
session_->registerService("sMicrophone", qi::AnyObject( microphone_ptr ) );
mic_ = microphone_ptr;
qi::AnyObject microphone = session_ -> service("sMicrophone");
microphone.call<qi::AnyValue>("init");
}
/*
Publishes all the data in the deque
*/
void Microphone::publish_buffer_cb() {
if (buff_.empty()) {
//RCUTILS_LOG_INFO("Buffer empty!");
return;
}
lhw_interfaces::msg::Audio msg = lhw_interfaces::msg::Audio();
std::vector<uint8_t> data_vec(buff_.begin(), buff_.end());
int mean = calculate_buff_mean();
//RCUTILS_LOG_INFO("RMS: %d", mean);
// TODO: Make the publishing of the mean a separate message
msg.mean = mean;
msg.data = data_vec;
buff_.clear();
pub_->publish(msg);
}
// TODO: Fix mean value, currently there is no difference
// between silence and when someone is talking
int Microphone::calculate_buff_mean() {
// TODO: Possibly have to loop through buff and add together every 2 values (8 bit + 8 bit = 16 bit)
std::vector<int16_t> data_16(buff_.begin(), buff_.end());
const int count = data_16.size();
int mean_ans = 0;
if (count != 0) {
auto abs_val = [](int val, int sum){return sum + std::abs(val);};
int mean = std::accumulate(data_16.begin(), data_16.end(), 0, abs_val);
//RCUTILS_LOG_INFO("MEAN: %d", mean);
mean_ans = mean / count;
}
//RCUTILS_LOG_INFO("COUNT: %d", count);
return mean_ans;
}
void Microphone::processRemote(
const int & nbOfChannels,
const int & nbrOfSamplesByChannel,
const qi::AnyValue & timestamp,
const qi::AnyValue & buffer
) {
// This needs to spin to make it possible for us to get the data from the is_talking callback
rclcpp::spin_some(to_std(mic_));
if (is_talking) return; // Stops pepper from hearing itself
std::pair<char*, size_t> charBuffer = buffer.unwrap().asRaw();
uint8_t* data = (uint8_t*)charBuffer.first;
buff_.insert(buff_.end(), data, data + charBuffer.second);
}
} // namespace lhw_qi
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(lhw_qi::Microphone)
#include "lhw_qi/microphone_component.hpp"
#include <rcutils/cmdline_parser.h>
template<typename T>
void do_release(typename boost::shared_ptr<T> const&, T*)
{
}
template<typename T>
typename std::shared_ptr<T> to_std(typename boost::shared_ptr<T> const& p)
{
return
std::shared_ptr<T>(
p.get(),
boost::bind(& do_release<T>, p, _1));
}
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
// Parse the command line options.
auto ip = std::string("localhost");
char * cli_option = rcutils_cli_get_option(argv, argv + argc, "-i");
if (nullptr != cli_option) {
ip = std::string(cli_option);
}
auto port = std::string("9559");
cli_option = rcutils_cli_get_option(argv, argv + argc, "-p");
if (nullptr != cli_option) {
port = std::string(cli_option);
}
// Connect to naoqi
qi::ApplicationSession app(argc, argv, 0, "tcp://" + ip + ":" + port);
RCUTILS_LOG_INFO("Attempting connection to %s on port %s", ip.c_str(), port.c_str());
app.startSession();
qi::SessionPtr session = app.session();
RCUTILS_LOG_INFO("Session started");
auto microphone = boost::make_shared<lhw_qi::Microphone>(rclcpp::NodeOptions());
microphone->initiate(session, microphone);
app.run();
rclcpp::spin(to_std(microphone));
rclcpp::shutdown();
return 0;
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment