LocalBehaviorControllerNode.cpp
Go to the documentation of this file.
1 /* Copyright (c) 2017, TU Wien
2  All rights reserved.
3 
4  Redistribution and use in source and binary forms, with or without
5  modification, are permitted provided that the following conditions are met:
6  * Redistributions of source code must retain the above copyright
7  notice, this list of conditions and the following disclaimer.
8  * Redistributions in binary form must reproduce the above copyright
9  notice, this list of conditions and the following disclaimer in the
10  documentation and/or other materials provided with the distribution.
11  * Neither the name of the <organization> nor the
12  names of its contributors may be used to endorse or promote products
13  derived from this software without specific prior written permission.
14 
15  THIS SOFTWARE IS PROVIDED BY TU Wien ''AS IS'' AND ANY
16  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18  DISCLAIMED. IN NO EVENT SHALL TU Wien BE LIABLE FOR ANY
19  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 */
26 #include <ros/ros.h>
28 #include <tf/transform_datatypes.h>
29 
30 int main(int argc, char **argv)
31 {
32  ros::init(argc, argv, "local_behavior_controller_node");
34 
36 
37  return 0;
38 }
39 
41 {
43  : n_(n), n_param_("~")
44 {
46  robot_step_ = -1;
47  robot_route_ = tuw_multi_robot_msgs::Route();
48 
49  n_param_.param<std::string>("robot_name", robot_name_, "r0");
50  ROS_INFO("robot name = %s", robot_name_.c_str());
51 
52  n_param_.param<double>("robot_radius", robot_radius_, robot_radius_);
53 
54  n_param_.param<double>("robot_default_radius", robotDefaultRadius_, 0.3);
55 
56  n_param_.param<std::string>("path_topic", topic_path_, "path");
57 
58  n_param_.param<std::string>("route_topic", topic_route_, "route");
59 
60  n_param_.param<std::string>("robotInfo_topic", topic_robot_info_, "/robot_info");
61 
62  n_param_.param<std::string>("pose_topic", topic_pose_, "pose");
63 
64  n_param_.param<std::string>("frame_map", frame_map_, "map");
65 
66  n_param_.param<std::string>("frame_map", frame_map_, "map");
67 
68  n_param_.param<double>("update_rate", update_rate_, 1.0);
69 
70  subPose_ = n.subscribe<geometry_msgs::PoseWithCovarianceStamped>(topic_pose_, 1,
72  subRobotInfo_ = n.subscribe<tuw_multi_robot_msgs::RobotInfo>(topic_robot_info_, 10000, &LocalBehaviorControllerNode::subRobotInfoCb, this);
73 
74  subRoute_ = n.subscribe<tuw_multi_robot_msgs::Route>(topic_route_, 1,
76 
77  pubRobotInfo_ = n.advertise<tuw_multi_robot_msgs::RobotInfo>(topic_robot_info_, 10000);
78  pubPath_ = n.advertise<nav_msgs::Path>(topic_path_, 1);
79 
80 
81  ros::Rate r(update_rate_);
82 
83  while (ros::ok())
84  {
85  r.sleep();
86  ros::spinOnce();
88  }
89 }
90 
91 void LocalBehaviorControllerNode::publishPath(std::vector<Eigen::Vector3d> _p)
92 {
93  nav_msgs::Path path;
94  ros::Time now = ros::Time::now();
95  path.header.stamp = now;
96  path.header.frame_id = frame_map_;
97 
98  for(auto&& p : _p)
99  {
100  geometry_msgs::PoseStamped ps;
101  ps.header.stamp = now;
102  ps.header.frame_id = frame_map_;
103 
104  ps.pose.position.x = p[0];
105  ps.pose.position.y = p[1];
106 
107  Eigen::Quaternion<float> q;
108  q = Eigen::AngleAxisf(p[2], Eigen::Vector3f::UnitZ());
109 
110  ps.pose.orientation.x = q.x();
111  ps.pose.orientation.y = q.y();
112  ps.pose.orientation.z = q.z();
113  ps.pose.orientation.w = q.w();
114 
115  path.poses.push_back(ps);
116  }
117  pubPath_.publish(path);
118 }
119 
121 {
122  if(robot_route_.segments.size() == 0)
123  return;
124 
125  std::vector<Eigen::Vector3d> path;
126  // std::vector<PathSegment> seg_path;
127 
128  auto&& seg = robot_route_.segments.begin();
129  seg += robot_step_;
130 
131  bool valid = true;
132 
133  // go through all segments in the route
134  for(; valid && seg != robot_route_.segments.end(); seg++)
135  {
136  // go through all preconditions and check if they are fulfilled
137  for(auto&& prec : seg->preconditions)
138  {
139  if(prec.robot_id != robot_name_ && robot_steps_[prec.robot_id] < prec.current_route_segment)
140  {
141  valid = false;
142  }
143  }
144 
145  // add segments to path as long as the prec. are fulfilled
146  if(valid)
147  {
148  Eigen::Vector3d pose;
149 
150  double r, p, y;
151  tf::Quaternion q(seg->end.orientation.x, seg->end.orientation.y, seg->end.orientation.z, seg->end.orientation.w);
152  tf::Matrix3x3(q).getRPY(r, p, y);
153 
154  pose[0] = seg->end.position.x;
155  pose[1] = seg->end.position.y;
156  pose[2] = y;
157 
158  path.emplace_back(pose);
159  }
160  }
161 
162  if(path.size() > 1)
163  {
164  publishPath(path);
165  }
166 }
167 
168 void LocalBehaviorControllerNode::subRouteCb(const tuw_multi_robot_msgs::Route::ConstPtr &_route)
169 {
170  robot_route_ = *_route;
171 
172  if(robot_route_.segments.size() == 0)
173  return;
174 
175  // clear when new route arrives
176  robot_step_ = 0;
177 
178  std::vector<PathSegment> seg_path;
179 
180  // go through all segments in the route
181  for(auto&& seg : robot_route_.segments)
182  {
183  PathSegment path_seg;
184  path_seg.start[0] = seg.start.position.x;
185  path_seg.start[1] = seg.start.position.y;
186  path_seg.goal[0] = seg.end.position.x;
187  path_seg.goal[1] = seg.end.position.y;
188  path_seg.width = seg.width;
189 
190  seg_path.emplace_back(path_seg);
191  }
192 
193  observer_.init(seg_path);
194 }
195 
196 void LocalBehaviorControllerNode::subPoseCb(const geometry_msgs::PoseWithCovarianceStampedConstPtr &_pose)
197 {
198  robot_pose_ = _pose->pose;
199  bool changed = true;
200 
201  robot_step_ = observer_.getStep(Eigen::Vector2d(_pose->pose.pose.position.x, _pose->pose.pose.position.y), changed);
202 }
203 
205  const tuw_multi_robot_msgs::RobotInfo_<std::allocator<void> >::ConstPtr &_robot_info)
206 {
207  robot_steps_[_robot_info->sync.robot_id] = _robot_info->sync.current_route_segment;
208  updatePath();
209 }
210 
212 {
213  tuw_multi_robot_msgs::RobotInfo ri;
214  ri.header.stamp = ros::Time::now();
215  ri.header.frame_id = frame_map_;
216  ri.robot_name = robot_name_;
217  ri.pose = robot_pose_;
218  ri.shape = ri.SHAPE_CIRCLE;
219  ri.shape_variables.push_back(robot_radius_);
220  ri.sync.robot_id = robot_name_;
221  ri.sync.current_route_segment = robot_step_;
222  ri.mode = ri.MODE_NA;
223  ri.status = ri.STATUS_STOPPED; // TODO
224  ri.good_id = ri.GOOD_NA;
225 
227 }
228 } // namespace tuw_multi_robot_route_to_path
int getStep(const Eigen::Vector2d &_odom, bool &_changed)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int init(const std::vector< PathSegment > &_path)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::NodeHandle n_param_
Node handler to the current node.
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void subRouteCb(const tuw_multi_robot_msgs::Route::ConstPtr &_route)
static Time now()
void subPoseCb(const geometry_msgs::PoseWithCovarianceStampedConstPtr &_pose)
ROSCPP_DECL void spinOnce()
void subRobotInfoCb(const tuw_multi_robot_msgs::RobotInfo::ConstPtr &_robot_info)
int main(int argc, char **argv)


tuw_multi_robot_local_behavior_controller
Author(s): Benjamin Binder
autogenerated on Mon Jun 10 2019 15:42:36