Skip to content
Snippets Groups Projects
Commit b19579f7 authored by Tommy Persson's avatar Tommy Persson
Browse files

Adding parameters.

parent 92abf346
No related branches found
No related tags found
No related merge requests found
......@@ -155,6 +155,7 @@ add_executable(axis_imu
# )
target_link_libraries(axis_imu
curl
${catkin_LIBRARIES}
)
......
......@@ -14,6 +14,8 @@
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <curl/curl.h>
std::string frame_id;
ros::Publisher imu_pub;
......@@ -223,28 +225,56 @@ int main(int argc, char *argv[]) {
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<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;
std::ostringstream os;
os << "http://" << camera_ip << "local/motionloggerudp/control.cgi?cmd=start&target_ip=" << driver_ip
os << "http://" << "root:" << root_passwd << "@" << 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();
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);
/* 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));
/* always cleanup */
curl_easy_cleanup(curl);
} else {
ROS_ERROR("Could not create curl object, exiting");
exit(1);
}
ROS_INFO("COMMAND URL: %s", url.c_str());
imu_pub = nh.advertise<sensor_msgs::Imu>("imu",1);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment