Esempio n. 1
0
def reverse_robot():
    print "Reversing Robot\n"
    import time

    global vel_lin_buffer, vel_ang_buffer, reversing
    global cmd_vel_pub, restored_bump_pub
    reversing = True
    for l, a in zip(reversed(vel_lin_buffer), reversed(vel_ang_buffer)):
        lin_vec = Vector3(-l[0], -l[1], -l[2])
        ang_vec = Vector3(-a[0], -a[1], -a[2])
        time.sleep(0.1)
        twist_msg = Twist()
        twist_msg.linear = lin_vec
        twist_msg.angular = ang_vec
        cmd_vel_pub.publish(twist_msg)

    for _ in range(10):
        time.sleep(0.05)
        twist_msg = Twist()
        twist_msg.linear = Vector3(0, 0, 0)
        twist_msg.angular = Vector3(0, 0, 0)

        cmd_vel_pub.publish(twist_msg)
    reversing = False
    restored_bump_pub.publish(True)
Esempio n. 2
0
def square():
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
    rospy.init_node('square', anonymous=True)

    pause = Twist()
    pause.linear = Vector3(0, 0, 0)
    pause.angular = Vector3(0, 0, 0)

    forward = Twist()
    forward.linear = Vector3(forward_velocity, 0, 0)
    forward.angular = Vector3(0, 0, 0)

    rotate = Twist()
    rotate.linear = Vector3(0, 0, 0)
    rotate.angular = Vector3(0, 0, rotate_velocity)

    cmds = [pause, forward, pause, rotate] * 4 + [pause]
    cmd_step = 0
    sleep_times = {pause: 0.5, forward: 2, rotate: 0.5}

    while not rospy.is_shutdown() and cmd_step < len(cmds):
        cmd = cmds[cmd_step]
        sleep_time = sleep_times[cmd]
        # rospy.loginfo(cmd, sleep_time)
        pub.publish(cmd)
        rospy.sleep(sleep_time)
        cmd_step += 1
 def publishVelocities(self, listOfVelocities):
     """
     Input: takes a list of Velocities, alternating angular and linear.
     Output: publishes velocities for a certain amount of time to the rostopic
     """
     r = rospy.Rate(20)
     r.sleep()
     for i in range(len(listOfVelocities)):
         print(listOfVelocities[i])
         if (i % 2 == 1):
             output = Twist()
             output.linear = Vector3(listOfVelocities[i], 0, 0)
             output.angular = Vector3(0, 0, 0)
             now = rospy.get_time()
             while (now + 1.0 >
                    rospy.get_time()) and (not rospy.is_shutdown()):
                 self.velpub.publish(output)
                 r.sleep()
         else:
             if (listOfVelocities[i] != 0):
                 output = Twist()
                 if (listOfVelocities[i] > 3.14):
                     listOfVelocities[i] = listOfVelocities[i] - 6.28
                 output.linear = Vector3(0, 0, 0)
                 output.angular = Vector3(0, 0, -listOfVelocities[i])
                 now = rospy.get_time()
                 while (now + 1.0 >
                        rospy.get_time()) and (not rospy.is_shutdown()):
                     self.velpub.publish(output)
                     r.sleep()
Esempio n. 4
0
def turtle(cmd):

    print("cmd: ", cmd)

    # rostopic pub /turtle1/cmd_vel geometry_msgs/Twist -r 1 -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, -1.8]'

    # $ rostopic type /turtle1/cmd_vel
    # geometry_msgs/Twist

    # $ rosmsg show geometry_msgs/Twist
    # geometry_msgs/Vector3 linear
    #   float64 x
    #   float64 y
    #   float64 z
    # geometry_msgs/Vector3 angular
    #   float64 x
    #   float64 y
    #   float64 z

    pub_twist = Twist()

    pub_twist.linear = Vector3(x=0.0, y=0.0, z=0.0)
    pub_twist.angular = Vector3(x=0.0, y=0.0, z=0.0)

    if cmd == "顺时针旋转。":
        pub_twist.linear = Vector3(x=2.0, y=0.0, z=0.0)
        pub_twist.angular = Vector3(x=0.0, y=0.0, z=-1.8)

    if cmd == "逆时针旋转。":
        pub_twist.linear = Vector3(x=2.0, y=0.0, z=0.0)
        pub_twist.angular = Vector3(x=0.0, y=0.0, z=1.8)

    pub_turtle.publish(pub_twist)
Esempio n. 5
0
def robotDoLoop():
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
    rospy.init_node('robotDoLoop', anonymous=True)

    do_nothing = Twist()
    do_nothing.linear = Vector3(0, 0, 0)
    do_nothing.angular = Vector3(0, 0, 0)

    go_forward = Twist()
    go_forward.linear = Vector3(forward_velocity, 0, 0)
    go_forward.angular = Vector3(0, 0, 0)

    do_rotation = Twist()
    do_rotation.linear = Vector3(0, 0, 0)
    do_rotation.angular = Vector3(0, 0, rotate_velocity)

    cmds = [do_nothing, go_forward, do_nothing, do_rotation] * 4 + [do_nothing]
    cmd_step = 0
    sleep_times = {do_nothing: 0.5, go_forward: 2.0, do_rotation: 0.5}

    # Wait 5 seconds, then run the loop
    rospy.sleep(5)

    while not rospy.is_shutdown() and cmd_step < len(cmds):
        cmd = cmds[cmd_step]
        sleep_time = sleep_times[cmd]
        # rospy.loginfo(cmd, sleep_time)
        pub.publish(cmd)
        rospy.sleep(sleep_time)
        cmd_step += 1
Esempio n. 6
0
def goalPub():
	global currentGoal
	goalCount = 0

	goalinworld = rospy.Publisher('robot_2/goal',Twist,queue_size=10)
	goal = Twist()
	goal.linear = Vector3(currentGoal.x,currentGoal.y,0.0)
	goal.angular = Vector3(0.0,0.0,0.0)

	rate = rospy.Rate(10.0)
	while not rospy.is_shutdown():
		result = goal2_client()
		if result.robot2_thereOrNot == 1:
			goalCount = goalCount + 1

		if goalCount>=1:
			currentGoal.x = -13.0
			currentGoal.y = -10.0
		else:
			currentGoal.x = -13.0
			currentGoal.y = -5.0

		goal.linear = Vector3(currentGoal.x,currentGoal.y,0.0)
		goal.angular = Vector3(0.0,0.0,0.0)

		rospy.loginfo(goalCount)
		rospy.loginfo(goal.linear)
		goalinworld.publish(goal)
		rate.sleep()
Esempio n. 7
0
def pub_movement(movement):
    if not isinstance(movement, type("")):
        return False

    move_msg = Twist()
    if movement == "stop":
        move_msg.angular = 0
        move_msg.linear = 0

    elif movement == "left":
        move_msg.angular = -robot_speed * 0.5
        move_msg.linear = 0

    elif movement == "right":
        move_msg.angular = robot_speed * 0.5
        move_msg.linear = 0

    elif movement == "forward":
        move_msg.angular = 0
        move_msg.linear = robot_speed

    elif movement == "backward":
        move_msg.angular = 0
        move_msg.linear = -robot_speed

    movement_pub.publish()
