35 #include <sensor_msgs/LaserScan.h> 36 #include <sensor_msgs/PointCloud.h> 37 #include <geometry_msgs/PoseStamped.h> 38 #include <geometry_msgs/Polygon.h> 39 #include <nav_msgs/Odometry.h> 42 #include <mrpt/version.h> 45 #include <mrpt/nav/reactive/CReactiveNavigationSystem.h> 46 #include <mrpt/maps/CSimplePointsMap.h> 50 #include <mrpt/version.h> 51 #if MRPT_VERSION >= 0x199 52 #include <mrpt/system/CTimeLogger.h> 53 #include <mrpt/config/CConfigFileMemory.h> 54 #include <mrpt/config/CConfigFile.h> 56 using namespace mrpt::config;
82 ros::init(argc, argv,
"mrpt_reactivenav2d");
139 m_parent.
m_profiler,
"getCurrentPoseAndSpeeds");
145 "getCurrentPoseAndSpeeds.lookupTransform_sensor");
162 #if MRPT_VERSION >= 0x199 167 curOdometry = curPose;
170 MRPT_TODO(
"Retrieve current speeds from odometry");
172 "[getCurrentPoseAndSpeeds] Latest pose: %s",
175 curVel.
vx = curV * cos(curPose.
phi);
176 curVel.
vy = curV * sin(curPose.
phi);
195 const double v = vel_cmd_diff_driven->
lin_vel;
196 const double w = vel_cmd_diff_driven->
ang_vel;
198 "changeSpeeds: v=%7.4f m/s w=%8.3f deg/s", v,
200 geometry_msgs::Twist
cmd;
207 bool stop(
bool isEmergency)
override 212 return changeSpeeds(vel_cmd);
232 MRPT_TODO(
"TODO: Check age of obstacles!");
242 mrpt::kinematics::CVehicleVelCmd::Ptr ret =
243 mrpt::kinematics::CVehicleVelCmd::Ptr(
263 : m_auxinit(argc, argv),
266 m_1st_time_init(false),
267 m_target_allowed_distance(0.40
f),
269 m_pub_topic_reactive_nav_goal(
"reactive_nav_goal"),
270 m_sub_topic_local_obstacles(
"local_map_pointcloud"),
271 m_sub_topic_robot_shape(
""),
272 m_frameid_reference(
"/map"),
273 m_frameid_robot(
"base_link"),
274 m_save_nav_log(false),
275 m_reactive_if(*this),
276 m_reactive_nav_engine(m_reactive_if)
279 std::string cfg_file_reactive;
281 "cfg_file_reactive", cfg_file_reactive, cfg_file_reactive);
283 "target_allowed_distance", m_target_allowed_distance,
284 m_target_allowed_distance);
285 m_localn.
param(
"nav_period", m_nav_period, m_nav_period);
287 "frameid_reference", m_frameid_reference, m_frameid_reference);
288 m_localn.
param(
"frameid_robot", m_frameid_robot, m_frameid_robot);
290 "topic_robot_shape", m_sub_topic_robot_shape,
291 m_sub_topic_robot_shape);
292 m_localn.
param(
"save_nav_log", m_save_nav_log, m_save_nav_log);
296 !cfg_file_reactive.empty(),
297 "Mandatory param 'cfg_file_reactive' is missing!");
300 "Config file not found: '%s'", cfg_file_reactive.c_str());
311 catch (std::exception& e)
314 "Exception initializing reactive navigation engine:\n%s",
325 if (!m_sub_topic_robot_shape.empty())
327 m_sub_robot_shape = m_nh.
subscribe<geometry_msgs::Polygon>(
328 m_sub_topic_robot_shape, 1,
331 "Params say robot shape will arrive via topic '%s'... waiting " 333 m_sub_topic_robot_shape.c_str());
341 m_pub_cmd_vel = m_nh.
advertise<geometry_msgs::Twist>(
"cmd_vel", 1);
347 m_sub_nav_goal = m_nh.
subscribe<geometry_msgs::PoseStamped>(
348 m_pub_topic_reactive_nav_goal, 1,
350 m_sub_local_obs = m_nh.
subscribe<sensor_msgs::PointCloud>(
351 m_sub_topic_local_obstacles, 1,
369 "[navigateTo] Starting navigation to %s",
385 navParams.target = target_info;
391 std::lock_guard<std::mutex> csl(m_reactive_nav_engine_cs);
392 m_reactive_nav_engine.
navigate(&navParams);
401 if (!m_1st_time_init)
403 m_1st_time_init =
true;
405 "[ReactiveNav2DNode] Initializing reactive navigation " 408 std::lock_guard<std::mutex> csl(m_reactive_nav_engine_cs);
412 "[ReactiveNav2DNode] Reactive navigation engine init done!");
422 geometry_msgs::PoseStamped trg = *trg_ptr;
424 "Nav target received via topic sub: (%.03f,%.03f, %.03fdeg) " 426 trg.pose.position.x, trg.pose.position.y,
427 trg.pose.orientation.z * 180.0 /
M_PI, trg.header.frame_id.c_str());
430 if (trg.header.frame_id != m_frameid_reference)
434 geometry_msgs::PoseStamped trg2;
446 trg.pose.position.x, trg.pose.position.y, trg.pose.orientation.z));
451 std::lock_guard<std::mutex> csl(m_last_obstacles_cs);
460 "[onRosSetRobotShape] Robot shape received via topic: " 464 poly.resize(newShape->points.size());
465 for (
size_t i = 0; i < newShape->points.size(); i++)
467 poly[i].x = newShape->points[i].x;
468 poly[i].y = newShape->points[i].y;
472 std::lock_guard<std::mutex> csl(m_reactive_nav_engine_cs);
479 int main(
int argc,
char** argv)
ros::NodeHandle m_localn
"~"
void enableLogFile(bool enable)
void onRosGoalReceived(const geometry_msgs::PoseStampedConstPtr &trg_ptr)
TCLAP::CmdLine cmd("carmen2rawlog", ' ', MRPT_getVersion().c_str())
ReactiveNav2DNode(int argc, char **argv)
void publish(const boost::shared_ptr< M > &message) const
ros::Subscriber m_sub_robot_shape
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool ros2mrpt(const sensor_msgs::PointCloud &msg, mrpt::maps::CSimplePointsMap &obj)
virtual bool startWatchdog(float T_ms) override
GLubyte GLubyte GLubyte GLubyte w
bool BASE_IMPEXP fileExists(const std::string &fileName)
MyReactiveInterface m_reactive_if
virtual void sendNavigationEndDueToErrorEvent()
mrpt::system::TTimeStamp now()
ros::Subscriber m_sub_nav_goal
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
tf::TransformListener m_tf_listener
Use to retrieve TF data.
mrpt::kinematics::CVehicleVelCmd::Ptr getStopCmd() override
virtual void sendNavigationStartEvent()
void onRosLocalObstacles(const sensor_msgs::PointCloudConstPtr &obs)
void onRosSetRobotShape(const geometry_msgs::PolygonConstPtr &newShape)
std::string m_sub_topic_robot_shape
virtual void navigate(const TNavigationParams *params)
std::mutex m_last_obstacles_cs
MRPT_TODO("Modify ping to run on Windows + Test this")
void initialize() MRPT_OVERRIDE
std::string m_sub_topic_local_obstacles
ROSCPP_DECL void spin(Spinner &spinner)
ReactiveNav2DNode & m_parent
ros::Publisher m_pub_cmd_vel
std::string m_frameid_robot
virtual bool stopWatchdog() override
ros::Subscriber m_sub_local_obs
mrpt::math::TPose2D target_coords
float targetAllowedDistance
bool senseObstacles(CSimplePointsMap &obstacles, mrpt::system::TTimeStamp ×tamp) override
virtual void loadConfigFile(const mrpt::utils::CConfigFileBase &c) MRPT_OVERRIDE
bool changeSpeeds(const mrpt::kinematics::CVehicleVelCmd &vel_cmd) override
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
#define ROS_ASSERT_MSG(cond,...)
TAuxInitializer m_auxinit
Just to make sure ROS is init first.
bool m_1st_time_init
Reactive initialization done?
std::mutex m_reactive_nav_engine_cs
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
virtual void navigationStep() MRPT_OVERRIDE
std::string m_pub_topic_reactive_nav_goal
int main(int argc, char **argv)
ros::NodeHandle m_nh
The node handle.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
MyReactiveInterface(ReactiveNav2DNode &parent)
virtual void sendNavigationEndEvent()
void convert(const ros::Time &src, mrpt::system::TTimeStamp &des)
mrpt::kinematics::CVehicleVelCmd::Ptr getEmergencyStopCmd() override
void onDoNavigation(const ros::TimerEvent &)
virtual void sendWaySeemsBlockedEvent()
#define ROS_INFO_STREAM(args)
double m_target_allowed_distance
void asString(std::string &s) const
bool getCurrentPoseAndSpeeds(mrpt::math::TPose2D &curPose, mrpt::math::TTwist2D &curVel, mrpt::system::TTimeStamp ×tamp, mrpt::math::TPose2D &curOdometry, std::string &frame_id) override
CSimplePointsMap m_last_obstacles
void changeRobotShape(const mrpt::math::CPolygon &shape)
TAuxInitializer(int argc, char **argv)
CReactiveNavigationSystem m_reactive_nav_engine
ROSCPP_DECL void spinOnce()
ros::Timer m_timer_run_nav
bool stop(bool isEmergency) override
std::string m_frameid_reference
void navigateTo(const mrpt::math::TPose2D &target)
Issue a navigation command.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f