[meta-arago] [meta-processor-sdk 0/7] Add ROS navigation demo with obstacle avoidance

[Hongmei Gou]

The major changes include:
* Update mmwave-ros-pkg-master to version 2.5.1
* Add hector-trajectory-server and its dependent packages
* Add patches for rotate-recovery and move-base
* Add mmwave-ros-autonomous-robotics package

Hongmei Gou (7):
mmwave-ros-pkg-master: version update and add setup.bash
hector*: add hector-trajectory-server and its dependent packages
packagegroup-ros-world: add hector* packages
rotate-recovery: add patch for ROS navigation demo
move-base: add patch for ROS navigation demo
mmwave-ros-autonomous-robotics: added for ROS navigation demo with
obstacle avoidance
packagegroup-ros-ti: add mmwave-ros-autonomous-robotics demo

.../hector-map-tools_0.3.5.bb} | 34 +----
.../hector-nav-msgs_0.3.5.bb} | 34 +----
.../hector-trajectory-server_0.3.5.bb} | 35 +----
...navigation-rotate-recovery-mmwave-changes.patch | 88 ++++++++++++
.../0002-navigation-move-base-mmwave-changes.patch | 22 +++
.../move-base_1.12.14.bbappend} | 13 +-
.../rotate-recovery_1.12.14.bbappend} | 13 +-
recipes-ros/packagegroups/packagegroup-ros-ti.bb | 3 +
...p-ros-ti.bb => packagegroup-ros-world.bbappend} | 15 +-
...-turtlebot-bringup-description-launch-xml.patch | 12 ++
...e-launchers-navigation-visualization-rviz.patch | 151 +++++++++++++++++++++
...-turtlebot-navigation-costmap-params-yaml.patch | 22 +++
.../0002-turtlebot-bringup-minimal-launch.patch | 26 ++++
recipes-ros/ros-mmwave/files/setup.bash | 9 ++
....bb => mmwave-ros-autonomous-robotics_2.5.1.bb} | 63 ++++++---
...ter_0.1.0.bb => mmwave-ros-pkg-master_2.5.1.bb} | 18 ++-
16 files changed, 411 insertions(+), 147 deletions(-)
copy recipes-ros/{ros-mmwave/mmwave-ros-pkg-master_0.1.0.bb => hector-slam/hector-map-tools_0.3.5.bb} (2%)
copy recipes-ros/{ros-mmwave/mmwave-ros-pkg-master_0.1.0.bb => hector-slam/hector-nav-msgs_0.3.5.bb} (2%)
copy recipes-ros/{ros-mmwave/mmwave-ros-pkg-master_0.1.0.bb => hector-slam/hector-trajectory-server_0.3.5.bb} (2%)
create mode 100644 recipes-ros/navigation/files/0001-navigation-rotate-recovery-mmwave-changes.patch
create mode 100644 recipes-ros/navigation/files/0002-navigation-move-base-mmwave-changes.patch
copy recipes-ros/{packagegroups/packagegroup-ros-ti.bb => navigation/move-base_1.12.14.bbappend} (1%)
copy recipes-ros/{packagegroups/packagegroup-ros-ti.bb => navigation/rotate-recovery_1.12.14.bbappend} (1%)
copy recipes-ros/packagegroups/{packagegroup-ros-ti.bb => packagegroup-ros-world.bbappend} (1%)
create mode 100644 recipes-ros/ros-mmwave/files/0001-turtlebot-bringup-description-launch-xml.patch
create mode 100644 recipes-ros/ros-mmwave/files/0001-turtlebot-mmwave-launchers-navigation-visualization-rviz.patch
create mode 100644 recipes-ros/ros-mmwave/files/0001-turtlebot-navigation-costmap-params-yaml.patch
create mode 100644 recipes-ros/ros-mmwave/files/0002-turtlebot-bringup-minimal-launch.patch
create mode 100644 recipes-ros/ros-mmwave/files/setup.bash
copy recipes-ros/ros-mmwave/{mmwave-ros-pkg-master_0.1.0.bb => mmwave-ros-autonomous-robotics_2.5.1.bb} (5%)
rename recipes-ros/ros-mmwave/{mmwave-ros-pkg-master_0.1.0.bb => mmwave-ros-pkg-master_2.5.1.bb} (69%)

[Hongmei Gou]

Signed-off-by: Hongmei Gou <***@ti.com>
---
recipes-ros/ros-mmwave/files/setup.bash | 9 +++++++++
...-master_0.1.0.bb => mmwave-ros-pkg-master_2.5.1.bb} | 18 ++++++++++++------
2 files changed, 21 insertions(+), 6 deletions(-)
create mode 100644 recipes-ros/ros-mmwave/files/setup.bash
rename recipes-ros/ros-mmwave/{mmwave-ros-pkg-master_0.1.0.bb => mmwave-ros-pkg-master_2.5.1.bb} (69%)

diff --git a/recipes-ros/ros-mmwave/files/setup.bash b/recipes-ros/ros-mmwave/files/setup.bash
new file mode 100644
index 0000000..ead7a36
--- /dev/null
+++ b/recipes-ros/ros-mmwave/files/setup.bash
@@ -0,0 +1,9 @@
+export ROS_ROOT=/opt/ros/indigo
+export PATH=$PATH:/opt/ros/indigo/bin
+export LD_LIBRARY_PATH=/opt/ros/indigo/lib
+export PYTHONPATH=/usr/lib/python3.5/site-packages:/opt/ros/indigo/lib/python3.5/site-packages
+export ROS_MASTER_URI=http://$SITARA_IP_ADDR:11311
+export ROS_IP=$SITARA_IP_ADDR
+export CMAKE_PREFIX_PATH=/opt/ros/indigo
+export ROS_PACKAGE_PATH=/opt/ros/indigo/share
+touch /opt/ros/indigo/.catkin
diff --git a/recipes-ros/ros-mmwave/mmwave-ros-pkg-master_0.1.0.bb b/recipes-ros/ros-mmwave/mmwave-ros-pkg-master_2.5.1.bb
similarity index 69%
rename from recipes-ros/ros-mmwave/mmwave-ros-pkg-master_0.1.0.bb
rename to recipes-ros/ros-mmwave/mmwave-ros-pkg-master_2.5.1.bb
index 9a984f5..143dd78 100644
--- a/recipes-ros/ros-mmwave/mmwave-ros-pkg-master_0.1.0.bb
+++ b/recipes-ros/ros-mmwave/mmwave-ros-pkg-master_2.5.1.bb
@@ -2,12 +2,16 @@ DESCRIPTION = "This package contains ROS driver for mmWave TI sensor."
SECTION = "devel"
LICENSE = "BSD"

