Skip to content

Commit

Permalink
Merge pull request #17 from sonia-auv/develop
Browse files Browse the repository at this point in the history 
Develop
  • Loading branch information
@supertoto29
supertoto29 authored May 29, 2022
2 parents d3295f8 + e72cf22 commit 2cd6ca9
Show file tree
Hide file tree
Showing 3 changed files with 232 additions and 49 deletions.
2 changes: 1 addition & 1 deletion package.xml
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>provider_imu</name>
<version>1.2.2</version>
<version>1.2.3</version>
<description>The provider_imu package </description>

<author>Thibaut Mattio</author>
Expand Down
233 changes: 197 additions & 36 deletions src/provider_imu_node.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,31 @@ namespace provider_IMU
ProviderIMUNode::ProviderIMUNode(const ros::NodeHandlePtr &_nh)
: nh(_nh), configuration(_nh), serialConnection(configuration.getTtyPort())
{
// Publisher
publisher = nh->advertise<sensor_msgs::Imu>("/provider_imu/imu_info", 100);
dvl_subscriber = nh->subscribe<geometry_msgs::Twist>("/proc_nav/dvl_velocity", 100, &ProviderIMUNode::dvl_velocity, this);
tare_srv = nh->advertiseService("/provider_imu/tare", &ProviderIMUNode::tare, this);
indoor_srv = nh->advertiseService("/provider_imu/indoor", &ProviderIMUNode::indoormode, this);

// Subscribers
dvl_subscriber = nh->subscribe<geometry_msgs::Twist>("/proc_nav/dvl_velocity", 100,
&ProviderIMUNode::dvl_velocity, this);
vpe_basic_control = nh->subscribe<std_msgs::UInt8MultiArray>("/provider_imu/vpe_basic_control", 10,
&ProviderIMUNode::vpe_basic_control_callback, this);
magnetometer_calibration_control = nh->subscribe<std_msgs::UInt8MultiArray>("/provider_imu/magnetometer_calibration_control", 10,
&ProviderIMUNode::magnetometer_calibration_control_callback, this);
delta_theta_delta_velocity = nh->subscribe<std_msgs::UInt8MultiArray>("/provider_imu/delta_theta_delta_velocity", 10,
&ProviderIMUNode::delta_theta_delta_velocity_callback, this);
imu_filtering_configuration = nh->subscribe<std_msgs::UInt8MultiArray>("/provider_imu/imu_filtering_configuration", 10,
&ProviderIMUNode::imu_filtering_configuration_callback, this);

// Service
tare_srv = nh->advertiseService("/provider_imu/tare", &ProviderIMUNode::tare, this);
reset_srv = nh->advertiseService("/provider_imu/reset", &ProviderIMUNode::reset, this);
factory_reset_srv = nh->advertiseService("/provider_imu/factory_reset", &ProviderIMUNode::factory_reset, this);
magnetic_disturbance_srv = nh->advertiseService("/provider_imu/magnetic_disturbance", &ProviderIMUNode::magnetic_disturbance, this);
acceleration_disturbance_srv = nh->advertiseService("/provider_imu/acceleration_disturbance", &ProviderIMUNode::acceleration_disturbance, this);
velocity_compensation_srv = nh->advertiseService("/provider_imu/velocity_compensation", &ProviderIMUNode::velocity_compensation, this);
asyn_output_pause_srv = nh->advertiseService("/provider_imu/pause", &ProviderIMUNode::asyn_output_pause, this);

// Threads
reader_thread = std::thread(std::bind(&ProviderIMUNode::reader, this));
error_thread = std::thread(std::bind(&ProviderIMUNode::send_error, this));
register_15_thread = std::thread(std::bind(&ProviderIMUNode::send_register_15, this));
Expand Down Expand Up @@ -110,35 +130,184 @@ namespace provider_IMU
return true;
}

/**
* @brief Switch the transmission mode to indoor or outdoor
*
* @param std_srvs contains the std_srvs SetBool service
*/
bool ProviderIMUNode::indoormode(std_srvs::SetBool::Request &indoormodeRsq, std_srvs::SetBool::Response &indoormodeRsp)
bool ProviderIMUNode::reset(std_srvs::Empty::Request &tareRsq, std_srvs::Empty::Response &tareRsp)
{

if (indoormodeRsq.data) {

serialConnection.transmit("$VNWRG,35,1,2,1,1*73/n");
ros::Duration(0.1).sleep();
serialConnection.transmit("$VNRST*4D\n");
ros::Duration(0.1).sleep();

ROS_INFO("IMU reset finished");
return true;
}

indoormodeRsp.message = "IMU in indoor mode";
ROS_INFO_STREAM("IMU in indoor mode");
bool ProviderIMUNode::factory_reset(std_srvs::Empty::Request &tareRsq, std_srvs::Empty::Response &tareRsp)
{
serialConnection.transmit("$VNRFS*5F\n");
ros::Duration(0.1).sleep();

} else {
ROS_INFO("IMU factory reset finished");
return true;
}

serialConnection.transmit("$VNWRG,35,1,0,1,1*71/n");
ros::Duration(0.1).sleep();
bool ProviderIMUNode::magnetic_disturbance(std_srvs::SetBool::Request &rsq, std_srvs::SetBool::Response &rsp)
{
std::stringstream ss;

indoormodeRsp.message = "IMU in absolute mode";
ROS_INFO_STREAM("IMU in absolute mode");

}
indoormodeRsp.success = true;
writer_mutex.lock();
ss << "$VNKMD," << std::to_string((uint8_t)rsq.data);
std::string send_data = ss.str();
appendChecksum(send_data);

serialConnection.transmit(send_data);
ros::Duration(0.1).sleep();

writer_mutex.unlock();
return true;
}

