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; }