Which part in this code that I should change to make it move from different angle

asked 2020-07-11 01:16:36 -0500

irsyad gravatar image

updated 2021-04-24 02:38:45 -0500

this is code of follow demo , did someone can show me the part that I can change the code, so it will follow from it right means from different angle

import rospy
import os
import pickle
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
import numpy as np

class follower:
    def __init__(self):
        rospy.loginfo('Follower node initialized')
        self.config_dir = os.path.join(os.path.dirname(__file__))
        self.config_dir = self.config_dir.replace('nodes', 'config')
        self.pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
        self.clf = pickle.load(open(self.config_dir + '/clf', "rb"))
        self.clf2 = pickle.load(open(self.config_dir + '/clf2', "rb"))
        self.labels = {'30_0':0, '30_l':1, '30_r':2, '45_0':3, '45_l':4, '45_r':5,'15_0':6, 'empty':7}
        rospy.loginfo('Tree initialized')
        self.follow()

    def check_people(self):
        laser_data=[]
        laser_data_set=[]
        result=[]
        ret = 0
        self.msg = rospy.wait_for_message("scan_filtered", LaserScan)

        for i in range(70,-2,-1) + range(359, 289,-1):

            if   np.nan_to_num( self.msg.intensities[i] ) != 0 :
                 laser_data.append(np.nan_to_num(self.msg.intensities[i]))

            elif (i+1) in range(70,-2,-1) + range(359, 289,-1) and (i-1) in range(70,-2,-1) + range(359, 289,-1) and np.nan_to_num(self.msg.intensities[i]) == 0:
                 laser_data.append((np.nan_to_num(self.msg.intensities[i+1])+np.nan_to_num(self.msg.intensities[i-1]))/2)

            else :
                 laser_data.append(np.nan_to_num(self.msg.intensities[i]))

        laser_data_set.append(laser_data)

        [x for (x , y) in self.labels.iteritems() if y == self.clf2.predict(laser_data_set) ] ## Predict the position

        if result == ['empty']:
            ret = 0

        else:
            ret = 1

        return ret

    def laser_scan(self):
        data_test=[]
        data_test_set=[]
        self.msg = rospy.wait_for_message("scan_filtered", LaserScan)

        for i in range(70,-2,-1) + range(359, 289,-1):

            if   np.nan_to_num( self.msg.ranges[i] ) != 0 :
                 data_test.append(np.nan_to_num(self.msg.ranges[i]))

            elif (i+1) in range(70,-2,-1) + range(359, 289,-1) and (i-1) in range(70,-2,-1) + range(359, 289,-1) and np.nan_to_num(self.msg.ranges[i]) == 0:
                 data_test.append((np.nan_to_num(self.msg.ranges[i+1])+np.nan_to_num(self.msg.ranges[i-1]))/2)

            else :
                 data_test.append(np.nan_to_num(self.msg.ranges[i]))

        data_test_set.append(data_test)

        return [x for (x , y) in self.labels.iteritems() if y == self.clf.predict(data_test_set) ] 

    def follow(self):
        while not rospy.is_shutdown():
            check = self.check_people()
            if  check == 1:
                x = self.laser_scan()
                twist = Twist()
                ## Do something according to each position##
                if  x == ['30_0']:
                    twist.linear.x  = 0.13;         twist.angular.z = 0.0;
                elif x== ['30_l']:
                    twist.linear.x  = 0.10;         twist.angular.z = 0.4;
                elif x== ['30_r']:
                    twist.linear.x  = 0.10;         twist.angular.z = -0.4;
                elif x== ['45_0']:
                    twist.linear.x  = 0.13;         twist.angular.z = 0.0;
                elif x== ['45_l']:
                    twist.linear.x  = 0.10;         twist.angular.z = 0.3;
                elif x== ['45_r']:
                    twist.linear.x  = 0.10;         twist.angular.z = -0.3;
                elif x== ['15_0']:
                    twist.linear.x  = 0.0;          twist.angular.z = 0.0;
                elif x== ['empty']:
                    twist.linear.x  = 0.0;          twist.angular.z = 0.0;
                else:
                    twist.linear.x  = 0.0;          twist ...
(more)
edit retag flag offensive close merge delete

Comments

1

I reformatted this, but it is still a badly posed question with almost no context. It is unclear what you have tried and what you want to do. Please read the instructions on how to ask a good question.

fvd gravatar image fvd  ( 2020-07-21 03:15:39 -0500 )edit