int main(int argc, char* argv[])
{
if (argc == 4) { // change this to 2 if running from rosrun, 4 if from launch file
if (strcmp(argv[1], "--quiet") == 0) {
tacit = true;
ROS_INFO("shhhh... be vewy, vewy, quiet.");
}
}
ros::init(argc, argv, "Calculate_Orientation_server");
ros::NodeHandle berk;
imuFilter.reset();
ros::ServiceServer service = berk.advertiseService("Calculate_Orientation", calculate );
VERBOSE ROS_INFO("Ready to calculate orientation.");
VERBOSE ROS_INFO("Roll: %f, Pitch: %f, Yaw: %f", deg_from_rad(imuFilter.getRoll()),
deg_from_rad(imuFilter.getPitch()), deg_from_rad(imuFilter.getYaw()));
//Listening for reset messages
ros::Subscriber resetListen = berk.subscribe("imu_reset", 1, imu_reset_callback);
ros::spin();
return 0;
}
void SVGPathWriter::arcTo(double rx, double ry, double angle,
bool large_arc, bool sweep, Point const &p)
{
_setCommand('A');
_current_pars.push_back(rx);
_current_pars.push_back(ry);
_current_pars.push_back(deg_from_rad(angle));
_current_pars.push_back(large_arc ? 1. : 0.);
_current_pars.push_back(sweep ? 1. : 0.);
_current_pars.push_back(p[X]);
_current_pars.push_back(p[Y]);
_current = _quad_tangent = _cubic_tangent = p;
if (!_optimize) {
flush();
}
}
bool calculate(ros_imu::imu_filter::Request &request, ros_imu::imu_filter::Response &response)
{
//Update filter with IMU data
ros_imu::spatialRaw raw = request.rawIMU;
VERBOSE ROS_INFO("Server Side, Raw Phidget Data");
VERBOSE ROS_INFO("a_x: %f, a_y: %f, a_z:%f", raw.a_x, raw.a_y, raw.a_z);
VERBOSE ROS_INFO("w_x: %f, w_y: %f, w_z:%f", raw.w_x, raw.w_y, raw.w_z);
imuFilter.updateFilter(raw.w_x, raw.w_y, raw.w_z,
raw.a_x, raw.a_y, raw.a_z);
imuFilter.computeEuler();
double rotation[3][3];
VERBOSE ROS_INFO("Roll: %f, Pitch: %f, Yaw: %f", deg_from_rad(imuFilter.getRoll()),
deg_from_rad(imuFilter.getPitch()), deg_from_rad(imuFilter.getYaw()));
response.rpy.roll = deg_from_rad(imuFilter.getRoll());
response.rpy.pitch = deg_from_rad(imuFilter.getPitch());
response.rpy.yaw = deg_from_rad(imuFilter.getYaw());
imuFilter.getRotationMatrix(rotation);
for(int i =0; i<3; i++) {
response.rot.row1[i] = rotation[0][i];
response.rot.row2[i] = rotation[1][i];
response.rot.row3[i] = rotation[2][i];
}
//Filling up pose header
// response.pose.header.seq = seqNum;
//response.pose.header.stamp = raw.timestamp;
//response.pose.header.frame_id = 1;
//Artificially setting to (0,0,0) position for testing
//response.pose.pose.position.x = 0;
//response.pose.pose.position.y = 0;
//response.pose.pose.position.z = 0;
imuFilter.getOrientation( response.orientation.x,
response.orientation.y,
response.orientation.z,
response.orientation.w);
return true;
}