-LIC_FILES_CHKSUM = "file://docs/TI_mmWave_ROS_Driver_manifest.pdf;md5=da4f1cae75b0590538edcb7e23a325d8"
-SRC_URI[base.md5sum] = "e85370690db8652d9d706aa77facce6a"
-SRC_URI[base.sha256sum] = "1c88f501f5e9827299b8172d5895e54cb0178a65879f1d1092761cfe0e48bae4"
-SRC_URI = "http://dev.ti.com/tirex/content/mmwave_industrial_toolbox_2_1_1/labs/lab0006-ros-driver/lab0006_ros_driver_pjt/ti_mmwave_rospkg_v1p2p1.zip;name=base;subdir=${P}"
-SRC_URI += "file://append_launch"
-SRC_URI += "file://append_cfg"
+LIC_FILES_CHKSUM = "file://docs/TI_mmWave_ROS_Driver_manifest.pdf;md5=2f41c03cb07673fdbb8bc449e1d179ae"
+SRC_URI[base.md5sum] = "58594aee8e222ff7b259067ad7da0094"
+SRC_URI[base.sha256sum] = "06a93099e85ef47ad70f96db86a0601ace59512d8cc5c5ea0609285c1497dca1"
+SRC_URI = "http://dev.ti.com/tirex/content/mmwave_industrial_toolbox_2_5_1/labs/lab0006-ros-driver/lab0006_ros_driver_pjt/ti_mmwave_rospkg_v1p4.zip;name=base;subdir=${P}"
+SRC_URI += "file://append_launch \
+ file://append_cfg \
+ file://setup.bash \
+"
+
+PR = "r0"