Esempio n. 8
0
	def turn180(self):
		print "Doing turn now!"
		trans, rot = self.tfFrames.lookupTransform('/odom','/base_link',rospy.Time(0))
		Angle = euler_from_quaternion(rot)
		origAng = Angle[2]
		output = Twist()
		#print(euler_from_quaternion(rot))
		output.linear = Vector3(0,0,0)
		output.angular = Vector3(0,0,self.roomba_turn_speed)
		self.pub.publish(output)
		trans, rot = self.tfFrames.lookupTransform('/odom','/base_link',rospy.Time(0))
		Angle = euler_from_quaternion(rot)
		newAng = Angle[2]
		r = rospy.Rate(50)
		while(self.angle_diff(newAng,origAng) > -2.6) and not rospy.is_shutdown():
			self.pub.publish(output)
			trans, rot = self.tfFrames.lookupTransform('/odom','/base_link',rospy.Time(0))
			Angle = euler_from_quaternion(rot)
			newAng = Angle[2]
			r.sleep()
			#print(self.angle_diff(newAng,origAng))
		
		output = Twist()
		output.linear = Vector3(self.roomba_speed,0,0)
		output.angular = Vector3(0,0,0)
		self.pub.publish(output)
def callback(data):
    rospy.loginfo("Recieved {0}".format(data.data))
    twist = Twist()
    if data.data # If data present an object was detected, move forward
        twist.linear.x = .10; twist.linear.y = 0.0; twist.linear.z = 0.0;
        twist.angular. = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0;
    else #if not present, no object detected, spin
        twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0;
        twist.angular. = 0.0; twist.angular.y = 0.0; twist.angular.z = .5;

    pub.publish(twist)
Esempio n. 10
0
    def roll(self, color, speed, heading, time=5):
        t = Twist()
        t.linear = Vector3(speed, 0, 0)
        t.angular = Vector3(0, 0, heading)
        self.pub_sphero_cmd_vel[color].publish(t)
        rospy.sleep(time)

        t = Twist()
        t.linear = Vector3(0, 0, 0)
        t.angular = Vector3(0, 0, 0)
        self.pub_sphero_cmd_vel[color].publish(t)
Esempio n. 11
0
    def _parse_joy(self, joy=None):
        if self._msg_type == 'twist':
            cmd = Twist()
        else:
            cmd = Accel()
        if joy is not None:
            # Linear velocities:
            l = Vector3(0, 0, 0)

            if self._axes['x'] > -1 and abs(joy.axes[self._axes['x']]) > self._deadzone:
                l.x += self._axes_gain['x'] * joy.axes[self._axes['x']]

            if self._axes['y'] > -1 and abs(joy.axes[self._axes['y']]) > self._deadzone:
                l.y += self._axes_gain['y'] * joy.axes[self._axes['y']]

            if self._axes['z'] > -1 and abs(joy.axes[self._axes['z']]) > self._deadzone:
                l.z += self._axes_gain['z'] * joy.axes[self._axes['z']]

            if self._axes['xfast'] > -1 and abs(joy.axes[self._axes['xfast']]) > self._deadzone:
                l.x += self._axes_gain['xfast'] * joy.axes[self._axes['xfast']]

            if self._axes['yfast'] > -1 and abs(joy.axes[self._axes['yfast']]) > self._deadzone:
                l.y += self._axes_gain['yfast'] * joy.axes[self._axes['yfast']]

            if self._axes['zfast'] > -1 and abs(joy.axes[self._axes['zfast']]) > self._deadzone:
                l.z += self._axes_gain['zfast'] * joy.axes[self._axes['zfast']]

            # Angular velocities:
            a = Vector3(0, 0, 0)

            if self._axes['roll'] > -1 and abs(joy.axes[self._axes['roll']]) > self._deadzone:
                a.x += self._axes_gain['roll'] * joy.axes[self._axes['roll']]

            if self._axes['rollfast'] > -1 and abs(joy.axes[self._axes['rollfast']]) > self._deadzone:
                a.x += self._axes_gain['rollfast'] * joy.axes[self._axes['rollfast']]

            if self._axes['pitch'] > -1 and abs(joy.axes[self._axes['pitch']]) > self._deadzone:
                a.y += self._axes_gain['pitch'] * joy.axes[self._axes['pitch']]

            if self._axes['pitchfast'] > -1 and abs(joy.axes[self._axes['pitchfast']]) > self._deadzone:
                a.y += self._axes_gain['pitchfast'] * joy.axes[self._axes['pitchfast']]

            if self._axes['yaw'] > -1 and abs(joy.axes[self._axes['yaw']]) > self._deadzone:
                a.z += self._axes_gain['yaw'] * joy.axes[self._axes['yaw']]

            if self._axes['yawfast'] > -1 and abs(joy.axes[self._axes['yawfast']]) > self._deadzone:
                a.z += self._axes_gain['yawfast'] * joy.axes[self._axes['yawfast']]

            cmd.linear = l
            cmd.angular = a
        else:
            cmd.linear = Vector3(0, 0, 0)
            cmd.angular = Vector3(0, 0, 0)
        return cmd
def reverse_robot():
    global logger
    logger.info("Reversing Robot ...\n")
    import time
    global vel_lin_buffer, vel_ang_buffer, reversing
    global cmd_vel_pub, restored_bump_pub
    global curr_cam_data, save_pose_data, image_dir, save_img_seq
    reversing = True

    logger.debug("Posting cmd_vel messages backwards with a %.2f delay",
                 utils.REVERSE_PUBLISH_DELAY)
    for l, a in zip(reversed(vel_lin_buffer), reversed(vel_ang_buffer)):
        lin_vec = Vector3(-l[0], -l[1], -l[2])
        ang_vec = Vector3(-a[0], -a[1], -a[2])
        twist_msg = Twist()
        twist_msg.linear = lin_vec
        twist_msg.angular = ang_vec
        rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
        cmd_vel_pub.publish(twist_msg)
    # publish last twist message so the robot reverse a bit more
    for _ in range(5):
        rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
        cmd_vel_pub.publish(twist_msg)

    logger.debug("Finished posting cmd_vel messages backwards ...\n")
    rospy.sleep(0.5)
    logger.debug("Posting zero cmd_vel messages with %.2f delay",
                 utils.ZERO_VEL_PUBLISH_DELAY)
    for _ in range(100):
        rospy.sleep(utils.ZERO_VEL_PUBLISH_DELAY)
        twist_msg = Twist()
        twist_msg.linear = Vector3(0, 0, 0)
        twist_msg.angular = Vector3(0, 0, 0)

        cmd_vel_pub.publish(twist_msg)
    logger.debug("Finished posting zero cmd_vel messages ...\n")
    reversing = False
    restored_bump_pub.publish(True)

    if image_dir is not None:
        if save_img_seq:
            pickle.dump(
                curr_cam_data,
                open(image_dir + os.sep + 'images_' + str(episode) + '.pkl',
                     'wb'))
            curr_cam_data = []
        pickle.dump(
            save_pose_data,
            open(image_dir + os.sep + 'pose_' + str(episode) + '.pkl', 'wb'))
        save_pose_data = []

    logger.info("Reverse finished ...\n")
