controller_action_node.cpp
Go to the documentation of this file.
2 
4 {
6 {
7  nh_ = ros::NodeHandle("~");
8 
9  if (!nh_.getParam("loop_rate", loop_rate_))
10  {
11  ROS_WARN_STREAM("Missing loop_rate parameter for "
12  << ros::this_node::getName() << ". Using default.");
13  loop_rate_ = 100;
14  }
15 
16  got_first_ = false;
17  joint_state_sub_ = nh_.subscribe("/joint_states", 1,
19  state_pub_ = nh_.advertise<sensor_msgs::JointState>("/joint_command", 1);
20 }
21 
23 
25 {
27  ros::Time prev_time = ros::Time::now();
28  sensor_msgs::JointState command;
29  bool was_running = false;
30 
31  while (ros::ok())
32  {
33  if (got_first_) // TODO: Maybe keep track of how long was it since the last
34  // joint states msg received?
35  {
36  command = controller.updateControl(state_, ros::Time::now() - prev_time);
37  if (controller.isActive())
38  {
39  ROS_DEBUG_THROTTLE(10, "Controller is active, publishing");
40  was_running = true;
41  state_pub_.publish(command);
42  }
43  else
44  {
45  if (was_running)
46  {
47  state_pub_.publish(command); // publish the last command msg
48  was_running = false;
49  }
50  ROS_DEBUG_THROTTLE(10, "Controller is not active, skipping");
51  }
52  }
53  else
54  {
55  ROS_WARN_THROTTLE(10, "No joint state received");
56  }
57 
58  prev_time = ros::Time::now();
59  ros::spinOnce();
60  r.sleep();
61  }
62 }
63 
65  const sensor_msgs::JointState::ConstPtr &msg)
66 {
67  ROS_INFO_ONCE("Joint state received!");
68  state_ = *msg;
69  got_first_ = true;
70 }
71 } // namespace generic_control_toolbox
#define ROS_INFO_ONCE(...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL const std::string & getName()
void publish(const boost::shared_ptr< M > &message) const
ROSLIB_DECL std::string command(const std::string &cmd)
#define ROS_WARN_THROTTLE(period,...)
void jointStatesCb(const sensor_msgs::JointState::ConstPtr &msg)
bool getParam(const std::string &key, std::string &s) const
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_WARN_STREAM(args)
bool sleep()
virtual sensor_msgs::JointState updateControl(const sensor_msgs::JointState &current_state, const ros::Duration &dt)=0
static Time now()
ROSCPP_DECL void spinOnce()
#define ROS_DEBUG_THROTTLE(period,...)


generic_control_toolbox
Author(s): diogo
autogenerated on Mon Feb 28 2022 22:24:37