Merge pull request #63 from kartavya2000/slam

Edits for SLAM

navigation_layer/mapping/CMakeLists.txt

@@ -57,25 +57,24 @@ find_package(Boost REQUIRED COMPONENTS system)
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+#add_message_files(
+ #  FILES
+ # slam_msg.msg
+ #)
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+  slam_msg.srv
+  slam_srv.srv
+)
 
 ## Generate actions in the 'action' folder
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES actionlib_msgs std_msgs nav_msgs
-# )
+generate_messages(
+  DEPENDENCIES actionlib_msgs std_msgs nav_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##

navigation_layer/mapping/include/slam_server.h

@@ -0,0 +1,30 @@
+#ifndef SLAM_SERVER_H
+#define SLAM_SERVER_H
+
+#include "slam_filter.h"
+#include "ros/ros.h"
+#include "mapping/slam_msg.h"
+#include "mapping/slam_srv.h"
+#include "util.h"
+
+namespace mapping{
+class SlamClient {
+    public:
+        int observe_landmark(const ros::NodeHandlePtr &n_, std::string obj_id, double m_x,double m_y, double m_z, double u_x, double u_y, double u_z, int uncertainty);
+        vec6 request_landmark(const ros::NodeHandlePtr &n_, std::string obj_id);
+        vec3 request_position(const ros::NodeHandlePtr &n_);
+};
+
+class SlamServer {
+    private:
+        SlamFilter *filter_;
+
+    public:
+        SlamServer(SlamFilter *filter);
+        bool observe(mapping::slam_msg::Request &msg, mapping::slam_msg::Response &rep);
+        bool slam_do(mapping::slam_srv::Request &req, mapping::slam_srv::Response &res);
+        void Listen(const ros::NodeHandlePtr &n_);
+        void Landmark_observe(const ros::NodeHandlePtr n_);
+};
+}
+#endif

navigation_layer/mapping/include/util.h

@@ -1,10 +1,15 @@
+#ifndef UTIL_H
+#define UTIL_H
+
 #include <string>
 #include <Eigen/Dense>
 
 typedef Eigen::Matrix<float, 3, 1> vec3;
 typedef Eigen::Matrix<float, 3, 3> mat3;
 typedef Eigen::Matrix<float, 6, 1> vec6;
 typedef Eigen::Matrix<float, Eigen::Dynamic, 1> vecX;
+typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> matX;
+
 
 struct Observation {
     std::string id;
@@ -14,4 +19,5 @@ struct Observation {
     int u_x;
     int u_y;
     int u_z;
-};
+};
+#endif

navigation_layer/mapping/package.xml

@@ -69,6 +69,7 @@
     <exec_depend>rospy</exec_depend>
     <exec_depend>std_msgs</exec_depend>
     <exec_depend>message_generation</exec_depend>
+    <exec_depend>message_runtime</exec_depend>
     <exec_depend>grid_map_core</exec_depend>
     <exec_depend>grid_map_msgs</exec_depend>
     <exec_depend>grid_map_ros</exec_depend>

navigation_layer/mapping/src/slam_clsrv.cpp

@@ -0,0 +1,120 @@
+#include "slam_server.h"
+
+namespace mapping{
+
+//SLAM CLIENT DEFINTIONS
+int SlamClient::observe_landmark(const ros::NodeHandlePtr &n_,std::string obj_id, double m_x, double m_y, double m_z, double u_x=0, 
+            double u_y=0, double u_z=0, int uncertainty=0)
+{
+    mapping::slam_msg msg;
+    msg.request.m_x=m_x;
+    msg.request.m_y=m_y;
+    msg.request.m_z=m_z;
+    msg.request.id=obj_id;
+
+    if(uncertainty==0)
+    {
+        msg.request.m_x=m_x;
+        msg.request.m_y=m_y;
+        msg.request.m_z=m_z;
+    }
+    else
+    {
+        msg.request.m_x=uncertainty;
+        msg.request.m_y=uncertainty;
+        msg.request.m_z=uncertainty;
+    }
+    ros::ServiceClient observe=n_->serviceClient<mapping::slam_msg>("observe_landmark"); //yet to see
+    if(observe.call(msg))
+    {
+        if(msg.response.rep==1) return 1;
+        else return 0;
+    } 
+    else 
+        ROS_ERROR("REQUEST FAILED");
+}
+
+vec6 SlamClient::request_landmark(const ros::NodeHandlePtr &n_, std::string obj_id)
+{
+    ros::ServiceClient slclient=n_->serviceClient<mapping::slam_srv>("request_slam"); 
+    mapping::slam_srv srv;
+    srv.request.id=obj_id;
+    if(slclient.call(srv))
+    {
+        vec6 result;
+        result << (float)srv.response.m_x,(float)srv.response.m_y,(float)srv.response.m_z,
+                       (float)srv.response.u_x,(float)srv.response.u_y,(float)srv.response.u_z;
+        return result;
+    }
+    else
+    {
+        ROS_ERROR("REQUEST FAILED");
+    }
+}
+
+vec3 SlamClient::request_position(const ros::NodeHandlePtr &n_)
+{
+    ros::ServiceClient slclient=n_->serviceClient<mapping::slam_srv>("request_slam"); 
+    mapping::slam_srv srv;
+    srv.request.id="";
+    if(slclient.call(srv))
+    {
+        vec3 result;
+        result << (float)srv.response.m_x,(float)srv.response.m_y,(float)srv.response.m_z;
+        return result;
+    }
+    else
+    {
+        ROS_ERROR("REQUEST FAILED");
+    }
+}
+
+//SLAM SERVER DEFINITIONS
+SlamServer::SlamServer(SlamFilter *filter)
+    : filter_{filter} {
+
+}
+bool SlamServer::slam_do(mapping::slam_srv::Request &req, mapping::slam_srv::Response &res)
+{
+
+    if (req.id == "") {
+        vec3 pos {filter_->GetState()};
+        res.m_x=pos(0,0);
+        res.m_y=pos(1,0);
+        res.m_z=pos(2,0);
+        }
+    else {
+        vec6 state {filter_->GetState(req.id)};
+        res.m_x=state(0,0);
+        res.m_y=state(1,0);
+        res.m_z=state(2,0);
+        res.u_x=state(3,0);
+        res.u_y=state(4,0);
+        res.u_z=state(5,0);
+        }
+    return true;
+    }
+
+void SlamServer::Listen(const ros::NodeHandlePtr &n_) {
+    while (true) 
+    {
+        ros::ServiceServer slsrv=n_->advertiseService("request_slam",&SlamServer::slam_do,this);
+    }
+    std::cout << "Finishing Up..." << std::endl;
+}
+
+bool SlamServer::observe(mapping::slam_msg::Request &msg, mapping::slam_msg::Response &rep)
+{
+    std::string id {msg.id.c_str()};
+    vec3 relpos {float(msg.m_x), float(msg.m_y), float(msg.m_z)};
+    mat3 cov {vec3(float(msg.u_x), float(msg.u_y), float(msg.u_z)).asDiagonal()};
+    filter_->Landmark(id,relpos, cov);
+    rep.rep=1;
+    return true;
+}
+void SlamServer::Landmark_observe(const ros::NodeHandlePtr n_)
+{
+    ros::ServiceServer sub=n_->advertiseService("observe_landmark",&SlamServer::observe,this);
+}
+
+}

navigation_layer/mapping/srv/slam_msg.srv

@@ -0,0 +1,9 @@
+string id
+float32 m_x
+float32 m_y
+float32 m_z
+float32 u_x
+float32 u_y
+float32 u_z
+---
+int8 rep

navigation_layer/mapping/srv/slam_srv.srv

@@ -0,0 +1,8 @@
+string id
+---
+float32 m_x
+float32 m_y
+float32 m_z
+float32 u_x
+float32 u_y
+float32 u_z