simpleVelocityNavigationWithLaser

app/simpleVelocityNavigationExamples/scripts/simpleVelocityNavigationExample1.xml

@@ -36,13 +36,13 @@
 
 <module>
    <name>yarpdev</name>
-   <parameters>--device Rangefinder2DWrapper --subdevice fakeLaser --period 10 --name /ikart/laser:o --test use_mapfile --map_file C:\\software\\maps\\map_test.map</parameters>
+   <parameters>--device Rangefinder2DWrapper --subdevice fakeLaser --period 10 --name /robot_2wheels/laser:o --test use_mapfile --map_file C:\\software\\maps\\map_test.map</parameters>
    <node>console</node>
 </module>
 
 <module>
    <name>yarpdev</name>
-   <parameters>--device navigation2DServer --subdevice simpleVelocityNavigation --context simpleVelocityNavigationExamples --from robotGoto_robot_2wheels.ini</parameters>
+   <parameters>--device navigation2DServer --subdevice simpleVelocityNavigation --context simpleVelocityNavigationExamples --from simpleVelocityControl.ini</parameters>
    <node>console</node>
 </module>
 

app/simpleVelocityNavigationExamples/scripts/simpleVelocityNavigationExample2.xml

@@ -0,0 +1,61 @@
+<application>
+<name>simpleVelocityNavigation (with Laser) EXAMPLE2</name>
+
+<dependencies>
+</dependencies>
+
+<module>
+   <name>yarpdev</name>
+   <parameters>--device transformServer --ROS "(enable_ros_publisher 0)" "(enable_ros_subscriber 0)"</parameters>
+   <node>console</node>
+</module>
+
+<module>
+   <name>yarpdev</name>
+   <parameters>--device fakeMotionControl --name /robot_2wheels/mobile_base --GENERAL::Joints 2</parameters>
+   <node>console</node>
+</module>
+
+<module>
+   <name>baseControl</name>
+   <parameters>--context baseControl_SIM --from robot_2wheels.ini --skip_robot_interface_check</parameters>
+   <node>console</node>
+</module>
+
+<module>
+   <name>yarpdev</name>
+   <parameters>--device localization2DServer --subdevice odomLocalizer --context simpleVelocityNavigationExamples --from localizationServer.ini</parameters>
+   <node>console</node>
+</module>
+
+<module>
+   <name>yarpdev</name>
+   <parameters>--device map2DServer --mapCollectionContext mapsExample</parameters>
+   <node>console</node>
+</module>
+
+<module>
+   <name>yarpdev</name>
+   <parameters>--device Rangefinder2DWrapper --subdevice fakeLaser --period 10 --name /robot_2wheels/laser:o --test use_mapfile --map_file C:\\software\\maps\\map_test.map</parameters>
+   <node>console</node>
+</module>
+
+<module>
+   <name>yarpdev</name>
+   <parameters>--device navigation2DServer --subdevice simpleVelocityNavigationWithLaser --context simpleVelocityNavigationExamples --from simpleVelocityControl.ini</parameters>
+   <node>console</node>
+</module>
+
+<connection>
+  <from>/simpleVelocityNavigation/control:o</from>
+  <to>/baseControl/control:i</to>
+  <protocol>udp</protocol>
+</connection>
+
+<connection>
+  <from>/baseControl/odometry:o</from>
+  <to>/fakeLaser/location:i</to>
+  <protocol>tcp</protocol>
+</connection>
+
+</application>

src/common/CMakeLists.txt

@@ -8,12 +8,14 @@ set(PROJECT_NAME navigation_lib)
 set(LIBRARY_TARGET_NAME ${PROJECT_NAME})
 
 set(${LIBRARY_TARGET_NAME}_SRC
-        movable_localization_device/movable_localization_device.cpp)
+        movable_localization_device/movable_localization_device.cpp
+        obstacle_detection_avoidance/obstacles.cpp)
 
 
 set(${LIBRARY_TARGET_NAME}_HDR
         movable_localization_device/movable_localization_device.h
-        include/navigation_defines.h)
+        include/navigation_defines.h
+        obstacle_detection_avoidance/obstacles.h)
 
 add_library(${LIBRARY_TARGET_NAME} ${${LIBRARY_TARGET_NAME}_SRC} ${${LIBRARY_TARGET_NAME}_HDR})
 add_library(${PROJECT_NAME}::${LIBRARY_TARGET_NAME} ALIAS ${LIBRARY_TARGET_NAME})
