fix error

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(ieuagv_localization)
+project(advoard_localization)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
@@ -104,7 +104,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
-#  LIBRARIES ieuagv_localization
+#  LIBRARIES advoard_localization
 #  CATKIN_DEPENDS roscpp rospy std_msgs
 #  DEPENDS system_lib
 )
@@ -122,7 +122,7 @@ include_directories(
 
 ## Declare a C++ library
 # add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/ieuagv_localization.cpp
+#   src/${PROJECT_NAME}/advoard_localization.cpp
 # )
 
 ## Add cmake target dependencies of the library
@@ -133,7 +133,7 @@ include_directories(
 ## 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(${PROJECT_NAME}_node src/ieuagv_localization_node.cpp)
+# add_executable(${PROJECT_NAME}_node src/advoard_localization_node.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -197,7 +197,7 @@ include_directories(
 #############
 
 ## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_ieuagv_localization.cpp)
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_advoard_localization.cpp)
 # if(TARGET ${PROJECT_NAME}-test)
 #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
 # endif()

launch/kalman_localization.launch

@@ -1,3 +1,3 @@
 <launch>
-    <node pkg="ieuagv_localization" name="kalman_filter_localization" type="kalman_filter_localization.py"/>
+    <node pkg="advoard_localization" name="kalman_filter_localization" type="kalman_filter_localization.py"/>
 </launch>

launch/lidar_uwb_initial_pose.launch

@@ -1,4 +1,4 @@
 <launch>
-    <include file="$(find ieuagv_localization)/launch/kalman_localization.launch" />
-    <node pkg="ieuagv_localization" name="initialpose_lidar_uwb" type="initialpose_lidar_uwb.py" />
+    <include file="$(find advoard_localization)/launch/kalman_localization.launch" />
+    <node pkg="advoard_localization" name="initialpose_lidar_uwb" type="initialpose_lidar_uwb.py" />
 </launch>

launch/uwb_initial_pose.launch

@@ -1,4 +1,4 @@
 <launch>
-  <node pkg="ieuagv_pozyx" name="kalman_filter_localization" type="kalman_filter_localization.py"/>
-  <node pkg="ieuagv_pozyx" name="initialpose_uwb_average" type="initialpose_uwb_average.py" >
+  <node pkg="advoard_pozyx" name="kalman_filter_localization" type="kalman_filter_localization.py"/>
+  <node pkg="advoard_pozyx" name="initialpose_uwb_average" type="initialpose_uwb_average.py" >
 </launch>

package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>ieuagv_localization</name>
+  <name>advoard_localization</name>
   <version>0.0.1</version>
-  <description>The ieuagv_localization package</description>
+  <description>The advoard_localization package</description>
   <maintainer email="[email protected]">Bostanci</maintainer>
   <license>BSD</license>
   <author email="[email protected]">Bekir Bostanci</author>

src/kalman_filter_localization.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 '''
 __author__ = "Bekir Bostanci"
 __license__ = "BSD"
@@ -14,7 +14,9 @@
 
 from nav_msgs.msg import Odometry
 from geometry_msgs.msg import Pose
-from ieu_agv.msg import  uwb_data
+from pozyx_simulation.msg import  uwb_data
+from geometry_msgs.msg import PointStamped
+
 import math
 
 import tf 
@@ -30,6 +32,7 @@
 
 rospy.init_node('kalman_filter_localization', anonymous=True)
 pub = rospy.Publisher('localization_data_topic', Pose, queue_size=10)
+pub_ = rospy.Publisher('position_kalman', PointStamped, queue_size=10)
 r = rospy.Rate(1)
 
 
@@ -47,7 +50,7 @@ def prediction_step(Odometry):
     y = mu[1]
     theta = mu[2]
 
-    delta_vel = Odometry.twist.twist.linear.x *1000           # redefine r1                  odom=>twist=>linear=>x 
+    delta_vel = Odometry.twist.twist.linear.x            # redefine r1                  odom=>twist=>linear=>x 
     delta_w = Odometry.twist.twist.angular.z                  # redefine t                   odom=>twist=>angular=>z
 
     noise = 0.1**2
@@ -151,6 +154,12 @@ def publish_data(pose_x,pose_y):
     robot_pos.orientation.w = 0.0
     pub.publish(robot_pos)
 
+    robot_pos_ = PointStamped()
+    robot_pos_.header.stamp = rospy.Time.now() 
+    robot_pos_.header.frame_id = "map" 
+    robot_pos_.point = robot_pos.position
+    pub_.publish(robot_pos_)
+
 
 def get_anchors_pos():
     max_anchor = 100
@@ -166,14 +175,6 @@ def get_anchors_pos():
         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
             break
 
-    sensor_pos = np.dot(sensor_pos,1000)
-
-
-    if sensor_pos == [] :
-        rospy.logwarn("There is not found any anchors. Function is working again.")    
-        get_anchors_pos()
-
-
     return sensor_pos
 
 if __name__ == "__main__":
@@ -183,4 +184,4 @@ def get_anchors_pos():
     rospy.Subscriber("odom", Odometry, subscribe_odom_data)
     rospy.Subscriber("uwb_data_topic", uwb_data, subscribe_uwb_data)
 
-    rospy.spin()
+    rospy.spin()

src/sqrrange_leastsqr_localization.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 '''
 __author__ = "Bekir Bostanci"
 __license__ = "BSD"