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, "object_message_generator");
ros::NodeHandle node_priv("~");
ros::NodeHandle node("");
moveit_object_handling::ObjectMessageGenerator generator(node_priv, node);
ros::spin();
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "camnode", ros::init_options::AnonymousName);
ros::NodeHandle node;
ros::NodeHandle node_priv("~");
DynamicReconfigureCameraConfig config_srv;
DynamicReconfigureCameraConfig::CallbackType cb;
CameraNode* cn = new CameraNode(node_priv, argc,argv);
cb = boost::bind(&CameraNode::config_callback, cn, _1, _2);
config_srv.setCallback(cb);
return cn->run();
}
int main(int argc, char **argv)
{
ROS_INFO("Starting pid with node name %s", node_name.c_str());
// Initialize ROS stuff
ros::init(argc, argv, node_name); // Note node_name can be overidden by launch file
ros::NodeHandle node;
ros::NodeHandle node_priv("~");
while (ros::Time(0) == ros::Time::now())
{
ROS_INFO("controller spinning waiting for time to become non-zero");
sleep(1);
}
// Get params if specified in launch file or as params on command-line, set defaults
node_priv.param<double>("Kp", Kp, 1.0);
node_priv.param<double>("Ki", Ki, 0.0);
node_priv.param<double>("Kd", Kd, 0.0);
node_priv.param<double>("upper_limit", upper_limit, 1000.0);
node_priv.param<double>("lower_limit", lower_limit, -1000.0);
node_priv.param<double>("windup_limit", windup_limit, 1000.0);
node_priv.param<double>("cutoff_frequency", cutoff_frequency, -1.0);
node_priv.param<std::string>("topic_from_controller", topic_from_controller, "control_effort");
node_priv.param<std::string>("topic_from_plant", topic_from_plant, "rpms");
node_priv.param<std::string>("setpoint_topic", setpoint_topic, "setpoint");
node_priv.param<double>("max_loop_frequency", max_loop_frequency, 1.0);
node_priv.param<double>("min_loop_frequency", min_loop_frequency, 1000.0);
// Update params if specified as command-line options, & print settings
print_parameters();
if (not validate_parameters())
{
std::cout << "Error: invalid parameter\n";
}
// instantiate publishers & subscribers
control_effort_pub = node.advertise<std_msgs::Float64>(topic_from_controller, 1);
ros::Subscriber sub = node.subscribe(topic_from_plant, 1, plant_state_callback );
// COmment for now not to have changes. We want constant rpms
//ros::Subscriber setpoint_sub = node.subscribe(setpoint_topic, 1, setpoint_callback );
ros::Subscriber pid_enabled_sub = node.subscribe("pid_enable", 1, pid_enable_callback );
/* // configure dynamic reconfiguration
dynamic_reconfigure::Server<pid::PidConfig> config_server;
dynamic_reconfigure::Server<pid::PidConfig>::CallbackType f;
f = boost::bind(&reconfigure_callback, _1, _2);
config_server.setCallback(f);
*/
/*
// initialize diagnostics
diags = new PidControllerDiags;
diag_status.level = diagnostic_msgs::DiagnosticStatus::OK;
diag_status.message = "PID status nominal";
diags->diag_updater.setHardwareID(node_name);
diags->diag_updater.add(diags->freq_status);
diags->diag_updater.add("PID status", get_pid_diag_status);
*/
// Respond to inputs until shut down
ros::spin();
return 0;
}
