diff --git a/include/emc/communication.h b/include/emc/communication.h index 04c7d5c..e6aca73 100644 --- a/include/emc/communication.h +++ b/include/emc/communication.h @@ -23,34 +23,6 @@ namespace emc { -struct MapConfig{ - - /** - * @brief The width of each pixel in the map. - */ - double mapResolution; - - /** - * @brief The X coordinate of the centre of the map. - */ - double mapOffsetX; - - /** - * @brief The Y coordinate of the centre of the map. - */ - double mapOffsetY; - - /** - * @brief The rotation of the map. - */ - double mapOrientation; - - /** - * @brief Indicates if the map is initialised. if this is false, do not use the data. - */ - bool mapInitialised; -}; - class Communication { @@ -84,9 +56,6 @@ class Communication // Postion data void sendPoseEstimate(const geometry_msgs::Transform& pose); - // Map data - bool getMapConfig(MapConfig& config); - private: // Base velocity reference @@ -140,17 +109,9 @@ class Communication void bumperfCallback(const std_msgs::BoolConstPtr& msg); void bumperbCallback(const std_msgs::BoolConstPtr& msg); - // Map data - - ros::CallbackQueue mapdata_cb_queue_; - - ros::Subscriber sub_mapdata_; - - MapConfig mapconfig; + // pose publishing std::string robot_frame_name; - void mapCallback(const nav_msgs::MapMetaData::ConstPtr& msg); - /* // Control effort data diff --git a/src/communication.cpp b/src/communication.cpp index 82e5660..2ca1256 100644 --- a/src/communication.cpp +++ b/src/communication.cpp @@ -53,9 +53,6 @@ Communication::Communication(std::string /*robot_name*/) ros::SubscribeOptions bumper_b_sub_options = ros::SubscribeOptions::create(bumper_b_param, 1, boost::bind(&Communication::bumperbCallback, this, _1), ros::VoidPtr(), &bumper_b_cb_queue_); sub_bumper_b_ = nh.subscribe(bumper_b_sub_options); - ros::SubscribeOptions mapdata_sub_options = ros::SubscribeOptions::create("/map_metadata", 1, boost::bind(&Communication::mapCallback, this, _1), ros::VoidPtr(), &mapdata_cb_queue_); - sub_mapdata_ = nh.subscribe(mapdata_sub_options); - pub_base_ref_ = nh.advertise(base_ref_param, 1); pub_open_door_ = nh.advertise(open_door_param, 1); @@ -228,13 +225,6 @@ void Communication::sendPoseEstimate(const geometry_msgs::Transform& pose) pub_tf2->sendTransform(transformStamped); } -bool Communication::getMapConfig(MapConfig& config) { - if (!mapconfig.mapInitialised) - mapdata_cb_queue_.callAvailable(); //try to initialise the map - config = mapconfig; - return mapconfig.mapInitialised; -} - // ---------------------------------------------------------------------------------------------------- void Communication::laserCallback(const sensor_msgs::LaserScanConstPtr& msg) @@ -263,28 +253,6 @@ void Communication::bumperbCallback(const std_msgs::BoolConstPtr& msg) bumper_b_msg_ = msg; } -void Communication::mapCallback(const nav_msgs::MapMetaData::ConstPtr& msg) -{ - tf2::Quaternion q(msg->origin.orientation.x, - msg->origin.orientation.y, - msg->origin.orientation.z, - msg->origin.orientation.w); - - tf2::Matrix3x3 T(q); - - double roll, pitch, yaw; - T.getRPY(roll, pitch, yaw); - - mapconfig.mapOrientation = yaw + M_PI/2; - - mapconfig.mapResolution = msg->resolution; - - mapconfig.mapOffsetX = (msg->width * mapconfig.mapResolution / 2) * cos(yaw)-(msg->height * mapconfig.mapResolution / 2) * sin(yaw) + msg->origin.position.x; - mapconfig.mapOffsetY = (msg->width * mapconfig.mapResolution / 2) * sin(yaw)+(msg->height * mapconfig.mapResolution / 2) * cos(yaw) + msg->origin.position.y; - mapconfig.mapInitialised = true; - ROS_INFO_STREAM("Map data loaded"); - sub_mapdata_.shutdown(); -} // ---------------------------------------------------------------------------------------------------- /* void Communication::controlEffortCallback(const emc_system::controlEffortConstPtr& msg) diff --git a/src/io.cpp b/src/io.cpp index bac1441..cecc447 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -95,12 +95,6 @@ bool IO::ok() bool IO::sendPath(std::vector> path, std::array color, double width, int id) { - MapConfig mapData; - if (!comm_->getMapConfig(mapData)) - { - ROS_ERROR_STREAM("No map data supplied"); - return false; - } visualization_msgs::Marker pathMarker; pathMarker.header.frame_id = "map"; pathMarker.header.stamp = ros::Time(); @@ -136,21 +130,6 @@ bool IO::sendPath(std::vector> path, std::array c ROS_WARN_STREAM("point at index " << std::distance(path.begin(), it) << " has unused dimensions (expected 2 or 3, got " << (*it).size() << ")."); } } - - tf2::Transform tfPoint; - tfPoint.setOrigin(tf2::Vector3(point.x, point.y, point.z)); - tfPoint.setRotation(tf2::Quaternion(0, 0, 0, 1)); - tf2::Transform mapOffset; - mapOffset.setOrigin(tf2::Vector3(mapData.mapOffsetX, mapData.mapOffsetY, 0)); - tf2::Quaternion q; - q.setRPY(0, 0, mapData.mapOrientation); - mapOffset.setRotation(q); - - tfPoint = mapOffset * tfPoint; - point.x = tfPoint.getOrigin().x(); - point.y = tfPoint.getOrigin().y(); - point.z = tfPoint.getOrigin().z(); - pathMarker.points.push_back(point); } if (pathMarker.points.size() < 2)