From c39c2f65da75acaa4358e8a10f7c232199034335 Mon Sep 17 00:00:00 2001
From: Tommy Persson <tommy.persson@liu.se>
Date: Mon, 18 Mar 2019 15:52:54 +0100
Subject: [PATCH] Adding parameters.
---
src/axis_imu.cc | 35 ++++++++++++++++++++++++++++-------
1 file changed, 28 insertions(+), 7 deletions(-)
diff --git a/src/axis_imu.cc b/src/axis_imu.cc
index 4c30a62..cd6fdf9 100644
--- a/src/axis_imu.cc
+++ b/src/axis_imu.cc
@@ -19,6 +19,9 @@
std::string frame_id;
ros::Publisher imu_pub;
+sensor_msgs::Imu imu_msg;
+boost::mutex mutex;
+
// 1 byte header ('A' eller 'G') | float64 timestamp | float64 x | float64 y | float64 z
//
@@ -36,6 +39,7 @@ ros::Publisher imu_pub;
//
// Max sample rate för gyrot är 2kHz och för accelerometern 1kHz, och detta styrs med gyro_rate_div och accel_rate_div i ovanstående http-anrop.
//
+// Fix rate ut, 1kHz
@@ -137,8 +141,9 @@ private:
return;
}
- sensor_msgs::Imu imu_msg;
-
+ {
+ boost::mutex::scoped_lock lock(mutex);
+
/* Fill the IMU message */
// Fill the header
@@ -177,10 +182,8 @@ private:
// mag_msg.magnetic_field.x = data.b[0]*1e-6;
// mag_msg.magnetic_field.y = data.b[1]*1e-6;
// mag_msg.magnetic_field.z = data.b[2]*1e-6;
+ }
- // ROS_ERROR("publish imu");
- // Publish the messages
- imu_pub.publish(imu_msg);
// mag_pub.publish(mag_msg);
start_receive();
@@ -216,6 +219,13 @@ void runUDPServer(int port) {
}
}
+void timer_callback(const ros::TimerEvent&) {
+ boost::mutex::scoped_lock lock(mutex);
+ // ROS_ERROR("publish imu");
+ // Publish the messages
+ imu_pub.publish(imu_msg);
+}
+
int main(int argc, char *argv[]) {
ros::init(argc, argv, "axis_imu");
@@ -244,14 +254,20 @@ int main(int argc, char *argv[]) {
int gyro_div = 2000/rate;
int accel_div = 1000/rate;
+ // currently fixed rate out so set div to 0
+
+ gyro_div = 0;
+ accel_div = 0;
+
std::ostringstream os;
- // os << "http://" << "root:" << root_passwd << "@" << camera_ip
os << "http://" << camera_ip
<< "/local/motionloggerudp/control.cgi?cmd=start&target_ip=" << driver_ip
<< "&accel_enabled=1&accel_rate_div=" << accel_div << "&gyro_enabled=1&gyro_rate_div=" << gyro_div;
std::string url = os.str();
+ ROS_INFO("COMMAND URL: %s", url.c_str());
+
CURL *curl;
CURLcode res;
@@ -261,6 +277,10 @@ int main(int argc, char *argv[]) {
curl_easy_setopt(curl, CURLOPT_URL, url);
/* example.com is redirected, so we tell libcurl to follow redirection */
curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1L);
+ // curl_easy_setopt(curl, CURLOPT_HTTPAUTH, CURLAUTH_DIGEST);
+ curl_easy_setopt(curl, CURLOPT_HTTPAUTH, CURLAUTH_ANY);
+ curl_easy_setopt(curl, CURLOPT_USERNAME, "root");
+ curl_easy_setopt(curl, CURLOPT_PASSWORD, root_passwd);
/* Perform the request, res will get the return code */
res = curl_easy_perform(curl);
@@ -279,10 +299,11 @@ int main(int argc, char *argv[]) {
exit(1);
}
- ROS_INFO("COMMAND URL: %s", url.c_str());
imu_pub = nh.advertise<sensor_msgs::Imu>("imu",1);
+ ros::Timer timer = nh.createTimer(ros::Duration(1.0/rate), timer_callback);
+
boost::thread sthread (spin_thread);
ROS_ERROR("runUDPServer");
--
GitLab