Esempio n. 13
0
    def control_backup(self):
        v_pallet = self.pallet_position - self.robot_position
        distance = np.linalg.norm(v_pallet)

        if (distance < BACKUP_DISTANCE):
            cmd = Twist()
            cmd.linear = Vector3(-BACKUP_SPEED, 0, 0)
            cmd.angular = Vector3(0, 0, 0)
            self.wheel_cmd_publisher.publish(cmd)
        else:
            cmd = Twist()
            cmd.linear = Vector3(0, 0, 0)
            cmd.angular = Vector3(0, 0, 0)
            self.wheel_cmd_publisher.publish(cmd)
            self.state = CONTROL_STATE_DONE
Esempio n. 14
0
  def coordinate_to_action(self, msg):
    x = msg.x
    y = msg.y
    r = msg.z

    depth_proportion = -0.025
    depth_intercept = 1.35
    depth = r*depth_proportion + depth_intercept
    # print depth
    y_transform = self.frame_height/2 - y
    x_transform = x-self.frame_width/2
    angle_diff = math.tan(x_transform/depth)
    print "x: ", x_transform
    print "y: ",y
    print "d: ", depth
    print "a: ", angle_diff

    twist = Twist()

    lin_proportion = 0#-(0.5-depth)*0.1
    twist.linear = Vector3(lin_proportion, 0, 0)

    turn_proportion = 0*(angle_diff)

    twist.angular = Vector3(0, 0, turn_proportion)

    self.move_pub.publish(twist.linear, twist.angular)
Esempio n. 15
0
    def _move_tick(self, linear_vector=Vector3(0.0, 0.0, 0.0),
                         angular_vector=Vector3(0.0, 0.0, 0.0)):
        twist_msg = Twist()
        twist_msg.linear = linear_vector
        twist_msg.angular = angular_vector

        self._base_publisher.publish(twist_msg)
Esempio n. 16
0
    def move_base(self, x, y, z):
        print "MOVING"

        twist_msg = Twist()
        twist_msg.linear = Vector3(x, 0.0, 0.0)
        twist_msg.angular = Vector3(0.0, 0.0, z)
        self.cmd_vel_pub.publish(twist_msg)
Esempio n. 17
0
def move_robot(distance):
    """
    moves the robot straight on specified distance 
    """

    global pub
    global is_currently_moving
    is_currently_moving = True

    move_command = Twist()

    # no turning 
    move_command.angular = Vector3(0, 0, 0)

    # just moving forward
    move_command.linear = Vector3(SPEED, 0, 0)

    # moving forward for specified distance
    t_0 = rospy.Time.now().to_sec()
    current_distance = 0
    while current_distance < distance and movement_is_ok():
        pub.publish(move_command)
        t_1 = rospy.Time.now().to_sec()
        current_distance = SPEED*(t_1-t_0)
    
    move_command.linear = Vector3(0,0,0)
    pub.publish(move_command)
    is_currently_moving = False
Esempio n. 18
0
    def turn_towards_target(self, angle):
        current_yaw = get_yaw_from_pose(self.current_pose.pose.pose)

        # convert angle to radian value in [-pi, pi]
        angle = math.remainder(angle, 2 * np.pi)

        destination_yaw = current_yaw + angle
        if destination_yaw > np.pi:
            destination_yaw -= (2 * np.pi)
        elif destination_yaw < -np.pi:
            destination_yaw += (2 * np.pi)

        r = rospy.Rate(60)
        # turn until we face within 1 degree of destination angle
        while abs(
                get_yaw_from_pose(self.current_pose.pose.pose) -
                destination_yaw) > (np.pi / 180):
            # turn pi/12 rad/sec
            turn_msg = Twist()
            turn_vel = np.sign(destination_yaw - get_yaw_from_pose(
                self.current_pose.pose.pose)) * (np.pi / 12)
            turn_msg.angular = Vector3(0, 0, turn_vel)
            self.movement_pub.publish(turn_msg)
            r.sleep()
            print("{0}/{1}: {2}".format(
                str(get_yaw_from_pose(self.current_pose.pose.pose)),
                str(destination_yaw),
                str(
                    abs(
                        get_yaw_from_pose(self.current_pose.pose.pose) -
                        destination_yaw))))

        # Halt
        self.movement_pub.publish(Twist())
Esempio n. 19
0
    def turn_towards_target(self, angle):
        #wait for first pose
        while (not self.seen_first_pose):
            time.sleep(1)

        current_yaw = get_yaw_from_pose(self.current_pose.pose.pose)

        # convert angle to radian value in [-pi, pi]
        angle = math.remainder(angle, 2 * np.pi)
        destination_yaw = current_yaw + angle
        if destination_yaw > np.pi:
            destination_yaw = -np.pi + (destination_yaw - np.pi)

        r = rospy.Rate(60)
        # turn until we face within 1 degree of destination angle
        while abs(
                get_yaw_from_pose(self.current_pose.pose.pose) -
                destination_yaw) > (np.pi / 180):
            # turn pi/16 rad/sec
            turn_msg = Twist()
            turn_vel = np.sign(destination_yaw - get_yaw_from_pose(
                self.current_pose.pose.pose)) * (np.pi / 16)
            turn_msg.angular = Vector3(0, 0, turn_vel)
            self.movement_pub.publish(turn_msg)
            r.sleep()
        # Halt
        self.movement_pub.publish(Twist())
	def notifyClientUpdated(self, topic):
		#rank the votes and convert to a numpy array
		linearCandidates = self.getLinearCandidates()
		angularCandidates = self.getAngularCandidates()

		linearRankings, angularRankings = rankings = self.calculateRankings(rospy.get_rostime())

		#sum the rankings of each candidate
		linearBordaCount = np.sum(linearRankings, axis=0)
		angularBordaCount = np.sum(angularRankings, axis=0)

		#highest ranked candidate
		bestLinearCandidateIndex = np.argmax(linearBordaCount)
		bestAngularCandidateIndex = np.argmax(angularBordaCount)
		bestLinearCandidate = linearCandidates[bestLinearCandidateIndex]
		bestAngularCandidate = angularCandidates[bestAngularCandidateIndex]

		#publish the best ranked action
		rospy.loginfo("aggregator realtime_weighted_borda_full: %s - %s",(bestLinearCandidate,bestAngularCandidate),self.weights)

		action=Twist()
		if bestLinearCandidate:
			action.linear = bestLinearCandidate.linear
		if bestAngularCandidate:
			action.angular = bestAngularCandidate.angular
		self.publish(action)
Esempio n. 21
0
    def talker_direct(self):
        self.motor_counter += 1
        current_counter = self.motor_counter
        pub = rospy.Publisher('lego_robot/cmd_vel_med', Twist, queue_size=10)

        max_speed = 800
        # running the robot at duty_cycle_sp of 100 gave me a speed of 880 something in the air
        # running on the ground it got like 800

        publishFrequency = 60
        rate = rospy.Rate(publishFrequency)  # 60hz
        counter = 0

        while not rospy.is_shutdown() and not self.stopped:
            if self.motor_counter != current_counter:
                return
            # print(counter)
            twist = Twist()
            angular = geometry_msgs.msg.Vector3()
            linear = geometry_msgs.msg.Vector3()

            angular.x = math.radians(max_speed * self.duty_cycle_sp / 100)
            # angular.x = kwargs['speed_sp'] * math.pi / 180

            counter += 1

            twist.angular = angular
            twist.linear = linear

            # rospy.loginfo(twist)
            pub.publish(twist)

            rate.sleep()

        self.coast(pub, math.radians(max_speed * self.duty_cycle_sp / 100))
