fix frames (#35)

Align with frame conventions as established in tue-robotics/emc_simulator#63
tue-robotics · Feb 14, 2024 · 2e8a2c8 · 2e8a2c8
2 parents 95c4ba2 + 4b01bd4
commit 2e8a2c8
Show file tree

Hide file tree

Showing 3 changed files with 1 addition and 93 deletions.
diff --git 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
@@ -53,9 +53,6 @@ Communication::Communication(std::string /*robot_name*/)
     ros::SubscribeOptions bumper_b_sub_options = ros::SubscribeOptions::create<std_msgs::Bool>(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<nav_msgs::MapMetaData>("/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<geometry_msgs::Twist>(base_ref_param, 1);
 
     pub_open_door_ = nh.advertise<std_msgs::Empty>(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
@@ -95,12 +95,6 @@ bool IO::ok()
 
 bool IO::sendPath(std::vector<std::vector<double>> path, std::array<double, 3> 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<std::vector<double>> path, std::array<double, 3> 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)