setpoint_node.cpp
Go to the documentation of this file.
1 /***************************************************************************/
36 #include "ros/ros.h"
37 #include "std_msgs/Float64.h"
38 
39 int main(int argc, char** argv)
40 {
41  ros::init(argc, argv, "setpoint_node");
42  ROS_INFO("Starting setpoint publisher");
43  ros::NodeHandle setpoint_node;
44 
45  while (ros::ok() && ros::Time(0) == ros::Time::now())
46  {
47  ROS_INFO("Setpoint_node spinning, waiting for time to become non-zero");
48  sleep(1);
49  }
50 
51  std_msgs::Float64 setpoint;
52  setpoint.data = 1.0;
53  ros::Publisher setpoint_pub = setpoint_node.advertise<std_msgs::Float64>("setpoint", 1);
54 
55  ros::Rate loop_rate(0.2); // change setpoint every 5 seconds
56 
57  while (ros::ok())
58  {
59  ros::spinOnce();
60 
61  setpoint_pub.publish(setpoint); // publish twice so graph gets it as a step
62  setpoint.data = 0 - setpoint.data;
63  setpoint_pub.publish(setpoint);
64 
65  loop_rate.sleep();
66  }
67 }
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_INFO(...)
ROSCPP_DECL bool ok()
double setpoint
Definition: autotune.cpp:64
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static Time now()
int main(int argc, char **argv)
ROSCPP_DECL void spinOnce()


pid
Author(s): Andy Zelenak , Paul Bouchier
autogenerated on Mon Apr 15 2019 02:41:32