From e392b6609cfcea4a2a7a07b3f8022f1705d772fe Mon Sep 17 00:00:00 2001 From: fmessmer Date: Thu, 15 Feb 2024 15:04:35 +0100 Subject: [PATCH] remove cob_interactive_teleop --- cob_command_tools/package.xml | 1 - cob_interactive_teleop/CHANGELOG.rst | 173 ---------------- cob_interactive_teleop/CMakeLists.txt | 26 --- .../config/interactive_teleop.yaml | 9 - .../interactive_markers_tools.h | 42 ---- .../cob_interactive_teleop/parameters_list.h | 50 ----- .../teleop_cob_marker.h | 142 ------------- .../cob_interactive_teleop/topics_list.h | 34 --- .../launch/interactive_teleop.launch | 15 -- .../launch/interactive_teleop_test.xml | 11 - cob_interactive_teleop/package.xml | 28 --- cob_interactive_teleop/rosdoc.yaml | 6 - .../src/cob_interactive_teleop.cpp | 35 ---- .../src/interactive_markers_tools.cpp | 74 ------- .../src/teleop_cob_marker.cpp | 195 ------------------ 15 files changed, 841 deletions(-) delete mode 100644 cob_interactive_teleop/CHANGELOG.rst delete mode 100644 cob_interactive_teleop/CMakeLists.txt delete mode 100644 cob_interactive_teleop/config/interactive_teleop.yaml delete mode 100644 cob_interactive_teleop/include/cob_interactive_teleop/interactive_markers_tools.h delete mode 100644 cob_interactive_teleop/include/cob_interactive_teleop/parameters_list.h delete mode 100644 cob_interactive_teleop/include/cob_interactive_teleop/teleop_cob_marker.h delete mode 100644 cob_interactive_teleop/include/cob_interactive_teleop/topics_list.h delete mode 100644 cob_interactive_teleop/launch/interactive_teleop.launch delete mode 100644 cob_interactive_teleop/launch/interactive_teleop_test.xml delete mode 100644 cob_interactive_teleop/package.xml delete mode 100644 cob_interactive_teleop/rosdoc.yaml delete mode 100644 cob_interactive_teleop/src/cob_interactive_teleop.cpp delete mode 100644 cob_interactive_teleop/src/interactive_markers_tools.cpp delete mode 100644 cob_interactive_teleop/src/teleop_cob_marker.cpp diff --git a/cob_command_tools/package.xml b/cob_command_tools/package.xml index 9a025951..ac2b3c99 100644 --- a/cob_command_tools/package.xml +++ b/cob_command_tools/package.xml @@ -16,7 +16,6 @@ cob_command_gui cob_dashboard cob_helper_tools - cob_interactive_teleop cob_monitoring cob_script_server cob_teleop diff --git a/cob_interactive_teleop/CHANGELOG.rst b/cob_interactive_teleop/CHANGELOG.rst deleted file mode 100644 index 005244eb..00000000 --- a/cob_interactive_teleop/CHANGELOG.rst +++ /dev/null @@ -1,173 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_interactive_teleop -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.6.33 (2023-11-06) -------------------- - -0.6.32 (2023-04-29) -------------------- - -0.6.31 (2023-01-04) -------------------- - -0.6.30 (2022-11-17) -------------------- - -0.6.29 (2022-07-29) -------------------- - -0.6.28 (2022-03-15) -------------------- - -0.6.27 (2022-01-12) -------------------- - -0.6.26 (2021-11-26) -------------------- - -0.6.25 (2021-08-02) -------------------- - -0.6.24 (2021-07-02) -------------------- - -0.6.23 (2021-07-01) -------------------- - -0.6.22 (2021-05-10) -------------------- - -0.6.21 (2021-04-06) -------------------- - -0.6.20 (2021-01-25) -------------------- - -0.6.19 (2020-12-02) -------------------- -* Merge pull request `#287 `_ from fmessmer/fix_catkin_lint - fix catkin_lint -* fix catkin_lint -* Contributors: Felix Messmer, fmessmer - -0.6.18 (2020-10-21) -------------------- - -0.6.17 (2020-10-17) -------------------- -* Merge pull request `#284 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Contributors: Felix Messmer, fmessmer - -0.6.16 (2020-03-18) -------------------- -* Merge pull request `#271 `_ from fmessmer/ci_updates - [travis] ci updates -* catkin_lint fixes -* Contributors: Felix Messmer, fmessmer - -0.6.15 (2019-11-07) -------------------- - -0.6.14 (2019-08-07) -------------------- - -0.6.13 (2019-07-19) ------------------- - -0.6.12 (2019-06-07) -------------------- - -0.6.11 (2019-04-05) -------------------- - -0.6.10 (2019-03-14) -------------------- - -0.6.9 (2018-07-21) ------------------- - -0.6.8 (2018-07-21) ------------------- - -0.6.7 (2018-01-07) ------------------- -* Merge remote-tracking branch 'origin/indigo_release_candidate' into indigo_dev -* Merge pull request `#197 `_ from ipa-fxm/APACHE_license - use license apache 2.0 -* Merge pull request `#199 `_ from ipa320/ipa-rmb-patch-1 - Changed maintainer -* Changed maintainer -* use license apache 2.0 -* Contributors: Felix Messmer, Richard Bormann, ipa-fxm, ipa-uhr-mk - -0.6.6 (2017-07-17) ------------------- -* manually fix changelog -* Contributors: ipa-fxm - -0.6.5 (2016-10-10) ------------------- - -0.6.4 (2016-04-01) ------------------- - -0.6.3 (2015-08-25) ------------------- -* boost revision -* do not install headers in executable-only packages -* explicit dependency for boost -* remove obsolete autogenerated mainpage.dox files -* remove FILES_MATCHING -* remove trailing whitespaces -* migrate to package format 2 -* sort dependencies -* critically review dependencies -* Contributors: ipa-fxm - -0.6.2 (2015-06-17) ------------------- -* cleanup CMakeLists -* Contributors: ipa-fxm - -0.6.1 (2014-12-15) ------------------- - -0.6.0 (2014-09-18) ------------------- -* fix cppcheck warnings -* Contributors: Florian Weisshardt - -0.5.2 (2014-08-28) ------------------- -* Update package.xml -* Contributors: Florian Weisshardt - -0.5.1 (2014-03-20) ------------------- -* removed compiler warning -* Fixed CMakefiles for teleop stuff. -* merged catkin version -* Initial catkinization. -* Ability to disable 1-DOF and rotation controls -* Ability to change z position of the marker -* move marker higher -* Fix of the teleop when using /map fixed frame in Rviz -* Launch file arguments added -* New launch file to start the teleop -* small fixes for interactive teleop -* Unnecessary files removed -* Revision of the interactive teleop (parameters added, etc.) -* removed blacklist -* update stack -* Removed unnecessary dependencies -* Some minor changes -* Manifest updated -* Manifest updated -* cleanup in interactive teleop -* Include and source directories fixed -* Include and source directory updated -* New COB interactive teleop -* Contributors: abubeck, but-spanel, ipa-fmw diff --git a/cob_interactive_teleop/CMakeLists.txt b/cob_interactive_teleop/CMakeLists.txt deleted file mode 100644 index 38621705..00000000 --- a/cob_interactive_teleop/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_interactive_teleop) - -find_package(catkin REQUIRED COMPONENTS geometry_msgs interactive_markers roscpp std_msgs tf visualization_msgs) - -catkin_package( - CATKIN_DEPENDS geometry_msgs roscpp std_msgs tf visualization_msgs -) - -### Build ### -include_directories(include ${catkin_INCLUDE_DIRS}) - -add_executable(interactive_teleop src/${PROJECT_NAME}.cpp src/interactive_markers_tools.cpp src/teleop_cob_marker.cpp) -add_dependencies(interactive_teleop ${catkin_EXPORTED_TARGETS}) -target_link_libraries(interactive_teleop ${catkin_LIBRARIES}) - -### Install ### -install(TARGETS interactive_teleop - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY config launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/cob_interactive_teleop/config/interactive_teleop.yaml b/cob_interactive_teleop/config/interactive_teleop.yaml deleted file mode 100644 index 1f76858c..00000000 --- a/cob_interactive_teleop/config/interactive_teleop.yaml +++ /dev/null @@ -1,9 +0,0 @@ -max_vel_x: 0.3 -max_vel_y: 0.2 -max_vel_th: 0.3 - -scale_linear: 0.5 -scale_angular: 0.25 - -z_pos: 0.1 -disable_driver: false diff --git a/cob_interactive_teleop/include/cob_interactive_teleop/interactive_markers_tools.h b/cob_interactive_teleop/include/cob_interactive_teleop/interactive_markers_tools.h deleted file mode 100644 index 27b708c9..00000000 --- a/cob_interactive_teleop/include/cob_interactive_teleop/interactive_markers_tools.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef INTERACTIVE_MARKERS_TOOLS_H_ -#define INTERACTIVE_MARKERS_TOOLS_H_ - -#include -#include - -namespace interactive_markers -{ -/** - * @brief Makes circle from markers and assigns it to specified control - * @param control is control to which the circle will be assigned - * @param radius is circle's radius - * @param width is circle's width - * @param color is circle's color - */ -void makeCircle(visualization_msgs::InteractiveMarkerControl &control, - float radius, - float width, - std_msgs::ColorRGBA color - ); - -} - -#endif /* INTERACTIVE_MARKERS_TOOLS_H_ */ - diff --git a/cob_interactive_teleop/include/cob_interactive_teleop/parameters_list.h b/cob_interactive_teleop/include/cob_interactive_teleop/parameters_list.h deleted file mode 100644 index 1ca9d9ea..00000000 --- a/cob_interactive_teleop/include/cob_interactive_teleop/parameters_list.h +++ /dev/null @@ -1,50 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef COB_INTERACTIVE_TELEOP_PARAMETERS_LIST_H -#define COB_INTERACTIVE_TELEOP_PARAMETERS_LIST_H - -#include - -namespace cob_interactive_teleop -{ - -/** - * Names of parameters - */ -const std::string MAX_VEL_X_PARAM = "max_vel_x"; -const std::string MAX_VEL_Y_PARAM = "max_vel_y"; -const std::string MAX_VEL_TH_PARAM = "max_vel_th"; -const std::string SCALE_LINEAR_PARAM = "scale_linear"; -const std::string SCALE_ANGULAR_PARAM = "scale_angular"; -const std::string Z_POS_PARAM = "z_pos"; -const std::string DISABLE_DRIVER_PARAM = "disable_driver"; - -/** - * Default parameter values - */ -const double DEFAULT_MAX_VEL_X = 0.1; -const double DEFAULT_MAX_VEL_Y = 0.1; -const double DEFAULT_MAX_VEL_TH = 0.1; -const double DEFAULT_SCALE_LINEAR = 0.1; -const double DEFAULT_SCALE_ANGULAR = 0.1; -const double DEFAULT_Z_POS = 0.15; - -} - -#endif // COB_INTERACTIVE_TELEOP_PARAMETERS_LIST_H - diff --git a/cob_interactive_teleop/include/cob_interactive_teleop/teleop_cob_marker.h b/cob_interactive_teleop/include/cob_interactive_teleop/teleop_cob_marker.h deleted file mode 100644 index e8d98ac5..00000000 --- a/cob_interactive_teleop/include/cob_interactive_teleop/teleop_cob_marker.h +++ /dev/null @@ -1,142 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef TELEOPCOBMARKER_H_ -#define TELEOPCOBMARKER_H_ - -#include -#include -#include -#include - -#include -#include - -namespace cob_interactive_teleop -{ - -const std::string MARKER_DRIVER_NAME = "marker_driver"; -const std::string MARKER_NAVIGATOR_NAME = "marker_navigator"; -const std::string CONTROL_MOVE_NAME = "control_move"; -const std::string CONTROL_STRAFE_NAME = "control_strafe"; -const std::string CONTROL_ROTATE_NAME = "control_rotate"; -const std::string CONTROL_NAVIGATION_NAME = "controle_navigation"; - -typedef boost::shared_ptr InteractiveMarkerServerPtr; - - -/** - * @brief Motion parameters for the interactive COB teleop. - */ -struct TeleopCOBParams -{ - double max_vel_x, max_vel_y, max_vel_th; - double scale_linear, scale_angular; - double z_pos; - bool disable_driver; - - /** - * @brief Constructor initializes all parameters to default values - */ - TeleopCOBParams() - : max_vel_x(DEFAULT_MAX_VEL_X) - , max_vel_y(DEFAULT_MAX_VEL_Y) - , max_vel_th(DEFAULT_MAX_VEL_TH) - , scale_linear(DEFAULT_SCALE_LINEAR) - , scale_angular(DEFAULT_SCALE_ANGULAR) - , z_pos(DEFAULT_Z_POS) - , disable_driver(false) - {} -}; - - -/** - * @brief This class handles COB driving using Interactive Markers. - * @author Tomas Lokaj - * - * @see http://www.ros.org/wiki/cob_interactive_teleop - */ -class TeleopCOBMarker -{ -public: - /** - * @brief Constructor - */ - TeleopCOBMarker(); - - /** - * @brief Destructor - */ - virtual ~TeleopCOBMarker() - { - server_->erase(MARKER_DRIVER_NAME); - server_->erase(MARKER_NAVIGATOR_NAME); - server_->applyChanges(); - } - - /** - * @brief Returns reference to the motion parameters - */ - TeleopCOBParams& getParams() { return params_; } - - /** - * @brief Changes position of the marekrs to the default state - */ - void reinitMarkers(); - -private: - /** - * @brief Markers feedback - */ - void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); - - /** - * @brief Limits a given velocity - */ - double limitVel(double vel, double limit); - - /** - * @brief Gives the sign (1, -1 or 0) of value - */ - int sign(double value); - - /** - * @brief Creates Interactive Markers - */ - void createMarkers(); - -private: - // Interactive Marker Server - InteractiveMarkerServerPtr server_; - - // Movement publisher - ros::Publisher pub_; - - // Node handles - ros::NodeHandle n_, pn_; - - // Initial position - geometry_msgs::Pose initial_pose_; - - // Teleop parameters - TeleopCOBParams params_; -}; - -} - -#endif /* TELEOPCOBMARKER_H_ */ - diff --git a/cob_interactive_teleop/include/cob_interactive_teleop/topics_list.h b/cob_interactive_teleop/include/cob_interactive_teleop/topics_list.h deleted file mode 100644 index 0aea4462..00000000 --- a/cob_interactive_teleop/include/cob_interactive_teleop/topics_list.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#ifndef COB_INTERACTIVE_TELEOP_TOPICS_LIST_H -#define COB_INTERACTIVE_TELEOP_TOPCIS_LIST_H - -#include - -namespace cob_interactive_teleop -{ - -/** - * Published topics - */ -const std::string BASE_CONTROLLER_COMMAND_TOPIC = "/base_controller/command"; - -} - -#endif // COB_INTERACTIVE_TELEOP_TOPCIS_LIST_H - diff --git a/cob_interactive_teleop/launch/interactive_teleop.launch b/cob_interactive_teleop/launch/interactive_teleop.launch deleted file mode 100644 index d8a10533..00000000 --- a/cob_interactive_teleop/launch/interactive_teleop.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/cob_interactive_teleop/launch/interactive_teleop_test.xml b/cob_interactive_teleop/launch/interactive_teleop_test.xml deleted file mode 100644 index 1bb3c006..00000000 --- a/cob_interactive_teleop/launch/interactive_teleop_test.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/cob_interactive_teleop/package.xml b/cob_interactive_teleop/package.xml deleted file mode 100644 index 1926b0bf..00000000 --- a/cob_interactive_teleop/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - cob_interactive_teleop - 0.6.33 - COB teleop interactive marker for RViz provided by dcgm-robotics@FIT group. - - Richard Bormann - Michal Spanel - - Apache 2.0 - - http://ros.org/wiki/cob_interactive_teleop - - - catkin - - geometry_msgs - interactive_markers - roscpp - std_msgs - tf - visualization_msgs - - rviz - - - - - diff --git a/cob_interactive_teleop/rosdoc.yaml b/cob_interactive_teleop/rosdoc.yaml deleted file mode 100644 index ff87169d..00000000 --- a/cob_interactive_teleop/rosdoc.yaml +++ /dev/null @@ -1,6 +0,0 @@ -- builder: epydoc - output_dir: python -- builder: doxygen - name: C++ API - output_dir: c++ - file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' diff --git a/cob_interactive_teleop/src/cob_interactive_teleop.cpp b/cob_interactive_teleop/src/cob_interactive_teleop.cpp deleted file mode 100644 index caf723cc..00000000 --- a/cob_interactive_teleop/src/cob_interactive_teleop.cpp +++ /dev/null @@ -1,35 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include - -using namespace cob_interactive_teleop; - -/** - * @brief Just a main function which runs COB marker teleop node - */ -int main(int argc, char** argv) -{ - ros::init(argc, argv, "cob_interactive_teleop"); - - new TeleopCOBMarker(); - - ROS_INFO("COB Interactive Teleop is running..."); - - ros::spin(); -} - diff --git a/cob_interactive_teleop/src/interactive_markers_tools.cpp b/cob_interactive_teleop/src/interactive_markers_tools.cpp deleted file mode 100644 index 19424d4f..00000000 --- a/cob_interactive_teleop/src/interactive_markers_tools.cpp +++ /dev/null @@ -1,74 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include - -namespace interactive_markers -{ - -void makeCircle(visualization_msgs::InteractiveMarkerControl &control, - float radius, - float width, - std_msgs::ColorRGBA color - ) -{ - visualization_msgs::Marker marker; - - marker.pose.orientation = control.orientation; - marker.type = visualization_msgs::Marker::TRIANGLE_LIST; - marker.scale.x = radius; - marker.scale.y = radius; - marker.scale.z = radius; - marker.color = color; - - int steps = 36; - std::vector circle1, circle2; - - geometry_msgs::Point v1, v2; - - for (int i = 0; i < steps; i++) - { - float a = float(i) / float(steps) * M_PI * 2.0; - - v1.y = 0.5 * cos(a); - v1.z = 0.5 * sin(a); - - v2.y = (1 + width) * v1.y; - v2.z = (1 + width) * v1.z; - - circle1.push_back(v1); - circle2.push_back(v2); - } - - for (int i = 0; i < steps; i++) - { - int i1 = i; - int i2 = (i + 1) % steps; - - marker.points.clear(); - marker.points.push_back(circle1[i1]); - marker.points.push_back(circle2[i1]); - marker.points.push_back(circle1[i2]); - marker.points.push_back(circle2[i1]); - marker.points.push_back(circle2[i2]); - marker.points.push_back(circle1[i2]); - - control.markers.push_back(marker); - } -} - -} diff --git a/cob_interactive_teleop/src/teleop_cob_marker.cpp b/cob_interactive_teleop/src/teleop_cob_marker.cpp deleted file mode 100644 index e17d341e..00000000 --- a/cob_interactive_teleop/src/teleop_cob_marker.cpp +++ /dev/null @@ -1,195 +0,0 @@ -/* - * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - - -#include -#include - -#include -#include - -//#include - -using namespace interactive_markers; -using namespace visualization_msgs; - -namespace cob_interactive_teleop -{ - -TeleopCOBMarker::TeleopCOBMarker() : pn_("~") -{ - pn_.param(MAX_VEL_X_PARAM, params_.max_vel_x, params_.max_vel_x); - pn_.param(MAX_VEL_Y_PARAM, params_.max_vel_y, params_.max_vel_y); - pn_.param(MAX_VEL_TH_PARAM, params_.max_vel_th, params_.max_vel_th); - pn_.param(SCALE_LINEAR_PARAM, params_.scale_linear, params_.scale_linear); - pn_.param(SCALE_ANGULAR_PARAM, params_.scale_angular, params_.scale_angular); - pn_.param(Z_POS_PARAM, params_.z_pos, params_.z_pos); - pn_.param(DISABLE_DRIVER_PARAM, params_.disable_driver, params_.disable_driver); - - ROS_INFO("max_vel_x = %f", params_.max_vel_x); - ROS_INFO("max_vel_y = %f", params_.max_vel_y); - ROS_INFO("max_vel_th = %f", params_.max_vel_th); - ROS_INFO("scale_linear = %f", params_.scale_linear); - ROS_INFO("scale_angular = %f", params_.scale_angular); - ROS_INFO("z_pos = %f", params_.z_pos); - ROS_INFO("disable_driver = %d", int(params_.disable_driver)); - - server_.reset(new InteractiveMarkerServer("cob_interactive_teleop", "", false)); - pub_ = n_.advertise(BASE_CONTROLLER_COMMAND_TOPIC, 1); - - initial_pose_ = geometry_msgs::Pose(); - initial_pose_.position.z = params_.z_pos; - - // wait for transform to the base_link -/* tf::StampedTransform transform; - tf::TransformListener listener; - try { - listener.waitForTransform("/base_link", "/base_footprint", ros::Time(0), ros::Duration(10.0) ); - listener.lookupTransform("/base_link", "/base_footprint", ros::Time(0), transform); - } - catch (tf::TransformException ex) { - ROS_INFO("Wait for transform failed..."); - }*/ - - createMarkers(); -} - -void TeleopCOBMarker::processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) -{ - geometry_msgs::Twist twist; - - if (feedback->marker_name == MARKER_DRIVER_NAME) - { - twist.linear.x = limitVel(params_.scale_linear * feedback->pose.position.x, params_.max_vel_x); - twist.linear.y = limitVel(params_.scale_linear * feedback->pose.position.y, params_.max_vel_y); - twist.angular.z = limitVel(params_.scale_angular * tf::getYaw(feedback->pose.orientation), params_.max_vel_th); - } - else if (feedback->marker_name == MARKER_NAVIGATOR_NAME) - { - // TODO: use /odom_combined frame to calculate moving direction. for now we use the y displacement for commanding the angular velocity - twist.linear.x = limitVel(params_.scale_linear * feedback->pose.position.x, params_.max_vel_x); - twist.angular.z = sign(feedback->pose.position.x) * limitVel(params_.scale_linear * feedback->pose.position.y, params_.max_vel_th); - } - else - return; - - pub_.publish(twist); - - reinitMarkers(); -} - -void TeleopCOBMarker::reinitMarkers() -{ - if( !params_.disable_driver ) - { - server_->setPose(MARKER_DRIVER_NAME, initial_pose_); - } - server_->setPose(MARKER_NAVIGATOR_NAME, initial_pose_); - server_->applyChanges(); -} - -double TeleopCOBMarker::limitVel(double vel, double limit) -{ - if (fabs(vel) >= limit) - { - return limit*sign(vel); - } - else - { - return vel; - } -} - -int TeleopCOBMarker::sign(double value) -{ - if (value > 0) - return 1; - else if (value <0) - return -1; - else - return 0; -} - -void TeleopCOBMarker::createMarkers() -{ - InteractiveMarker marker_driver; - marker_driver.name = MARKER_DRIVER_NAME; - marker_driver.header.frame_id = "/base_link"; -// marker_driver.header.stamp = ros::Time::now(); - marker_driver.header.stamp = ros::Time(0); - marker_driver.pose = initial_pose_; - marker_driver.scale = 1.5; - - InteractiveMarkerControl control; - control.name = CONTROL_ROTATE_NAME; - control.orientation_mode = InteractiveMarkerControl::FIXED; - control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; - control.orientation.x = 0; - control.orientation.y = 1; - control.orientation.z = 0; - control.orientation.w = 1; - marker_driver.controls.push_back(control); - - control.name = CONTROL_MOVE_NAME; - control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; - control.orientation.x = 1; - control.orientation.y = 0; - control.orientation.z = 0; - control.orientation.w = 1; - marker_driver.controls.push_back(control); - - control.name = CONTROL_STRAFE_NAME; - control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; - control.orientation.x = 0; - control.orientation.y = 0; - control.orientation.z = 1; - control.orientation.w = 1; - marker_driver.controls.push_back(control); - - InteractiveMarker marker_navigator; - marker_navigator.name = MARKER_NAVIGATOR_NAME; - marker_navigator.header.frame_id = "/base_link"; -// marker_navigator.header.stamp = ros::Time::now(); - marker_navigator.header.stamp = ros::Time(0); - marker_navigator.pose = initial_pose_; - marker_navigator.scale = 1.5; - - InteractiveMarkerControl floorControl; - floorControl.name = CONTROL_NAVIGATION_NAME; - floorControl.orientation_mode = InteractiveMarkerControl::INHERIT; - floorControl.interaction_mode = InteractiveMarkerControl::MOVE_PLANE; - floorControl.orientation.x = 0; - floorControl.orientation.y = 1; - floorControl.orientation.z = 0; - floorControl.orientation.w = 1; - std_msgs::ColorRGBA c; - c.r = 1.0; - c.g = 1.0; - c.b = 0.0; - c.a = 0.7; - makeCircle(floorControl, 0.1, 10.0, c); - marker_navigator.controls.push_back(floorControl); - - if( !params_.disable_driver ) - { - server_->insert(marker_driver, boost::bind(&TeleopCOBMarker::processFeedback, this, _1)); - } - server_->insert(marker_navigator, boost::bind(&TeleopCOBMarker::processFeedback, this, _1)); - server_->applyChanges(); -} - -} -