publishers.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #include <locomotor/publishers.h>
36 #include <nav_2d_utils/path_ops.h>
38 #include <nav_msgs/Path.h>
39 #include <geometry_msgs/Twist.h>
40 #include <string>
41 
42 namespace locomotor
43 {
45 {
46  std::string topic, publish_type;
47  nh.param("global_plan_topic", topic, std::string("global_plan"));
48  nh.param("global_plan_type", publish_type, std::string("Path3D"));
49  nh.param("global_plan_epsilon", global_plan_epsilon_, 0.1);
50 
51  if (publish_type == "Path2D")
52  {
54  pub_ = nh.advertise<nav_2d_msgs::Path2D>(topic, 1);
55  }
56  else if (publish_type == "")
57  {
59  }
60  else
61  {
62  if (publish_type != "Path3D")
63  {
64  ROS_ERROR_NAMED("Locomotor", "Unknown global_plan_type \"%s\". Using Path3D instead.", publish_type.c_str());
65  }
67  pub_ = nh.advertise<nav_msgs::Path>(topic, 1);
68  }
69 }
70 
71 void PathPublisher::publishPath(const nav_2d_msgs::Path2D& global_plan)
72 {
73  if (pub_.getNumSubscribers() == 0) return;
74 
75  nav_2d_msgs::Path2D to_publish = global_plan;
76  if (global_plan_epsilon_ >= 0.0)
77  {
78  to_publish = nav_2d_utils::compressPlan(global_plan, global_plan_epsilon_);
79  }
80  switch (path_type_)
81  {
82  case PathType::PATH_2D:
83  pub_.publish(to_publish);
84  break;
85  case PathType::PATH_3D:
87  break;
88  case PathType::NO_PATH:
89  break;
90  };
91 }
92 
94 {
95  std::string topic, publish_type;
96  nh.param("twist_topic", topic, std::string("cmd_vel"));
97  nh.param("twist_type", publish_type, std::string("Twist3D"));
98 
99  ros::NodeHandle global_nh;
100  if (publish_type == "Twist2D")
101  {
102  twist_type_ = TwistType::TWIST_2D;
103  pub_ = global_nh.advertise<nav_2d_msgs::Twist2D>(topic, 1);
104  }
105  else if (publish_type == "Twist2DStamped")
106  {
107  twist_type_ = TwistType::TWIST_2D_STAMPED;
108  pub_ = global_nh.advertise<nav_2d_msgs::Twist2DStamped>(topic, 1);
109  }
110  else if (publish_type == "")
111  {
112  twist_type_ = TwistType::NO_TWIST;
113  }
114  else
115  {
116  if (publish_type != "Twist3D")
117  {
118  ROS_ERROR_NAMED("Locomotor", "Unknown twist_type \"%s\". Using Twist3D instead.", publish_type.c_str());
119  }
120  twist_type_ = TwistType::TWIST_3D;
121  pub_ = global_nh.advertise<geometry_msgs::Twist>(topic, 1);
122  }
123 }
124 
125 void TwistPublisher::publishTwist(const nav_2d_msgs::Twist2DStamped& command)
126 {
127  if (pub_.getNumSubscribers() == 0) return;
128  switch (twist_type_)
129  {
130  case TwistType::TWIST_2D:
131  pub_.publish(command.velocity);
132  break;
134  pub_.publish(command);
135  break;
136  case TwistType::TWIST_3D:
137  pub_.publish(nav_2d_utils::twist2Dto3D(command.velocity));
138  break;
139  case TwistType::NO_TWIST:
140  break;
141  };
142 }
143 
144 } // namespace locomotor
void publish(const boost::shared_ptr< M > &message) const
TwistPublisher(ros::NodeHandle &nh)
Definition: publishers.cpp:93
nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path &path)
void publishPath(const nav_2d_msgs::Path2D &global_plan)
Definition: publishers.cpp:71
geometry_msgs::Twist twist2Dto3D(const nav_2d_msgs::Twist2D &cmd_vel_2d)
PathPublisher(ros::NodeHandle &nh)
Definition: publishers.cpp:44
bool param(const std::string &param_name, T &param_val, const T &default_val) const
nav_2d_msgs::Path2D compressPlan(const nav_2d_msgs::Path2D &input_path, double epsilon=0.1)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
uint32_t getNumSubscribers() const
#define ROS_ERROR_NAMED(name,...)
ros::Publisher pub_
Definition: publishers.h:54
void publishTwist(const nav_2d_msgs::Twist2DStamped &command)
Definition: publishers.cpp:125


locomotor
Author(s):
autogenerated on Sun Jan 10 2021 04:08:39