# rospy.loginfo('smm: dx '+str(x_new-self.x))
            y_new = (self.y - v_hat/w_hat*math.cos(self.theta)
                - v_hat/w_hat*math.cos(self.theta+w_hat*dt))
            # rospy.loginfo('smm: dy '+str(y_new-self.y))

        theta_new = self.theta + w_hat*dt + y_hat*dt

        new_odom = Odometry()
        new_odom.pose.pose.position.x = x_new
        new_odom.pose.pose.position.y = y_new
        new_odom.pose.pose.orientation = heading_to_quaternion(theta_new)
        new_odom.twist.twist.linear.x = v_hat
        new_odom.twist.twist.angular.z = w_hat
        new_odom.header.frame_id = self.frame_id

        return new_odom

    # pylint: disable=unused-argument
    # it will be used when implmenented properly
    def sample_normal(self, unused_b):
        # TODO(buckbaskin): change to sample normal distribution
        # norm(mean=0, stdev = b)
        return 0


if __name__ == '__main__':
    # pylint: disable=invalid-name
    # leader is a fine name, it's not a constant
    leader = ForceLeader()
    leader.run_server(20)
예제 #2
0
        des_speed = .5 # m/s
        dx = end.pose.pose.position.x - start.pose.pose.position.x
        dy = end.pose.pose.position.y - start.pose.pose.position.y

        heading = math.atan2(dy, dx)
        dx = des_speed*math.cos(heading)*dt
        dy = des_speed*math.sin(heading)*dt

        distance = math.sqrt(dx*dx+dy*dy)
        steps = math.floor(distance/des_speed)

        for i in range(1, int(steps)):
            odo = Odometry()
            odo.header.frame_id = 'odom'
            odo.pose.pose.point = Point(x=start.x+i*dx, y=start.y+i*dy)
            odo.pose.pose.orientation = heading_to_quaternion(heading)
            odo.twist.twist.linear = Vector3(x=des_speed)
            odo.twist.twist.angular = Vector3()
            self.targets.append(odo)

        if rvs:
            self.index = len(self.targets)-2
        else:
            self.index = 0

if __name__ == '__main__':
    # pylint: disable=invalid-name
    # leader is a fine name, it's not a constant
    leader = ExampleLeader()
    leader.run_server()