# do not use rpath
EXTRA_OECMAKE_append = " -DCMAKE_SKIP_RPATH=ON"
@@ -27,8 +31,10 @@ do_install_append() {
cp -r ${S}/launch ${D}${ros_datadir}/ti_mmwave_rospkg/
cp ${WORKDIR}/append_launch/*.launch ${D}${ros_datadir}/ti_mmwave_rospkg/launch/.
cp ${WORKDIR}/append_cfg/*.bt ${D}${ros_datadir}/ti_mmwave_rospkg/cfg/.
+ cp ${WORKDIR}/setup.bash ${D}${ros_prefix}
}

FILES_${PN} += "${ros_libdir}/ti_mmwave_rospkg"
+FILES_${PN} += "${ros_prefix}/setup.bash"

ROS_SPN = "ti_mmwave_rospkg"

[Hongmei Gou]

Signed-off-by: Hongmei Gou <***@ti.com>
---
recipes-ros/packagegroups/packagegroup-ros-world.bbappend | 7 +++++++
1 file changed, 7 insertions(+)
create mode 100644 recipes-ros/packagegroups/packagegroup-ros-world.bbappend

diff --git a/recipes-ros/packagegroups/packagegroup-ros-world.bbappend b/recipes-ros/packagegroups/packagegroup-ros-world.bbappend
new file mode 100644
index 0000000..57d255c
--- /dev/null
+++ b/recipes-ros/packagegroups/packagegroup-ros-world.bbappend
@@ -0,0 +1,7 @@
+PR_append = ".tisdk0"
+
+RDEPENDS_${PN} += "\
+ hector-map-tools \
+ hector-nav-msgs \
+ hector-trajectory-server \
+"

[Hongmei Gou]

Signed-off-by: Hongmei Gou <***@ti.com>
---
recipes-ros/hector-slam/hector-map-tools_0.3.5.bb | 8 ++++++++
recipes-ros/hector-slam/hector-nav-msgs_0.3.5.bb | 8 ++++++++
recipes-ros/hector-slam/hector-trajectory-server_0.3.5.bb | 9 +++++++++
3 files changed, 25 insertions(+)
create mode 100644 recipes-ros/hector-slam/hector-map-tools_0.3.5.bb
create mode 100644 recipes-ros/hector-slam/hector-nav-msgs_0.3.5.bb
create mode 100644 recipes-ros/hector-slam/hector-trajectory-server_0.3.5.bb

diff --git a/recipes-ros/hector-slam/hector-map-tools_0.3.5.bb b/recipes-ros/hector-slam/hector-map-tools_0.3.5.bb
new file mode 100644
index 0000000..4ac5a0c
--- /dev/null
+++ b/recipes-ros/hector-slam/hector-map-tools_0.3.5.bb
@@ -0,0 +1,8 @@
+DESCRIPTION = "hector_map_tools contains some functions related to accessing information from OccupancyGridMap maps."
+SECTION = "devel"
+LICENSE = "BSD"
+LIC_FILES_CHKSUM = "file://package.xml;beginline=17;endline=17;md5=d566ef916e9dedc494f5f793a6690ba5"
+
+DEPENDS = "nav-msgs libeigen"
+
+require recipes-ros/hector-slam/hector-slam.inc
diff --git a/recipes-ros/hector-slam/hector-nav-msgs_0.3.5.bb b/recipes-ros/hector-slam/hector-nav-msgs_0.3.5.bb
new file mode 100644
index 0000000..e8c82c4
--- /dev/null
+++ b/recipes-ros/hector-slam/hector-nav-msgs_0.3.5.bb
@@ -0,0 +1,8 @@
+DESCRIPTION = "hector_nav_msgs contains messages and services used in the hector_slam stack."
+SECTION = "devel"
+LICENSE = "BSD"
+LIC_FILES_CHKSUM = "file://package.xml;beginline=16;endline=16;md5=d566ef916e9dedc494f5f793a6690ba5"
+
+DEPENDS = "cmake-modules nav-msgs geometry-msgs message-generation"
+
+require recipes-ros/hector-slam/hector-slam.inc
diff --git a/recipes-ros/hector-slam/hector-trajectory-server_0.3.5.bb b/recipes-ros/hector-slam/hector-trajectory-server_0.3.5.bb
new file mode 100644
index 0000000..f6afe95
--- /dev/null
+++ b/recipes-ros/hector-slam/hector-trajectory-server_0.3.5.bb
@@ -0,0 +1,9 @@
+DESCRIPTION = "hector_trajectory_server keeps track of tf trajectories extracted from tf data and \
+ makes this data accessible via a service and topic"
+SECTION = "devel"
+LICENSE = "BSD"
+LIC_FILES_CHKSUM = "file://package.xml;beginline=16;endline=16;md5=d566ef916e9dedc494f5f793a6690ba5"
+
+DEPENDS = "cmake-modules tf roscpp hector-nav-msgs nav-msgs hector-map-tools"
+
+require recipes-ros/hector-slam/hector-slam.inc

[Jacob Stiffler]

Post by Hongmei Gou
---
recipes-ros/hector-slam/hector-map-tools_0.3.5.bb | 8 ++++++++
recipes-ros/hector-slam/hector-nav-msgs_0.3.5.bb | 8 ++++++++
recipes-ros/hector-slam/hector-trajectory-server_0.3.5.bb | 9 +++++++++
3 files changed, 25 insertions(+)
create mode 100644 recipes-ros/hector-slam/hector-map-tools_0.3.5.bb
create mode 100644 recipes-ros/hector-slam/hector-nav-msgs_0.3.5.bb
create mode 100644 recipes-ros/hector-slam/hector-trajectory-server_0.3.5.bb
diff --git a/recipes-ros/hector-slam/hector-map-tools_0.3.5.bb b/recipes-ros/hector-slam/hector-map-tools_0.3.5.bb
new file mode 100644
index 0000000..4ac5a0c
--- /dev/null
+++ b/recipes-ros/hector-slam/hector-map-tools_0.3.5.bb
@@ -0,0 +1,8 @@
+DESCRIPTION = "hector_map_tools contains some functions related to accessing information from OccupancyGridMap maps."

There should be a short SUMMARY. The longer DESCRIPTION is optional.

Post by Hongmei Gou
+SECTION = "devel"
+LICENSE = "BSD"
+LIC_FILES_CHKSUM = "file://package.xml;beginline=17;endline=17;md5=d566ef916e9dedc494f5f793a6690ba5"
+
+DEPENDS = "nav-msgs libeigen"
+
+require recipes-ros/hector-slam/hector-slam.inc
diff --git a/recipes-ros/hector-slam/hector-nav-msgs_0.3.5.bb b/recipes-ros/hector-slam/hector-nav-msgs_0.3.5.bb
new file mode 100644
index 0000000..e8c82c4
--- /dev/null
+++ b/recipes-ros/hector-slam/hector-nav-msgs_0.3.5.bb
@@ -0,0 +1,8 @@
+DESCRIPTION = "hector_nav_msgs contains messages and services used in the hector_slam stack."
+SECTION = "devel"
+LICENSE = "BSD"
+LIC_FILES_CHKSUM = "file://package.xml;beginline=16;endline=16;md5=d566ef916e9dedc494f5f793a6690ba5"
+
+DEPENDS = "cmake-modules nav-msgs geometry-msgs message-generation"
+
+require recipes-ros/hector-slam/hector-slam.inc
diff --git a/recipes-ros/hector-slam/hector-trajectory-server_0.3.5.bb b/recipes-ros/hector-slam/hector-trajectory-server_0.3.5.bb
new file mode 100644
index 0000000..f6afe95
--- /dev/null
+++ b/recipes-ros/hector-slam/hector-trajectory-server_0.3.5.bb
@@ -0,0 +1,9 @@
+DESCRIPTION = "hector_trajectory_server keeps track of tf trajectories extracted from tf data and \
+ makes this data accessible via a service and topic"
+SECTION = "devel"
+LICENSE = "BSD"
+LIC_FILES_CHKSUM = "file://package.xml;beginline=16;endline=16;md5=d566ef916e9dedc494f5f793a6690ba5"
+
+DEPENDS = "cmake-modules tf roscpp hector-nav-msgs nav-msgs hector-map-tools"
+
+require recipes-ros/hector-slam/hector-slam.inc

[Hongmei Gou]

Signed-off-by: Hongmei Gou <***@ti.com>
---
...navigation-rotate-recovery-mmwave-changes.patch | 88 ++++++++++++++++++++++
.../navigation/rotate-recovery_1.12.14.bbappend | 5 ++
2 files changed, 93 insertions(+)
create mode 100644 recipes-ros/navigation/files/0001-navigation-rotate-recovery-mmwave-changes.patch
create mode 100644 recipes-ros/navigation/rotate-recovery_1.12.14.bbappend

diff --git a/recipes-ros/navigation/files/0001-navigation-rotate-recovery-mmwave-changes.patch b/recipes-ros/navigation/files/0001-navigation-rotate-recovery-mmwave-changes.patch
new file mode 100644
index 0000000..089ac34
--- /dev/null
+++ b/recipes-ros/navigation/files/0001-navigation-rotate-recovery-mmwave-changes.patch
@@ -0,0 +1,88 @@
+diff --git a/rotate_recovery/src/rotate_recovery.cpp b/rotate_recovery/src/rotate_recovery.cpp
+index 64e9999..65959e8 100644
+--- a/rotate_recovery/src/rotate_recovery.cpp
++++ b/rotate_recovery/src/rotate_recovery.cpp
+@@ -3,6 +3,7 @@
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
++* Copyright (c) 2017, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+@@ -38,7 +39,7 @@
+ #include <pluginlib/class_list_macros.h>
+
+ //register this planner as a RecoveryBehavior plugin
+-PLUGINLIB_DECLARE_CLASS(rotate_recovery, RotateRecovery, rotate_recovery::RotateRecovery, nav_core::RecoveryBehavior)
++PLUGINLIB_EXPORT_CLASS(rotate_recovery::RotateRecovery, nav_core::RecoveryBehavior)
+
+ namespace rotate_recovery {
+ RotateRecovery::RotateRecovery(): global_costmap_(NULL), local_costmap_(NULL),
+@@ -54,7 +55,7 @@ void RotateRecovery::initialize(std::string name, tf::TransformListener* tf,
+
+ //get some parameters from the parameter server
+ ros::NodeHandle private_nh("~/" + name_);
+- ros::NodeHandle blp_nh("~/TrajectoryPlannerROS");
++ ros::NodeHandle blp_nh("~/DWAPlannerROS");
+
+ //we'll simulate every degree by default
+ private_nh.param("sim_granularity", sim_granularity_, 0.017);
+@@ -88,8 +89,13 @@ void RotateRecovery::runBehavior(){
+ ROS_ERROR("The costmaps passed to the RotateRecovery object cannot be NULL. Doing nothing.");
+ return;
+ }
+- ROS_WARN("Rotate recovery behavior started.");
++ ROS_WARN("mmWave customized rotate recovery behavior started.");
+
++ ROS_WARN("Clearing costmaps...");
++ global_costmap_->resetLayers();
++ local_costmap_->resetLayers();
++
++ ROS_WARN("Performing rotation...");
+ ros::Rate r(frequency_);
+ ros::NodeHandle n;
+ ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
+@@ -101,15 +107,15 @@ void RotateRecovery::runBehavior(){
+
+ bool got_180 = false;
+
+- double start_offset = 0 - angles::normalize_angle(tf::getYaw(global_pose.getRotation()));
++ double start_offset = angles::normalize_angle_positive(tf::getYaw(global_pose.getRotation()));
+ while(n.ok()){
+ local_costmap_->getRobotPose(global_pose);
+
+- double norm_angle = angles::normalize_angle(tf::getYaw(global_pose.getRotation()));
+- current_angle = angles::normalize_angle(norm_angle + start_offset);
++ double norm_angle = angles::normalize_angle_positive(tf::getYaw(global_pose.getRotation()));
++ current_angle = angles::normalize_angle_positive(norm_angle - start_offset);
+
+ //compute the distance left to rotate
+- double dist_left = M_PI - current_angle;
++ double dist_left = (2 * M_PI) - current_angle;
+
+ double x = global_pose.getOrigin().x(), y = global_pose.getOrigin().y();
+
+@@ -142,12 +148,20 @@ void RotateRecovery::runBehavior(){
+ vel_pub.publish(cmd_vel);
+
+ //makes sure that we won't decide we're done right after we start
+- if(current_angle < 0.0)
++ if ((fabs(current_angle - M_PI) < M_PI/2) && (got_180 == false))
++ {
+ got_180 = true;
++ }
+
+ //if we're done with our in-place rotation... then return
+- if(got_180 && current_angle >= (0.0 - tolerance_))
++ if(got_180 && ((current_angle >= (2*M_PI - tolerance_)) || (current_angle <= (M_PI/2))))
++ {
++ cmd_vel.angular.z = 0;
++ vel_pub.publish(cmd_vel);
++
++ ROS_WARN("Completed rotation...");
+ return;
++ }
+
+ r.sleep();
+ }
diff --git a/recipes-ros/navigation/rotate-recovery_1.12.14.bbappend b/recipes-ros/navigation/rotate-recovery_1.12.14.bbappend
new file mode 100644
index 0000000..435a3d6
--- /dev/null
+++ b/recipes-ros/navigation/rotate-recovery_1.12.14.bbappend
@@ -0,0 +1,5 @@
+PR_append = ".tisdk0"
+
+FILESEXTRAPATHS_prepend := "${THISDIR}/files:"
+
+SRC_URI += "file://0001-navigation-rotate-recovery-mmwave-changes.patch;patchdir=.."

[Jacob Stiffler]

Post by Hongmei Gou
---
...navigation-rotate-recovery-mmwave-changes.patch | 88 ++++++++++++++++++++++
.../navigation/rotate-recovery_1.12.14.bbappend | 5 ++
2 files changed, 93 insertions(+)
create mode 100644 recipes-ros/navigation/files/0001-navigation-rotate-recovery-mmwave-changes.patch
create mode 100644 recipes-ros/navigation/rotate-recovery_1.12.14.bbappend
diff --git a/recipes-ros/navigation/files/0001-navigation-rotate-recovery-mmwave-changes.patch b/recipes-ros/navigation/files/0001-navigation-rotate-recovery-mmwave-changes.patch
new file mode 100644
index 0000000..089ac34
--- /dev/null
+++ b/recipes-ros/navigation/files/0001-navigation-rotate-recovery-mmwave-changes.patch
@@ -0,0 +1,88 @@
+diff --git a/rotate_recovery/src/rotate_recovery.cpp b/rotate_recovery/src/rotate_recovery.cpp

Patches should have an "Upstream-Status".

https://www.openembedded.org/wiki/Commit_Patch_Message_Guidelines#Patch_Header_Recommendations:_Upstream-Status

Post by Hongmei Gou
+index 64e9999..65959e8 100644
+--- a/rotate_recovery/src/rotate_recovery.cpp
++++ b/rotate_recovery/src/rotate_recovery.cpp
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2009, Willow Garage, Inc.
++* Copyright (c) 2017, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ #include <pluginlib/class_list_macros.h>
+
+ //register this planner as a RecoveryBehavior plugin
+-PLUGINLIB_DECLARE_CLASS(rotate_recovery, RotateRecovery, rotate_recovery::RotateRecovery, nav_core::RecoveryBehavior)
++PLUGINLIB_EXPORT_CLASS(rotate_recovery::RotateRecovery, nav_core::RecoveryBehavior)
+
+ namespace rotate_recovery {
+ RotateRecovery::RotateRecovery(): global_costmap_(NULL), local_costmap_(NULL),
+
+ //get some parameters from the parameter server
+ ros::NodeHandle private_nh("~/" + name_);
+- ros::NodeHandle blp_nh("~/TrajectoryPlannerROS");
++ ros::NodeHandle blp_nh("~/DWAPlannerROS");
+
+ //we'll simulate every degree by default
+ private_nh.param("sim_granularity", sim_granularity_, 0.017);
+ ROS_ERROR("The costmaps passed to the RotateRecovery object cannot be NULL. Doing nothing.");
+ return;
+ }
+- ROS_WARN("Rotate recovery behavior started.");
++ ROS_WARN("mmWave customized rotate recovery behavior started.");
+
++ ROS_WARN("Clearing costmaps...");
++ global_costmap_->resetLayers();
++ local_costmap_->resetLayers();
++
++ ROS_WARN("Performing rotation...");
+ ros::Rate r(frequency_);
+ ros::NodeHandle n;
+ ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
+
+ bool got_180 = false;
+
+- double start_offset = 0 - angles::normalize_angle(tf::getYaw(global_pose.getRotation()));
++ double start_offset = angles::normalize_angle_positive(tf::getYaw(global_pose.getRotation()));
+ while(n.ok()){
+ local_costmap_->getRobotPose(global_pose);
+
+- double norm_angle = angles::normalize_angle(tf::getYaw(global_pose.getRotation()));
+- current_angle = angles::normalize_angle(norm_angle + start_offset);
++ double norm_angle = angles::normalize_angle_positive(tf::getYaw(global_pose.getRotation()));
++ current_angle = angles::normalize_angle_positive(norm_angle - start_offset);
+
+ //compute the distance left to rotate
+- double dist_left = M_PI - current_angle;
++ double dist_left = (2 * M_PI) - current_angle;
+
+ double x = global_pose.getOrigin().x(), y = global_pose.getOrigin().y();
+
+ vel_pub.publish(cmd_vel);
+
+ //makes sure that we won't decide we're done right after we start
+- if(current_angle < 0.0)
++ if ((fabs(current_angle - M_PI) < M_PI/2) && (got_180 == false))
++ {
+ got_180 = true;
++ }
+
+ //if we're done with our in-place rotation... then return
+- if(got_180 && current_angle >= (0.0 - tolerance_))
++ if(got_180 && ((current_angle >= (2*M_PI - tolerance_)) || (current_angle <= (M_PI/2))))
++ {
++ cmd_vel.angular.z = 0;
++ vel_pub.publish(cmd_vel);
++
++ ROS_WARN("Completed rotation...");
+ return;
++ }
+
+ r.sleep();
+ }
diff --git a/recipes-ros/navigation/rotate-recovery_1.12.14.bbappend b/recipes-ros/navigation/rotate-recovery_1.12.14.bbappend
new file mode 100644
index 0000000..435a3d6
--- /dev/null
+++ b/recipes-ros/navigation/rotate-recovery_1.12.14.bbappend
@@ -0,0 +1,5 @@
+PR_append = ".tisdk0"
+
+FILESEXTRAPATHS_prepend := "${THISDIR}/files:"
+
+SRC_URI += "file://0001-navigation-rotate-recovery-mmwave-changes.patch;patchdir=.."

[Hongmei Gou]

Signed-off-by: Hongmei Gou <***@ti.com>
---
...-turtlebot-bringup-description-launch-xml.patch | 12 ++
...e-launchers-navigation-visualization-rviz.patch | 151 +++++++++++++++++++++
...-turtlebot-navigation-costmap-params-yaml.patch | 22 +++
.../0002-turtlebot-bringup-minimal-launch.patch | 26 ++++
.../mmwave-ros-autonomous-robotics_2.5.1.bb | 55 ++++++++
5 files changed, 266 insertions(+)
create mode 100644 recipes-ros/ros-mmwave/files/0001-turtlebot-bringup-description-launch-xml.patch
create mode 100644 recipes-ros/ros-mmwave/files/0001-turtlebot-mmwave-launchers-navigation-visualization-rviz.patch
create mode 100644 recipes-ros/ros-mmwave/files/0001-turtlebot-navigation-costmap-params-yaml.patch
create mode 100644 recipes-ros/ros-mmwave/files/0002-turtlebot-bringup-minimal-launch.patch
create mode 100644 recipes-ros/ros-mmwave/mmwave-ros-autonomous-robotics_2.5.1.bb

diff --git a/recipes-ros/ros-mmwave/files/0001-turtlebot-bringup-description-launch-xml.patch b/recipes-ros/ros-mmwave/files/0001-turtlebot-bringup-description-launch-xml.patch
new file mode 100644
index 0000000..cc4a794
--- /dev/null
+++ b/recipes-ros/ros-mmwave/files/0001-turtlebot-bringup-description-launch-xml.patch
@@ -0,0 +1,12 @@
+--- a/launch/includes/description.launch.xml
++++ b/launch/includes/description.launch.xml
+@@ -6,7 +6,9 @@
+ <arg name="stacks"/>
+ <arg name="3d_sensor"/>
+
++<!--
+ <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find turtlebot_description)/robots/$(arg base)_$(arg stacks)_$(arg 3d_sensor).urdf.xacro'" />
+ <param name="robot_description" command="$(arg urdf_file)" />
++ -->
+
+ </launch>
diff --git a/recipes-ros/ros-mmwave/files/0001-turtlebot-mmwave-launchers-navigation-visualization-rviz.patch b/recipes-ros/ros-mmwave/files/0001-turtlebot-mmwave-launchers-navigation-visualization-rviz.patch
new file mode 100644
index 0000000..3956c52
--- /dev/null
+++ b/recipes-ros/ros-mmwave/files/0001-turtlebot-mmwave-launchers-navigation-visualization-rviz.patch
@@ -0,0 +1,151 @@
+--- a/launch/navigation_visualization.rviz
++++ b/launch/navigation_visualization.rviz
+@@ -250,36 +250,6 @@
+ Value: true
+ Visual Enabled: true
+ - Alpha: 1
+- Axes Length: 1
+- Axes Radius: 0.100000001
+- Class: rviz/PoseWithCovariance
+- Color: 255; 25; 0
+- Covariance:
+- Orientation:
+- Alpha: 0.5
+- Color: 255; 255; 127
+- Color Style: Unique
+- Frame: Local
+- Offset: 1
+- Scale: 1
+- Value: true
+- Position:
+- Alpha: 0.300000012
+- Color: 204; 51; 204
+- Scale: 1
+- Value: true
+- Value: true
+- Enabled: true
+- Head Length: 0.300000012
+- Head Radius: 0.100000001
+- Name: PoseWithCovariance
+- Shaft Length: 1
+- Shaft Radius: 0.0500000007
+- Shape: Arrow
+- Topic: /initialpose
+- Unreliable: false
+- Value: true
+- - Alpha: 1
+ Buffer Length: 1
+ Class: rviz/Path
+ Color: 255; 0; 0
+@@ -343,6 +313,36 @@
+ Unreliable: false
+ Use Timestamp: false
+ Value: true
++ - Alpha: 1
++ Axes Length: 1
++ Axes Radius: 0.1
++ Class: rviz_plugin_covariance/PoseWithCovariance
++ Color: 255; 25; 0
++ Covariance:
++ Orientation:
++ Alpha: 0.5
++ Color: 255; 255; 127
++ Color Style: Unique
++ Frame: Local
++ Offset: 1
++ Scale: 1
++ Value: true
++ Position:
++ Alpha: 0.3
++ Color: 204; 51; 204
++ Scale: 1
++ Value: true
++ Value: true
++ Enabled: true
++ Head Length: 0.3
++ Head Radius: 0.1
++ Name: PoseWithCovariance
++ Shaft Length: 1
++ Shaft Radius: 0.05
++ Shape: Arrow
++ Topic: /initialpose
++ Unreliable: false
++ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+--- a/launch/navigation_visualization_2.rviz
++++ b/launch/navigation_visualization_2.rviz
+@@ -251,35 +251,6 @@
+ Visual Enabled: true
+ - Alpha: 1
+ Axes Length: 1
+- Axes Radius: 0.100000001
+- Class: rviz/PoseWithCovariance
+- Color: 255; 25; 0
+- Covariance:
+- Orientation:
+- Alpha: 0.5
+- Color: 255; 255; 127
+- Color Style: Unique
+- Frame: Local
+- Offset: 1
+- Scale: 1
+- Value: true
+- Position:
+- Alpha: 0.300000012
+- Color: 204; 51; 204
+- Scale: 1
+- Value: true
+- Value: true
+- Enabled: false
+- Head Length: 0.300000012
+- Head Radius: 0.100000001
+- Name: PoseWithCovariance
+- Shaft Length: 1
+- Shaft Radius: 0.0500000007
+- Shape: Arrow
+- Topic: /initialpose
+- Unreliable: false
+- Value: false
+- - Alpha: 1
+ Buffer Length: 1
+ Class: rviz/Path
+ Color: 255; 0; 0
+@@ -373,6 +344,36 @@
+ Unreliable: false
+ Use Timestamp: false
+ Value: true
++ - Alpha: 1
++ Axes Length: 1
++ Axes Radius: 0.1
++ Class: rviz_plugin_covariance/PoseWithCovariance
++ Color: 255; 25; 0
++ Covariance:
++ Orientation:
++ Alpha: 0.5
++ Color: 255; 255; 127
++ Color Style: Unique
++ Frame: Local
++ Offset: 1
++ Scale: 1
++ Value: true
++ Position:
++ Alpha: 0.3
++ Color: 204; 51; 204
++ Scale: 1
++ Value: true
++ Value: true
++ Enabled: true
++ Head Length: 0.3
++ Head Radius: 0.1
++ Name: PoseWithCovariance
++ Shaft Length: 1
++ Shaft Radius: 0.05
++ Shape: Arrow
++ Topic: /initialpose
++ Unreliable: false
++ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
diff --git a/recipes-ros/ros-mmwave/files/0001-turtlebot-navigation-costmap-params-yaml.patch b/recipes-ros/ros-mmwave/files/0001-turtlebot-navigation-costmap-params-yaml.patch
new file mode 100644
index 0000000..7fba277
--- /dev/null
+++ b/recipes-ros/ros-mmwave/files/0001-turtlebot-navigation-costmap-params-yaml.patch
@@ -0,0 +1,22 @@
+--- a/param/global_costmap_params.yaml
++++ b/param/global_costmap_params.yaml
+@@ -1,7 +1,7 @@
+ global_costmap:
+ global_frame: /map
+ robot_base_frame: /base_footprint
+- update_frequency: 15
++ update_frequency: 8
+ publish_frequency: 5
+ static_map: false
+ rolling_window: true
+--- a/param/local_costmap_params.yaml
++++ b/param/local_costmap_params.yaml
+@@ -1,7 +1,7 @@
+ local_costmap:
+ global_frame: /odom
+ robot_base_frame: /base_footprint
+- update_frequency: 20
++ update_frequency: 15
+ publish_frequency: 10
+ static_map: false
+ rolling_window: true
diff --git a/recipes-ros/ros-mmwave/files/0002-turtlebot-bringup-minimal-launch.patch b/recipes-ros/ros-mmwave/files/0002-turtlebot-bringup-minimal-launch.patch
new file mode 100644
index 0000000..e09f750
--- /dev/null
+++ b/recipes-ros/ros-mmwave/files/0002-turtlebot-bringup-minimal-launch.patch
@@ -0,0 +1,26 @@
+--- a/launch/minimal.launch
++++ b/launch/minimal.launch
+@@ -1,11 +1,10 @@
+ <launch>
+ <!-- Turtlebot -->
+ <arg name="base" default="kobuki" doc="mobile base type [create, roomba]"/>
+- <arg name="battery" default="$(env TURTLEBOT_BATTERY)" doc="kernel provided locatio for battery info, use /proc/acpi/battery/BAT0 in 2.6 or earlier kernels." />
+- <arg name="stacks" default="$(env TURTLEBOT_STACKS)" doc="stack type displayed in visualisation/simulation [circles, hexagons]"/>
+- <arg name="3d_sensor" default="radar" doc="3d sensor types [kinect, asux_xtion_pro, radar]"/>
+- <arg name="simulation" default="$(env TURTLEBOT_SIMULATION)" doc="set flags to indicate this turtle is run in simulation mode."/>
+- <arg name="serialport" default="$(env TURTLEBOT_SERIAL_PORT)" doc="used by create to configure the port it is connected on [/dev/ttyUSB0, /dev/ttyS0]"/>
++ <arg name="stacks" default="hexagons" doc="stack type displayed in visualisation/simulation [circles, hexagons]"/>
++ <arg name="3d_sensor" default="radar" doc="3d sensor types [kinect, asux_xtion_pro, radar]"/>
++ <arg name="simulation" default="false" doc="set flags to indicate this turtle is run in simulation mode."/>
++ <arg name="serialport" default="/dev/kobuki" doc="used by create to configure the port it is connected on [/dev/ttyUSB0, /dev/ttyS0]"/>
+
+ <param name="/use_sim_time" value="$(arg simulation)"/>
+
+@@ -18,7 +17,4 @@
+ <arg name="base" value="$(arg base)" />
+ <arg name="serialport" value="$(arg serialport)" />
+ </include>
+- <include unless="$(eval arg('battery') == 'None')" file="$(find turtlebot_bringup)/launch/includes/netbook.launch.xml">
+- <arg name="battery" value="$(arg battery)" />
+- </include>
+ </launch>
diff --git a/recipes-ros/ros-mmwave/mmwave-ros-autonomous-robotics_2.5.1.bb b/recipes-ros/ros-mmwave/mmwave-ros-autonomous-robotics_2.5.1.bb
new file mode 100644
index 0000000..1c391e2
--- /dev/null
+++ b/recipes-ros/ros-mmwave/mmwave-ros-autonomous-robotics_2.5.1.bb
@@ -0,0 +1,55 @@
+DESCRIPTION = "This package contains mmwave autonomous robotics demos"
+SECTION = "devel"
+LICENSE = "BSD"
+
+LIC_FILES_CHKSUM = "file://docs/Autonomous_Robotics_with_ROS_for_mmWave_manifest.pdf;md5=052cbb2cc0847a82abb159996f974c35"
+SRC_URI[base.md5sum] = "561921297c73a7ead40bf7486ee8f4f9"
+SRC_URI[base.sha256sum] = "768661b88bd31f083660287045dc49a9441d15cf10b0c48e7d29377a23864191"
+SRC_URI = "http://dev.ti.com/tirex/content/mmwave_industrial_toolbox_2_5_1/labs/lab0008-autonomous-robotics-ros/lab0008_autonomous_robotics_ros/ti_mmwave_ros_map_nav_v1p1.gz;name=base;subdir=${P}"
+
+SRC_URI += "file://0001-turtlebot-bringup-description-launch-xml.patch;patchdir=${WORKDIR}/${P}/turtlebot/turtlebot/turtlebot_bringup \
+ file://0002-turtlebot-bringup-minimal-launch.patch;patchdir=${WORKDIR}/${P}/turtlebot/turtlebot/turtlebot_bringup \
+ file://0001-turtlebot-navigation-costmap-params-yaml.patch;patchdir=${WORKDIR}/${P}/turtlebot/turtlebot_apps/turtlebot_navigation \
+ file://0001-turtlebot-mmwave-launchers-navigation-visualization-rviz.patch;patchdir=${S} \
+"
+
+S = "${WORKDIR}/${P}/turtlebot_mmwave_launchers"
+
+PR = "r1"
+
+inherit catkin
+
+RDEPENDS_${PN} += "bash"
+
+tar_xf() {
+ tar xf ${WORKDIR}/ti_mmwave_ros_map_nav_v1p1 -C ${WORKDIR}/${P}
+}
+
+do_unpack[postfuncs] += "tar_xf"
+
+do_install_append() {
+ install -d ${D}${ros_datadir}/turtlebot_mmwave_launchers
+ cp -r ${S}/launch ${D}${ros_datadir}/turtlebot_mmwave_launchers
+ cp -r ${S}/scripts ${D}${ros_datadir}/turtlebot_mmwave_launchers
+
+ install -d ${D}${ros_datadir}/ti_mmwave_rospkg
+ cp -r ${WORKDIR}/${P}/ti_mmwave_rospkg/cfg ${D}${ros_datadir}/ti_mmwave_rospkg
+
+ install -d ${D}${ros_datadir}/turtlebot_bringup
+ install -d ${D}${ros_datadir}/turtlebot_description
+ cp -r ${WORKDIR}/${P}/turtlebot/turtlebot/turtlebot_bringup/* ${D}${ros_datadir}/turtlebot_bringup
+ cp -r ${WORKDIR}/${P}/turtlebot/turtlebot/turtlebot_description/* ${D}${ros_datadir}/turtlebot_description
+ rm -rf ${D}${ros_datadir}/turtlebot_bringup/patches
+
+ install -d ${D}${ros_datadir}/turtlebot_navigation
+ cp -r ${WORKDIR}/${P}/turtlebot/turtlebot_apps/turtlebot_navigation/* ${D}${ros_datadir}/turtlebot_navigation
+ rm -rf ${D}${ros_datadir}/turtlebot_navigation/patches
+}
+
+FILES_${PN} += "${ros_datadir}/turtlebot_mmwave_launchers"
+FILES_${PN} += "${ros_datadir}/ti_mmwave_rospkg"
+FILES_${PN} += "${ros_datadir}/turtlebot_bringup"
+FILES_${PN} += "${ros_datadir}/turtlebot_description"
+FILES_${PN} += "${ros_datadir}/turtlebot_navigation"
+
+ROS_SPN = "turtlebot_mmwave_launchers"

[Hongmei Gou]

Signed-off-by: Hongmei Gou <***@ti.com>
---
recipes-ros/packagegroups/packagegroup-ros-ti.bb | 3 +++
1 file changed, 3 insertions(+)

diff --git a/recipes-ros/packagegroups/packagegroup-ros-ti.bb b/recipes-ros/packagegroups/packagegroup-ros-ti.bb
index 52d07e7..b3e02f9 100644
--- a/recipes-ros/packagegroups/packagegroup-ros-ti.bb
+++ b/recipes-ros/packagegroups/packagegroup-ros-ti.bb
@@ -3,10 +3,13 @@ LICENSE = "MIT"

inherit packagegroup

+PR = "r1"
+
PACKAGES = "${PN}"

RDEPENDS_${PN} = "\
mmwave-ros-pkg-master \
serial-ros \
octomap-server \
+ mmwave-ros-autonomous-robotics \
"

[Hongmei Gou]

Signed-off-by: Hongmei Gou <***@ti.com>
---
.../0002-navigation-move-base-mmwave-changes.patch | 22 ++++++++++++++++++++++
recipes-ros/navigation/move-base_1.12.14.bbappend | 5 +++++
2 files changed, 27 insertions(+)
create mode 100644 recipes-ros/navigation/files/0002-navigation-move-base-mmwave-changes.patch
create mode 100644 recipes-ros/navigation/move-base_1.12.14.bbappend

diff --git a/recipes-ros/navigation/files/0002-navigation-move-base-mmwave-changes.patch b/recipes-ros/navigation/files/0002-navigation-move-base-mmwave-changes.patch
new file mode 100644
index 0000000..0ab97b1
--- /dev/null
+++ b/recipes-ros/navigation/files/0002-navigation-move-base-mmwave-changes.patch
@@ -0,0 +1,22 @@
+diff --git a/move_base/src/move_base.cpp b/move_base/src/move_base.cpp
+index c608d37..43e0e8c 100644
+--- a/move_base/src/move_base.cpp
++++ b/move_base/src/move_base.cpp
+@@ -3,6 +3,7 @@
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
++* Copyright (c) 2017, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+@@ -662,6 +663,9 @@ namespace move_base {
+ controller_costmap_ros_->start();
+ }
+
++ // Perform initial rotation
++ recovery_behaviors_[recovery_index_]->runBehavior();
++
+ //we want to make sure that we reset the last time we had a valid plan and control
+ last_valid_control_ = ros::Time::now();
+ last_valid_plan_ = ros::Time::now();
diff --git a/recipes-ros/navigation/move-base_1.12.14.bbappend b/recipes-ros/navigation/move-base_1.12.14.bbappend
new file mode 100644
index 0000000..fd3ef69
--- /dev/null
+++ b/recipes-ros/navigation/move-base_1.12.14.bbappend
@@ -0,0 +1,5 @@
+PR_append = ".tisdk0"
+
+FILESEXTRAPATHS_prepend := "${THISDIR}/files:"
+
+SRC_URI += "file://0002-navigation-move-base-mmwave-changes.patch;patchdir=.."

[Jacob Stiffler]

These are pushed. Thank you.

- Jake

Post by Hongmei Gou
* Update mmwave-ros-pkg-master to version 2.5.1
* Add hector-trajectory-server and its dependent packages
* Add patches for rotate-recovery and move-base
* Add mmwave-ros-autonomous-robotics package
mmwave-ros-pkg-master: version update and add setup.bash
hector*: add hector-trajectory-server and its dependent packages
packagegroup-ros-world: add hector* packages
rotate-recovery: add patch for ROS navigation demo
move-base: add patch for ROS navigation demo
mmwave-ros-autonomous-robotics: added for ROS navigation demo with
obstacle avoidance
packagegroup-ros-ti: add mmwave-ros-autonomous-robotics demo
.../hector-map-tools_0.3.5.bb} | 34 +----
.../hector-nav-msgs_0.3.5.bb} | 34 +----
.../hector-trajectory-server_0.3.5.bb} | 35 +----
...navigation-rotate-recovery-mmwave-changes.patch | 88 ++++++++++++
.../0002-navigation-move-base-mmwave-changes.patch | 22 +++
.../move-base_1.12.14.bbappend} | 13 +-
.../rotate-recovery_1.12.14.bbappend} | 13 +-
recipes-ros/packagegroups/packagegroup-ros-ti.bb | 3 +
...p-ros-ti.bb => packagegroup-ros-world.bbappend} | 15 +-
...-turtlebot-bringup-description-launch-xml.patch | 12 ++
...e-launchers-navigation-visualization-rviz.patch | 151 +++++++++++++++++++++
...-turtlebot-navigation-costmap-params-yaml.patch | 22 +++
.../0002-turtlebot-bringup-minimal-launch.patch | 26 ++++
recipes-ros/ros-mmwave/files/setup.bash | 9 ++
....bb => mmwave-ros-autonomous-robotics_2.5.1.bb} | 63 ++++++---
...ter_0.1.0.bb => mmwave-ros-pkg-master_2.5.1.bb} | 18 ++-
16 files changed, 411 insertions(+), 147 deletions(-)
copy recipes-ros/{ros-mmwave/mmwave-ros-pkg-master_0.1.0.bb => hector-slam/hector-map-tools_0.3.5.bb} (2%)
copy recipes-ros/{ros-mmwave/mmwave-ros-pkg-master_0.1.0.bb => hector-slam/hector-nav-msgs_0.3.5.bb} (2%)
copy recipes-ros/{ros-mmwave/mmwave-ros-pkg-master_0.1.0.bb => hector-slam/hector-trajectory-server_0.3.5.bb} (2%)
create mode 100644 recipes-ros/navigation/files/0001-navigation-rotate-recovery-mmwave-changes.patch
create mode 100644 recipes-ros/navigation/files/0002-navigation-move-base-mmwave-changes.patch
copy recipes-ros/{packagegroups/packagegroup-ros-ti.bb => navigation/move-base_1.12.14.bbappend} (1%)
copy recipes-ros/{packagegroups/packagegroup-ros-ti.bb => navigation/rotate-recovery_1.12.14.bbappend} (1%)
copy recipes-ros/packagegroups/{packagegroup-ros-ti.bb => packagegroup-ros-world.bbappend} (1%)
create mode 100644 recipes-ros/ros-mmwave/files/0001-turtlebot-bringup-description-launch-xml.patch
create mode 100644 recipes-ros/ros-mmwave/files/0001-turtlebot-mmwave-launchers-navigation-visualization-rviz.patch
create mode 100644 recipes-ros/ros-mmwave/files/0001-turtlebot-navigation-costmap-params-yaml.patch
create mode 100644 recipes-ros/ros-mmwave/files/0002-turtlebot-bringup-minimal-launch.patch
create mode 100644 recipes-ros/ros-mmwave/files/setup.bash
copy recipes-ros/ros-mmwave/{mmwave-ros-pkg-master_0.1.0.bb => mmwave-ros-autonomous-robotics_2.5.1.bb} (5%)
rename recipes-ros/ros-mmwave/{mmwave-ros-pkg-master_0.1.0.bb => mmwave-ros-pkg-master_2.5.1.bb} (69%)