bool ProviderIMUNode::acceleration_disturbance(std_srvs::SetBool::Request &rsq, std_srvs::SetBool::Response &rsp)
{
std::stringstream ss;

writer_mutex.lock();
ss << "$VNKAD," << std::to_string((uint8_t)rsq.data);
std::string send_data = ss.str();
appendChecksum(send_data);

serialConnection.transmit(send_data);
ros::Duration(0.1).sleep();

writer_mutex.unlock();
return true;
}

bool ProviderIMUNode::velocity_compensation(std_srvs::SetBool::Request &rsq, std_srvs::SetBool::Response &rsp)
{
std::stringstream ss;

writer_mutex.lock();
ss << "$VNWNV,51," << std::to_string((uint8_t)rsq.data) << ",0.1,0.01";
std::string send_data = ss.str();
appendChecksum(send_data);

serialConnection.transmit(send_data);
ros::Duration(0.1).sleep();

writer_mutex.unlock();
return true;
}

bool ProviderIMUNode::asyn_output_pause(std_srvs::SetBool::Request &rsq, std_srvs::SetBool::Response &rsp)
{
std::stringstream ss;

writer_mutex.lock();
ss << "$VNASY," << std::to_string((uint8_t)rsq.data);
std::string send_data = ss.str();
appendChecksum(send_data);

serialConnection.transmit(send_data);
ros::Duration(0.1).sleep();

writer_mutex.unlock();
return true;
}

void ProviderIMUNode::dvl_velocity(const geometry_msgs::Twist::ConstPtr& msg)
{
std::stringstream ss;

std::unique_lock<std::mutex> mlock(writer_mutex);
ss << "$VNWRG,50," << std::to_string((float)msg->linear.x) << "," << std::to_string((float)msg->linear.y) << "," << std::to_string((float)msg->linear.z);
std::string send_data = ss.str();
appendChecksum(send_data);

serialConnection.transmit(send_data);
}

void ProviderIMUNode::asyn_data_frequency_callback(const std_msgs::UInt8::ConstPtr& msg)
{
std::stringstream ss;

writer_mutex.lock();

ss << "$VNWNV,07," << std::to_string(msg->data);
std::string send_data = ss.str();
appendChecksum(send_data);

serialConnection.transmit(send_data);
ros::Duration(0.5).sleep();

writer_mutex.unlock();
}

void ProviderIMUNode::vpe_basic_control_callback(const std_msgs::UInt8MultiArray::ConstPtr& msg)
{
std::stringstream ss;

writer_mutex.lock();

ss << "$VNWNV,35," << std::to_string(msg->data.at(0)) << "," << std::to_string(msg->data.at(1)) << "," << std::to_string(msg->data.at(2))
<< "," << std::to_string(msg->data.at(3));
std::string send_data = ss.str();
appendChecksum(send_data);

serialConnection.transmit(send_data);
ros::Duration(0.5).sleep();

writer_mutex.unlock();
}

void ProviderIMUNode::magnetometer_calibration_control_callback(const std_msgs::UInt8MultiArray::ConstPtr& msg)
{
std::stringstream ss;

writer_mutex.lock();

ss << "$VNWNV,44," << std::to_string(msg->data.at(0)) << "," << std::to_string(msg->data.at(1)) << "," << std::to_string(msg->data.at(2));
std::string send_data = ss.str();
appendChecksum(send_data);

serialConnection.transmit(send_data);
ros::Duration(0.5).sleep();

writer_mutex.unlock();
}

void ProviderIMUNode::delta_theta_delta_velocity_callback(const std_msgs::UInt8MultiArray::ConstPtr& msg)
{
std::stringstream ss;

writer_mutex.lock();

ss << "$VNWNV,82," << std::to_string(msg->data.at(0)) << "," << std::to_string(msg->data.at(1)) << "," << std::to_string(msg->data.at(2))
<< "," << std::to_string(msg->data.at(3));
std::string send_data = ss.str();
appendChecksum(send_data);

serialConnection.transmit(send_data);
ros::Duration(0.5).sleep();

writer_mutex.unlock();
}

