From 1e5b8dd0d4dbb35e852a582795e97f1a172bc6a0 Mon Sep 17 00:00:00 2001 From: Tongtybj Date: Sun, 12 Jun 2016 10:05:59 +0900 Subject: [PATCH 1/2] Visualize height of UAV (both esitamted state and ground truth) in rviz and modify the config of rviz about task1 and task3 --- jsk_mbzirc_tasks/CMakeLists.txt | 20 ++- jsk_mbzirc_tasks/config/task3.rviz | 62 +++++++- .../mbzirc_gazebo_uav_state_display_plugin.h | 88 +++++++++++ .../launch/jsk_mbzirc_task_1.launch | 13 +- .../launch/jsk_mbzirc_task_3.launch | 26 +++- ...mbzirc_gazebo_uav_state_display_plugin.cpp | 138 ++++++++++++++++++ 6 files changed, 340 insertions(+), 7 deletions(-) create mode 100644 jsk_mbzirc_tasks/include/jsk_mbzirc_tasks/mbzirc_gazebo_uav_state_display_plugin.h create mode 100644 jsk_mbzirc_tasks/src/mbzirc_gazebo_uav_state_display_plugin.cpp diff --git a/jsk_mbzirc_tasks/CMakeLists.txt b/jsk_mbzirc_tasks/CMakeLists.txt index b13d4c7..53cd158 100644 --- a/jsk_mbzirc_tasks/CMakeLists.txt +++ b/jsk_mbzirc_tasks/CMakeLists.txt @@ -4,8 +4,10 @@ project(jsk_mbzirc_tasks) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp) +find_package(catkin REQUIRED COMPONENTS roscpp gazebo_ros) +# Depend on system install of Gazebo and SDFormat +find_package(gazebo REQUIRED) ################################### ## catkin specific configuration ## @@ -14,14 +16,28 @@ catkin_package( CATKIN_DEPENDS ) -include_directories(include ${catkin_INCLUDE_DIRS}) +include_directories(include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS}) add_executable(uav_teleop_keyboard src/uav_teleop_keyboard.cpp) target_link_libraries(uav_teleop_keyboard ${catkin_LIBRARIES}) +# build plugins +add_library(mbzirc_gazebo_uav_state_display_plugin src/mbzirc_gazebo_uav_state_display_plugin.cpp) +target_link_libraries(mbzirc_gazebo_uav_state_display_plugin ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +############# +## Install ## +############# + ############# ## Install ## ############# +install(TARGETS + mbzirc_gazebo_uav_state_display_plugin + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ) + install(DIRECTORY launch urdf config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) diff --git a/jsk_mbzirc_tasks/config/task3.rviz b/jsk_mbzirc_tasks/config/task3.rviz index fdd3e6d..c7de566 100644 --- a/jsk_mbzirc_tasks/config/task3.rviz +++ b/jsk_mbzirc_tasks/config/task3.rviz @@ -1,13 +1,16 @@ Panels: - Class: rviz/Displays - Help Height: 78 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 + - /OverlayText1 + - /OverlayText2 + - /OverlayText3 Splitter Ratio: 0.5 - Tree Height: 124 + Tree Height: 202 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -129,6 +132,7 @@ Visualization Manager: Value: true Visibility: Grid: true + OverlayText: true Remaining Time: true UAV1 LaserScan: true UAV1 RobotModel: true @@ -342,6 +346,60 @@ Visualization Manager: text size: 64 top: 10 width: 370 + - Background Alpha: 0.8 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.8 + Foreground Color: 25; 255; 240 + Name: OverlayText + Overtake Color Properties: true + Overtake Position Properties: true + Topic: /UAV1/ground_truth_overlay + Value: true + font: DejaVu Sans Mono + height: 60 + left: 10 + line width: 2 + text size: 20 + top: 10 + width: 330 + - Background Alpha: 0.8 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.8 + Foreground Color: 25; 255; 240 + Name: OverlayText + Overtake Color Properties: true + Overtake Position Properties: true + Topic: /UAV2/ground_truth_overlay + Value: true + font: DejaVu Sans Mono + height: 60 + left: 10 + line width: 2 + text size: 20 + top: 80 + width: 330 + - Background Alpha: 0.8 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.8 + Foreground Color: 25; 255; 240 + Name: OverlayText + Overtake Color Properties: true + Overtake Position Properties: true + Topic: /UAV3/ground_truth_overlay + Value: true + font: DejaVu Sans Mono + height: 60 + left: 10 + line width: 2 + text size: 20 + top: 150 + width: 330 Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/jsk_mbzirc_tasks/include/jsk_mbzirc_tasks/mbzirc_gazebo_uav_state_display_plugin.h b/jsk_mbzirc_tasks/include/jsk_mbzirc_tasks/mbzirc_gazebo_uav_state_display_plugin.h new file mode 100644 index 0000000..cae5376 --- /dev/null +++ b/jsk_mbzirc_tasks/include/jsk_mbzirc_tasks/mbzirc_gazebo_uav_state_display_plugin.h @@ -0,0 +1,88 @@ +/* + * Copyright (c) 2016, JSK Robotics Laboratory, The University of Tokyo + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef JSK_MBZIRC_TASKS_MBZIRC_GAZEBO_UAV_STATE_DISPLAY_PLUGIN_H +#define JSK_MBZIRC_TASKS_MBZIRC_GAZEBO_UAV_STATE_DISPLAY_PLUGIN_H + +#include +#include +#include +#include +#include + +// ROS +#include +#include + +#include +#include + +// Boost +#include +#include + +#include +#include + +namespace gazebo +{ +class GazeboUavState : public ModelPlugin +{ +public: + GazeboUavState(); + virtual ~GazeboUavState(); + +protected: + virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + virtual void Update(); + +private: + ros::NodeHandle* node_handle_; + + ros::Subscriber state_sub_, ground_truth_sub_; + ros::Publisher string_state_pub_, string_ground_truth_pub_; + + ros::CallbackQueue callback_queue_; + event::ConnectionPtr update_connection_; + + physics::WorldPtr world_; + physics::LinkPtr link_; + std::string link_name_; + + std::string state_topic_, ground_truth_topic_; + + // parameters + std::string robot_name_space_; + + void stateCallback(const nav_msgs::OdometryConstPtr&); + void groundTruthCallback(const nav_msgs::OdometryConstPtr&); +}; +} // namespace gazebo + +#endif // JSK_MBZIRC_TASKS_MBZIRC_GAZEBO_UAV_STATE_DISPLAY_PLUGIN_H diff --git a/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_1.launch b/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_1.launch index 1748d56..5b4bd16 100644 --- a/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_1.launch +++ b/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_1.launch @@ -39,17 +39,26 @@ + + + + + - - + + diff --git a/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_3.launch b/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_3.launch index 8fa8e30..4a5531b 100644 --- a/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_3.launch +++ b/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_3.launch @@ -25,6 +25,14 @@ + + + + @@ -35,6 +43,14 @@ + + + + @@ -45,10 +61,18 @@ + + + + - diff --git a/jsk_mbzirc_tasks/src/mbzirc_gazebo_uav_state_display_plugin.cpp b/jsk_mbzirc_tasks/src/mbzirc_gazebo_uav_state_display_plugin.cpp new file mode 100644 index 0000000..eed0a18 --- /dev/null +++ b/jsk_mbzirc_tasks/src/mbzirc_gazebo_uav_state_display_plugin.cpp @@ -0,0 +1,138 @@ +/* + * Copyright (c) 2016, JSK Robotics Laboratory, The University of Tokyo + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +namespace gazebo +{ + +GazeboUavState::GazeboUavState() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboUavState::~GazeboUavState() +{ + event::Events::DisconnectWorldUpdateBegin(update_connection_); + node_handle_->shutdown(); + delete node_handle_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboUavState::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + world_ = _model->GetWorld(); + link_ = _model->GetLink(); + link_name_ = link_->GetName(); + + // default parameters + robot_name_space_.clear(); + state_topic_.clear(); + ground_truth_topic_.clear(); + + // load parameters from sdf + if (_sdf->HasElement("robotNamespace")) + robot_name_space_ = _sdf->GetElement("robotNamespace")->Get(); + if (_sdf->HasElement("stateTopic")) + state_topic_ = _sdf->GetElement("stateTopic")->Get(); + if (_sdf->HasElement("groundTruthTopic")) + ground_truth_topic_ = _sdf->GetElement("groundTruthTopic")->Get(); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + int argc = 0; + char** argv = NULL; + ros::init(argc, argv, "gazebo", ros::init_options::NoSigintHandler | ros::init_options::AnonymousName); + } + + node_handle_ = new ros::NodeHandle(robot_name_space_); + + if (!state_topic_.empty()) + { + ros::SubscribeOptions ops = ros::SubscribeOptions::create( + state_topic_, 1, + boost::bind(&GazeboUavState::stateCallback, this, _1), + ros::VoidPtr(), &callback_queue_); + state_sub_ = node_handle_->subscribe(ops); + } + + if (!ground_truth_topic_.empty()) + { + ros::SubscribeOptions ops = ros::SubscribeOptions::create( + ground_truth_topic_, 1, + boost::bind(&GazeboUavState::groundTruthCallback, this, _1), + ros::VoidPtr(), &callback_queue_); + ground_truth_sub_ = node_handle_->subscribe(ops); + } + + string_state_pub_ = node_handle_->advertise("string_state", 1); + string_ground_truth_pub_ = node_handle_->advertise("string_ground_truth", 1); + + update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboUavState::Update, this)); +} + +//////////////////////////////////////////////////////////////////////////////// +// Callbacks +void GazeboUavState::stateCallback(const nav_msgs::OdometryConstPtr& state) +{ + std::stringstream ss; + std_msgs::String msg_state; + /* right now, just pub height(z) */ + ss << state->pose.pose.position.z; + msg_state.data = "[" + robot_name_space_ + "state] z: " + ss.str() + "[m]"; + string_state_pub_.publish(msg_state); +} + +void GazeboUavState::groundTruthCallback(const nav_msgs::OdometryConstPtr& state) +{ + std::stringstream ss; + std_msgs::String msg_state; + /* right now, just pub height(z) */ + ss << state->pose.pose.position.z; + msg_state.data = "[" + robot_name_space_ + "ground_truth] z: " + ss.str() + "[m]"; + string_ground_truth_pub_.publish(msg_state); +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboUavState::Update() +{ + // handle callbacks + callback_queue_.callAvailable(); +} + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboUavState) + +} // namespace gazebo + From 82bdb4a32ce494bcf252167bbd8217619c7244b7 Mon Sep 17 00:00:00 2001 From: Tongtybj Date: Tue, 14 Jun 2016 12:10:14 +0900 Subject: [PATCH 2/2] Add type assigment in relay_field.py --- jsk_mbzirc_tasks/CMakeLists.txt | 20 +-- jsk_mbzirc_tasks/config/task1.rviz | 38 +++++ jsk_mbzirc_tasks/config/task3.rviz | 22 ++- .../mbzirc_gazebo_uav_state_display_plugin.h | 88 ----------- .../launch/jsk_mbzirc_task_1.launch | 14 +- .../launch/jsk_mbzirc_task_3.launch | 26 ++-- jsk_mbzirc_tasks/script/relay_field_728.py | 4 + ...mbzirc_gazebo_uav_state_display_plugin.cpp | 138 ------------------ 8 files changed, 74 insertions(+), 276 deletions(-) delete mode 100644 jsk_mbzirc_tasks/include/jsk_mbzirc_tasks/mbzirc_gazebo_uav_state_display_plugin.h delete mode 100644 jsk_mbzirc_tasks/src/mbzirc_gazebo_uav_state_display_plugin.cpp diff --git a/jsk_mbzirc_tasks/CMakeLists.txt b/jsk_mbzirc_tasks/CMakeLists.txt index 53cd158..b13d4c7 100644 --- a/jsk_mbzirc_tasks/CMakeLists.txt +++ b/jsk_mbzirc_tasks/CMakeLists.txt @@ -4,10 +4,8 @@ project(jsk_mbzirc_tasks) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp gazebo_ros) +find_package(catkin REQUIRED COMPONENTS roscpp) -# Depend on system install of Gazebo and SDFormat -find_package(gazebo REQUIRED) ################################### ## catkin specific configuration ## @@ -16,28 +14,14 @@ catkin_package( CATKIN_DEPENDS ) -include_directories(include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS}) +include_directories(include ${catkin_INCLUDE_DIRS}) add_executable(uav_teleop_keyboard src/uav_teleop_keyboard.cpp) target_link_libraries(uav_teleop_keyboard ${catkin_LIBRARIES}) -# build plugins -add_library(mbzirc_gazebo_uav_state_display_plugin src/mbzirc_gazebo_uav_state_display_plugin.cpp) -target_link_libraries(mbzirc_gazebo_uav_state_display_plugin ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - -############# -## Install ## -############# - ############# ## Install ## ############# -install(TARGETS - mbzirc_gazebo_uav_state_display_plugin - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ) - install(DIRECTORY launch urdf config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) diff --git a/jsk_mbzirc_tasks/config/task1.rviz b/jsk_mbzirc_tasks/config/task1.rviz index d553591..dbf03f5 100644 --- a/jsk_mbzirc_tasks/config/task1.rviz +++ b/jsk_mbzirc_tasks/config/task1.rviz @@ -7,6 +7,8 @@ Panels: - /Global Options1 - /Status1 - /OverlayText1 + - /OverlayText2 + - /OverlayText3 Splitter Ratio: 0.5 Tree Height: 428 - Class: rviz/Selection @@ -151,6 +153,42 @@ Visualization Manager: text size: 32 top: 10 width: 500 + - Background Alpha: 0.8 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.8 + Foreground Color: 25; 255; 240 + Name: OverlayText + Overtake Color Properties: true + Overtake Position Properties: true + Topic: /state_overlay + Value: true + font: DejaVu Sans Mono + height: 40 + left: 10 + line width: 2 + text size: 24 + top: 70 + width: 250 + - Background Alpha: 0.8 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.8 + Foreground Color: 255; 0; 0 + Name: OverlayText + Overtake Color Properties: true + Overtake Position Properties: true + Topic: /ground_truth_overlay + Value: true + font: DejaVu Sans Mono + height: 30 + left: 10 + line width: 2 + text size: 18 + top: 110 + width: 200 Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/jsk_mbzirc_tasks/config/task3.rviz b/jsk_mbzirc_tasks/config/task3.rviz index c7de566..d6e3b0d 100644 --- a/jsk_mbzirc_tasks/config/task3.rviz +++ b/jsk_mbzirc_tasks/config/task3.rviz @@ -116,7 +116,6 @@ Visualization Manager: Size (m): 0.01 Style: Flat Squares Topic: /UAV1/scan - Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true @@ -128,7 +127,6 @@ Visualization Manager: Overlay Alpha: 0.5 Queue Size: 2 Transport Hint: raw - Unreliable: false Value: true Visibility: Grid: true @@ -200,7 +198,7 @@ Visualization Manager: Max Color: 255; 255; 255 Max Intensity: 999999 Min Color: 0; 0; 0 - Min Intensity: 0 + Min Intensity: -6.80389e-27 Name: UAV2 LaserScan Position Transformer: XYZ Queue Size: 10 @@ -209,7 +207,6 @@ Visualization Manager: Size (m): 0.01 Style: Flat Squares Topic: /UAV2/scan - Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true @@ -221,10 +218,10 @@ Visualization Manager: Overlay Alpha: 0.5 Queue Size: 2 Transport Hint: raw - Unreliable: false Value: true Visibility: Grid: true + OverlayText: true Remaining Time: true UAV1 Camera: true UAV1 LaserScan: true @@ -301,7 +298,6 @@ Visualization Manager: Size (m): 0.01 Style: Flat Squares Topic: /UAV3/scan - Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true @@ -313,10 +309,10 @@ Visualization Manager: Overlay Alpha: 0.5 Queue Size: 2 Transport Hint: raw - Unreliable: false Value: true Visibility: Grid: true + OverlayText: true Remaining Time: true UAV1 Camera: true UAV1 LaserScan: true @@ -358,12 +354,12 @@ Visualization Manager: Topic: /UAV1/ground_truth_overlay Value: true font: DejaVu Sans Mono - height: 60 + height: 30 left: 10 line width: 2 text size: 20 top: 10 - width: 330 + width: 250 - Background Alpha: 0.8 Background Color: 0; 0; 0 Class: jsk_rviz_plugin/OverlayText @@ -376,12 +372,12 @@ Visualization Manager: Topic: /UAV2/ground_truth_overlay Value: true font: DejaVu Sans Mono - height: 60 + height: 30 left: 10 line width: 2 text size: 20 top: 80 - width: 330 + width: 250 - Background Alpha: 0.8 Background Color: 0; 0; 0 Class: jsk_rviz_plugin/OverlayText @@ -394,12 +390,12 @@ Visualization Manager: Topic: /UAV3/ground_truth_overlay Value: true font: DejaVu Sans Mono - height: 60 + height: 30 left: 10 line width: 2 text size: 20 top: 150 - width: 330 + width: 250 Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/jsk_mbzirc_tasks/include/jsk_mbzirc_tasks/mbzirc_gazebo_uav_state_display_plugin.h b/jsk_mbzirc_tasks/include/jsk_mbzirc_tasks/mbzirc_gazebo_uav_state_display_plugin.h deleted file mode 100644 index cae5376..0000000 --- a/jsk_mbzirc_tasks/include/jsk_mbzirc_tasks/mbzirc_gazebo_uav_state_display_plugin.h +++ /dev/null @@ -1,88 +0,0 @@ -/* - * Copyright (c) 2016, JSK Robotics Laboratory, The University of Tokyo - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef JSK_MBZIRC_TASKS_MBZIRC_GAZEBO_UAV_STATE_DISPLAY_PLUGIN_H -#define JSK_MBZIRC_TASKS_MBZIRC_GAZEBO_UAV_STATE_DISPLAY_PLUGIN_H - -#include -#include -#include -#include -#include - -// ROS -#include -#include - -#include -#include - -// Boost -#include -#include - -#include -#include - -namespace gazebo -{ -class GazeboUavState : public ModelPlugin -{ -public: - GazeboUavState(); - virtual ~GazeboUavState(); - -protected: - virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); - virtual void Update(); - -private: - ros::NodeHandle* node_handle_; - - ros::Subscriber state_sub_, ground_truth_sub_; - ros::Publisher string_state_pub_, string_ground_truth_pub_; - - ros::CallbackQueue callback_queue_; - event::ConnectionPtr update_connection_; - - physics::WorldPtr world_; - physics::LinkPtr link_; - std::string link_name_; - - std::string state_topic_, ground_truth_topic_; - - // parameters - std::string robot_name_space_; - - void stateCallback(const nav_msgs::OdometryConstPtr&); - void groundTruthCallback(const nav_msgs::OdometryConstPtr&); -}; -} // namespace gazebo - -#endif // JSK_MBZIRC_TASKS_MBZIRC_GAZEBO_UAV_STATE_DISPLAY_PLUGIN_H diff --git a/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_1.launch b/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_1.launch index 5b4bd16..d4323a3 100644 --- a/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_1.launch +++ b/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_1.launch @@ -38,19 +38,19 @@ - + + 'text: m.data' --wait-for-start" /> + args="/state /state_overlay jsk_rviz_plugins/OverlayText + 'text: [m.pose.pose.position.z, str]' --wait-for-start" /> - + diff --git a/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_3.launch b/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_3.launch index 4a5531b..4d134fc 100644 --- a/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_3.launch +++ b/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_3.launch @@ -27,12 +27,12 @@ + args="/$(arg uav1_name)/state /$(arg uav1_name)/state_overlay jsk_rviz_plugins/OverlayText + 'text: [m.pose.pose.position.z, str]' --wait-for-start" /> + args="/$(arg uav1_name)/ground_truth/state /$(arg uav1_name)/ground_truth_overlay jsk_rviz_plugins/OverlayText + 'text: [m.pose.pose.position.z, str]' --wait-for-start" /> @@ -45,12 +45,12 @@ + args="/$(arg uav2_name)/state /$(arg uav2_name)/state_overlay jsk_rviz_plugins/OverlayText + 'text: [m.pose.pose.position.z, str]' --wait-for-start" /> + args="/$(arg uav2_name)/ground_truth/state /$(arg uav2_name)/ground_truth_overlay jsk_rviz_plugins/OverlayText + 'text: [m.pose.pose.position.z, str]' --wait-for-start" /> @@ -63,12 +63,14 @@ + args="/$(arg uav3_name)/state /$(arg uav3_name)/state_overlay jsk_rviz_plugins/OverlayText + 'text: [m.pose.pose.position.z, str]' --wait-for-start" /> + + args="/$(arg uav3_name)/ground_truth/state /$(arg uav3_name)/ground_truth_overlay jsk_rviz_plugins/OverlayText + 'text: [m.pose.pose.position.z, str]' --wait-for-start" /> + diff --git a/jsk_mbzirc_tasks/script/relay_field_728.py b/jsk_mbzirc_tasks/script/relay_field_728.py index 8f3c660..a513777 100755 --- a/jsk_mbzirc_tasks/script/relay_field_728.py +++ b/jsk_mbzirc_tasks/script/relay_field_728.py @@ -28,6 +28,7 @@ import rostopic import genpy import std_msgs +import __builtin__ __author__ = 'www.kentaro.wada@gmail.com (Kentaro Wada)' @@ -38,6 +39,9 @@ def _eval_in_dict_impl(dict_, globals_, locals_): type_ = type(v) if type_ is dict: res[k] = _eval_in_dict_impl(v, globals_, locals_) + elif (type_ is list): + val = eval(v[0], globals_, locals_) + res[k] = getattr(__builtin__, v[1])(val) elif (type_ is str) or (type_ is unicode): try: res[k] = eval(v, globals_, locals_) diff --git a/jsk_mbzirc_tasks/src/mbzirc_gazebo_uav_state_display_plugin.cpp b/jsk_mbzirc_tasks/src/mbzirc_gazebo_uav_state_display_plugin.cpp deleted file mode 100644 index eed0a18..0000000 --- a/jsk_mbzirc_tasks/src/mbzirc_gazebo_uav_state_display_plugin.cpp +++ /dev/null @@ -1,138 +0,0 @@ -/* - * Copyright (c) 2016, JSK Robotics Laboratory, The University of Tokyo - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include - -namespace gazebo -{ - -GazeboUavState::GazeboUavState() -{ -} - -//////////////////////////////////////////////////////////////////////////////// -// Destructor -GazeboUavState::~GazeboUavState() -{ - event::Events::DisconnectWorldUpdateBegin(update_connection_); - node_handle_->shutdown(); - delete node_handle_; -} - -//////////////////////////////////////////////////////////////////////////////// -// Load the controller -void GazeboUavState::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) -{ - world_ = _model->GetWorld(); - link_ = _model->GetLink(); - link_name_ = link_->GetName(); - - // default parameters - robot_name_space_.clear(); - state_topic_.clear(); - ground_truth_topic_.clear(); - - // load parameters from sdf - if (_sdf->HasElement("robotNamespace")) - robot_name_space_ = _sdf->GetElement("robotNamespace")->Get(); - if (_sdf->HasElement("stateTopic")) - state_topic_ = _sdf->GetElement("stateTopic")->Get(); - if (_sdf->HasElement("groundTruthTopic")) - ground_truth_topic_ = _sdf->GetElement("groundTruthTopic")->Get(); - - // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) - { - int argc = 0; - char** argv = NULL; - ros::init(argc, argv, "gazebo", ros::init_options::NoSigintHandler | ros::init_options::AnonymousName); - } - - node_handle_ = new ros::NodeHandle(robot_name_space_); - - if (!state_topic_.empty()) - { - ros::SubscribeOptions ops = ros::SubscribeOptions::create( - state_topic_, 1, - boost::bind(&GazeboUavState::stateCallback, this, _1), - ros::VoidPtr(), &callback_queue_); - state_sub_ = node_handle_->subscribe(ops); - } - - if (!ground_truth_topic_.empty()) - { - ros::SubscribeOptions ops = ros::SubscribeOptions::create( - ground_truth_topic_, 1, - boost::bind(&GazeboUavState::groundTruthCallback, this, _1), - ros::VoidPtr(), &callback_queue_); - ground_truth_sub_ = node_handle_->subscribe(ops); - } - - string_state_pub_ = node_handle_->advertise("string_state", 1); - string_ground_truth_pub_ = node_handle_->advertise("string_ground_truth", 1); - - update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboUavState::Update, this)); -} - -//////////////////////////////////////////////////////////////////////////////// -// Callbacks -void GazeboUavState::stateCallback(const nav_msgs::OdometryConstPtr& state) -{ - std::stringstream ss; - std_msgs::String msg_state; - /* right now, just pub height(z) */ - ss << state->pose.pose.position.z; - msg_state.data = "[" + robot_name_space_ + "state] z: " + ss.str() + "[m]"; - string_state_pub_.publish(msg_state); -} - -void GazeboUavState::groundTruthCallback(const nav_msgs::OdometryConstPtr& state) -{ - std::stringstream ss; - std_msgs::String msg_state; - /* right now, just pub height(z) */ - ss << state->pose.pose.position.z; - msg_state.data = "[" + robot_name_space_ + "ground_truth] z: " + ss.str() + "[m]"; - string_ground_truth_pub_.publish(msg_state); -} - -//////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboUavState::Update() -{ - // handle callbacks - callback_queue_.callAvailable(); -} - -// Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboUavState) - -} // namespace gazebo -