Esempio n. 22
0
    def talker_timed(self, kwargs):
        self.motor_counter += 1
        current_counter = self.motor_counter
        pub = rospy.Publisher('lego_robot/cmd_vel_med', Twist, queue_size=10)

        publishFrequency = 60
        rate = rospy.Rate(publishFrequency)  # 60hz
        counter = 0

        t_end = time.time() + kwargs['time_sp'] / 1000

        while not rospy.is_shutdown(
        ) and not self.stopped and time.time() < t_end:
            if self.motor_counter != current_counter:
                return
            # print(counter)
            twist = Twist()
            angular = geometry_msgs.msg.Vector3()
            linear = geometry_msgs.msg.Vector3()

            angular.x = math.radians(kwargs['speed_sp'])
            # angular.x = kwargs['speed_sp'] * math.pi / 180

            counter += 1

            twist.angular = angular
            twist.linear = linear

            # rospy.loginfo(twist)
            pub.publish(twist)

            rate.sleep()

        self.coast(pub, math.radians(kwargs['speed_sp']))
Esempio n. 23
0
    def coast(self, pub, currentSpeed):
        '''
        The idea of this function is to simulate coasting.
        I've set it so that on coasting, the wheel slows down 0.5 radians ever 0.1 seconds.
        So that's 5 radians per second.
        '''
        coastStep = 0.5
        totalSteps = int(math.floor(currentSpeed / coastStep))
        current_counter = self.motor_counter
        for i in range(0, totalSteps):
            if self.motor_counter != current_counter:
                return
            if i == totalSteps - 1:
                currentSpeed = 0
            else:
                currentSpeed -= coastStep

            twist = Twist()
            angular = geometry_msgs.msg.Vector3()
            linear = geometry_msgs.msg.Vector3()

            angular.x = currentSpeed

            twist.angular = angular
            twist.linear = linear

            # rospy.loginfo(twist)
            pub.publish(twist)
            time.sleep(0.1)
Esempio n. 24
0
    def daemon_worker(self):
        try:
            cmd_pub = rospy.Publisher(self.config["cmd_vel"],
                                      Twist,
                                      queue_size=10)
            rospy.loginfo("Driver Daemon online")

            while not rospy.core.is_shutdown():
                try:
                    if self.flag == DriverStatus.PREEMPTING:
                        cx, cy, cr = self.get_current_loc()
                        tx, ty, _ = self.target_goal

                        delta_angle = math.atan2(ty - cy, tx - cx)
                        delta_distance = math.sqrt((ty - cy)**2 + (tx - cx)**2)

                        ANG_vel = delta_angle - cr
                        LIN_vel = delta_distance * self.GAIN_L

                        # * Fix ANG_vel
                        if ANG_vel > math.pi:
                            ANG_vel -= 2 * math.pi
                        elif ANG_vel < -math.pi:
                            ANG_vel += 2 * math.pi

                        ANG_vel *= self.GAIN_A

                        rospy.logwarn_throttle(
                            0.2,
                            "Da={:.3f} :: Dd={:.3f} :: Va={:.3f} :: Vl={:.3f}".
                            format(delta_angle, delta_distance, ANG_vel,
                                   LIN_vel),
                        )
                        msg = Twist()
                        linear = Vector3()
                        angular = Vector3()

                        linear.x = LIN_vel
                        linear.y = 0
                        linear.z = ANG_vel
                        angular.x = 0
                        angular.y = 0
                        angular.z = 0

                        msg.linear = linear
                        msg.angular = angular
                        cmd_pub.publish(msg)

                except Exception:
                    rospy.logerr("Error in Driver... Recovering.")
                    exc_type, exc_value, exc_traceback = sys.exc_info()
                    traceback.print_exception(exc_type,
                                              exc_value,
                                              exc_traceback,
                                              limit=2,
                                              file=sys.stdout)

                rospy.rostime.wallsleep(0.01)
        except Exception:
            rospy.logfatal("Driver Daemon crashed or halted!")
Esempio n. 25
0
    def positionUpdateCallback(self, position):
        # gets called whenever we receive a new position. It will then update the motorcomand

        if (not (self.active)):
            return  #if we are not active we will return imediatly without doing anything

        angleX = position.angleX
        distance = position.distance

        rospy.loginfo('Angle: {}, Distance: {}, '.format(angleX, distance))

        # call the PID controller to update it and get new speeds
        [uncliped_ang_speed,
         uncliped_lin_speed] = self.PID_controller.update([angleX, distance])

        # clip these speeds to be less then the maximal speed specified above
        angularSpeed = np.clip(-uncliped_ang_speed, -self.max_speed,
                               self.max_speed)
        linearSpeed = np.clip(-uncliped_lin_speed, -self.max_speed,
                              self.max_speed)

        # create the Twist message to send to the cmd_vel topic
        velocity = Twist()
        velocity.linear = Vector3(linearSpeed, 0, 0.)
        velocity.angular = Vector3(0., 0., angularSpeed)
        rospy.loginfo('linearSpeed: {}, angularSpeed: {}'.format(
            linearSpeed, angularSpeed))
        self.cmdVelPublisher.publish(velocity)
Esempio n. 26
0
    def talker_forever(self, kwargs):
        self.motor_counter += 1
        current_counter = self.motor_counter

        pub = rospy.Publisher('lego_robot/cmd_vel_' + self.wheel,
                              Twist,
                              queue_size=10)

        publishFrequency = 60
        rate = rospy.Rate(publishFrequency)  # 60hz
        counter = 0

        while not rospy.is_shutdown() and not self.stopped:
            if self.motor_counter != current_counter:
                return
            # print(counter, self.wheel, self.motor_counter)
            twist = Twist()
            angular = geometry_msgs.msg.Vector3()
            linear = geometry_msgs.msg.Vector3()
            angular.x = math.radians(kwargs['speed_sp'])
            counter += 1
            twist.angular = angular
            twist.linear = linear
            # rospy.loginfo(twist)
            pub.publish(twist)
            rate.sleep()

        self.coast(pub, math.radians(kwargs['speed_sp']))
Esempio n. 27
0
	def positionUpdateCallback(self, twist):

	   angleX= twist.angular.z
	   distance = twist.linear.x

           dis_error = self.dis_setPoint - distance
	   ang_error = self.ang_setPoint - angleX
           
           linearSpeed  = -dis_error*self.dis_Kp - self.dis_last_error*self.dis_Kd
           angularSpeed =  ang_error*self.ang_Kp + self.ang_last_error*self.ang_Kd

           if(linearSpeed>self.line_max_speed):
	      linearSpeed=self.line_max_speed
           if(linearSpeed<-self.line_max_speed):
	      linearSpeed=-self.line_max_speed
           if(angularSpeed>self.angular_max_speed):
	      angularSpeed=self.angular_max_speed
           if(angularSpeed<-self.angular_max_speed):
	      angularSpeed=-self.angular_max_speed
           if(angleX==0):
              angularSpeed=0.0
           if(distance==0):
              linearSpeed=0.0

           self.dis_last_error = dis_error
           self.ang_last_error = ang_error
	   # create the Twist message to send to the cmd_vel topic
	   velocity = Twist()	
	   velocity.linear = Vector3(linearSpeed,0,0.)
	   velocity.angular= Vector3(0., 0.,angularSpeed)
	   rospy.loginfo('linearSpeed: {}, angularSpeed: {}'.format(linearSpeed, angularSpeed))

	   self.cmdVelPublisher.publish(velocity)
