From 23e75d66c86b9018d20ac492a1b37914c659c420 Mon Sep 17 00:00:00 2001 From: Emiliano Borghi Date: Sat, 11 Jan 2020 19:13:14 -0300 Subject: [PATCH] Fix Linting --- .travis.yml | 5 +- ca_bringup/package.xml | 2 +- ca_bumper2pc/CMakeLists.txt | 25 +- .../include/ca_bumper2pc/ca_bumper2pc.hpp | 10 +- ca_bumper2pc/package.xml | 9 +- ca_bumper2pc/src/ca_bumper2pc.cpp | 48 +-- ca_description/CMakeLists.txt | 6 + ca_description/package.xml | 1 + ca_driver/CMakeLists.txt | 12 +- .../include/create_driver/create_driver.h | 58 ++-- ca_driver/package.xml | 1 + ca_driver/src/create_driver.cpp | 294 +++++++++--------- ca_gazebo/CMakeLists.txt | 101 +++--- .../include/ca_gazebo/create_bumper_plugin.h | 120 +++---- ca_gazebo/package.xml | 7 +- ca_gazebo/src/create_bumper_plugin.cpp | 151 ++++----- ca_gazebo/src/model_pose_publisher_plugin.cpp | 234 ++++++++------ ca_msgs/package.xml | 7 +- ca_node/CMakeLists.txt | 8 +- ca_node/src/nodelet/CMakeLists.txt | 8 +- ca_tools/CMakeLists.txt | 18 +- ca_tools/package.xml | 5 +- ca_tools/scripts/key_teleop.py | 21 +- navigation/ca_localization/CMakeLists.txt | 22 +- navigation/ca_localization/package.xml | 2 +- navigation/ca_move_base/CMakeLists.txt | 108 +------ navigation/ca_move_base/package.xml | 2 +- .../ca_safety_controller/CMakeLists.txt | 45 ++- .../safety_controller.hpp | 17 +- navigation/ca_safety_controller/package.xml | 17 +- .../ca_safety_controller/src/nodelet.cpp | 12 +- navigation/ca_slam/package.xml | 2 +- navigation/ca_swarm/CMakeLists.txt | 27 +- navigation/ca_swarm/package.xml | 38 +-- navigation/ca_swarm/scripts/box_client.py | 35 ++- navigation/ca_visual_odometry/CMakeLists.txt | 6 + navigation/ca_visual_odometry/package.xml | 4 +- sensors/ca_imu/CMakeLists.txt | 43 +-- sensors/ca_imu/package.xml | 2 +- 39 files changed, 809 insertions(+), 724 deletions(-) diff --git a/.travis.yml b/.travis.yml index 4aa82cb25..4cc41b685 100644 --- a/.travis.yml +++ b/.travis.yml @@ -49,5 +49,6 @@ script: # Run tests - catkin_make run_tests && catkin_test_results # Lint package files - # - sudo apt-get install python-catkin-lint - # - catkin lint -W2 --strict --explain src \ No newline at end of file + - sudo apt-get install python-catkin-lint + - catkin lint -W2 --strict --explain src/create_autonomy --ignore plugin_missing_install --ignore missing_depend --ignore target_name_collision + - catkin_make roslint \ No newline at end of file diff --git a/ca_bringup/package.xml b/ca_bringup/package.xml index 79422042f..c40efddd1 100644 --- a/ca_bringup/package.xml +++ b/ca_bringup/package.xml @@ -2,7 +2,7 @@ ca_bringup 0.0.0 - The ca_bringup package + ca_bringup spawns the nodes that the real robot uses Emiliano Borghi diff --git a/ca_bumper2pc/CMakeLists.txt b/ca_bumper2pc/CMakeLists.txt index 5c49c3a46..2f9c9d5b1 100644 --- a/ca_bumper2pc/CMakeLists.txt +++ b/ca_bumper2pc/CMakeLists.txt @@ -1,26 +1,32 @@ cmake_minimum_required(VERSION 2.8.3) project(ca_bumper2pc) find_package(catkin REQUIRED COMPONENTS - roscpp + ca_msgs nodelet pluginlib - ca_msgs + roscpp + roslint sensor_msgs ) catkin_package( INCLUDE_DIRS include - LIBRARIES ca_bumper2pc_nodelet - CATKIN_DEPENDS roscpp nodelet pluginlib sensor_msgs ca_msgs + LIBRARIES ${PROJECT_NAME}_nodelet + CATKIN_DEPENDS + ca_msgs + nodelet + pluginlib + roscpp + sensor_msgs ) include_directories(include ${catkin_INCLUDE_DIRS}) -add_library(ca_bumper2pc_nodelet src/ca_bumper2pc.cpp) -add_dependencies(ca_bumper2pc_nodelet ${catkin_EXPORTED_TARGETS}) -target_link_libraries(ca_bumper2pc_nodelet ${catkin_LIBRARIES}) +add_library(${PROJECT_NAME}_nodelet src/${PROJECT_NAME}.cpp) +add_dependencies(${PROJECT_NAME}_nodelet ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME}_nodelet ${catkin_LIBRARIES}) -install(TARGETS ca_bumper2pc_nodelet +install(TARGETS ${PROJECT_NAME}_nodelet DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) install(DIRECTORY include/${PROJECT_NAME}/ @@ -32,3 +38,6 @@ install(DIRECTORY plugins install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + +set(ROSLINT_CPP_OPTS "--filter=-build/c++11") +roslint_cpp() diff --git a/ca_bumper2pc/include/ca_bumper2pc/ca_bumper2pc.hpp b/ca_bumper2pc/include/ca_bumper2pc/ca_bumper2pc.hpp index 0a9f2ffed..8ecfdc144 100644 --- a/ca_bumper2pc/include/ca_bumper2pc/ca_bumper2pc.hpp +++ b/ca_bumper2pc/include/ca_bumper2pc/ca_bumper2pc.hpp @@ -68,9 +68,9 @@ class Bumper2PcNodelet : public nodelet::Nodelet { public: Bumper2PcNodelet() - : P_INF_X(+100*sin(0.34906585)), - P_INF_Y(+100*cos(0.34906585)), - N_INF_Y(-100*cos(0.34906585)), + : P_INF_X(+100 * sin(0.34906585)), + P_INF_Y(+100 * cos(0.34906585)), + N_INF_Y(-100 * cos(0.34906585)), ZERO(0), prev_bumper(0), prev_cliff(0) { } ~Bumper2PcNodelet() { } @@ -94,7 +94,7 @@ class Bumper2PcNodelet : public nodelet::Nodelet ros::Publisher pointcloud_pub_; ros::Subscriber cliff_event_subscriber_ ; ros::Subscriber bumper_event_subscriber_; - + sensor_msgs::PointCloud2 pointcloud_; /** @@ -105,6 +105,6 @@ class Bumper2PcNodelet : public nodelet::Nodelet void cliffSensorCB(const ca_msgs::Cliff::ConstPtr& msg_cliff); }; -} // namespace create_bumper2pc +} // namespace create_bumper2pc #endif // _CA_BUMPER2PC_HPP_ diff --git a/ca_bumper2pc/package.xml b/ca_bumper2pc/package.xml index 52946da3e..e475db133 100644 --- a/ca_bumper2pc/package.xml +++ b/ca_bumper2pc/package.xml @@ -15,16 +15,17 @@ catkin - roscpp + ca_msgs nodelet pluginlib - ca_msgs + roscpp + roslint sensor_msgs - roscpp + ca_msgs nodelet pluginlib - ca_msgs + roscpp sensor_msgs diff --git a/ca_bumper2pc/src/ca_bumper2pc.cpp b/ca_bumper2pc/src/ca_bumper2pc.cpp index 3dd4385e9..25e4afa97 100644 --- a/ca_bumper2pc/src/ca_bumper2pc.cpp +++ b/ca_bumper2pc/src/ca_bumper2pc.cpp @@ -3,7 +3,7 @@ * * @brief Bumper to pointcloud nodelet class implementation. * - * + * Copyright 2020 * **/ @@ -15,6 +15,8 @@ #include "ca_bumper2pc/ca_bumper2pc.hpp" +#include + namespace create_bumper2pc { @@ -23,13 +25,14 @@ void Bumper2PcNodelet::bumperSensorCB(const ca_msgs::Bumper::ConstPtr& msg_bump) if (pointcloud_pub_.getNumSubscribers() == 0) return; - // We publish just one "no events" pc (with all three points far away) and stop spamming when bumper/cliff conditions disappear - if (! (msg_bump->is_left_pressed || msg_bump->is_right_pressed) && ! prev_bumper) + // We publish just one "no events" pc (with all three points far away) + // and stop spamming when bumper/cliff conditions disappear + if (!(msg_bump->is_left_pressed || msg_bump->is_right_pressed) && !prev_bumper) return; prev_bumper = msg_bump->is_left_pressed || msg_bump->is_right_pressed; - // For any of {left/center/right} with no bumper/cliff event, we publish a faraway point that won't get used + // For any of {left/center/right} with no bumper/cliff event, we publish a faraway point that won't get used if (msg_bump->is_left_pressed && !(msg_bump->is_right_pressed)) { memcpy(&pointcloud_.data[0 * pointcloud_.point_step + pointcloud_.fields[0].offset], &p_side_x_, sizeof(float)); @@ -50,7 +53,7 @@ void Bumper2PcNodelet::bumperSensorCB(const ca_msgs::Bumper::ConstPtr& msg_bump) memcpy(&pointcloud_.data[1 * pointcloud_.point_step + pointcloud_.fields[0].offset], &P_INF_X, sizeof(float)); } - if (msg_bump->is_right_pressed && !(msg_bump->is_left_pressed)) + if (msg_bump->is_right_pressed && !(msg_bump->is_left_pressed)) { memcpy(&pointcloud_.data[2 * pointcloud_.point_step + pointcloud_.fields[0].offset], &p_side_x_, sizeof(float)); memcpy(&pointcloud_.data[2 * pointcloud_.point_step + pointcloud_.fields[1].offset], &n_side_y_, sizeof(float)); @@ -71,15 +74,22 @@ void Bumper2PcNodelet::cliffSensorCB(const ca_msgs::Cliff::ConstPtr& msg_cliff) if (pointcloud_pub_.getNumSubscribers() == 0) return; - // We publish just one "no events" pc (with all three points far away) and stop spamming when bumper/cliff conditions disappear - if ( !(msg_cliff->is_cliff_left || msg_cliff->is_cliff_front_left || msg_cliff->is_cliff_front_right || msg_cliff->is_cliff_right) - && !prev_cliff) + // We publish just one "no events" pc (with all three points far away) + // and stop spamming when bumper/cliff conditions disappear + if (!(msg_cliff->is_cliff_left || + msg_cliff->is_cliff_front_left || + msg_cliff->is_cliff_front_right || + msg_cliff->is_cliff_right) + && !prev_cliff) return; - prev_cliff = msg_cliff->is_cliff_left || msg_cliff->is_cliff_front_left || msg_cliff->is_cliff_front_right || msg_cliff->is_cliff_right; + prev_cliff = msg_cliff->is_cliff_left || + msg_cliff->is_cliff_front_left || + msg_cliff->is_cliff_front_right || + msg_cliff->is_cliff_right; + - - // For any of {left/center/right} with no bumper/cliff event, we publish a faraway point that won't get used + // For any of {left/center/right} with no bumper/cliff event, we publish a faraway point that won't get used if (msg_cliff->is_cliff_left) { memcpy(&pointcloud_.data[0 * pointcloud_.point_step + pointcloud_.fields[0].offset], &p_side_x_, sizeof(float)); @@ -126,15 +136,17 @@ void Bumper2PcNodelet::onInit() // them will probably fail. std::string base_link_frame; double r, h, angle; - nh.param("pointcloud_radius", r, 0.25); pc_radius_ = r; - nh.param("pointcloud_height", h, 0.04); pc_height_ = h; - nh.param("side_point_angle", angle, 0.34906585); + nh.param("pointcloud_radius", r, 0.25); + pc_radius_ = r; + nh.param("pointcloud_height", h, 0.04); + pc_height_ = h; + nh.param("side_point_angle", angle, 0.34906585); nh.param("base_link_frame", base_link_frame, "/base_link"); // Lateral points x/y coordinates; we need to store float values to memcopy later - p_side_x_ = + pc_radius_*sin(angle); // angle degrees from vertical - p_side_y_ = + pc_radius_*cos(angle); // angle degrees from vertical - n_side_y_ = - pc_radius_*cos(angle); // angle degrees from vertical + p_side_x_ = + pc_radius_ * sin(angle); // angle degrees from vertical + p_side_y_ = + pc_radius_ * cos(angle); // angle degrees from vertical + n_side_y_ = - pc_radius_ * cos(angle); // angle degrees from vertical // Prepare constant parts of the pointcloud message to be published pointcloud_.header.frame_id = base_link_frame; @@ -180,7 +192,7 @@ void Bumper2PcNodelet::onInit() ROS_INFO("Bumper/cliff pointcloud configured at distance %f and height %f from base frame", pc_radius_, pc_height_); } -} // namespace create_bumper2pc +} // namespace create_bumper2pc PLUGINLIB_EXPORT_CLASS(create_bumper2pc::Bumper2PcNodelet, nodelet::Nodelet); diff --git a/ca_description/CMakeLists.txt b/ca_description/CMakeLists.txt index 6b0c0aa0d..0bd025399 100644 --- a/ca_description/CMakeLists.txt +++ b/ca_description/CMakeLists.txt @@ -10,6 +10,12 @@ catkin_package() roslaunch_add_file_check(launch) +install(DIRECTORY scripts/ + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + USE_SOURCE_PERMISSIONS + PATTERN ".svn" EXCLUDE +) + install( DIRECTORY launch meshes urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/ca_description/package.xml b/ca_description/package.xml index d6fc20ea4..a82d778e9 100644 --- a/ca_description/package.xml +++ b/ca_description/package.xml @@ -15,6 +15,7 @@ catkin roslaunch + tf joint_state_publisher robot_state_publisher diff --git a/ca_driver/CMakeLists.txt b/ca_driver/CMakeLists.txt index 9db024f99..938e7ebb9 100644 --- a/ca_driver/CMakeLists.txt +++ b/ca_driver/CMakeLists.txt @@ -3,15 +3,15 @@ project(ca_driver) add_compile_options(-std=c++11) -find_package(libcreate REQUIRED) - find_package(catkin REQUIRED COMPONENTS ca_msgs diagnostic_msgs diagnostic_updater geometry_msgs + libcreate nav_msgs roscpp + roslint sensor_msgs std_msgs tf @@ -24,17 +24,16 @@ catkin_package( diagnostic_msgs diagnostic_updater geometry_msgs + libcreate nav_msgs roscpp sensor_msgs std_msgs tf - DEPENDS libcreate ) include_directories( include - ${libcreate_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ) @@ -58,6 +57,9 @@ install(DIRECTORY include FILES_MATCHING PATTERN "*.h" ) -install(DIRECTORY launch config +install(DIRECTORY config launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + +set(ROSLINT_CPP_OPTS "--filter=-build/c++11, -runtime/references") +roslint_cpp() diff --git a/ca_driver/include/create_driver/create_driver.h b/ca_driver/include/create_driver/create_driver.h index 5a59ee700..71a5d34e1 100644 --- a/ca_driver/include/create_driver/create_driver.h +++ b/ca_driver/include/create_driver/create_driver.h @@ -26,10 +26,11 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef CREATE_AUTONOMY_CREATE_DRIVER_H -#define CREATE_AUTONOMY_CREATE_DRIVER_H +#ifndef CREATE_DRIVER_CREATE_DRIVER_H +#define CREATE_DRIVER_CREATE_DRIVER_H #include +#include #include #include @@ -59,33 +60,40 @@ namespace create { static const uint8_t SONG_0_LENGTH = 2; -static const uint8_t SONG_0_NOTES [] = {64,54}; -static const float SONG_0_DURATIONS [] = {1.0,1.0}; +static const uint8_t SONG_0_NOTES [] = {64, 54}; +static const float SONG_0_DURATIONS [] = {1.0, 1.0}; static const uint8_t SONG_1_LENGTH = 16; -static const uint8_t SONG_1_NOTES [] = {105,103,100,96,98,107,101,108,105,103,100,96,98,107,103,108}; -static const float SONG_1_DURATIONS [] = {0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1}; +static const uint8_t SONG_1_NOTES [] = + {105, 103, 100, 96, 98, 107, 101, 108, 105, 103, 100, 96, 98, 107, 103, 108}; +static const float SONG_1_DURATIONS [] = + {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; static const uint8_t SONG_2_LENGTH = 16; -static const uint8_t SONG_2_NOTES [] = {84,107,84,107,84,107,84,107,84,107,84,107,84,107,84,107}; -static const float SONG_2_DURATIONS [] = {0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1}; +static const uint8_t SONG_2_NOTES [] = {84, 107, 84, 107, 84, 107, 84, 107, 84, 107, 84, 107, 84, 107, 84, 107}; +static const float SONG_2_DURATIONS [] = + {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1}; static const uint8_t SONG_3_LENGTH = 11; -static const uint8_t SONG_3_NOTES [] = {59,59,59,59,62,61,61,59,59,59,59}; -static const float SONG_3_DURATIONS [] = {1.0,0.75,0.25,1.0,0.75,0.25,0.7,0.25,0.7,0.25,1.0}; +static const uint8_t SONG_3_NOTES [] = {59, 59, 59, 59, 62, 61, 61, 59, 59, 59, 59}; +static const float SONG_3_DURATIONS [] = {1.0, 0.75, 0.25, 1.0, 0.75, 0.25, 0.7, 0.25, 0.7, 0.25, 1.0}; static const uint8_t SONG_4_LENGTH = 16; -static const uint8_t SONG_4_NOTES [] = {79,86,84,83,81,91,86,84,83,81,91,86,84,83,84,81}; -static const float SONG_4_DURATIONS [] = {0.9,0.8,0.2,0.2,0.2,0.8,0.7,0.2,0.2,0.2,0.8,0.7,0.2,0.2,0.2,0.9}; +static const uint8_t SONG_4_NOTES [] = {79, 86, 84, 83, 81, 91, 86, 84, 83, 81, 91, 86, 84, 83, 84, 81}; +static const float SONG_4_DURATIONS [] = + {0.9, 0.8, 0.2, 0.2, 0.2, 0.8, 0.7, 0.2, 0.2, 0.2, 0.8, 0.7, 0.2, 0.2, 0.2, 0.9}; // Unused covariances must have a large value static const double MAX_DBL = 1e10; -static const double COVARIANCE[36] = {1e-5, 0.0, 0.0, 0.0, 0.0, 0.0, +static const double COVARIANCE[36] = + { + 1e-5, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, MAX_DBL, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, MAX_DBL, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, MAX_DBL, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1e-3}; + 0.0, 0.0, 0.0, 0.0, 0.0, 1e-3 + }; class CreateDriver { @@ -133,11 +141,11 @@ class CreateDriver void mainMotorCallback(const std_msgs::Float32ConstPtr& msg); bool update(); - void updateBatteryDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); - void updateSafetyDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); - void updateSerialDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); - void updateModeDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); - void updateDriverDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat); + void updateBatteryDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); + void updateSafetyDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); + void updateSerialDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); + void updateModeDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); + void updateDriverDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); void publishOdom(); void publishAngle(); void publishJointState(); @@ -151,10 +159,11 @@ class CreateDriver void publishIsWall(); void publishOvercurrent(); - inline float normalizeAngle(float angle) { - angle = std::fmod(angle + M_PI,2*M_PI); + inline float normalizeAngle(float angle) + { + angle = std::fmod(angle + M_PI, 2 * M_PI); if (angle < 0) - angle += 2*M_PI; + angle += 2 * M_PI; return angle - M_PI; }; @@ -203,9 +212,8 @@ class CreateDriver virtual void spin(); virtual void spinOnce(); - }; // class CreateDriver -} //namespace ccreate +} // namespace create -#endif // CREATE_AUTONOMY_CREATE_DRIVER_H +#endif // CREATE_DRIVER_CREATE_DRIVER_H diff --git a/ca_driver/package.xml b/ca_driver/package.xml index fef504aad..b11d06ad5 100644 --- a/ca_driver/package.xml +++ b/ca_driver/package.xml @@ -21,6 +21,7 @@ libcreate nav_msgs roscpp + roslint sensor_msgs std_msgs tf diff --git a/ca_driver/src/create_driver.cpp b/ca_driver/src/create_driver.cpp index f9cbed560..76c2c6f94 100644 --- a/ca_driver/src/create_driver.cpp +++ b/ca_driver/src/create_driver.cpp @@ -28,7 +28,9 @@ POSSIBILITY OF SUCH DAMAGE. #include #include + #include +#include namespace create { @@ -49,13 +51,20 @@ CreateDriver::CreateDriver(ros::NodeHandle& nh, ros::NodeHandle& ph) priv_nh_.param("latch_cmd_duration", latch_duration_, 0.2); priv_nh_.param("publish_tf", publish_tf_, true); - if (robot_model_name == "ROOMBA_400") { + if (robot_model_name == "ROOMBA_400") + { model_ = create::RobotModel::ROOMBA_400; - } else if (robot_model_name == "CREATE_1") { + } + else if (robot_model_name == "CREATE_1") + { model_ = create::RobotModel::CREATE_1; - } else if (robot_model_name == "CREATE_2") { + } + else if (robot_model_name == "CREATE_2") + { model_ = create::RobotModel::CREATE_2; - } else { + } + else + { ROS_FATAL_STREAM("[CREATE] Robot model \"" + robot_model_name + "\" is not known."); ros::shutdown(); return; @@ -142,7 +151,7 @@ CreateDriver::CreateDriver(ros::NodeHandle& nh, ros::NodeHandle& ph) wheeldrop_pub_ = nh.advertise("wheeldrop", 30); wheel_joint_pub_ = nh.advertise("joint_states", 10); wall_pub_ = nh.advertise("wall", 30); - //overcurrent_pub_ = nh.advertise("overcurrent", 30); + // overcurrent_pub_ = nh.advertise("overcurrent", 30); // Setup diagnostics diagnostics_.add("Battery Status", this, &CreateDriver::updateBatteryDiagnostics); @@ -243,20 +252,22 @@ void CreateDriver::setASCIICallback(const std_msgs::UInt8MultiArrayConstPtr& msg void CreateDriver::playSongCallback(const std_msgs::UInt8ConstPtr& msg) { - if (msg->data == 4){ - robot_->defineSong(3, SONG_4_LENGTH, SONG_4_NOTES, SONG_4_DURATIONS); - robot_->playSong(3); - } - - else if (msg->data == 3){ - robot_->defineSong(3, SONG_3_LENGTH, SONG_3_NOTES, SONG_3_DURATIONS); - robot_->playSong(3); - } + if (msg->data == 4) + { + robot_->defineSong(3, SONG_4_LENGTH, SONG_4_NOTES, SONG_4_DURATIONS); + robot_->playSong(3); + } - else{ - robot_->playSong(msg->data); - } + else if (msg->data == 3) + { + robot_->defineSong(3, SONG_3_LENGTH, SONG_3_NOTES, SONG_3_DURATIONS); + robot_->playSong(3); + } + else + { + robot_->playSong(msg->data); + } } void CreateDriver::dockCallback(const std_msgs::EmptyConstPtr& msg) @@ -294,7 +305,7 @@ bool CreateDriver::update() publishCliffInfo(); publishWheeldrop(); publishIsWall(); - //publishOvercurrent(); + // publishOvercurrent(); // If last velocity command was sent longer than latch duration, stop robot if (ros::Time::now() - last_cmd_vel_time_ >= ros::Duration(latch_duration_)) @@ -305,7 +316,7 @@ bool CreateDriver::update() return true; } -void CreateDriver::updateBatteryDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) +void CreateDriver::updateBatteryDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat) { const float charge = robot_->getBatteryCharge(); const float capacity = robot_->getBatteryCapacity(); @@ -338,31 +349,34 @@ void CreateDriver::updateBatteryDiagnostics(diagnostic_updater::DiagnosticStatus switch (charging_state) { - case create::CHARGE_NONE: - stat.add("Charging state", "Not charging"); - break; - case create::CHARGE_RECONDITION: - stat.add("Charging state", "Reconditioning"); - break; - case create::CHARGE_FULL: - stat.add("Charging state", "Full charging"); - break; - case create::CHARGE_TRICKLE: - stat.add("Charging state", "Trickle charging"); - break; - case create::CHARGE_WAITING: - stat.add("Charging state", "Waiting"); - break; - case create::CHARGE_FAULT: - stat.add("Charging state", "Fault"); - break; - } -} - -void CreateDriver::updateSafetyDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) + case create::CHARGE_NONE: + stat.add("Charging state", "Not charging"); + break; + case create::CHARGE_RECONDITION: + stat.add("Charging state", "Reconditioning"); + break; + case create::CHARGE_FULL: + stat.add("Charging state", "Full charging"); + break; + case create::CHARGE_TRICKLE: + stat.add("Charging state", "Trickle charging"); + break; + case create::CHARGE_WAITING: + stat.add("Charging state", "Waiting"); + break; + case create::CHARGE_FAULT: + stat.add("Charging state", "Fault"); + break; + } +} + +void CreateDriver::updateSafetyDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat) { const bool is_wheeldrop = robot_->isLeftWheel() || robot_->isRightWheel(); - const bool is_cliff = robot_->isCliffLeft() || robot_->isCliffFrontLeft() || robot_->isCliffFrontRight() || robot_->isCliffRight(); + const bool is_cliff = robot_->isCliffLeft() || + robot_->isCliffFrontLeft() || + robot_->isCliffFrontRight() || + robot_->isCliffRight(); if (is_wheeldrop) { stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Wheeldrop detected"); @@ -380,7 +394,7 @@ void CreateDriver::updateSafetyDiagnostics(diagnostic_updater::DiagnosticStatusW stat.add("Cliff", is_cliff); } -void CreateDriver::updateSerialDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) +void CreateDriver::updateSerialDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat) { const bool is_connected = robot_->connected(); const uint64_t corrupt_packets = robot_->getNumCorruptPackets(); @@ -404,30 +418,30 @@ void CreateDriver::updateSerialDiagnostics(diagnostic_updater::DiagnosticStatusW stat.add("Total packets", total_packets); } -void CreateDriver::updateModeDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) +void CreateDriver::updateModeDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat) { const create::CreateMode mode = robot_->getMode(); switch (mode) { - case create::MODE_UNAVAILABLE: - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Unknown mode of operation"); - break; - case create::MODE_OFF: - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Mode is set to OFF"); - break; - case create::MODE_PASSIVE: - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Mode is set to PASSIVE"); - break; - case create::MODE_SAFE: - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Mode is set to SAFE"); - break; - case create::MODE_FULL: - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Mode is set to FULL"); - break; + case create::MODE_UNAVAILABLE: + stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Unknown mode of operation"); + break; + case create::MODE_OFF: + stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Mode is set to OFF"); + break; + case create::MODE_PASSIVE: + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Mode is set to PASSIVE"); + break; + case create::MODE_SAFE: + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Mode is set to SAFE"); + break; + case create::MODE_FULL: + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Mode is set to FULL"); + break; } } -void CreateDriver::updateDriverDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat) +void CreateDriver::updateDriverDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat) { if (is_running_slowly_) { @@ -490,34 +504,34 @@ void CreateDriver::publishOdom() void CreateDriver::publishAngle() { - const float orientation_stddev = 5.0*M_PI/180.0; // 5 degrees - /* - * According to Roomba Open Interface specs, the angle has to be - * divided by 0.324056 to get the angle in degrees. - * So, to get it in radians, the number has to be divided by 180 too. - */ - orientation_ += (robot_->getAngle() * M_PI / 58.33008); - const float yaw = CreateDriver::normalizeAngle(orientation_); - const float yaw_2 = yaw / 2.; - angle_msg_.pose.pose.orientation.x = cos(yaw_2); - angle_msg_.pose.pose.orientation.w = sin(yaw_2); - angle_msg_.header.seq += 1; - angle_msg_.header.stamp = ros::Time::now(); - angle_msg_.pose.covariance[35] = orientation_stddev * orientation_stddev; - angle_pub_.publish(angle_msg_); + const float orientation_stddev = 5.0 * M_PI / 180.0; // 5 degrees + /* + * According to Roomba Open Interface specs, the angle has to be + * divided by 0.324056 to get the angle in degrees. + * So, to get it in radians, the number has to be divided by 180 too. + */ + orientation_ += (robot_->getAngle() * M_PI / 58.33008); + const float yaw = CreateDriver::normalizeAngle(orientation_); + const float yaw_2 = yaw / 2.; + angle_msg_.pose.pose.orientation.x = cos(yaw_2); + angle_msg_.pose.pose.orientation.w = sin(yaw_2); + angle_msg_.header.seq += 1; + angle_msg_.header.stamp = ros::Time::now(); + angle_msg_.pose.covariance[35] = orientation_stddev * orientation_stddev; + angle_pub_.publish(angle_msg_); } void CreateDriver::publishJointState() { - // Publish joint states - float wheelRadius = model_.getWheelDiameter() / 2.0; + // Publish joint states + float wheelRadius = model_.getWheelDiameter() / 2.0; - joint_state_msg_.header.stamp = ros::Time::now(); - joint_state_msg_.position[0] = robot_->getLeftWheelDistance() / wheelRadius; - joint_state_msg_.position[1] = robot_->getRightWheelDistance() / wheelRadius; - joint_state_msg_.velocity[0] = robot_->getRequestedLeftWheelVel() / wheelRadius; - joint_state_msg_.velocity[1] = robot_->getRequestedRightWheelVel() / wheelRadius; - wheel_joint_pub_.publish(joint_state_msg_); + joint_state_msg_.header.stamp = ros::Time::now(); + joint_state_msg_.position[0] = robot_->getLeftWheelDistance() / wheelRadius; + joint_state_msg_.position[1] = robot_->getRightWheelDistance() / wheelRadius; + joint_state_msg_.velocity[0] = robot_->getRequestedLeftWheelVel() / wheelRadius; + joint_state_msg_.velocity[1] = robot_->getRequestedRightWheelVel() / wheelRadius; + wheel_joint_pub_.publish(joint_state_msg_); } void CreateDriver::publishBatteryInfo() @@ -539,28 +553,28 @@ void CreateDriver::publishBatteryInfo() charging_state_msg_.header.stamp = ros::Time::now(); switch (charging_state) { - case create::CHARGE_NONE: - charging_state_msg_.state = charging_state_msg_.CHARGE_NONE; - break; - case create::CHARGE_RECONDITION: - charging_state_msg_.state = charging_state_msg_.CHARGE_RECONDITION; - break; + case create::CHARGE_NONE: + charging_state_msg_.state = charging_state_msg_.CHARGE_NONE; + break; + case create::CHARGE_RECONDITION: + charging_state_msg_.state = charging_state_msg_.CHARGE_RECONDITION; + break; - case create::CHARGE_FULL: - charging_state_msg_.state = charging_state_msg_.CHARGE_FULL; - break; + case create::CHARGE_FULL: + charging_state_msg_.state = charging_state_msg_.CHARGE_FULL; + break; - case create::CHARGE_TRICKLE: - charging_state_msg_.state = charging_state_msg_.CHARGE_TRICKLE; - break; + case create::CHARGE_TRICKLE: + charging_state_msg_.state = charging_state_msg_.CHARGE_TRICKLE; + break; - case create::CHARGE_WAITING: - charging_state_msg_.state = charging_state_msg_.CHARGE_WAITING; - break; + case create::CHARGE_WAITING: + charging_state_msg_.state = charging_state_msg_.CHARGE_WAITING; + break; - case create::CHARGE_FAULT: - charging_state_msg_.state = charging_state_msg_.CHARGE_FAULT; - break; + case create::CHARGE_FAULT: + charging_state_msg_.state = charging_state_msg_.CHARGE_FAULT; + break; } charging_state_pub_.publish(charging_state_msg_); } @@ -598,7 +612,7 @@ void CreateDriver::publishOmniChar() uint8_t ir_char = robot_->getIROmni(); uint16_msg_.data = ir_char; omni_char_pub_.publish(uint16_msg_); - // TODO: Publish info based on character, such as dock in sight + // TODO(@eborghi10): Publish info based on character, such as dock in sight } void CreateDriver::publishMode() @@ -607,21 +621,21 @@ void CreateDriver::publishMode() mode_msg_.header.stamp = ros::Time::now(); switch (mode) { - case create::MODE_OFF: - mode_msg_.mode = mode_msg_.MODE_OFF; - break; - case create::MODE_PASSIVE: - mode_msg_.mode = mode_msg_.MODE_PASSIVE; - break; - case create::MODE_SAFE: - mode_msg_.mode = mode_msg_.MODE_SAFE; - break; - case create::MODE_FULL: - mode_msg_.mode = mode_msg_.MODE_FULL; - break; - default: - ROS_ERROR("[CREATE] Unknown mode detected"); - break; + case create::MODE_OFF: + mode_msg_.mode = mode_msg_.MODE_OFF; + break; + case create::MODE_PASSIVE: + mode_msg_.mode = mode_msg_.MODE_PASSIVE; + break; + case create::MODE_SAFE: + mode_msg_.mode = mode_msg_.MODE_SAFE; + break; + case create::MODE_FULL: + mode_msg_.mode = mode_msg_.MODE_FULL; + break; + default: + ROS_ERROR("[CREATE] Unknown mode detected"); + break; } mode_pub_.publish(mode_msg_); } @@ -675,28 +689,28 @@ void CreateDriver::publishCliffInfo() void CreateDriver::publishWheeldrop() { - wheeldrop_msg_.header.stamp = ros::Time::now(); - wheeldrop_msg_.is_left_dropped = robot_->isLeftWheel(); - wheeldrop_msg_.is_right_dropped = robot_->isRightWheel(); + wheeldrop_msg_.header.stamp = ros::Time::now(); + wheeldrop_msg_.is_left_dropped = robot_->isLeftWheel(); + wheeldrop_msg_.is_right_dropped = robot_->isRightWheel(); - wheeldrop_pub_.publish(wheeldrop_msg_); + wheeldrop_pub_.publish(wheeldrop_msg_); } void CreateDriver::publishIsWall() { - is_wall_msg_.data = robot_->isWall(); + is_wall_msg_.data = robot_->isWall(); - wall_pub_.publish(is_wall_msg_); + wall_pub_.publish(is_wall_msg_); } void CreateDriver::publishOvercurrent() { - is_overcurrent_msg_.is_left_wheel_overcurrent = robot_->isLeftWheelOvercurrent(); - is_overcurrent_msg_.is_right_wheel_overcurrent = robot_->isRightWheelOvercurrent(); - is_overcurrent_msg_.is_main_brush_overcurrent = robot_->isMainBrushOvercurrent(); - is_overcurrent_msg_.is_side_brush_overcurrent = robot_->isSideBrushOvercurrent(); + is_overcurrent_msg_.is_left_wheel_overcurrent = robot_->isLeftWheelOvercurrent(); + is_overcurrent_msg_.is_right_wheel_overcurrent = robot_->isRightWheelOvercurrent(); + is_overcurrent_msg_.is_main_brush_overcurrent = robot_->isMainBrushOvercurrent(); + is_overcurrent_msg_.is_side_brush_overcurrent = robot_->isSideBrushOvercurrent(); - overcurrent_pub_.publish(is_overcurrent_msg_); + overcurrent_pub_.publish(is_overcurrent_msg_); } void CreateDriver::spinOnce() @@ -708,31 +722,29 @@ void CreateDriver::spinOnce() void CreateDriver::spin() { - -try{ - ros::Rate rate(loop_hz_); - while (ros::ok()) + try { - spinOnce(); - - is_running_slowly_ = !rate.sleep(); - if (is_running_slowly_) + ros::Rate rate(loop_hz_); + while (ros::ok()) { - ROS_WARN("[CREATE] Loop running slowly."); + spinOnce(); + + is_running_slowly_ = !rate.sleep(); + if (is_running_slowly_) + { + ROS_WARN("[CREATE] Loop running slowly."); + } } } -} catch (std::runtime_error& ex) { ROS_FATAL_STREAM("[CREATE] Runtime error: " << ex.what()); } - - } -}//create namespace +} // namespace create int main(int argc, char** argv) { diff --git a/ca_gazebo/CMakeLists.txt b/ca_gazebo/CMakeLists.txt index c4b4a8a3c..16be94abc 100644 --- a/ca_gazebo/CMakeLists.txt +++ b/ca_gazebo/CMakeLists.txt @@ -1,46 +1,55 @@ -if( ${CMAKE_SYSTEM_PROCESSOR} MATCHES "arm" ) - message( STATUS "Avoid compiling ca_gazebo because of the ARM architecture" ) -else() - cmake_minimum_required(VERSION 2.8.3) - set(CMAKE_CXX_FLAGS "-std=c++11") - project(ca_gazebo) - - find_package(gazebo REQUIRED) - - find_package(catkin REQUIRED COMPONENTS - gazebo_ros - gazebo_plugins - roscpp - ca_msgs - ) - - catkin_package( - INCLUDE_DIRS include - LIBRARIES create_bumper_plugin - CATKIN DEPENDS - gazebo_ros - gazebo_plugins - roscpp - ca_msgs - ) - - link_directories(${GAZEBO_LIBRARY_DIRS}) - include_directories(include - ${catkin_INCLUDE_DIRS} - ${GAZEBO_INCLUDE_DIRS} - ) - - add_library(create_bumper_plugin src/create_bumper_plugin.cpp) - add_dependencies(create_bumper_plugin ${catkin_EXPORTED_TARGETS}) - target_link_libraries(create_bumper_plugin - ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) - - add_library(model_pose_publisher_plugin src/model_pose_publisher_plugin.cpp) - target_link_libraries(model_pose_publisher_plugin - ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) - - install(DIRECTORY launch worlds models - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - - message(STATUS "ca_gazebo compiled") -endif() +cmake_minimum_required(VERSION 2.8.3) +project(ca_gazebo) + +add_compile_options(-std=c++11) + +find_package(gazebo REQUIRED) + +find_package(catkin REQUIRED COMPONENTS + ca_msgs + gazebo_plugins + gazebo_ros + roscpp + roslint + tf2 + tf2_ros +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES + create_bumper_plugin + model_pose_publisher_plugin +) + +include_directories(include + ${catkin_INCLUDE_DIRS} + ${GAZEBO_INCLUDE_DIRS} +) + +add_library(create_bumper_plugin src/create_bumper_plugin.cpp) +add_dependencies(create_bumper_plugin ${catkin_EXPORTED_TARGETS}) +target_link_libraries(create_bumper_plugin + ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) + +add_library(model_pose_publisher_plugin src/model_pose_publisher_plugin.cpp) +target_link_libraries(model_pose_publisher_plugin + ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) + +install(DIRECTORY launch models worlds + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(TARGETS + create_bumper_plugin + model_pose_publisher_plugin + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +set(ROSLINT_CPP_OPTS "--filter=-build/c++11") +roslint_cpp() diff --git a/ca_gazebo/include/ca_gazebo/create_bumper_plugin.h b/ca_gazebo/include/ca_gazebo/create_bumper_plugin.h index 4312c45ac..3f3e8fdc6 100644 --- a/ca_gazebo/include/ca_gazebo/create_bumper_plugin.h +++ b/ca_gazebo/include/ca_gazebo/create_bumper_plugin.h @@ -1,4 +1,5 @@ -#pragma once +#ifndef CA_GAZEBO_CREATE_BUMPER_PLUGIN_H +#define CA_GAZEBO_CREATE_BUMPER_PLUGIN_H /* * Gazebo - Outdoor Multi-Robot Simulator @@ -26,10 +27,9 @@ * Date: 24 Sept 2008 */ #include +#include #include -#include - #include #include #include @@ -41,63 +41,69 @@ #include #include -#include +#include namespace gazebo { - /// \brief A Bumper controller - class CreateBumperPlugin : public SensorPlugin +/// \brief A Bumper controller +class CreateBumperPlugin : public SensorPlugin +{ + /// Constructor +public: + CreateBumperPlugin(); + + /// Destructor +public: + virtual ~CreateBumperPlugin(); + + /// \brief Load the plugin + /// \param take in SDF root element +public: + void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// Update the controller +protected: + void OnUpdate(); + +private: + inline float normalizeAngle(float angle) { - /// Constructor - public: CreateBumperPlugin(); - - /// Destructor - public: virtual ~CreateBumperPlugin(); - - /// \brief Load the plugin - /// \param take in SDF root element - public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); - - /// Update the controller - protected: void OnUpdate(); - - private: - - inline float normalizeAngle(float angle) { - angle = std::fmod(angle + M_PI,2*M_PI); - if (angle < 0) - angle += 2*M_PI; - return angle - M_PI; - }; - - /// \brief pointer to ros node - std::shared_ptr rosnode_; - ros::Publisher contact_pub_; - ros::Subscriber gts_sub_; - ca_msgs::Bumper bumper_event_; - void PublishBumperMsg(); - - void GtsCb(const nav_msgs::Odometry::ConstPtr& msg); - double robot_heading_; - - bool bumper_left_was_pressed_; - bool bumper_center_was_pressed_; - bool bumper_right_was_pressed_; - bool bumper_left_is_pressed_; - bool bumper_center_is_pressed_; - bool bumper_right_is_pressed_; - - /// \brief set topic name of broadcast - std::string bumper_topic_name_; - std::string frame_name_; - - sensors::ContactSensorPtr bumper_; - - /// \brief for setting ROS namespace - std::string robot_namespace_; - - // Pointer to the update event connection - event::ConnectionPtr update_connection_; + angle = std::fmod(angle + M_PI, 2 * M_PI); + if (angle < 0) + angle += 2 * M_PI; + return angle - M_PI; }; -} + /// \brief pointer to ros node + std::shared_ptr rosnode_; + ros::Publisher contact_pub_; + ros::Subscriber gts_sub_; + ca_msgs::Bumper bumper_event_; + void PublishBumperMsg(); + + void GtsCb(const nav_msgs::Odometry::ConstPtr& msg); + double robot_heading_; + + bool bumper_left_was_pressed_; + bool bumper_center_was_pressed_; + bool bumper_right_was_pressed_; + bool bumper_left_is_pressed_; + bool bumper_center_is_pressed_; + bool bumper_right_is_pressed_; + + /// \brief set topic name of broadcast + std::string bumper_topic_name_; + std::string frame_name_; + + sensors::ContactSensorPtr bumper_; + + /// \brief for setting ROS namespace + std::string robot_namespace_; + + // Pointer to the update event connection + event::ConnectionPtr update_connection_; +}; + +} // namespace gazebo + +#endif // CA_GAZEBO_CREATE_BUMPER_PLUGIN_H diff --git a/ca_gazebo/package.xml b/ca_gazebo/package.xml index 12428a23f..87bee1353 100644 --- a/ca_gazebo/package.xml +++ b/ca_gazebo/package.xml @@ -2,22 +2,23 @@ ca_gazebo 0.0.1 - The ca_gazebo package + Simulation plugins to simulate the robot Emiliano Borghi BSD - ca_msgs catkin + + ca_msgs gazebo_plugins gazebo_ros roscpp + roslint tf2 tf2_ros ca_description - ca_msgs gazebo_plugins gazebo_ros roscpp diff --git a/ca_gazebo/src/create_bumper_plugin.cpp b/ca_gazebo/src/create_bumper_plugin.cpp index de0bb2c4a..89c393b62 100644 --- a/ca_gazebo/src/create_bumper_plugin.cpp +++ b/ca_gazebo/src/create_bumper_plugin.cpp @@ -25,15 +25,16 @@ */ #include "ca_gazebo/create_bumper_plugin.h" +#include namespace gazebo { CreateBumperPlugin::CreateBumperPlugin() -: SensorPlugin() -, bumper_left_was_pressed_(false) -, bumper_center_was_pressed_(false) -, bumper_right_was_pressed_(false) + : SensorPlugin() + , bumper_left_was_pressed_(false) + , bumper_center_was_pressed_(false) + , bumper_right_was_pressed_(false) { } @@ -45,66 +46,66 @@ CreateBumperPlugin::~CreateBumperPlugin() void CreateBumperPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) { - ROS_INFO_STREAM("Loading gazebo bumper"); - - this->bumper_ = - std::dynamic_pointer_cast(_parent); - if (!this->bumper_) - { - gzerr << "ContactPlugin requires a ContactSensor.\n"; - return; - } - - this->robot_namespace_ = ""; - if (_sdf->HasElement("robotNamespace")) - this->robot_namespace_ = _sdf->Get("robotNamespace") + "/"; - - // "publishing contact/collisions to this topic name: " << this->bumper_topic_name_ << std::endl; - this->bumper_topic_name_ = "bumper_base"; - if (_sdf->HasElement("bumperTopicName")) - this->bumper_topic_name_ = _sdf->Get("bumperTopicName"); - - // "transform contact/collisions pose, forces to this body (link) name: " << this->frame_name_ << std::endl; - if (!_sdf->HasElement("frameName")) - { - ROS_INFO("bumper plugin missing , defaults to world"); - this->frame_name_ = "world"; - } - else - this->frame_name_ = _sdf->Get("frameName"); - - this->bumper_event_.header.frame_id = this->frame_name_; - - ROS_INFO("Loaded with values: robotNamespace = %s, bumperTopicName = %s, frameName = %s", - this->robot_namespace_.c_str(), this->bumper_topic_name_.c_str(),this->frame_name_.c_str()); - - if (!ros::isInitialized()) - { - int argc = 0; - char** argv = NULL; - ros::init(argc,argv,"bumper_node",ros::init_options::AnonymousName); - } - - this->rosnode_.reset(new ros::NodeHandle(this->robot_namespace_)); - - // resolve tf prefix - std::string prefix; - this->rosnode_->getParam(std::string("tf_prefix"), prefix); - this->frame_name_ = tf::resolve(prefix, this->frame_name_); - - this->contact_pub_ = this->rosnode_->advertise(this->bumper_topic_name_, 5); - - this->gts_sub_ = this->rosnode_->subscribe("gts", 1, &CreateBumperPlugin::GtsCb, this); - - // Listen to the update event. This event is broadcast every - // simulation iteration. - this->update_connection_ = this->bumper_->ConnectUpdated( - boost::bind(&CreateBumperPlugin::OnUpdate, this)); - - // Make sure the parent sensor is active. - this->bumper_->SetActive(true); - - ROS_INFO("Bumper plugin loaded"); + ROS_INFO_STREAM("Loading gazebo bumper"); + + this->bumper_ = + std::dynamic_pointer_cast(_parent); + if (!this->bumper_) + { + gzerr << "ContactPlugin requires a ContactSensor.\n"; + return; + } + + this->robot_namespace_ = ""; + if (_sdf->HasElement("robotNamespace")) + this->robot_namespace_ = _sdf->Get("robotNamespace") + "/"; + + // "publishing contact/collisions to this topic name: " << this->bumper_topic_name_ << std::endl; + this->bumper_topic_name_ = "bumper_base"; + if (_sdf->HasElement("bumperTopicName")) + this->bumper_topic_name_ = _sdf->Get("bumperTopicName"); + + // "transform contact/collisions pose, forces to this body (link) name: " << this->frame_name_ << std::endl; + if (!_sdf->HasElement("frameName")) + { + ROS_INFO("bumper plugin missing , defaults to world"); + this->frame_name_ = "world"; + } + else + this->frame_name_ = _sdf->Get("frameName"); + + this->bumper_event_.header.frame_id = this->frame_name_; + + ROS_INFO("Loaded with values: robotNamespace = %s, bumperTopicName = %s, frameName = %s", + this->robot_namespace_.c_str(), this->bumper_topic_name_.c_str(), this->frame_name_.c_str()); + + if (!ros::isInitialized()) + { + int argc = 0; + char** argv = NULL; + ros::init(argc, argv, "bumper_node", ros::init_options::AnonymousName); + } + + this->rosnode_.reset(new ros::NodeHandle(this->robot_namespace_)); + + // resolve tf prefix + std::string prefix; + this->rosnode_->getParam(std::string("tf_prefix"), prefix); + this->frame_name_ = tf::resolve(prefix, this->frame_name_); + + this->contact_pub_ = this->rosnode_->advertise(this->bumper_topic_name_, 5); + + this->gts_sub_ = this->rosnode_->subscribe("gts", 1, &CreateBumperPlugin::GtsCb, this); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->update_connection_ = this->bumper_->ConnectUpdated( + boost::bind(&CreateBumperPlugin::OnUpdate, this)); + + // Make sure the parent sensor is active. + this->bumper_->SetActive(true); + + ROS_INFO("Bumper plugin loaded"); } void CreateBumperPlugin::OnUpdate() @@ -113,7 +114,7 @@ void CreateBumperPlugin::OnUpdate() this->bumper_left_is_pressed_ = false; this->bumper_center_is_pressed_ = false; this->bumper_right_is_pressed_ = false; - + // Get all the contacts. msgs::Contacts contacts; contacts = this->bumper_->Contacts(); @@ -126,18 +127,19 @@ void CreateBumperPlugin::OnUpdate() { // using the force normals below, since the contact position is given in world coordinates // also negating the normal, because it points from contact to robot centre - const double global_contact_angle = std::atan2(-contacts.contact(i).normal(0).y(), -contacts.contact(i).normal(0).x()); + const double global_contact_angle = std::atan2( + -contacts.contact(i).normal(0).y(), -contacts.contact(i).normal(0).x()); const double relative_contact_angle = normalizeAngle(global_contact_angle - this->robot_heading_); - if ((relative_contact_angle <= (M_PI/2)) && (relative_contact_angle > (M_PI/6))) + if ((relative_contact_angle <= (M_PI / 2)) && (relative_contact_angle > (M_PI / 6))) { this->bumper_left_is_pressed_ = true; } - else if ((relative_contact_angle <= (M_PI/6)) && (relative_contact_angle >= (-M_PI/6))) + else if ((relative_contact_angle <= (M_PI / 6)) && (relative_contact_angle >= (-M_PI / 6))) { this->bumper_center_is_pressed_ = true; } - else if ((relative_contact_angle < (-M_PI/6)) && (relative_contact_angle >= (-M_PI/2))) + else if ((relative_contact_angle < (-M_PI / 6)) && (relative_contact_angle >= (-M_PI / 2))) { this->bumper_right_is_pressed_ = true; } @@ -195,10 +197,10 @@ void CreateBumperPlugin::GtsCb(const nav_msgs::Odometry::ConstPtr& msg) { // Get current robot heading (yaw angle) const tf::Quaternion q( - msg->pose.pose.orientation.x, - msg->pose.pose.orientation.y, - msg->pose.pose.orientation.z, - msg->pose.pose.orientation.w); + msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z, + msg->pose.pose.orientation.w); tf::Matrix3x3 m(q); double roll, pitch; m.getRPY(roll, pitch, this->robot_heading_); @@ -206,4 +208,5 @@ void CreateBumperPlugin::GtsCb(const nav_msgs::Odometry::ConstPtr& msg) // Register this plugin with the simulator GZ_REGISTER_SENSOR_PLUGIN(CreateBumperPlugin); -} + +} // namespace gazebo diff --git a/ca_gazebo/src/model_pose_publisher_plugin.cpp b/ca_gazebo/src/model_pose_publisher_plugin.cpp index c9ca98fd1..8a6f42aa3 100644 --- a/ca_gazebo/src/model_pose_publisher_plugin.cpp +++ b/ca_gazebo/src/model_pose_publisher_plugin.cpp @@ -1,3 +1,29 @@ +/* + * Copyright 2020 + * Emiliano Borghi + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/* + * Desc: Model pose publisher + * Author: Emiliano Borghi + * Date: 11 January 2020 + */ + + #include #include #include @@ -15,110 +41,126 @@ #include "geometry_msgs/Pose.h" #include "nav_msgs/Odometry.h" -static const ros::Duration update_period = ros::Duration(0.01); // 10 ms +#include + +static const ros::Duration update_period = ros::Duration(0.01); // 10 ms + namespace gazebo { class ModelPosePublisherPlugin : public ModelPlugin { +public: + void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) + { + GZ_ASSERT(_parent, "ModelPosePublisherPlugin _parent pointer is NULL"); - public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) { - GZ_ASSERT(_parent, "ModelPosePublisherPlugin _parent pointer is NULL"); - - // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " - << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); - return; - } - ROS_INFO("Model pose publisher started!"); - - // Store the pointer to the model - this->model_ = _parent; - - ROS_INFO("model Name = %s", this->model_->GetName().c_str()); - - // Listen to the update event. This event is broadcast every - // simulation iteration. - this->updateConnection = event::Events::ConnectWorldUpdateBegin( - std::bind(&ModelPosePublisherPlugin::OnUpdate, this)); - this->prev_update_time_ = ros::Time::now(); - - this->rosnode_.reset(new ros::NodeHandle()); - this->pose_pub_ = this->rosnode_->advertise((this->model_->GetName() + "/pose").c_str(), 1); - this->odom_pub_ = this->rosnode_->advertise((this->model_->GetName() + "/odom").c_str(), 1); - this->link_ = this->model_->GetLink("link"); - - this->odom_msg_.header.frame_id = kMapFrameID_; + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; } + ROS_INFO("Model pose publisher started!"); + + // Store the pointer to the model + this->model_ = _parent; + + ROS_INFO("model Name = %s", this->model_->GetName().c_str()); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->updateConnection = event::Events::ConnectWorldUpdateBegin( + std::bind(&ModelPosePublisherPlugin::OnUpdate, this)); + this->prev_update_time_ = ros::Time::now(); + + this->rosnode_.reset(new ros::NodeHandle()); + this->pose_pub_ = this->rosnode_->advertise((this->model_->GetName() + "/pose").c_str(), 1); + this->odom_pub_ = this->rosnode_->advertise((this->model_->GetName() + "/odom").c_str(), 1); + this->link_ = this->model_->GetLink("link"); + + this->odom_msg_.header.frame_id = kMapFrameID_; + } - // Called by the world update start event - public: void OnUpdate() + // Called by the world update start event +public: + void OnUpdate() + { + if ((ros::Time::now() - this->prev_update_time_) < update_period) { - if ((ros::Time::now() - this->prev_update_time_) < update_period) { - return; - } - - // Pose message - geometry_msgs::PosePtr pose_msg(new geometry_msgs::Pose); - ignition::math::Pose3d pose = this->link_->WorldPose(); - pose_msg->position.x = pose.Pos().X(); - pose_msg->position.y = pose.Pos().Y(); - pose_msg->position.z = pose.Pos().Z(); - - pose_msg->orientation.x = pose.Rot().X(); - pose_msg->orientation.y = pose.Rot().Y(); - pose_msg->orientation.z = pose.Rot().Z(); - pose_msg->orientation.w = pose.Rot().W(); - - this->pose_pub_.publish(pose_msg); - - // Odom message - this->odom_msg_.header.seq += 1; - this->odom_msg_.header.stamp = ros::Time::now(); - this->odom_msg_.child_frame_id = this->model_->GetName(); - this->odom_msg_.pose.pose = *pose_msg; - // this->odom_msg_.pose.covariance; - this->odom_msg_.twist.twist.linear.x = this->link_->WorldLinearVel().X(); - this->odom_msg_.twist.twist.linear.y = this->link_->WorldLinearVel().Y(); - this->odom_msg_.twist.twist.linear.z = this->link_->WorldLinearVel().Z(); - this->odom_msg_.twist.twist.angular.x = this->link_->WorldAngularVel().X(); - this->odom_msg_.twist.twist.angular.y = this->link_->WorldAngularVel().Y(); - this->odom_msg_.twist.twist.angular.z = this->link_->WorldAngularVel().Z(); - // this->odom_msg_.twist.covariance; - this->odom_pub_.publish(this->odom_msg_); - - // Publish Tf - geometry_msgs::TransformStamped transformStamped; - transformStamped.header.stamp = ros::Time::now(); - transformStamped.header.frame_id= kMapFrameID_; - transformStamped.child_frame_id = this->model_->GetName(); - transformStamped.transform.translation.x = pose_msg->position.x; - transformStamped.transform.translation.y = pose_msg->position.y; - transformStamped.transform.translation.z = 0.0; - transformStamped.transform.rotation = pose_msg->orientation; - - this->br_.sendTransform(transformStamped); - - // Update time - this->prev_update_time_ = ros::Time::now(); + return; } - private: std::shared_ptr rosnode_; - private: ros::Publisher pose_pub_; - private: ros::Publisher odom_pub_; - private: nav_msgs::Odometry odom_msg_; - private: physics::ModelPtr model_; - private: physics::LinkPtr link_; - private: ros::Time prev_update_time_; - private: tf2_ros::TransformBroadcaster br_; - private: const std::string kMapFrameID_ = "map"; - - // Pointer to the update event connection - private: event::ConnectionPtr updateConnection; - }; - - // Register this plugin with the simulator - GZ_REGISTER_MODEL_PLUGIN(ModelPosePublisherPlugin) -} + // Pose message + geometry_msgs::PosePtr pose_msg(new geometry_msgs::Pose); + ignition::math::Pose3d pose = this->link_->WorldPose(); + pose_msg->position.x = pose.Pos().X(); + pose_msg->position.y = pose.Pos().Y(); + pose_msg->position.z = pose.Pos().Z(); + + pose_msg->orientation.x = pose.Rot().X(); + pose_msg->orientation.y = pose.Rot().Y(); + pose_msg->orientation.z = pose.Rot().Z(); + pose_msg->orientation.w = pose.Rot().W(); + + this->pose_pub_.publish(pose_msg); + + // Odom message + this->odom_msg_.header.seq += 1; + this->odom_msg_.header.stamp = ros::Time::now(); + this->odom_msg_.child_frame_id = this->model_->GetName(); + this->odom_msg_.pose.pose = *pose_msg; + // this->odom_msg_.pose.covariance; + this->odom_msg_.twist.twist.linear.x = this->link_->WorldLinearVel().X(); + this->odom_msg_.twist.twist.linear.y = this->link_->WorldLinearVel().Y(); + this->odom_msg_.twist.twist.linear.z = this->link_->WorldLinearVel().Z(); + this->odom_msg_.twist.twist.angular.x = this->link_->WorldAngularVel().X(); + this->odom_msg_.twist.twist.angular.y = this->link_->WorldAngularVel().Y(); + this->odom_msg_.twist.twist.angular.z = this->link_->WorldAngularVel().Z(); + // this->odom_msg_.twist.covariance; + this->odom_pub_.publish(this->odom_msg_); + + // Publish Tf + geometry_msgs::TransformStamped transformStamped; + transformStamped.header.stamp = ros::Time::now(); + transformStamped.header.frame_id = kMapFrameID_; + transformStamped.child_frame_id = this->model_->GetName(); + transformStamped.transform.translation.x = pose_msg->position.x; + transformStamped.transform.translation.y = pose_msg->position.y; + transformStamped.transform.translation.z = 0.0; + transformStamped.transform.rotation = pose_msg->orientation; + + this->br_.sendTransform(transformStamped); + + // Update time + this->prev_update_time_ = ros::Time::now(); + } + +private: + std::shared_ptr rosnode_; +private: + ros::Publisher pose_pub_; +private: + ros::Publisher odom_pub_; +private: + nav_msgs::Odometry odom_msg_; +private: + physics::ModelPtr model_; +private: + physics::LinkPtr link_; +private: + ros::Time prev_update_time_; +private: + tf2_ros::TransformBroadcaster br_; +private: + const std::string kMapFrameID_ = "map"; + + // Pointer to the update event connection +private: + event::ConnectionPtr updateConnection; +}; + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(ModelPosePublisherPlugin) + +} // namespace gazebo diff --git a/ca_msgs/package.xml b/ca_msgs/package.xml index 6e3d78d4c..a5912026b 100644 --- a/ca_msgs/package.xml +++ b/ca_msgs/package.xml @@ -14,11 +14,10 @@ catkin - message_generation - std_msgs - + message_generation + std_msgs + message_runtime - std_msgs diff --git a/ca_node/CMakeLists.txt b/ca_node/CMakeLists.txt index 1314d75a5..4136cd8c7 100644 --- a/ca_node/CMakeLists.txt +++ b/ca_node/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(catkin REQUIRED COMPONENTS find_package(Boost REQUIRED system thread) catkin_package( - LIBRARIES ca_nodelet ca_driver + LIBRARIES ${PROJECT_NAME}let ca_driver CATKIN_DEPENDS ca_description ca_msgs @@ -41,3 +41,9 @@ install(DIRECTORY launch plugins install(DIRECTORY scripts DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(DIRECTORY scripts + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + USE_SOURCE_PERMISSIONS + PATTERN ".svn" EXCLUDE +) diff --git a/ca_node/src/nodelet/CMakeLists.txt b/ca_node/src/nodelet/CMakeLists.txt index 937dce2cd..29add63ac 100644 --- a/ca_node/src/nodelet/CMakeLists.txt +++ b/ca_node/src/nodelet/CMakeLists.txt @@ -2,10 +2,10 @@ # NODELET ############################################################################## -add_library(ca_nodelet ca_nodelet.cpp) -add_dependencies(ca_nodelet ca_driver) -target_link_libraries(ca_nodelet) +add_library(${PROJECT_NAME}let ${PROJECT_NAME}let.cpp) +add_dependencies(${PROJECT_NAME}let ca_driver) +target_link_libraries(${PROJECT_NAME}let) -install(TARGETS ca_nodelet +install(TARGETS ${PROJECT_NAME}let DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) diff --git a/ca_tools/CMakeLists.txt b/ca_tools/CMakeLists.txt index 7cb10788a..4a8c49db5 100644 --- a/ca_tools/CMakeLists.txt +++ b/ca_tools/CMakeLists.txt @@ -1,15 +1,29 @@ cmake_minimum_required(VERSION 2.8.3) project(ca_tools) -find_package(catkin REQUIRED) +find_package(catkin REQUIRED COMPONENTS + ca_move_base + geometry_msgs + joy_teleop + rospy + roslint +) catkin_package( CATKIN_DEPENDS - joy_teleop geometry_msgs + joy_teleop rospy ) install(DIRECTORY config launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + +install(DIRECTORY scripts + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + USE_SOURCE_PERMISSIONS + PATTERN ".svn" EXCLUDE +) + +roslint_python() diff --git a/ca_tools/package.xml b/ca_tools/package.xml index 7f55f39bf..c5a8258c4 100644 --- a/ca_tools/package.xml +++ b/ca_tools/package.xml @@ -14,11 +14,14 @@ catkin + roslint + + ca_move_base geometry_msgs + joy_teleop rospy joy - joy_teleop diff --git a/ca_tools/scripts/key_teleop.py b/ca_tools/scripts/key_teleop.py index d032fccd6..8f2a3e246 100755 --- a/ca_tools/scripts/key_teleop.py +++ b/ca_tools/scripts/key_teleop.py @@ -13,6 +13,7 @@ import rospy from geometry_msgs.msg import Twist + class Velocity(object): def __init__(self, min_velocity, max_velocity, num_steps): @@ -38,6 +39,7 @@ def __call__(self, value, step): max_value = self._min + self._step_incr * (step - 1) return value * max_value + class TextWindow(): _screen = None @@ -60,7 +62,7 @@ def clear(self): def write_line(self, lineno, message): if lineno < 0 or lineno >= self._num_lines: - raise ValueError, 'lineno out of bounds' + raise ValueError('lineno out of bounds') height, width = self._screen.getmaxyx() y = (height / self._num_lines) * lineno x = 10 @@ -75,6 +77,7 @@ def refresh(self): def beep(self): curses.flash() + class KeyTeleop(): _interface = None @@ -127,10 +130,10 @@ def _get_twist(self, linear, angular): def _key_pressed(self, keycode): movement_bindings = { - curses.KEY_UP: ( 1, 0), - curses.KEY_DOWN: (-1, 0), - curses.KEY_LEFT: ( 0, 1), - curses.KEY_RIGHT: ( 0, -1), + curses.KEY_UP: (1, 0), + curses.KEY_DOWN: (-1, 0), + curses.KEY_LEFT: (0, 1), + curses.KEY_RIGHT: (0, -1), } speed_bindings = { ord(' '): (0, 0), @@ -190,10 +193,10 @@ def __init__(self, interface): self._linear = 0 movement_bindings = { - curses.KEY_UP: ( 1, 0), - curses.KEY_DOWN: (-1, 0), - curses.KEY_LEFT: ( 0, 1), - curses.KEY_RIGHT: ( 0, -1), + curses.KEY_UP: (1, 0), + curses.KEY_DOWN: (-1, 0), + curses.KEY_LEFT: (0, 1), + curses.KEY_RIGHT: (0, -1), } def run(self): diff --git a/navigation/ca_localization/CMakeLists.txt b/navigation/ca_localization/CMakeLists.txt index 39595c675..f56d0241f 100644 --- a/navigation/ca_localization/CMakeLists.txt +++ b/navigation/ca_localization/CMakeLists.txt @@ -1,16 +1,12 @@ -if( ${CMAKE_SYSTEM_PROCESSOR} MATCHES "arm" ) - message( STATUS "Avoid compiling ca_localization because it's an ARM architecture" ) -else() - cmake_minimum_required(VERSION 2.8.3) - project(ca_localization) +cmake_minimum_required(VERSION 2.8.3) +project(ca_localization) - find_package(catkin REQUIRED COMPONENTS - robot_localization - ) +find_package(catkin REQUIRED COMPONENTS + robot_localization +) - catkin_package() +catkin_package() - include_directories( - ${catkin_INCLUDE_DIRS} - ) -endif() +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/navigation/ca_localization/package.xml b/navigation/ca_localization/package.xml index 7387ed911..eb57fbecc 100644 --- a/navigation/ca_localization/package.xml +++ b/navigation/ca_localization/package.xml @@ -2,7 +2,7 @@ ca_localization 0.0.0 - The ca_localization package + Contains localization tools Emiliano Borghi diff --git a/navigation/ca_move_base/CMakeLists.txt b/navigation/ca_move_base/CMakeLists.txt index 1264dc23f..f2a9518f9 100644 --- a/navigation/ca_move_base/CMakeLists.txt +++ b/navigation/ca_move_base/CMakeLists.txt @@ -1,12 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(ca_move_base) -## Compile as C++11, supported in ROS Kinetic and newer add_compile_options(-std=c++11) -## 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 actionlib amcl @@ -16,110 +12,22 @@ find_package(catkin REQUIRED COMPONENTS tf ) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES ca_move_base -# CATKIN_DEPENDS actionlib move_base move_base_msgs roscpp tf -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} + CATKIN_DEPENDS + move_base_msgs ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/ca_move_base.cpp -# ) +include_directories(${catkin_INCLUDE_DIRS}) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide add_executable(send_goal src/send_robot_goal.cpp) -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - ## Specify libraries to link a library or executable target against target_link_libraries(send_goal ${catkin_LIBRARIES} ) -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_ca_move_base.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +install(TARGETS send_goal + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/navigation/ca_move_base/package.xml b/navigation/ca_move_base/package.xml index 04a274533..9f8599779 100644 --- a/navigation/ca_move_base/package.xml +++ b/navigation/ca_move_base/package.xml @@ -2,7 +2,7 @@ ca_move_base 0.0.0 - The ca_move_base package + Has navigations utilities Emiliano Borghi diff --git a/navigation/ca_safety_controller/CMakeLists.txt b/navigation/ca_safety_controller/CMakeLists.txt index ee922e61e..b0a44896a 100644 --- a/navigation/ca_safety_controller/CMakeLists.txt +++ b/navigation/ca_safety_controller/CMakeLists.txt @@ -1,29 +1,50 @@ cmake_minimum_required(VERSION 2.8.3) project(ca_safety_controller) -find_package(catkin REQUIRED COMPONENTS roscpp nodelet pluginlib std_msgs geometry_msgs ca_msgs yocs_controllers ecl_threads) + +add_compile_options(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + ca_msgs + ecl_threads + geometry_msgs + nodelet + pluginlib + roscpp + roslint + std_msgs + yocs_controllers +) catkin_package( INCLUDE_DIRS include - LIBRARIES ca_safety_controller_nodelet - CATKIN_DEPENDS roscpp nodelet pluginlib std_msgs geometry_msgs ca_msgs yocs_controllers ecl_threads + LIBRARIES ${PROJECT_NAME}_nodelet + CATKIN_DEPENDS + ca_msgs + ecl_threads + geometry_msgs + nodelet + pluginlib + roscpp + std_msgs + yocs_controllers ) include_directories(include - ${catkin_INCLUDE_DIRS}) + ${catkin_INCLUDE_DIRS}) -add_library(ca_safety_controller_nodelet src/nodelet.cpp) -add_dependencies(ca_safety_controller_nodelet ca_msgs_gencpp) -target_link_libraries(ca_safety_controller_nodelet ${catkin_LIBRARIES}) +add_library(${PROJECT_NAME}_nodelet src/nodelet.cpp) +add_dependencies(${PROJECT_NAME}_nodelet ca_msgs_gencpp) +target_link_libraries(${PROJECT_NAME}_nodelet ${catkin_LIBRARIES}) -install(TARGETS ca_safety_controller_nodelet +install(TARGETS ${PROJECT_NAME}_nodelet DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) -install(DIRECTORY plugins - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) -install(DIRECTORY launch +install(DIRECTORY launch plugins DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + +set(ROSLINT_CPP_OPTS "--filter=-build/c++11") +roslint_cpp() diff --git a/navigation/ca_safety_controller/include/ca_safety_controller/safety_controller.hpp b/navigation/ca_safety_controller/include/ca_safety_controller/safety_controller.hpp index 4a6120b3b..fb2042482 100644 --- a/navigation/ca_safety_controller/include/ca_safety_controller/safety_controller.hpp +++ b/navigation/ca_safety_controller/include/ca_safety_controller/safety_controller.hpp @@ -86,10 +86,10 @@ class SafetyController : public yocs::Controller bumper_right_pressed_(false), cliff_left_detected_(false), cliff_center_detected_(false), - cliff_right_detected_(false), + cliff_right_detected_(false), last_event_time_(ros::Time(0)), - msg_(new geometry_msgs::Twist()){}; - ~SafetyController(){}; + msg_(new geometry_msgs::Twist()) {}; + ~SafetyController() {}; /** * Set-up necessary publishers/subscribers and variables @@ -181,7 +181,7 @@ void SafetyController::enableCB(const std_msgs::EmptyConstPtr msg) } else { - ROS_INFO_STREAM("Controller was already enabled. [" << name_ <<"]"); + ROS_INFO_STREAM("Controller was already enabled. [" << name_ << "]"); } }; @@ -189,11 +189,11 @@ void SafetyController::disableCB(const std_msgs::EmptyConstPtr msg) { if (this->disable()) { - ROS_INFO_STREAM("Controller has been disabled. [" << name_ <<"]"); + ROS_INFO_STREAM("Controller has been disabled. [" << name_ << "]"); } else { - ROS_INFO_STREAM("Controller was already disabled. [" << name_ <<"]"); + ROS_INFO_STREAM("Controller was already disabled. [" << name_ << "]"); } }; @@ -326,8 +326,9 @@ void SafetyController::spin() velocity_command_publisher_.publish(msg_); } //if we want to extend the safety state and we're within the time, just keep sending msg_ - else if (time_to_extend_bump_cliff_events_ > ros::Duration(1e-10) && - ros::Time::now() - last_event_time_ < time_to_extend_bump_cliff_events_) { + else if (time_to_extend_bump_cliff_events_ > ros::Duration(1e-10) && + ros::Time::now() - last_event_time_ < time_to_extend_bump_cliff_events_) + { velocity_command_publisher_.publish(msg_); } } diff --git a/navigation/ca_safety_controller/package.xml b/navigation/ca_safety_controller/package.xml index f7761e848..7033e3705 100644 --- a/navigation/ca_safety_controller/package.xml +++ b/navigation/ca_safety_controller/package.xml @@ -19,23 +19,24 @@ catkin - roscpp + ca_msgs + ecl_threads + geometry_msgs nodelet pluginlib + roscpp + roslint std_msgs - geometry_msgs - ca_msgs yocs_controllers - ecl_threads - roscpp + ca_msgs + ecl_threads + geometry_msgs nodelet pluginlib + roscpp std_msgs - geometry_msgs - ca_msgs yocs_controllers - ecl_threads diff --git a/navigation/ca_safety_controller/src/nodelet.cpp b/navigation/ca_safety_controller/src/nodelet.cpp index 6f8133118..72a521f77 100644 --- a/navigation/ca_safety_controller/src/nodelet.cpp +++ b/navigation/ca_safety_controller/src/nodelet.cpp @@ -43,8 +43,10 @@ #include #include #include + #include "ca_safety_controller/safety_controller.hpp" +#include namespace ca { @@ -52,7 +54,7 @@ namespace ca class SafetyControllerNodelet : public nodelet::Nodelet { public: - SafetyControllerNodelet() : shutdown_requested_(false) { }; + SafetyControllerNodelet() : shutdown_requested_(false) { } ~SafetyControllerNodelet() { NODELET_DEBUG_STREAM("Waiting for update thread to finish."); @@ -83,8 +85,8 @@ class SafetyControllerNodelet : public nodelet::Nodelet void update() { ros::Rate spin_rate(10); - controller_->enable(); // enable the controller when loading the nodelet - while (! shutdown_requested_ && ros::ok()) + controller_->enable(); // enable the controller when loading the nodelet + while ( !shutdown_requested_ && ros::ok() ) { controller_->spin(); spin_rate.sleep(); @@ -96,7 +98,7 @@ class SafetyControllerNodelet : public nodelet::Nodelet bool shutdown_requested_; }; -} // namespace ca +} // namespace ca PLUGINLIB_EXPORT_CLASS(ca::SafetyControllerNodelet, - nodelet::Nodelet); + nodelet::Nodelet); diff --git a/navigation/ca_slam/package.xml b/navigation/ca_slam/package.xml index 4f766c7ed..57f8ee465 100644 --- a/navigation/ca_slam/package.xml +++ b/navigation/ca_slam/package.xml @@ -2,7 +2,7 @@ ca_slam 0.0.0 - The ca_slam package + Wrapper for SLAM Emiliano Borghi diff --git a/navigation/ca_swarm/CMakeLists.txt b/navigation/ca_swarm/CMakeLists.txt index 63354c9f4..5ed28324c 100644 --- a/navigation/ca_swarm/CMakeLists.txt +++ b/navigation/ca_swarm/CMakeLists.txt @@ -10,26 +10,32 @@ add_compile_options(-std=c++11) find_package(catkin REQUIRED COMPONENTS actionlib amcl + ca_move_base + geometry_msgs move_base move_base_msgs roscpp + roslint + rospy tf + tf2_ros ) ################################### ## catkin specific configuration ## ################################### -catkin_package() +catkin_package( + CATKIN_DEPENDS + geometry_msgs + move_base_msgs + rospy +) ########### ## Build ## ########### -include_directories( - ${catkin_INCLUDE_DIRS} - geometry_msgs - rospy -) +include_directories(${catkin_INCLUDE_DIRS}) ############# ## Install ## @@ -37,7 +43,10 @@ include_directories( ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination -install(PROGRAMS - scripts/swarm_controller_node - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(DIRECTORY scripts + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + USE_SOURCE_PERMISSIONS + PATTERN ".svn" EXCLUDE ) + +roslint_python() diff --git a/navigation/ca_swarm/package.xml b/navigation/ca_swarm/package.xml index a407b77fa..e6a61500a 100644 --- a/navigation/ca_swarm/package.xml +++ b/navigation/ca_swarm/package.xml @@ -2,36 +2,28 @@ ca_swarm 0.0.0 - The ca_swarm package + Scripts to control multiple robots Emiliano Borghi BSD catkin - actionlib - ca_move_base - move_base - move_base_msgs - roscpp - actionlib - amcl - move_base - move_base_msgs - roscpp - tf - tf2_ros - actionlib - amcl - ca_move_base + + roslint + + actionlib + amcl + ca_move_base + geometry_msgs + move_base + move_base_msgs + roscpp + rospy + tf + tf2_ros + dwa_local_planner - geometry_msgs - move_base - move_base_msgs - roscpp - rospy - tf - tf2_ros map_server diff --git a/navigation/ca_swarm/scripts/box_client.py b/navigation/ca_swarm/scripts/box_client.py index 1a45629da..cbbfc7fc1 100755 --- a/navigation/ca_swarm/scripts/box_client.py +++ b/navigation/ca_swarm/scripts/box_client.py @@ -9,6 +9,7 @@ from visualization_msgs.msg import Marker from math import radians, pi + class MoveBaseSquare(): def __init__(self): rospy.init_node('box_client', anonymous=False) @@ -20,7 +21,7 @@ def __init__(self): # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. - waypoints.append(Pose(Point(10.0, 0.62, 0.0), Quaternion(0,0,0,1))) + waypoints.append(Pose(Point(10.0, 0.62, 0.0), Quaternion(0, 0, 0, 1))) # Initialize the visualization markers for RViz self.init_markers() @@ -68,26 +69,26 @@ def __init__(self): i += 1 def move(self, goal): - # Send the goal pose to the MoveBaseAction server - self.move_base.send_goal(goal) - - # Allow 1 minute to get there - finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) - - # If we don't get there in time, abort the goal - if not finished_within_time: - self.move_base.cancel_goal() - rospy.loginfo("Timed out achieving goal") - else: - # We made it! - state = self.move_base.get_state() - if state == GoalStatus.SUCCEEDED: - rospy.loginfo("Goal succeeded!") + # Send the goal pose to the MoveBaseAction server + self.move_base.send_goal(goal) + + # Allow 1 minute to get there + finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) + + # If we don't get there in time, abort the goal + if not finished_within_time: + self.move_base.cancel_goal() + rospy.loginfo("Timed out achieving goal") + else: + # We made it! + state = self.move_base.get_state() + if state == GoalStatus.SUCCEEDED: + rospy.loginfo("Goal succeeded!") def init_markers(self): # Set up our waypoint markers marker_scale = 0.2 - marker_lifetime = 0 # 0 is forever + marker_lifetime = 0 # 0 is forever marker_ns = 'waypoints' marker_id = 0 marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0} diff --git a/navigation/ca_visual_odometry/CMakeLists.txt b/navigation/ca_visual_odometry/CMakeLists.txt index 974de5b19..05ea96586 100644 --- a/navigation/ca_visual_odometry/CMakeLists.txt +++ b/navigation/ca_visual_odometry/CMakeLists.txt @@ -37,3 +37,9 @@ include_directories( # include ${catkin_INCLUDE_DIRS} ) + +install(DIRECTORY scripts + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + USE_SOURCE_PERMISSIONS + PATTERN ".svn" EXCLUDE +) diff --git a/navigation/ca_visual_odometry/package.xml b/navigation/ca_visual_odometry/package.xml index 71d33cf86..b7cbec878 100644 --- a/navigation/ca_visual_odometry/package.xml +++ b/navigation/ca_visual_odometry/package.xml @@ -2,7 +2,7 @@ ca_visual_odometry 0.0.0 - The ca_visual_odometry package + ca_visual_odometry performs visual odometry localization Emiliano Borghi @@ -10,6 +10,8 @@ catkin + camera_calibration + camera_calibration gscam image_proc diff --git a/sensors/ca_imu/CMakeLists.txt b/sensors/ca_imu/CMakeLists.txt index 7b7e65ce3..9fb1e4d12 100644 --- a/sensors/ca_imu/CMakeLists.txt +++ b/sensors/ca_imu/CMakeLists.txt @@ -1,21 +1,28 @@ -if( ${CMAKE_SYSTEM_PROCESSOR} MATCHES "arm" ) - cmake_minimum_required(VERSION 2.8.3) - project(ca_imu) - ## Compile as C++11, supported in ROS Kinetic and newer - add_compile_options(-std=c++11) +cmake_minimum_required(VERSION 2.8.3) +project(ca_imu) +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) - ## Find catkin macros and libraries - find_package(catkin REQUIRED COMPONENTS - geometry_msgs - roscpp - sensor_msgs - std_msgs - tf - ) +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + roscpp + sensor_msgs + std_msgs + tf +) - catkin_package(CATKIN_DEPENDS roscpp) - include_directories(${catkin_INCLUDE_DIRS}) +catkin_package(CATKIN_DEPENDS + geometry_msgs + roscpp + sensor_msgs + std_msgs +) -else() - message( STATUS "Avoid compiling ca_imu because it's not an ARM architecture" ) -endif() +include_directories(${catkin_INCLUDE_DIRS}) + +install(DIRECTORY scripts + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + USE_SOURCE_PERMISSIONS + PATTERN ".svn" EXCLUDE +) diff --git a/sensors/ca_imu/package.xml b/sensors/ca_imu/package.xml index 41a87a241..c8a013706 100644 --- a/sensors/ca_imu/package.xml +++ b/sensors/ca_imu/package.xml @@ -2,7 +2,7 @@ ca_imu 0.0.0 - The ca_imu package + Driver to provide Imu readings Emiliano Borghi