int main(int argc, char **argv) {
ros::init(argc, argv, "pid_roll");
ros::NodeHandle nh;
ros::NodeHandle node_priv("~");
node_priv.param<double>("p",p,1.0);
node_priv.param<double>("i",i,1.0);
node_priv.param<double>("d",d,1.0);
node_priv.param<double>("im",im,.3);
node_priv.param<double>("dm",dm,-.3);
message_filters::Subscriber<imu_3dm_gx4::FilterOutput> imu_sub(nh, "/state/filter",1);
message_filters::Subscriber<geometry_msgs::Vector3Stamped> accel_sub(nh, "angular_set_pt",1);
dynamic_reconfigure::Server<riptide_navigation::pidConfig> server;
dynamic_reconfigure::Server<riptide_navigation::pidConfig>::CallbackType f;
f = boost::bind(&dyn_callback, _1, _2);
server.setCallback(f);
typedef message_filters::sync_policies::ApproximateTime<imu_3dm_gx4::FilterOutput, geometry_msgs::Vector3Stamped> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(100), imu_sub, accel_sub);
sync.registerCallback(boost::bind(&callback,_1,_2));
a_e = nh.advertise<geometry_msgs::Vector3Stamped>("accel_error_roll", 1);
ros::spin();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "imu_state");
ros::NodeHandle nh;
message_filters::Subscriber<sensor_msgs::Imu> imu_sub(nh, "imu/imu", 1);
message_filters::Subscriber<imu_3dm_gx4::FilterOutput> filter_sub(nh, "imu/filter", 1);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Imu, imu_3dm_gx4::FilterOutput> approx;
message_filters::Synchronizer<approx> sync(approx(10), imu_sub, filter_sub);
sync.registerCallback(boost::bind(&callback, _1, _2));
unifier = nh.advertise<sensor_msgs::Imu>("state/imu", 1);
// tform.getOrigin().setX(0.0);
// tform.getOrigin().setY(0.0);
// tform.getOrigin().setZ(0.0);
// tform.frame_id_ = "";
// tform.child_frame_id_ = "";
ros::spin();
}