void ProviderIMUNode::imu_filtering_configuration_callback(const std_msgs::UInt8MultiArray::ConstPtr& msg)
{
std::stringstream ss;

writer_mutex.lock();

ss << "$VNWNV,85," << std::to_string(msg->data.at(0)) << "," << std::to_string(msg->data.at(1)) << "," << std::to_string(msg->data.at(2))
<< "," << std::to_string(msg->data.at(4)) << "," << std::to_string(msg->data.at(5)) << "," << std::to_string(msg->data.at(6))
<< "," << std::to_string(msg->data.at(7)) << "," << std::to_string(msg->data.at(8)) << "," << std::to_string(msg->data.at(9));
std::string send_data = ss.str();
appendChecksum(send_data);

serialConnection.transmit(send_data);
ros::Duration(0.5).sleep();

writer_mutex.unlock();
}

/**
* @brief read the IMU serial port
*
Expand Down Expand Up @@ -195,6 +364,10 @@ namespace provider_IMU
error_str = std::string(buffer);
error_cond.notify_one();
}
else
{
ROS_INFO_STREAM(buffer);
}
}
}

Expand Down Expand Up @@ -428,16 +601,4 @@ namespace provider_IMU
}
}
}

void ProviderIMUNode::dvl_velocity(const geometry_msgs::Twist::ConstPtr& msg)
{
std::stringstream ss;

std::unique_lock<std::mutex> mlock(writer_mutex);
ss << "$VNWRG,50," << std::to_string((float)msg->linear.x) << "," << std::to_string((float)msg->linear.y) << "," << std::to_string((float)msg->linear.z);
std::string send_data = ss.str();
appendChecksum(send_data);

serialConnection.transmit(send_data);
}
}
46 changes: 34 additions & 12 deletions src/provider_imu_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,12 @@
#define INTERFACE_RS485_NODE_H

#include "driver/serial.h"

#include <sensor_msgs/Imu.h>
#include <std_srvs/Empty.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/UInt8MultiArray.h>
#include <std_srvs/SetBool.h>
#include <std_srvs/Empty.h>
#include <ros/ros.h>

#include <mutex>
Expand All @@ -31,11 +32,12 @@ namespace provider_IMU
const char* REG_15 = "QMR";
const char* REG_239 = "YBA";
const char* REG_240 = "YIA";

const char* ERR_STR = "ERR";

Configuration configuration;

ros::NodeHandlePtr nh;
Configuration configuration;
Serial serialConnection;

std::thread reader_thread;
std::thread register_15_thread;
std::thread register_239_thread;
Expand All @@ -60,7 +62,6 @@ namespace provider_IMU

std::mutex writer_mutex;


bool register_15_stop_thread = false;
bool register_239_stop_thread = false;
bool register_240_stop_thread = false;
Expand All @@ -72,23 +73,44 @@ namespace provider_IMU
bool confirmChecksum(std::string& data);

void send_information();
bool indoormode(std_srvs::SetBool::Request &indoormodeRsq, std_srvs::SetBool::Response &indoormodeRsp);

bool tare(std_srvs::Empty::Request &tareRsq, std_srvs::Empty::Response &tareRsp);

bool reset(std_srvs::Empty::Request &tareRsq, std_srvs::Empty::Response &tareRsp);
bool factory_reset(std_srvs::Empty::Request &tareRsq, std_srvs::Empty::Response &tareRsp);
bool magnetic_disturbance(std_srvs::SetBool::Request &rsq, std_srvs::SetBool::Response &rsp);
bool acceleration_disturbance(std_srvs::SetBool::Request &rsq, std_srvs::SetBool::Response &rsp);
bool velocity_compensation(std_srvs::SetBool::Request &rsq, std_srvs::SetBool::Response &rsp);
bool asyn_output_pause(std_srvs::SetBool::Request &rsq, std_srvs::SetBool::Response &rsp);

void dvl_velocity(const geometry_msgs::Twist::ConstPtr& msg);
void asyn_data_frequency_callback(const std_msgs::UInt8::ConstPtr& msg);
void vpe_basic_control_callback(const std_msgs::UInt8MultiArray::ConstPtr& msg);
void magnetometer_calibration_control_callback(const std_msgs::UInt8MultiArray::ConstPtr& msg);
void delta_theta_delta_velocity_callback(const std_msgs::UInt8MultiArray::ConstPtr& msg);
void imu_filtering_configuration_callback(const std_msgs::UInt8MultiArray::ConstPtr& msg);

void send_register_15();
void send_register_239();
void send_register_240();
void send_error();
void reader();

ros::NodeHandlePtr nh;
Serial serialConnection;

ros::ServiceServer tare_srv;
ros::ServiceServer indoor_srv;
ros::ServiceServer reset_srv;
ros::ServiceServer factory_reset_srv;
ros::ServiceServer magnetic_disturbance_srv;
ros::ServiceServer acceleration_disturbance_srv;
ros::ServiceServer velocity_compensation_srv;
ros::ServiceServer asyn_output_pause_srv;

ros::Publisher publisher;

ros::Subscriber dvl_subscriber;
ros::Subscriber asyn_data_frequency;
ros::Subscriber vpe_basic_control;
ros::Subscriber magnetometer_calibration_control;
ros::Subscriber delta_theta_delta_velocity;
ros::Subscriber imu_filtering_configuration;
};
}

Expand Down

0 comments on commit 2cd6ca9

Please sign in to comment.