Esempio n. 28
0
  def coordinate_to_action(self, msg):
    x = msg.x
    y = msg.y
    r = msg.z

    depth_proportion = -0.025
    depth_intercept = 1.35
    depth = r*depth_proportion + depth_intercept
    # print depth
    y_transform = self.frame_height/2 - y
    x_transform = x-self.frame_width/2
    angle_diff = x_transform
    print angle_diff

    if (angle_diff-10) < 0 and (angle_diff + 10) > 0:
      angle_diff = 0 

    if depth-.05<0.5 and depth+.05>0.5:
      depth = 0.5

    # print "x: ", x_transform
    # print "y: ",y
    # print "d: ", depth
    # print "a: ", angle_diff

    twist = Twist()

    lin_proportion = -(0.5-depth)*0.07
    twist.linear = Vector3(lin_proportion, 0, 0)

    turn_proportion = -0.0005*(angle_diff)

    twist.angular = Vector3(0, 0, turn_proportion)

    self.move_pub.publish(twist.linear, twist.angular)
Esempio n. 29
0
    def move_robot(distance, speed=0.2):
        """
        moves the robot straight on specified distance
        An imitation of code used in ros wiki
        http://wiki.ros.org/turtlesim/Tutorials/Rotating%20Left%20and%20Right

        @param distance distance to move in feet
        @param speed speed in which to move (default= 0.2)
        """
        # convert entered distance to meter
        distance = 0.3048 * distance

        pub = MovementCommands.__get_publisher()

        move_command = Twist()

        # no turning
        move_command.angular = Vector3(0, 0, 0)

        # just moving forward
        move_command.linear = Vector3(speed, 0, 0)

        # moving forward for specified distance
        t_0 = rospy.Time.now().to_sec()
        current_distance = 0
        while current_distance < distance and MovementCommands.is_cleared_to_go(
        ):
            pub.publish(move_command)
            t_1 = rospy.Time.now().to_sec()
            current_distance = speed * (t_1 - t_0)

        move_command.linear = Vector3(0, 0, 0)
        pub.publish(move_command)
        pub.unregister()
Esempio n. 30
0
def doCommand(load_location, drop_location):
    waypoints = loadWaypoints(load_location, drop_location)

    print "Waypoints loaded: "
    print waypoints

    currentTargetIndex = 0
    rate = rospy.Rate(10)
    keepDriving = True
    while not rospy.is_shutdown() and keepDriving:
        try:
            currentWaypoint = waypoints[currentTargetIndex]
            currentPosDiff, currentRotDiff = goToLocation(
                getWaypointName(currentWaypoint))

            if abs(currentPosDiff[0]) < tolerance and abs(
                    currentPosDiff[1]) < tolerance:
                print "Either something broke, or we're there!"
                currentTargetIndex += 1
                keepDriving = currentTargetIndex < len(waypoints)

        except:
            print(sys.exc_info())
            continue
            # continue
    message = Twist()
    message.linear = Vector3(0, 0, 0)
    message.angular = Vector3(0, 0, 0)
    zumy_pub.publish(message)

    print "We should be stopping."
Esempio n. 31
0
def getMsgFromTransform(trans, rot):
    omega, theta = eqf.quaternion_to_exp(np.array(rot))
    omega = np.reshape(omega, (3, 1))
    v = eqf.find_v(omega, theta, trans)
    twist = np.vstack((v, omega))

    linear = Vector3()
    computeScale = lambda x, y, z, target: (float(target) /
                                            (x * x + y * y + z * z))**(0.5)
    a = computeScale(v[0], v[1], v[2], speed)
    print "Scale", a
    linear.x = v[0] * a
    linear.y = v[1] * a
    linear.z = v[2] * a

    angular = Vector3()
    angular.x = omega[0] * theta / 2
    angular.y = omega[1] * theta / 2
    angular.z = omega[2] * theta / 2

    message = Twist()
    message.linear = linear
    message.angular = angular

    return message
Esempio n. 32
0
def drive(publisher, x, rz):
    """
    moves zumy to the goal
        
    Args:
    publisher: zumy publisher
    x: desired x velocity
    rz: desired z angular velocity

    Returns:
    """

    twistMessage = Twist()
    l = Vector3()
    l.x = x
    l.y = 0
    l.z = 0
    twistMessage.linear = l
    v = Vector3()
    v.x = 0
    v.y = 0
    v.z = rz
    twistMessage.angular = v
    # print(twistMessage)
    publisher.publish(twistMessage)
Esempio n. 33
0
def drive_1m():
    """Drive the simulated robot one meter forward"""

    # Get the current position of the robot and 1m in front
    x = get_current_position()[0]
    wanted_x = x + 1

    # Set the current node to publish to the cmd_vel topic
    speed_publisher = rospy.Publisher("cmd_vel", Twist, queue_size=10)

    # Create the node
    rospy.init_node("distance_controller", anonymous=True)

    # Set the rate to update the topic
    update_rate = rospy.Rate(10)

    # While ROS is running and the current X is less than the one we want to reach keep driving forward
    while not rospy.is_shutdown():
        pub_data = Twist()
        if x < wanted_x:
            pub_data.angular = Vector3(0, 0, 0)
            pub_data.linear = Vector3(0.2, 0, 0)
            speed_publisher.publish(pub_data)
            x = get_current_position()[0]
            update_rate.sleep()

        # If not stop the robot announce the maneuver is done then break out of the loop
        else:
            pub_data.linear = Vector3(0, 0, 0)
            speed_publisher.publish(pub_data)
            print("Maneuver Complete")
            break
Esempio n. 34
0
    def positionUpdateCallback(self, position):

        if (self.active):

            angleX = position.angleX
            distance = position.distance

            rospy.logwarn('Angle: {}, Distance: {}, '.format(angleX, distance))

            #CALLING PID CONTROLLER to update it and get new vectors of velocity
            [uncliped_ang_speed, uncliped_lin_speed
             ] = self.PID_controller.update([angleX, distance])

            # clip these speeds to be less then the maximal speed specified above
            angularSpeed = np.clip(-uncliped_ang_speed, -self.max_speed,
                                   self.max_speed)
            linearSpeed = np.clip(-uncliped_lin_speed, -self.max_speed,
                                  self.max_speed)

            # create the Twist message to send to the cmd_vel topic
            velocity = Twist()
            velocity.linear = Vector3(linearSpeed, 0, 0.)
            velocity.angular = Vector3(0., 0., angularSpeed)
            rospy.loginfo('linearSpeed: {}, angularSpeed: {}'.format(
                linearSpeed, angularSpeed))
            self.cmd_vel_Publisher.publish(velocity)
Esempio n. 35
0
def create_modelstate_message(coords, yaw, model_name='/'):
    """
    Create a model state message for husky robot
    """
    pose = Pose()
    p = Point()
    p.x = coords[0]
    p.y = coords[1]
    p.z = coords[2]
    pose.position = p
    
    qua = quaternion_from_euler(0, 0, yaw)
    q = Quaternion()
    q.x = qua[0]
    q.y = qua[1]
    q.z = qua[2]
    q.w = qua[3]
    pose.orientation = q
    
    twist = Twist()
    twist.linear = Vector3(0, 0, 0)
    twist.angular = Vector3(0, 0, 0)

    ms = ModelState()

    ms.model_name = model_name
    ms.pose = pose
    ms.twist = twist
    ms.reference_frame = 'sand_mine'

    return ms
Esempio n. 36
0
	def drive(self, lin_vel, ang_vel):
		"""Publishes specified linear and angular command velocities"""
		# Note: for the Neato platforms, only x-linear and z-rotational motion is possible
		twist = Twist()

		twist.linear = Vector3(lin_vel,0,0)
		twist.angular = Vector3(0,0,ang_vel)
		self.pub.publish(twist.linear, twist.angular)
Esempio n. 37
0
 def center_flag(self, flag):
     if not self.is_centered(flag):
         print "gotta align"
         print "angle is currently " + str(flag.angle)
         msg = Twist()
         msg.linear = Vector3(0, 0, 0)
         msg.angular = Vector3(0, 0, flag.angle)
         self.velocity_publisher.publish(msg)
def reverse_robot():
    global logger
    logger.info("Reversing Robot ...\n")
    import time
    global vel_lin_buffer,vel_ang_buffer,reversing
    global cmd_vel_pub,restored_bump_pub
    global curr_cam_data,save_pose_data,image_dir,save_img_seq
    reversing = True
    
    logger.debug("Posting cmd_vel messages backwards with a %.2f delay",utils.REVERSE_PUBLISH_DELAY)
    for l,a in zip(reversed(vel_lin_buffer),reversed(vel_ang_buffer)):
        lin_vec = Vector3(-l[0],-l[1],-l[2])
        ang_vec = Vector3(-a[0],-a[1],-a[2])
        twist_msg = Twist()
        twist_msg.linear = lin_vec
        twist_msg.angular = ang_vec
	rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
        cmd_vel_pub.publish(twist_msg)
    # publish last twist message so the robot reverse a bit more
    for _ in range(5):
	rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
	cmd_vel_pub.publish(twist_msg)

    logger.debug("Finished posting cmd_vel messages backwards ...\n")
    rospy.sleep(0.5)
    logger.debug("Posting zero cmd_vel messages with %.2f delay",utils.ZERO_VEL_PUBLISH_DELAY)
    for _ in range(100):
        rospy.sleep(utils.ZERO_VEL_PUBLISH_DELAY)
        twist_msg = Twist()
        twist_msg.linear = Vector3(0,0,0)
        twist_msg.angular = Vector3(0,0,0)

        cmd_vel_pub.publish(twist_msg)
    logger.debug("Finished posting zero cmd_vel messages ...\n")
    reversing = False
    restored_bump_pub.publish(True)
    
    if image_dir is not None:
        if save_img_seq:
            pickle.dump(curr_cam_data,open(image_dir+os.sep+'images_'+str(episode)+'.pkl','wb'))
            curr_cam_data=[]
        pickle.dump(save_pose_data,open(image_dir+os.sep+'pose_'+str(episode)+'.pkl','wb'))
        save_pose_data=[]

    logger.info("Reverse finished ...\n")
Esempio n. 39
0
    def do_GET(self):
        global publisher
        query_string = urlparse.urlparse(self.path).query
        parameters = urlparse.parse_qs(query_string)

        if 'type' not in parameters:
            try:
                if self.path == "/":
                    self.path = "/index.html"
                elif self.path == "favicon.ico":
                    return
                elif self.path == "map.gif":
                    # Draw robot position on map and send image back
                    draw_map()
                    return
                fname,ext = os.path.splitext(self.path)
                print "Loading file", self.path
                with open(os.path.join(os.getcwd(),self.path[1:])) as f:
                    self.send_response(200)
                    self.send_header('Content-type', types_map[ext])
                    self.end_headers()
                    self.wfile.write(f.read())
                return
            except IOError:
                self.send_error(404)
                return

        command_type = parameters['type'][0]
        
        if command_type == 'base':
            base_x = 0.0
            if 'x' in parameters:
                base_x = float(parameters['x'][0])
            base_y = 0.0
            if 'y' in parameters:
                base_y = float(parameters['y'][0])
            base_z = 0.0
            if 'z' in parameters:
                base_z = float(parameters['z'][0])
            twist_msg = Twist()
            twist_msg.linear = Vector3(base_x, base_y, 0.0)
            twist_msg.angular = Vector3(0.0, 0.0, base_z)
            mobile_base_velocity.publish(twist_msg)

        elif command_type == 'speak':
            text = parameters['say'][0]
            sr = SoundRequest()
            sr.sound = -3 #Say text
            sr.command = 1 #Play once
            sr.arg = text
            publisher.publish(sr)
            #os.system("espeak -s 155 -a 200 '" + text + "' ")


        # response
        self.send_response(204)
        return
    def base_action(self, x, y, z, theta_x, theta_y, theta_z):
        topic_name = '/base_controller/command'
        base_publisher = rospy.Publisher(topic_name, Twist)

        twist_msg = Twist()
        twist_msg.linear = Vector3(x, y, z)
        twist_msg.angular = Vector3(theta_x, theta_y, theta_z)
        
        base_publisher.publish(twist_msg)
Esempio n. 41
0
def stop_uav(pub):
   """
   Tells the UAV to hold at its current position.
   """
   zero = Vector3(0,0,0)
   msg = Twist()
   msg.linear = zero
   msg.angular = zero
   pub.publish(msg)
Esempio n. 42
0
def reverse_robot():
    global logger
    logger.info("Reversing Robot ...\n")
    import time
    global vel_lin_buffer,vel_ang_buffer,reversing
    global cmd_vel_pub,restored_bump_pub
    global curr_cam_data,save_pose_data,image_dir,save_img_seq
    reversing = True
    
    logger.debug("Posting cmd_vel messages backwards with a %.2f delay",utils.REVERSE_PUBLISH_DELAY)
    for l,a in zip(reversed(vel_lin_buffer),reversed(vel_ang_buffer)):
        lin_vec = Vector3(-l[0],-l[1],-l[2])
        ang_vec = Vector3(-a[0],-a[1],-a[2])
        twist_msg = Twist()
        twist_msg.linear = lin_vec
        twist_msg.angular = ang_vec
	rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
        cmd_vel_pub.publish(twist_msg)
    # publish last twist message so the robot reverse a bit more
    for _ in range(5):
	rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
	cmd_vel_pub.publish(twist_msg)

    logger.debug("Finished posting cmd_vel messages backwards ...\n")
    rospy.sleep(0.5)
    logger.debug("Posting zero cmd_vel messages with %.2f delay",utils.ZERO_VEL_PUBLISH_DELAY)
    for _ in range(100):
        rospy.sleep(utils.ZERO_VEL_PUBLISH_DELAY)
        twist_msg = Twist()
        twist_msg.linear = Vector3(0,0,0)
        twist_msg.angular = Vector3(0,0,0)

        cmd_vel_pub.publish(twist_msg)
    logger.debug("Finished posting zero cmd_vel messages ...\n")
    reversing = False
    restored_bump_pub.publish(True)

    logger.info("Reverse finished ...\n")
    
    #save_img_seq_thread = threading.Thread(target=save_img_sequence_pose_threading())
    #save_img_seq_thread.start()
    save_img_sequence_pose()
    logger.info('Saving images finished...')
