updated aruco detect launch files

aruco_markers_detect.launch

@@ -4,7 +4,7 @@
   <arg name="camera" default="/usb_cam"/>
   <arg name="image" default="image"/>
   <arg name="transport" default="raw"/>
-  <arg name="fiducial_len" default="0.14"/>
+  <arg name="fiducial_len" default="0.0845"/>
   <arg name="dictionary" default="7"/>
   <arg name="do_pose_estimation" default="true"/>
 

meas_transform.py

@@ -11,7 +11,7 @@ import math
 
 compname = getpass.getuser()
 
-dataset_path = "/home/"+ compname +"/full_tests/full_test_v2_3/"
+dataset_path = "/home/"+ compname +"/full_tests/full_test_v2_4/"
 
 def meas_to_meas_x(robot_label):
     meas_x_file_name = "Robot"+str(robot_label)+"_Measurement_x.dat"
@@ -35,7 +35,6 @@ def meas_to_meas_x(robot_label):
     with open(dataset_path+meas_file_name,'r+') as measure_file:
         for line in measure_file:
             if line[0] != '#':
-                time = float(line.split()[0])
                 subject_ID = line.split()[1]
                 range_cam = float(line.split()[2])
                 bearing = float(line.split()[3])
@@ -46,7 +45,7 @@ def meas_to_meas_x(robot_label):
                 
                 phi = math.atan2((range_cam*math.sin(bearing)), (range_cam*math.cos(bearing)+l))
                 r_cen = range_cam*math.sin(bearing)/math.sin(phi)
-                f.write(line.split()[0] + '\t' + subject_ID + '\t\t\t' + str(range_cam) + '\t\t\t'+ line.split()[3] +'\n')
+                f.write(line.split()[0] + '\t' + subject_ID + '\t\t\t' + str(r_cen) + '\t\t\t'+ str(phi) +'\n')
             
     f.close()
 

read_csv.py

@@ -12,7 +12,7 @@ from datetime import datetime
 import math
 
 compname = getpass.getuser()
-datapath = "/home/"+ compname +"/full_tests/full_test_v2_3/"
+datapath = "/home/"+ compname +"/full_tests/full_test_v2_4/"
 
 
 
@@ -60,7 +60,7 @@ def generate_landmarks_gt_file(landmark_names):
     gt_file_name = "Landmark_Groundtruth.dat"
     print(gt_file_name)
     f_r = open(datapath+gt_file_name, "w+")
-    f_r.write("# Time [sec] \t x [m] \t y [m] \n")
+    f_r.write("# Landmark_tags [sec] \t x [m] \t y [m] \n")
 
 
     input_data = list(csv.reader(open(datapath+"gt.csv", "r+")))
@@ -94,5 +94,5 @@ def generate_landmarks_gt_file(landmark_names):
 for robot_label in [1,2]:
     generate_robot_gt_file(robot_label)
 
-landmark_names = ["landmark0", "landmark1", "landmark2", "landmark3"]
+landmark_names = ["landmark0", "landmark1", "landmark2"]
 generate_landmarks_gt_file(landmark_names)
\ No newline at end of file

robot_controls/create2_motion_control_r1.py

@@ -33,8 +33,8 @@ class create2_motion_controller(object):
         self.evasion_time = 0
         self.end_node = time.time() + (60 * 5)
         self.compname = getpass.getuser()
-        #self.f = open("/home/"+ self.compname +"/catkin_ws/ros_colo_dataset/"+str(time.time())+"Robot1_Odometry.dat", "w+")
-        #self.f.write("# Time [sec] \t Velocity [m/s] \t Angular Velocity [rad/s] \n")
+        self.f = open("/home/"+ self.compname +"/catkin_ws/ros_colo_dataset/Robot1_Odometry_c"+str(time.time())+".dat", "w+")
+        self.f.write("# Time [sec] \t Velocity [m/s] \t Angular Velocity [rad/s] \n")
         self.movement_bindings = {
             'i':(1,0,0,0),
             'o':(1,0,0,-1),
@@ -49,7 +49,7 @@ class create2_motion_controller(object):
 
     def twist_msgs(self, lin_vel, ang_vel):
         rtime = time.time()
-        #self.f.write(str(rtime) + '\t\t' +str(lin_vel) + '\t\t' + str(ang_vel) + '\n')
+        self.f.write(str(rtime) + '\t\t' +str(lin_vel) + '\t\t' + str(ang_vel) + '\n')
         print(str(rtime) + '\t\t' +str(lin_vel) + '\t\t' + str(ang_vel) + '\n')
 
     def movements(self, key, linear, angular):
@@ -103,11 +103,9 @@ class create2_motion_controller(object):
         linear_vel = self.twist.linear.x
         angular_vel = self.twist.angular.z
 
-        linear_vel = (linear_vel-0.2)/2
-        if barriers[0]: # barrier on the left:
-            angular_vel = (angular_vel + 0.1)/2
-        if barriers[2]: # barrier on the right:
-            angular_vel = (angular_vel - 0.1)/2
+        linear_vel = (linear_vel-0.1)/2
+        angular_vel = (angular_vel - 0.2)/2
+
         self.twist.linear.x = linear_vel
         self.twist.angular.z = angular_vel
         print("back 0: ", self.twist)
@@ -130,7 +128,7 @@ class create2_motion_controller(object):
         '''
     def evasion(self, barriers):
         print("evasion")
-        linear_vel = 0
+        linear_vel = -0.1
         angular_vel = -0.2
         self.twist.linear.x = linear_vel
         self.twist.angular.z = angular_vel

robot_controls/create2_motion_control_r2.py

+#!/usr/bin/env python
+# license removed for brevity
+import numpy as np
+import rospy
+import sys, termios, tty
+import random
+from math import radians, degrees
+from ca_msgs.msg import Bumper
+import getpass
+import time
+from geometry_msgs.msg import Twist
+
+
+class create2_motion_controller(object):
+    """
+    docstring for create2_motion_controller
+
+    note: improve evasion to be smarter
+          make manual (optional)
+
+    """
+
+    def __init__(self):
+        self.control_pub = rospy.Publisher("cmd_vel", Twist, queue_size = 30)
+        self.state_sub = rospy.Subscriber("bumper", Bumper, self.robot_motion) #original_freq = 10hz
+        self.rate = rospy.Rate(10)
+        self.eva = False
+        #not sure for get_param
+        #self.lin = rospy.get_param('~linVel', .2)
+        #self.ang = rospy.get_param('~angVel', 1.0)
+        self.lin = 0.1
+        self.ang = 0.4
+        self.evasion_time = 0
+        self.end_node = time.time() + (60 * 5)
+        self.compname = getpass.getuser()
+        self.f = open("/home/"+ self.compname +"/catkin_ws/ros_colo_dataset/Robot2_Odometry_c"+str(time.time())+".dat", "w+")
+        self.f.write("# Time [sec] \t Velocity [m/s] \t Angular Velocity [rad/s] \n")
+        self.movement_bindings = {
+            'i':(1,0,0,0),
+            'o':(1,0,0,-1),
+            'j':(0,0,0,1),
+            'l':(0,0,0,-1),
+            'u':(1,0,0,1),
+            ',':(-1,0,0,0),
+            '.':(-1,0,0,1),
+            'm':(-1,0,0,-1),
+            }
+        self.twist = Twist()
+
+    def twist_msgs(self, lin_vel, ang_vel):
+        rtime = time.time()
+        self.f.write(str(rtime) + '\t\t' +str(lin_vel) + '\t\t' + str(ang_vel) + '\n')
+        print(str(rtime) + '\t\t' +str(lin_vel) + '\t\t' + str(ang_vel) + '\n')
+
+    def movements(self, key, linear, angular):
+        x = self.movement_bindings[key][0]
+        y = self.movement_bindings[key][1]
+        z = self.movement_bindings[key][2]
+        th = self.movement_bindings[key][3]
+        twist = Twist()
+        twist.linear.x = x*linear; twist.linear.y = y*linear; twist.linear.z = z*linear;
+        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*angular
+        self.control_pub.publish(twist)
+
+    def smooth_movements(self, linear_vel, angular_vel):
+
+        '''
+        control_input = [linear_vel, angular_vel]
+        self.twist.linear.x = 0 # [pos: forward velocity
+        self.twist.linear.y = 0 # no effects
+        self.twist.linear.z = 0 # no effects
+        self.twist.angular.x = 0  # no effects
+        self.twist.angular.y = 0  # no effects
+        self.twist.angular.z = 0.5 # angular velocity pos: counter-clock
+        '''
+        if 0.2 > linear_vel > 0:
+            self.twist.linear.x = linear_vel
+        elif linear_vel <= 0:
+            self.twist.linear.x = 0
+        else:
+            self.twist.linear.x = 0.2
+
+        if 0.1 > angular_vel > -0.1:
+            self.twist.angular.z = angular_vel
+        elif angular_vel > 0.1:
+            self.twist.angular.z = 0.1
+        else:
+            self.twist.angular.z = -0.1
+
+        self.twist.linear.x = 0.1
+        self.twist.angular.z = -0
+
+
+        self.twist_msgs(self.twist.linear.x, self.twist.angular.z)
+        self.control_pub.publish(self.twist)
+
+
+    def manual_control(self, manual_input):
+        pass
+
+    def backoff(self, barriers):
+        print("backoff")
+        linear_vel = self.twist.linear.x
+        angular_vel = self.twist.angular.z
+
+        linear_vel = (linear_vel-0.2)/2
+        angular_vel = (angular_vel - 0.2)/2
+
+        self.twist.linear.x = linear_vel
+        self.twist.angular.z = angular_vel
+        print("back 0: ", self.twist)
+        self.control_pub.publish(self.twist)
+        self.twist_msgs(self.twist.linear.x, self.twist.angular.z)        
+
+        '''
+        linear_vel = -0.1
+        if barriers[0]: # barrier on the left:
+            angular_vel = -0.2
+        elif barriers[1]:
+            angular_vel = -0.1
+        elif barriers[2]:
+            angular_vel = 0.2
+        self.twist.linear.x = linear_vel
+        self.twist.angular.z = angular_vel
+        print("back 1: ", self.twist)
+        self.control_pub.publish(self.twist)
+        self.twist_msgs(self.twist.linear.x, self.twist.angular.z)
+        '''
+    def evasion(self, barriers):
+        print("evasion")
+        linear_vel = 0
+        angular_vel = -0.2
+
+        self.twist.linear.x = linear_vel
+        self.twist.angular.z = angular_vel
+        print("back 0: ", self.twist)
+        self.control_pub.publish(self.twist)
+        self.twist_msgs(self.twist.linear.x, self.twist.angular.z)
+
+    def random_movement(self):
+        print("Movement")
+        delta_vel = random.uniform(-0.02, 0.02)
+        delta_ang_vel = random.uniform(-0.07, 0.07)
+        self.smooth_movements(self.twist.linear.x+delta_vel, self.twist.angular.z+delta_ang_vel)
+
+    def robot_motion(self, data):
+        if time.time() > self.end_node:
+            exit()
+        lef = data.is_light_left
+        lefron = data.is_light_front_left
+        lefcen =  data.is_light_center_left
+        rigcen = data.is_light_center_right
+        rigfron = data.is_light_front_right
+        rig = data.is_light_right
+
+        bumper_l = data.is_left_pressed
+        bumper_r = data.is_right_pressed
+
+        left_barrier = bumper_l | lefron | lef
+        front_barrier = lefcen | rigcen
+        right_barrier =  rigfron  | bumper_r | rig
+        barriers = [left_barrier, front_barrier, right_barrier]
+	#disabled rig and lef
+        barriers_detected = lefron | lefcen | rigcen | rigfron | bumper_l | bumper_r | rig | lef
+        if barriers_detected:
+            self.backoff(barriers)
+            self.evasion_time = time.time()+1
+        elif self.evasion_time >= time.time():
+            self.evasion(barriers)
+        else:
+            self.random_movement()
+
+if __name__ == '__main__':
+    rospy.init_node('create2_motion_controller', anonymous=True)
+    c = create2_motion_controller()
+    c.rate.sleep()
+    rospy.spin()

robot_controls/odom_record.py

@@ -7,22 +7,15 @@ import time
 import math
 import rethinkdb as r
 
-class odom_listener(object):
-    def __init__(self):
-        self.odom_sub = rospy.Subscriber("odom", odom, self.odom_record) #original_freq = 10hz
-        self.f = open("/home/"+ self.compname +"/catkin_ws/ros_colo_dataset/Robot1_Odometry.dat", "w+")
-        self.f.write("# Time [sec] \t Velocity [m/s] \t Angular Velocity [rad/s] \n")
-    def odom_record(self, data):
-        t_sec = data.header.stamp.secs
-        t_nsec = data.header.stamp.nsecs
-        rtime = time.time()
-        lin_vel = data.twist.linear.x
-        ang_vel = data.twist.angular.z 
-        lvel = 0
-        rvel = 0
-        self.f.write(str(rtime) + '\t\t' +str(lin_vel) + '\t\t' + str(ang_vel) + '\n')
-
-        rethink(t_sec, t_nsec, v, w, lvel, rvel)
+def callback(data):
+    t_sec = data.header.stamp.secs
+    t_nsec = data.header.stamp.nsecs
+    v = data.twist.linear.x
+    w = data.twist.angular.z 
+    first = f.read(1)
+    lvel = 0
+    rvel = 0
+    rethink(t_sec, t_nsec, v, w, lvel, rvel)
     #time.sleep(2/60)
 
 def rethink(tsec, nsec, linvel, angvel, lvel, rvel):
@@ -38,10 +31,12 @@ def rethink(tsec, nsec, linvel, angvel, lvel, rvel):
         "right wheel velocity": rvel,
         }).run(conn)
 
-if __name__== '__main__':
-    compname = getpass.getuser()
+def listener():
     rospy.init_node('listen_odom', anonymous=True)
-    l = odom_listener()
+    rospy.Subscriber("odom", JointState, callback)
     rospy.spin()
-	
+
+if __name__== '__main__':
+    compname = getpass.getuser()
+    listener()