28 #include <tf/transform_datatypes.h> 30 int main(
int argc,
char **argv)
32 ros::init(argc, argv,
"local_behavior_controller_node");
43 : n_(n), n_param_(
"~")
50 ROS_INFO(
"robot name = %s", robot_name_.c_str());
95 path.header.stamp = now;
100 geometry_msgs::PoseStamped ps;
101 ps.header.stamp = now;
104 ps.pose.position.x = p[0];
105 ps.pose.position.y = p[1];
107 Eigen::Quaternion<float> q;
108 q = Eigen::AngleAxisf(p[2], Eigen::Vector3f::UnitZ());
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();
115 path.poses.push_back(ps);
125 std::vector<Eigen::Vector3d> path;
134 for(; valid && seg !=
robot_route_.segments.end(); seg++)
137 for(
auto&& prec : seg->preconditions)
148 Eigen::Vector3d pose;
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);
154 pose[0] = seg->end.position.x;
155 pose[1] = seg->end.position.y;
158 path.emplace_back(pose);
178 std::vector<PathSegment> seg_path;
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;
190 seg_path.emplace_back(path_seg);
207 robot_steps_[_robot_info->sync.robot_id] = _robot_info->sync.current_route_segment;
213 tuw_multi_robot_msgs::RobotInfo ri;
218 ri.shape = ri.SHAPE_CIRCLE;
222 ri.mode = ri.MODE_NA;
223 ri.status = ri.STATUS_STOPPED;
224 ri.good_id = ri.GOOD_NA;
int getStep(const Eigen::Vector2d &_odom, bool &_changed)
void publish(const boost::shared_ptr< M > &message) const
tuw_multi_robot_msgs::Route robot_route_
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)
LocalBehaviorControllerNode(ros::NodeHandle &n)
std::string topic_robot_info_
double robotDefaultRadius_
ros::Subscriber subRoute_
int init(const std::vector< PathSegment > &_path)
ros::Publisher pubRobotInfo_
RobotStateObserver observer_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
ros::NodeHandle n_param_
Node handler to the current node.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void subRouteCb(const tuw_multi_robot_msgs::Route::ConstPtr &_route)
void publishPath(std::vector< Eigen::Vector3d > _p)
ros::Subscriber subRobotInfo_
void subPoseCb(const geometry_msgs::PoseWithCovarianceStampedConstPtr &_pose)
ROSCPP_DECL void spinOnce()
geometry_msgs::PoseWithCovariance robot_pose_
std::map< std::string, int > robot_steps_
void subRobotInfoCb(const tuw_multi_robot_msgs::RobotInfo::ConstPtr &_robot_info)
int main(int argc, char **argv)