def gui_bridge():	
	#setup ROS node
	rospy.init_node('gui_bridge')
	pub = rospy.Publisher('/golfcart_pilot/abs_cmd', Twist)
	
	#setup socket
	HOST = ''
	PORT = 50012
	s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
	s.bind((HOST, PORT))
	print 'Waiting for connection on port ', PORT
	s.listen(1)
	conn, addr = s.accept()
	print 'Connected by', addr
	
	rospy.sleep(0.5)	
	
	while not rospy.is_shutdown():
		try:
			data = conn.recv(1024)
			if data.count("{") > 1:
				data = data.split("}{")
				data = "{"+data[len(data)-1]
			guiCmd = eval(data)
			print guiCmd
			wheel_angle = float(guiCmd['wheel']) * 0.0174532925 # d2r
			speed = float(guiCmd['speed'])
		
			command = Twist()
			command.linear = Vector3 (speed,0,0)
			command.angular = Vector3 (0,wheel_angle,0)
			pub.publish(command)
			rospy.sleep(0.2) # 5Hz
		except socket.error:
			command = Twist()
			command.linear = Vector3 (0,0,0)
			command.angular = Vector3 (0,0,0)
			pub.publish(command)
			print 'gui_bridge: SOCKET ERROR OCCURED, STOPPING DRIVE OPERATIONS'
			break
			
	s.close()
Esempio n. 44
0
 def send_cmd_msg(self):
     msg = Twist()
     msg.linear = Vector3()
     msg.angular = Vector3()
     msg.linear.x = self.x
     msg.linear.y = 0
     msg.linear.z = 0
     msg.angular.x = 0
     msg.angular.y = 0
     msg.angular.z = self.omega
     self.publisher.publish(msg)        
Esempio n. 45
0
 def send_cmd_msg(self, speed, omega):
     #print('receieved odometry')
     msg = Twist()
     msg.linear = Vector3()
     msg.angular = Vector3()
     msg.linear.x = speed
     msg.linear.y = 0
     msg.linear.z = 0
     msg.angular.x = 0
     msg.angular.y = 0
     msg.angular.z = omega
     self.publisher.publish(msg)
Esempio n. 46
0
def stop_robot():
    import time
    global cmd_vel_pub
    global logger
    logger.debug("Posting 0 cmd_vel data ....")
    for _ in range(100):
	rospy.sleep(0.01)
	twist_msg = Twist()
	twist_msg.linear = Vector3(0,0,0)
	twist_msg.angular = Vector3(0,0,0)
	cmd_vel_pub.publish(twist_msg)
    logger.debug("Finished posting 0 cmd_vel data ...\n")
Esempio n. 47
0
        def move_base():
            
            topic_name = '/base_controller/command'
            base_publisher = rospy.Publisher(topic_name, Twist)

            twist_msg = Twist()
            if(self.direction == Base.FORWARD):
                twist_msg.linear = Vector3(0.5, 0.0, 0.0)
                twist_msg.angular = Vector3(0.0, 0.0, 0.0)
            elif(self.direction == Base.BACKWARD):
                twist_msg.linear = Vector3(-0.5, 0.0, 0.0)
                twist_msg.angular = Vector3(0.0, 0.0, 0.0)
            elif(self.direction == Base.LEFT):
                twist_msg.linear = Vector3(0.0, 0.5, 0.0)
                twist_msg.angular = Vector3(0.0, 0.0, 0.0)
            elif(self.direction == Base.RIGHT):
                twist_msg.linear = Vector3(0.0, -0.5, 0.0)
                twist_msg.angular = Vector3(0.0, 0.0, 0.0)
            elif(self.direction == Base.COUNTER):
                twist_msg.linear = Vector3(0.0, 0.0, 0.0)
                twist_msg.angular = Vector3(0.0, 0.0, 0.5)
            elif(self.direction == Base.CLOCKWISE):
                twist_msg.linear = Vector3(0.0, 0.0, 0.0)
                twist_msg.angular = Vector3(0.0, 0.0, -0.5)

            base_publisher.publish(twist_msg)
            self.gui.show_text_in_rviz("Base!")
Esempio n. 48
0
def reverse_robot():
    global logger
    logger.info("Reversing Robot ...\n")
    import time
    global vel_lin_buffer,vel_ang_buffer,reversing
    global cmd_vel_pub,restored_bump_pub
    reversing = True
    
    logger.debug("Posting cmd_vel messages backwards with a %.2f delay",utils.REVERSE_PUBLISH_DELAY)
    for l,a in zip(reversed(vel_lin_buffer),reversed(vel_ang_buffer)):
        lin_vec = Vector3(-l[0],-l[1],-l[2])
        ang_vec = Vector3(-a[0],-a[1],-a[2])
        twist_msg = Twist()
        twist_msg.linear = lin_vec
        twist_msg.angular = ang_vec
	rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
        cmd_vel_pub.publish(twist_msg)
    # publish last twist message so the robot reverse a bit more
    for _ in range(5):
	rospy.sleep(utils.REVERSE_PUBLISH_DELAY)
	cmd_vel_pub.publish(twist_msg)

    logger.debug("Finished posting cmd_vel messages backwards ...\n")
    rospy.sleep(0.5)
    logger.debug("Posting zero cmd_vel messages with %.2f delay",utils.ZERO_VEL_PUBLISH_DELAY)
    for _ in range(100):
        rospy.sleep(utils.ZERO_VEL_PUBLISH_DELAY)
        twist_msg = Twist()
        twist_msg.linear = Vector3(0,0,0)
        twist_msg.angular = Vector3(0,0,0)

        cmd_vel_pub.publish(twist_msg)
    logger.debug("Finished posting zero cmd_vel messages ...\n")
    reversing = False
    restored_bump_pub.publish(True)
    logger.info("Reverse finished ...\n")
Esempio n. 49
0
    def move_base(self, isForward):
        topic_name = '/base_controller/command'
        base_publisher = rospy.Publisher(topic_name, Twist)

        distance = 0.25
        if not isForward:
            distance *= -1

        twist_msg = Twist()
        twist_msg.linear = Vector3(distance, 0.0, 0.0)
        twist_msg.angular = Vector3(0.0, 0.0, 0.0)

        for x in range(0, 15):
            rospy.loginfo("Moving the base")
            base_publisher.publish(twist_msg)
            time.sleep(0.15)
        time.sleep(1.5)
Esempio n. 50
0
  def run(self, goal):
    rospy.loginfo('Rotating base')
    count = 0
    r = rospy.Rate(10)
    while count < 70:
      if self._as.is_preempt_requested():
        self._as.set_preempted()
        return

      twist_msg = Twist()
      twist_msg.linear = Vector3(0.0, 0.0, 0.0)
      twist_msg.angular = Vector3(0.0, 0.0, 1.0)

      self._base_publisher.publish(twist_msg)
      count = count + 1
      r.sleep()

    self._as.set_succeeded()    
