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/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 e172f48..b9bd884 100644 --- a/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_1.launch +++ b/jsk_mbzirc_tasks/launch/jsk_mbzirc_task_1.launch @@ -35,19 +35,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 d80f742..49b4ecb 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..d925586 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 - diff --git a/jsk_mbzirc_tasks/urdf/quadrotor_with_hokyo30lx_and_downward_cam.urdf.xacro b/jsk_mbzirc_tasks/urdf/quadrotor_with_hokyo30lx_and_downward_cam.urdf.xacro index 0ef91aa..76a1b45 100644 --- a/jsk_mbzirc_tasks/urdf/quadrotor_with_hokyo30lx_and_downward_cam.urdf.xacro +++ b/jsk_mbzirc_tasks/urdf/quadrotor_with_hokyo30lx_and_downward_cam.urdf.xacro @@ -11,7 +11,7 @@ - + @@ -24,12 +24,5 @@ - - - - state - ground_truth/state - -