diff --git a/CMakeLists.txt b/CMakeLists.txt
index b4bbc1ad17d583820b40d80ebbb54f0342aef0d8..b8e6e475d6a2e065120c9c0a151f311fe6960348 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -155,6 +155,7 @@ add_executable(axis_imu
# )
target_link_libraries(axis_imu
+ curl
${catkin_LIBRARIES}
)
diff --git a/README.md b/README.md
index 2e071acb38ebe10a261a4541d6fa6d32c0821db4..3890a54cd0fdc9917f23e2ad85698577b8fdb097 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,9 @@
# axis_imu
-Driver to Axis IMU.
\ No newline at end of file
+Driver to Axis IMU.
+
+
+Usage:
+```bash
+rosrun axis_imu axis_imu _frame_id:=imu_frame _rate:=100 _root_passwd:=pass _camera_ip:=192.168.7.19 _driver_ip:=192.168.7.160 _port:=5000
+```
\ No newline at end of file
diff --git a/data/Makefile b/data/Makefile
index 3484a560ea5fb54acfe1589b5b5d329ddc672c5a..7e130c62cdc1883f5063bd9f09be5dc1456bc493 100644
--- a/data/Makefile
+++ b/data/Makefile
@@ -1,9 +1,11 @@
+PCAPFILE=motionlogger_udp_0_5_0_doubles_network_byte_order.pcap
+
terra8:
- tcprewrite --infile=udp_dump_port_5000.pcap --outfile=changed.pcap --dstipmap=0.0.0.0/0:192.168.7.160/32 --fixcsum
+ tcprewrite --infile=$(PCAPFILE) --outfile=changed.pcap --dstipmap=0.0.0.0/0:192.168.7.160/32 --fixcsum
laptop:
- tcprewrite --infile=mypcap.pcap --outfile=changed.pcap --dstipmap=0.0.0.0/0:192.168.1.136/32 --fixcsum
+ tcprewrite --infile=$(PCAPFILE) --outfile=changed.pcap --dstipmap=0.0.0.0/0:192.168.1.136/32 --fixcsum
replay-terra8:
udpreplay -i ens1 -c 1000 changed.pcap
diff --git a/data/motionlogger_udp_0_4_0_doubles.pcap b/data/motionlogger_udp_0_4_0_doubles.pcap
new file mode 100644
index 0000000000000000000000000000000000000000..f84d0980ff1b797cc96be0828a486c80f7eeff98
Binary files /dev/null and b/data/motionlogger_udp_0_4_0_doubles.pcap differ
diff --git a/data/motionlogger_udp_0_5_0_doubles_network_byte_order.pcap b/data/motionlogger_udp_0_5_0_doubles_network_byte_order.pcap
new file mode 100644
index 0000000000000000000000000000000000000000..a9de2af658677bf41207e82f69d2c5613618cb08
Binary files /dev/null and b/data/motionlogger_udp_0_5_0_doubles_network_byte_order.pcap differ
diff --git a/src/axis_imu.cc b/src/axis_imu.cc
index 740d46aa91d2774bcc70e5339d03b1351ba8c499..c441513ecc3d302d375b2f6194ad5ece1e952839 100644
--- a/src/axis_imu.cc
+++ b/src/axis_imu.cc
@@ -14,15 +14,33 @@
#include <boost/asio.hpp>
#include <boost/bind.hpp>
+#include <curl/curl.h>
+
std::string frame_id;
ros::Publisher imu_pub;
+sensor_msgs::Imu imu_msg;
+boost::mutex mutex;
+
-// 1 byte header ('A' eller 'G') | float32 timestamp | float32 x | float32 y | float32 z
+// 1 byte header ('A' eller 'G') | float64 timestamp | float64 x | float64 y | float64 z
//
// Headern 'A' och 'G' står för accelerometer respektive gyro.
// timestamp är sekunder sedan kamerans uppstart
// x, y och z är i grader/s för gyrot och G för accelerometern.
+//
+// Data sent in network byte order
+//
+// Jag använder följande curl-kommando för att starta
+// curl --digest "[http://root:pass@[IP]/local/motionloggerudp/control.cgi?cmd=start&target_ip=10.92.1.19&accel_enabled=1&accel_rate_div=0&gyro_enabled=1&gyro_rate_div=0]http://root:pass@[IP]/local/motionloggerudp/control.cgi?cmd=start&target_ip=10.92.1.19&accel_enabled=1&accel_rate_div=0&gyro_enabled=1&gyro_rate_div=0"
+//
+// För att starta motionloggerudp (som är vad jag har kallat den nya varianten som inte lagrar på SD-kort) så anropar man t.ex.
+// http://[CAMERAIP]/local/motionloggerudp/control.cgi?cmd=start&target_ip=[TARGETIP]&accel_enabled=1&accel_rate_div=0&gyro_enabled=1&gyro_rate_div=0
+//
+// 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
+
using namespace std;
@@ -50,6 +68,7 @@ float bytesToFloat(unsigned char b0, unsigned char b1, unsigned char b2, unsign
return result;
}
+
class udp_server {
public:
udp_server(boost::asio::io_service& io_service, int port_number)
@@ -61,6 +80,25 @@ public:
}
private:
+
+ double bytesToDouble(unsigned int i) {
+ //unsigned char byte_array[] = { b0, b1, b2, b3, b4, b5, b6, b7 };
+ unsigned char byte_array[] = { recv_buffer_[i+7],
+ recv_buffer_[i+6],
+ recv_buffer_[i+5],
+ recv_buffer_[i+4],
+ recv_buffer_[i+3],
+ recv_buffer_[i+2],
+ recv_buffer_[i+1],
+ recv_buffer_[i+0]}; //netw byte order
+ double result;
+ std::copy(reinterpret_cast<const char*>(&byte_array[0]),
+ reinterpret_cast<const char*>(&byte_array[8]),
+ reinterpret_cast<char*>(&result));
+ return result;
+ }
+
+
void start_receive() {
socket_.async_receive_from(boost::asio::buffer(recv_buffer_), remote_endpoint_,
boost::bind(&udp_server::handle_receive, this,
@@ -71,22 +109,26 @@ private:
void handle_receive(const boost::system::error_code& error,
std::size_t bytes_transferred) {
if (!error || error == boost::asio::error::message_size) {
- // cerr << "Got message: " << bytes_transferred << " - " << recv_buffer_[0] << endl;
- float time = bytesToFloat(recv_buffer_[1], recv_buffer_[2], recv_buffer_[3], recv_buffer_[4]);
- float ax, ay, az, gx, gy, gz;
- if (recv_buffer_[0] == 'A') {
- have_acc_flag = true;
- ax = bytesToFloat(recv_buffer_[5], recv_buffer_[6], recv_buffer_[7], recv_buffer_[8]);
- ay = bytesToFloat(recv_buffer_[9], recv_buffer_[10], recv_buffer_[11], recv_buffer_[12]);
- az = bytesToFloat(recv_buffer_[13], recv_buffer_[14], recv_buffer_[15], recv_buffer_[16]);
- ROS_ERROR("ACCELEROMETER: %f - %f %f %f", time, ax, ay, az);
- }
+ // cerr << "Got message: " << bytes_transferred << " - " << recv_buffer_[0] << " - " << recv_buffer_[33] << endl;
+ // float time = bytesToFloat(recv_buffer_[1], recv_buffer_[2], recv_buffer_[3], recv_buffer_[4]);
+ double gtime = bytesToDouble(1);
+ //float ax, ay, az, gx, gy, gz;
+ double ax, ay, az, gx, gy, gz;
+ gx = bytesToDouble(9);
+ gy = bytesToDouble(17);
+ gz = bytesToDouble(25);
+
+ double atime = bytesToDouble(34);
+ ax = bytesToDouble(42);
+ ay = bytesToDouble(50);
+ az = bytesToDouble(58);
if (recv_buffer_[0] == 'G') {
have_gyro_flag = true;
- gx = bytesToFloat(recv_buffer_[5], recv_buffer_[6], recv_buffer_[7], recv_buffer_[8]);
- gy = bytesToFloat(recv_buffer_[9], recv_buffer_[10], recv_buffer_[11], recv_buffer_[12]);
- gz = bytesToFloat(recv_buffer_[13], recv_buffer_[14], recv_buffer_[15], recv_buffer_[16]);
- ROS_ERROR(" GYRO: %f - %f %f %f", time, gx, gy, gz);
+ ROS_ERROR(" GYRO: %f - %f %f %f", gtime, gx, gy, gz);
+ }
+ if (recv_buffer_[33] == 'A') {
+ have_acc_flag = true;
+ ROS_ERROR("ACCELEROMETER: %f - %f %f %f", atime, ax, ay, az);
}
if (!have_gyro_flag) {
@@ -99,8 +141,9 @@ private:
return;
}
- sensor_msgs::Imu imu_msg;
-
+ {
+ boost::mutex::scoped_lock lock(mutex);
+
/* Fill the IMU message */
// Fill the header
@@ -154,10 +197,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();
@@ -172,7 +213,7 @@ private:
boost::asio::ip::udp::udp::socket socket_;
boost::asio::ip::udp::udp::endpoint remote_endpoint_;
- boost::array<char, 128> recv_buffer_;
+ boost::array<unsigned char, 128> recv_buffer_;
bool have_gyro_flag;
bool have_acc_flag;
@@ -193,20 +234,91 @@ 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");
- ros::NodeHandle nh, private_nh;
+ ros::NodeHandle nh, private_nh("~");
int port;
int rate;
+ std::string camera_ip;
+ std::string driver_ip;
+ std::string root_passwd;
private_nh.param<int>("port", port, 5000);
private_nh.param<std::string>("frame_id", frame_id, "imu");
- private_nh.param("rate", rate, 200);
+ private_nh.param<int>("rate", rate, 200);
+ private_nh.param<std::string>("camera_ip", camera_ip, "192.168.7.19");
+ private_nh.param<std::string>("driver_ip", driver_ip, "192.168.7.160");
+ private_nh.param<std::string>("root_passwd", root_passwd, "pass");
+
+ ROS_INFO("Camera IP: %s", camera_ip.c_str());
+ ROS_INFO("Driver IP: %s", driver_ip.c_str());
+ ROS_INFO(" Port: %d", port);
+ ROS_INFO(" Frame Id: %s", frame_id.c_str());
+ ROS_INFO(" Rate: %d", rate);
+ ROS_INFO("Root pass: %s", root_passwd.c_str());
+
+ 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://" << 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;
+
+ curl = curl_easy_init();
+
+ if (curl) {
+ 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);
+ /* Check for errors */
+ if(res != CURLE_OK) {
+ fprintf(stderr, "curl_easy_perform() failed: %s\n",
+ curl_easy_strerror(res));
+ } else {
+ ROS_INFO("Curl call succeeded");
+ }
+
+ /* always cleanup */
+ curl_easy_cleanup(curl);
+ } else {
+ ROS_ERROR("Could not create curl object, exiting");
+ exit(1);
+ }
+
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");