Esempio n. 51
0
 def multi_dof_joint_state(cls, msg, obj):
     obj.header = decode.header(msg, obj.header, "")
     obj.joint_names = msg["joint_names"]
     for i in msg['_length_trans']:
         trans = Transform()
         trans.translation = decode.translation(msg, trans.translation, i)
         trans.rotation = decode.rotation(msg, trans.rotation, i)
         obj.transforms.append(trans)
     for i in msg['_length_twist']:
         tw = Twist()
         tw.linear = decode.linear(msg, tw.linear, i)
         tw.angular = decode.angular(msg, tw.angular, i)
         obj.twist.append(twist)
     for i in msg['_length_twist']:
         wr = Wrench()
         wr.force = decode.force(msg, wr.force, i)
         wr.torque = decode.torque(msg, wr.torque, i)
         obj.wrench.append(wr)
     return(obj)
	def notifyClientUpdated(self, topic):
		#rank the votes and convert to a numpy array
		linearCandidates = self.getLinearCandidates()
		angularCandidates = self.getAngularCandidates()

		linearRankings, angularRankings = self.calculateRankings(rospy.get_rostime())

		#calculate Kemeny-Young rank aggregation
		linear_min_dist, linear_best_rank = rankaggr_brute(linearRankings)
		angular_min_dist, angular_best_rank = rankaggr_brute(angularRankings)

		#publish the best ranked action
		action=Twist()
		if linear_best_rank:
			action.linear = linearCandidates[linear_best_rank.index(0)].linear
		if angular_best_rank:
			action.angular = angularCandidates[angular_best_rank.index(0)].angular

		rospy.loginfo("aggregator realtime_kemeny_full: {\nlinear: %s\nangular: %s\n}",linear_min_dist, angular_min_dist)
		self.publish(action)
Esempio n. 53
0
	def command_drive(data):
		# initialize the message components
		header = Header()
		foo  = TwistWithCovarianceStamped()
		bar = TwistWithCovariance()
		baz = Twist()
		linear = Vector3()
		angular = Vector3()

		# get some data to publish
		# fake covariance until we know better
		covariance = [1,0,0,0,0,0, 0,1,0,0,0,0, 0,0,1,0,0,0, 0,0,0,1,0,0, 0,0,0,0,1,0, 0,0,0,0,0,1]
		# to be overwritten when we decide what we want to go where
		linear.x = data.axes[1] * 15
		linear.y = 0
		linear.z = 0
		angular.x = 0
		angular.y = 0
		angular.z = data.axes[0] * 10
		
		# put it all together
		# Twist
		baz.linear = linear
		baz.angular = angular
		
		# TwistWithCovariance
		bar.covariance = covariance
		bar.twist = baz

		# Header
		header.seq = data.header.seq
		header.frame_id = frame_id
		header.stamp = stopwatch.now()
		# seq = seq + 1
		# TwistWithCovarianceStamped
		foo.header = header
		foo.twist = bar

		# publish and log
		rospy.loginfo(foo)
		pub.publish(foo)
Esempio n. 54
0
    def spin_base(self, rotate_count):
        # Adjust height and tuck arms before localization
        self._sound_client.say("Please wait while I orient myself.")
        self.torso.move(.15)
        self.saved_l_arm_pose = Milestone1GUI.TUCKED_UNDER_L_POS
        self.saved_r_arm_pose = Milestone1GUI.NAVIGATE_R_POS       
        self.move_arm('l', 5.0, True)
        self.move_arm('r', 5.0, True)
        self.l_gripper.close_gripper()
        self.r_gripper.close_gripper()
        
        if not rotate_count:
            rotate_count = 2
        topic_name = '/base_controller/command'
        base_publisher = rospy.Publisher(topic_name, Twist)

        twist_msg = Twist()
        twist_msg.linear = Vector3(0.0, 0.0, 0.0)
        twist_msg.angular = Vector3(0.0, 0.0, 0.5)
        start_time = rospy.get_rostime()
        while rospy.get_rostime() < start_time + rospy.Duration(15.0 * rotate_count):
            base_publisher.publish(twist_msg)
Esempio n. 55
0
def move_uav(pub,curState,curGoal):
   """
   Tells the UAV to move toward the current goal pose.

   Orientation is currently ignored, but at some point we will want to use this
   to control UAV yaw as well.
   """

   # Calculate the direction we want to move in
   diff = curGoal.position-curState.position
   dist = np.linalg.norm(diff)
   direction = diff/dist

   # Calculate the speed we want to move at, and use this to set
   # flight vector (direction*speed)
   speed = min(max(dist*speedScale,minSpeed),maxSpeed)
   flightVec = direction*speed

   str = "speed parameters: (min: %f max: %f scale: %f" % \
      (minSpeed,maxSpeed,speedScale)

   rospy.logdebug(str)
   rospy.logdebug("position: %s" % curState.position)
   rospy.logdebug("target: %s" % curGoal.position)
   rospy.logdebug("dist: %s" % dist)
   rospy.logdebug("speed: %s" % speed)
   rospy.logdebug("flightVec: %s" % flightVec)

   # publish Twist message to tell UAV to move according to the
   # flight vector
   msg = Twist()
   msg.angular = Vector3(0,0,0)
   msg.linear = Vector3()
   msg.linear.x = flightVec[0]
   msg.linear.y = flightVec[1]
   msg.linear.z = flightVec[2]
   pub.publish(msg)
Esempio n. 56
0
#!/usr/bin/env python
import roslib
roslib.load_manifest('rospy')
roslib.load_manifest('geometry_msgs')

import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Vector3

if __name__ == "__main__":
    rospy.init_node('base_test_node', anonymous=True)

    topic_name = '/base_controller/command'
    base_publisher = rospy.Publisher(topic_name, Twist)

    twist_msg = Twist()
    twist_msg.linear = Vector3(0.0, 0.1, 0.0)
    twist_msg.angular = Vector3(0.0, 0.0, 0.0)
    
    for i in range(100):
        base_publisher.publish(twist_msg)
from nav_msgs.msg import Odometry

rospy.init_node('drives_square')

#def process_odom(msg):
#	xdis = 


pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
#sub = rospy.Subscriber("/odom", Odometry, process_odom)

twist = Twist()
l0 = Vector3(0.0, 0.0, 0.0)
a0 = Vector3(0.0, 0.0, 0.0)
twist.linear = l0
twist.angular = a0

tl= 3.66 #time to 1 meter
ta= 1.7 #time to rotate 90 degrees
itime = rospy.get_time()

while not rospy.is_shutdown():
	seconds = rospy.get_time() - itime
	if seconds > 0 and seconds < tl:
		l0 = Vector3(0.5, 0.0, 0.0)
		a0 = Vector3(0.0, 0.0, 0.0)
	elif seconds > tl and seconds < tl+ta:
		l0 = Vector3(0.0, 0.0, 0.0)
		a0 = Vector3(0.0, 0.0, -1.0)
	elif seconds > tl+ta and seconds < 2*tl + ta:
		l0 = Vector3(0.5, 0.0, 0.0)