@@ -22,6 +24,7 @@ set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES VERSION       0}
                                                         PUBLIC_HEADER "${${LIBRARY_TARGET_NAME}_HDR}")
 
 target_include_directories(${LIBRARY_TARGET_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/movable_localization_device>"
+                                                         "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/obstacle_detection_avoidance>"
                                                          "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                          "$<INSTALL_INTERFACE:$<INSTALL_PREFIX>/${CMAKE_INSTALL_INCLUDEDIR}>")
 

src/navigationDevices/CMakeLists.txt

@@ -9,3 +9,4 @@ add_subdirectory(robotGotoDevice)
 add_subdirectory(robotPathPlannerDevice)
 add_subdirectory(rosNavigator)
 add_subdirectory(simpleVelocityNavigation)
+add_subdirectory(simpleVelocityNavigationWithLaser)

src/navigationDevices/robotGotoDevice/CMakeLists.txt

@@ -13,13 +13,14 @@ yarp_prepare_plugin(robotGotoDev
 
 set(CMAKE_INCLUDE_CURRENT_DIR ON)
 
-yarp_add_plugin(robotGotoDev robotGotoDev.h robotGotoDev.cpp robotGotoCtrl.h robotGotoCtrl.cpp obstacles.h obstacles.cpp )
+yarp_add_plugin(robotGotoDev robotGotoDev.h robotGotoDev.cpp robotGotoCtrl.h robotGotoCtrl.cpp)
 
 target_link_libraries(robotGotoDev YARP::YARP_os
                                    YARP::YARP_sig
                                    YARP::YARP_dev
                                    YARP::YARP_math
-                                   YARP::YARP_rosmsg)
+                                   YARP::YARP_rosmsg
+                                   navigation_lib)
 
 
 yarp_install(TARGETS robotGotoDev

src/navigationDevices/robotGotoDevice/robotGotoCtrl.h

@@ -45,7 +45,7 @@
 #include <yarp/rosmsg/visualization_msgs/MarkerArray.h>
 #include <yarp/rosmsg/geometry_msgs/PoseStamped.h>
 #include <yarp/rosmsg/nav_msgs/Path.h>
-#include "obstacles.h"
+#include <obstacles.h>
 
 using namespace std;
 using namespace yarp::os;

src/navigationDevices/simpleVelocityNavigation/simpleVelocityNavigation.cpp

@@ -1,19 +1,9 @@
 /*
- * Copyright (C)2018 ICub Facility - Istituto Italiano di Tecnologia
- * Author: Marco Randazzo
- * email:  [email protected]
- * website: www.robotcub.org
- * Permission is granted to copy, distribute, and/or modify this program
- * under the terms of the GNU General Public License, version 2 or any
- * later version published by the Free Software Foundation.
- *
- * A copy of the license can be found at
- * http://www.robotcub.org/icub/license/gpl.txt
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
- * Public License for more details
+�   Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT)
+�   All rights reserved.
+�
+�   This software may be modified and distributed under the terms of the
+�   GPL-2+ license. See the accompanying LICENSE file for details.
 */
 
 #include <yarp/os/LogStream.h>
@@ -134,19 +124,19 @@ void simpleVelocityNavigation::send_command(control_type control_data)
 
 bool simpleVelocityNavigation::gotoTargetByAbsoluteLocation(Map2DLocation loc)
 {
-    yError() << "gotoTargetByAbsoluteLocation() Not implemented by simpleVelocityNavigation";
+    yError() << "gotoTargetByAbsoluteLocation() Not implemented by " << m_localName;
     return NOT_IMPLEMENTED;
 }
 
 bool simpleVelocityNavigation::gotoTargetByRelativeLocation(double x, double y)
 {
-    yError() << "gotoTargetByRelativeLocation() Not implemented by simpleVelocityNavigation";
+    yError() << "gotoTargetByRelativeLocation() Not implemented by " << m_localName;
     return NOT_IMPLEMENTED;
 }
 
 bool simpleVelocityNavigation::gotoTargetByRelativeLocation(double x, double y, double theta)
 {
-    yError() << "gotoTargetByRelativeLocation() Not implemented by simpleVelocityNavigation";
+    yError() << "gotoTargetByRelativeLocation() Not implemented by " << m_localName;
     return NOT_IMPLEMENTED;
 }
 
@@ -168,54 +158,54 @@ bool simpleVelocityNavigation::getNavigationStatus(yarp::dev::NavigationStatusEn
 
 bool simpleVelocityNavigation::stopNavigation()
 {
-    yError() << "stopNavigation() Not implemented by simpleVelocityNavigation";
+    yError() << "stopNavigation() Not implemented by " << m_localName;
     return NOT_IMPLEMENTED;
 }
 
 bool simpleVelocityNavigation::getAbsoluteLocationOfCurrentTarget(Map2DLocation& target)
 {
-    yError() << "getAbsoluteLocationOfCurrentTarget() Not implemented by simpleVelocityNavigation";
+    yError() << "getAbsoluteLocationOfCurrentTarget() Not implemented by " << m_localName;
     return NOT_IMPLEMENTED;
 }
 
 bool simpleVelocityNavigation::getRelativeLocationOfCurrentTarget(double& x, double& y, double& theta)
 {
-    yError() << "getRelativeLocationOfCurrentTarget() Not implemented by simpleVelocityNavigation";
+    yError() << "getRelativeLocationOfCurrentTarget() Not implemented by " << m_localName;
     return NOT_IMPLEMENTED;
 }
 
 bool simpleVelocityNavigation::getAllNavigationWaypoints(Map2DPath& waypoints)
 {
-    yError() << "getAllNavigationWaypoints() Not implemented by simpleVelocityNavigation";
+    yError() << "getAllNavigationWaypoints() Not implemented by " << m_localName;
     return NOT_IMPLEMENTED;
 }
 
 bool simpleVelocityNavigation::getCurrentNavigationWaypoint(Map2DLocation& curr_waypoint)
 {
-    yError() << "getCurrentNavigationWaypoint() Not implemented by simpleVelocityNavigation";
+    yError() << "getCurrentNavigationWaypoint() Not implemented by " << m_localName;
     return NOT_IMPLEMENTED;
 }
 
 bool simpleVelocityNavigation::suspendNavigation(double time)
 {
-    yError() << "suspendNavigation() Not implemented by simpleVelocityNavigation";
+    yError() << "suspendNavigation() Not implemented by " << m_localName;
     return NOT_IMPLEMENTED;
 }
 
 bool simpleVelocityNavigation::resumeNavigation()
 {
-    yError() << "resumeNavigation() Not implemented by simpleVelocityNavigation";
+    yError() << "resumeNavigation() Not implemented by " << m_localName;
     return NOT_IMPLEMENTED;
 }
 
 bool simpleVelocityNavigation::recomputeCurrentNavigationPath()
 {
-    yError() << "recomputeCurrentNavigationPath() Not implemented by simpleVelocityNavigation";
+    yError() << "recomputeCurrentNavigationPath() Not implemented by " << m_localName;
     return NOT_IMPLEMENTED;
 }
 
 bool simpleVelocityNavigation::getCurrentNavigationMap(yarp::dev::NavigationMapTypeEnum map_type, MapGrid2D& map)
 {
-    yError() << "getCurrentNavigationMap() Not implemented by simpleVelocityNavigation";
+    yError() << "getCurrentNavigationMap() Not implemented by " << m_localName;
     return NOT_IMPLEMENTED;
 }

src/navigationDevices/simpleVelocityNavigation/simpleVelocityNavigation.h

@@ -1,19 +1,9 @@
-/* 
- * Copyright (C)2018 ICub Facility - Istituto Italiano di Tecnologia
- * Author: Marco Randazzo
- * email:  [email protected]
- * website: www.robotcub.org
- * Permission is granted to copy, distribute, and/or modify this program
- * under the terms of the GNU General Public License, version 2 or any
- * later version published by the Free Software Foundation.
- *
- * A copy of the license can be found at
- * http://www.robotcub.org/icub/license/gpl.txt
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
- * Public License for more details
+/*
+�   Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT)
+�   All rights reserved.
+�
+�   This software may be modified and distributed under the terms of the
+�   GPL-2+ license. See the accompanying LICENSE file for details.
 */
 
 #include <yarp/os/Network.h>
@@ -27,8 +17,8 @@
 #include <yarp/dev/ILocalization2D.h>
 #include <math.h>
 
-#ifndef NAV_DEVICE_TEMPLATE_H
-#define NAV_DEVICE_TEMPLATE_H
+#ifndef SIMPLE_NAV_VELOCTY_H
+#define SIMPLE_NAV_VELOCTY_H
 
 #define DEFAULT_THREAD_PERIOD 0.02 //s
 
@@ -60,8 +50,8 @@ class simpleVelocityNavigation : public yarp::os::PeriodicThread,
 public:
     simpleVelocityNavigation();
 
-private:
-    void send_command(control_type control_data);
+protected:
+    virtual void send_command(control_type control_data);
 
 public:
     virtual bool open(yarp::os::Searchable& config) override;
@@ -76,22 +66,22 @@ class simpleVelocityNavigation : public yarp::os::PeriodicThread,
     * @param loc the location to be reached
     * @return true/false if the command is accepted
     */
-    bool gotoTargetByAbsoluteLocation(yarp::dev::Nav2D::Map2DLocation loc) override;
+    virtual bool gotoTargetByAbsoluteLocation(yarp::dev::Nav2D::Map2DLocation loc) override;
 
     /**
     * Ask the robot to reach a position defined in the robot reference frame. The final orientation of the goal is unspecified.
     * @param x
     * @param y
     * @return true/false
     */
-    bool gotoTargetByRelativeLocation(double x, double y) override;
+    virtual bool gotoTargetByRelativeLocation(double x, double y) override;
 
     /**
     * //Sets a new relative target, expressed in local (robot) coordinate frame.
     * @param v a three-element vector (x,y,theta) representing the location to be reached
     * @return true/false if the command is accepted
     */
-    bool gotoTargetByRelativeLocation(double x, double y, double theta) override;
+    virtual bool gotoTargetByRelativeLocation(double x, double y, double theta) override;
 
     /**
     * Apply a velocity command. velocities are expressed in the robot reference frame
@@ -108,67 +98,67 @@ class simpleVelocityNavigation : public yarp::os::PeriodicThread,
     * @return a Map2DLocation containing data of the current target.
     * @return true if a target is currently available, false otherwise (in this case returned target is invalid)
     */
-    bool getAbsoluteLocationOfCurrentTarget(yarp::dev::Nav2D::Map2DLocation& target) override;
+    virtual bool getAbsoluteLocationOfCurrentTarget(yarp::dev::Nav2D::Map2DLocation& target) override;
 
     /**
     * //Gets the last target set through a setNewRelTarget command, expressed in absolute coordinates.
     * @param a Map2DLocation containing data of the current target.
     * @return true if a target is currently available, false otherwise (in this case returned target is invalid)
     */
-    bool getRelativeLocationOfCurrentTarget(double& x, double& y, double& theta) override;
+    virtual bool getRelativeLocationOfCurrentTarget(double& x, double& y, double& theta) override;
 
     /**
     * //Gets the status of the current navigation task. Typically stored into navigation_status variable.
     * @return the current navigation status expressed as NavigationStatusEnum.
     */
-    bool getNavigationStatus(yarp::dev::NavigationStatusEnum& status) override;
+    virtual bool getNavigationStatus(yarp::dev::NavigationStatusEnum& status) override;
 
     /**
     * //Stops the current navigation task.
     * @return true/false if the command is executed successfully.
     */
-    bool stopNavigation() override;
+    virtual bool stopNavigation() override;
 
     /**
     * //Pauses the current navigation task.
     * @return true/false if the command is executed successfully.
     */
-    bool suspendNavigation(double time) override;
+    virtual bool suspendNavigation(double time) override;
 
     /**
     * //Resumes a previously paused navigation task.
     * @return true/false if the command is executed successfully.
     */
-    bool resumeNavigation() override;
+    virtual bool resumeNavigation() override;
 
     /**
     * Returns the list of waypoints generated by the navigation algorithm
     * @param waypoints the list of waypoints generated by the navigation algorithm
     * @return true/false
     */
-    bool getAllNavigationWaypoints(yarp::dev::Nav2D::Map2DPath& waypoints) override;
+    virtual bool getAllNavigationWaypoints(yarp::dev::Nav2D::Map2DPath& waypoints) override;
 
     /**
     * Returns the current waypoint pursued by the navigation algorithm
     * @param curr_waypoint the current waypoint pursued by the navigation algorithm
     * @return true/false
     */
-    bool getCurrentNavigationWaypoint(yarp::dev::Nav2D::Map2DLocation& curr_waypoint) override;
+    virtual bool getCurrentNavigationWaypoint(yarp::dev::Nav2D::Map2DLocation& curr_waypoint) override;
 
     /**
     * Returns the current navigation map processed by the navigation algorithm
     * @param map_type the map to be requested (e.g. global, local, etc.)
     * @param map the map, currently used by the navigation algorithm
     * @return true/false
     */
-    bool getCurrentNavigationMap(yarp::dev::NavigationMapTypeEnum map_type, yarp::dev::Nav2D::MapGrid2D& map) override;
+    virtual bool getCurrentNavigationMap(yarp::dev::NavigationMapTypeEnum map_type, yarp::dev::Nav2D::MapGrid2D& map) override;
 
     /**
     * Forces the navigation system to recompute the path from the current robot position to the current goal.
     * If no goal has been set, the command has no effect.
     * @return true/false
     */
-    bool recomputeCurrentNavigationPath() override;
+    virtual bool recomputeCurrentNavigationPath() override;
 };
 
 #endif