Esempio n. 1
0
def ProcessXlateImage(data):
    global PrevDiameter
    global p_x
    global p_z_ang
    global TAG_DIAMETER
    global TagFound
    global LastTwist

    InputTags = data
    NewTwist = TwistStamped()

    if InputTags.tag_count > 0:
        TagFound = True
        # +Z_fwd_cam = +Z_base - points up
        # +Y_fwd_cam = +Y_base - points left
        # +X_fwd_cam = +X_base - points forward
        NewTwist.twist.linear.x = InputTags.tags[0].diameter - TAG_DIAMETER
        # NewTwist.twist.linear.y = InputTags.tags[0].x - ( FWD_IMAGE_WIDTH/2 ) #( IMAGE_WIDTH/2 ) - InputTags.tags[0].y
        # NewTwist.twist.linear.z = InputTags.tags[0].y - ( FWD_IMAGE_HEIGHT/2 ) #( IMAGE_HEIGHT/2 ) - InputTags.tags[0].x
        NewTwist.twist.angular.z = InputTags.tags[0].x - (IMAGE_WIDTH / 2)  # ( IMAGE_WIDTH/2 ) - InputTags.tags[0].y #
        # PrevDiameter = InputTags.tags[0].diameter

        # rospy.Publisher("image_pos", NewTwist )
        ProcessImagePosition(NewTwist)

        # Keep some history for when the tag disappears
        while len(PrevVector) >= MAX_HISTORY:
            PrevVector.pop(0)
        PrevVector.append(NewTwist.twist)
    else:
        # Extrapolate history
        try:
            NewTwist.twist = PrevVector.pop(0)
            print "Use History %d" % len(PrevVector)
            NewTwist.twist = LastTwist
            # Save off some history
        except IndexError, e:

            # Ran out of history
            NewTwist.twist.linear.x = 0
            NewTwist.twist.linear.y = 0
            NewTwist.twist.linear.z = 0
            NewTwist.twist.angular.z = 0
        if TagFound:
            p_x.Reset()
            p_z_ang.Reset()
        TagFound = False
        pub = rospy.Publisher("cmd_vel", Twist)
        pub.publish(NewTwist.twist)
Esempio n. 2
0
def motor_speed_send(a,b,c,d,e,f):

	global types_array

	pub = rospy.Publisher('robot/desired_velocity', TwistStamped, queue_size=10)
	r = rospy.Rate(5) 

	main = TwistStamped()
	stepper = Twist()
	stepper.linear.x = a
	stepper.linear.y = b
	stepper.linear.z = c
	stepper.angular.x = d
	stepper.angular.y = e
	stepper.angular.z = f

	main.twist = stepper
	now = rospy.get_rostime()
	main.header.stamp.secs = now.secs
	main.header.stamp.nsecs = now.nsecs
	main.header.frame_id = "robot/desired_velocity"
	
	pub.publish(main)
	rospy.loginfo("Stepper Motor sent %s", main)
	r.sleep()
Esempio n. 3
0
def pose_callback(data):
    # extract x and y position information
    x = data.pose.pose.position.x
    y = data.pose.pose.position.y
    # package and broadcast
    p = Point()
    p.x = x
    p.y = y
    # send it
    pub_beacon.publish(p)
    # and broadbast all odoms on a common channel
    pub_odom_all.publish(data)
    # publish pose info for viewing
    pose_out = PoseStamped()
    pose_out.header = data.header
    pose_out.header.frame_id = '/world'
    pose_out.pose = data.pose.pose
    pub_pose.publish(pose_out)
    # and twist stamped for DMS interface
    vel_out = TwistStamped()
    vel_out.header = data.header
    vel_out.header.frame_id = '/world'
    vel_out.twist = data.twist.twist
    pub_vel.publish(vel_out)
    # transform
    tf_broadcast.sendTransform(
        (x, y, 0.0),
        (data.pose.pose.orientation.x, data.pose.pose.orientation.y,
         data.pose.pose.orientation.z, data.pose.pose.orientation.w),
        rospy.Time.now(), frame_name, "/world")
Esempio n. 4
0
def publish():
    header = Header()
    header.stamp = rospy.Time.now()
    header.frame_id = 'map'

    for role, robot_id in WorldModel.assignments.items():
        if robot_id is not None:
            command = WorldModel.commands[role]

            if command.velocity_control_enable:
                msg = TwistStamped()
                msg.header = header
                msg.twist = command.target_velocity
                pubs_velocity[robot_id].publish(msg)
            else:
                send_pose = WorldModel.tf_listener.transformPose(
                    "map", command.target_pose)
                send_pose.header.stamp = rospy.Time.now()
                pubs_position[robot_id].publish(send_pose)

            pubs_kick_velocity[robot_id].publish(Float32(command.kick_power))

            status = AIStatus()
            status.avoidBall = command.avoid_ball
            status.avoidDefenceArea = command.avoid_defence_area
            status.do_chip = command.chip_enable
            status.dribble_power = command.dribble_power
            pubs_ai_status[robot_id].publish(status)

            command.reset_adjustments()
Esempio n. 5
0
def pose_callback(data):
  # extract x and y position information
  x = data.pose.pose.position.x
  y = data.pose.pose.position.y
  # package and broadcast
  p = Point()
  p.x = x
  p.y = y
  # send it
  pub_beacon.publish(p)
  # and broadbast all odoms on a common channel
  pub_odom_all.publish(data)
  # publish pose info for viewing
  pose_out = PoseStamped()
  pose_out.header = data.header
  pose_out.header.frame_id = '/world'
  pose_out.pose = data.pose.pose
  pub_pose.publish(pose_out)
  # and twist stamped for DMS interface
  vel_out = TwistStamped()
  vel_out.header = data.header
  vel_out.header.frame_id = '/world'
  vel_out.twist = data.twist.twist
  pub_vel.publish(vel_out)
  # transform
  tf_broadcast.sendTransform((x, y, 0.0),
                     (data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w),
                     rospy.Time.now(),frame_name,"/world")
Esempio n. 6
0
def callback(msg):
    global pub
    output = TwistStamped()
    output.header.stamp = rospy.Time.now()
    output.header.frame_id = sys.argv[1]
    output.twist = msg
    pub.publish(output)
Esempio n. 7
0
    def set_velocity(self, value, frame_id=None):
        if isinstance(value, TwistStamped):
            msg = value
        else:
            msg = TwistStamped()
            msg.header.frame_id = frame_id

            if isinstance(value, Twist):
                msg.twist = value
            else:
                if isinstance(value, Vector3):
                    msg.twist.linear = value
                else:
                    if isinstance(value, np.ndarray):
                        value = value.reshape((3, 1))
                        msg.twist.linear.x = value[0, 0]
                        msg.twist.linear.y = value[1, 0]
                        msg.twist.linear.z = value[2, 0]
                    elif isinstance(value, (list, tuple)):
                        msg.twist.linear.x = value[0]
                        msg.twist.linear.y = value[1]
                        msg.twist.linear.z = value[2]
                msg.twist.angular.x = 0
                msg.twist.angular.y = 0
                msg.twist.angular.z = 0
        if frame_id is not None:
            msg.header.frame_id = frame_id
        msg.header.stamp = rospy.Time.now()
        self._setpoint_vel = msg
        self._setpoint_mode = UAV.SetpointMode.VELOCITY
Esempio n. 8
0
	def onTwist(self,msg):
		newMsg = TwistStamped()
		head = std_msgs.msg.Header()
		newMsg.twist = msg
		head.stamp = rospy.Time.now()
		newMsg.header = head
		self.twist_pub.publish(newMsg)
def handle_convert(data):
    pub = rospy.Publisher('/navigation_vel', TwistStamped)
    m = TwistStamped()
    m.twist = data
    m.header = Header()
    m.header.stamp = rospy.Time.now()
    pub.publish(m)
def set_position():
    port = '/dev/ttyACM0'
    baud = 115200
    ser = serial.Serial(port, baud)
    temp = (ser.readline())
    a = ser.readline().rstrip()
    b = ser.readline().rstrip()
    c = ser.readline().rstrip()
    d = ser.readline().rstrip()
    e = ser.readline().rstrip()

    global hold
    global holdkm1
    global hold_refr
    hold = 0
    pos = TwistStamped()
    pos.twist = Twist()
    pos.header = Header()
    if int(a) > 1000:
        pos.twist.linear.z = 1
        print a
    elif int(b) > 1000:
        pos.twist.linear.x = 0.8
        print b
    elif int(c) > 1000:
        pos.twist.linear.x = -0.8
        print c
    elif int(d) > 1000:
        pos.twist.linear.y = 0.8
        print d
    elif int(e) > 1000:
        pos.twist.linear.y = -.8
        print e
    return pos
Esempio n. 11
0
    def control_update(self):
        control_region = self.valid_state()

        if control_region == 1:
            v_cmd, w_cmd = self.reachability_control()
        elif control_region == 2:
            v_cmd, w_cmd = self.forward_control()
        elif control_region == 3:
            v_cmd, w_cmd = self.right_control()
        elif control_region == 4:
            v_cmd, w_cmd = self.left_control()
        elif control_region == 5:
            v_cmd, w_cmd = self.reverse_control()
        else:
            v_cmd = 0.0
            w_cmd = 0.0

        v_cmd += compute_reciprocal_avoidance_vel(self.rel_z, 'robot')

        self.robot_u = np.array([v_cmd, w_cmd])
        control_msg = Twist()
        control_msg.linear.x = v_cmd
        control_msg.angular.z = w_cmd
        self.control_pub.publish(control_msg)

        # print current controller region
        # print('Current controller region: ' + self.region_map[control_region]),
        # print(chr(13)),

        # if environment == 'sim':
        rosbag_msg = TwistStamped()
        rosbag_msg.twist = control_msg
        rosbag_msg.header.stamp = rospy.Time.now()
        self.rosbag_pub.publish(rosbag_msg)
    def control_update(self):
        # Read user input from controller
        stick_x = self.joy.rightX()
        stick_y = self.joy.leftY()
        if stick_y >= 0:
            v_cmd = stick_y * V_MAX
            w_max_v = W_MAX1 + stick_y * (W_MAX2 - W_MAX1)
        else:
            v_cmd = stick_y * R_MAX
            w_max_v = W_MAX1

        w_cmd = -stick_x * w_max_v

        if self.rel_z is not None:
            v_cmd += compute_reciprocal_avoidance_vel(self.rel_z, 'robot')

        if environment == 'real':
            v_cmd *= REAL_WORLD_FACTOR
            w_cmd *= REAL_WORLD_FACTOR

        cmd_msg = Twist()
        cmd_msg.linear.x = v_cmd
        cmd_msg.angular.z = w_cmd
        self.cmd_pub.publish(cmd_msg)

        # if environment == 'sim':
        rosbag_msg = TwistStamped()
        rosbag_msg.twist = cmd_msg
        rosbag_msg.header.stamp = rospy.Time.now()
        self.rosbag_pub.publish(rosbag_msg)
 def update(self, state):
     if (self.target is None):
         rospy.logwarn("Target position for velocity controller is none.")
         return None
     # simplify variables a bit
     time = state.header.stamp.to_sec()
     position = state.pose.position
     orientation = state.pose.orientation
     # create output structure
     output = TwistStamped()
     output.header = state.header
     # output velocities
     linear = Vector3()
     angular = Vector3()
     # Control in X vel
     linear.x = self.X.update(self.target.position.x, position.x, time)
     # Control in Y vel
     linear.y = self.Y.update(self.target.position.y, position.y, time)
     # Control in Z vel
     linear.z = self.Z.update(self.target.position.z, position.z, time)
     # Control yaw (no x, y angular)
     # TODO
     output.twist = Twist()
     output.twist.linear = linear
     return output
def on_base_velocity_command(data):
    t = rospy.get_rostime()
    msg = TwistStamped()
    msg.header.stamp = t
    msg.header.frame_id = 'base'
    msg.twist = data

    velocity_pub.publish(msg)
Esempio n. 15
0
    def listener_callback(self, inMsg):
        outMsg = TwistStamped()
        outMsg.header = Header()
        outMsg.header.stamp = self.get_clock().now().to_msg()
        outMsg.header.frame_id = self.frame_id
        outMsg.twist = inMsg

        self.publisher_.publish(outMsg)
Esempio n. 16
0
    def outputTwist(self, twist):
        g_lateral_accel_limit = 0.8
        ERROR = 1e-8

        twistCmd = TwistStamped()
        twistCmd.twist = twist
        twistCmd.header.stamp = rospy.Time.now()

        return twistCmd
    def update(self, state):
        if (self.target is None):
            rospy.logwarn("Target position for velocity controller is none.")
            return None
        # simplify variables a bit
        time = state.header.stamp.to_sec()
        position = state.pose.position
        orientation = state.pose.orientation
        quat_arr = np.array(
            [orientation.x, orientation.y, orientation.z, orientation.w])
        att = euler_from_quaternion(quat_arr, 'sxyz')
        # create output structure
        output = TwistStamped()
        output.header = state.header
        # output velocities
        linear = Vector3()
        angular = Vector3()
        if (False):  #abs(att[2]-self.heading_target) > deg2rad(5.0)):
            # Control in X vel
            linear.x = self.X.update(position.x, position.x, time)
            # Control in Y vel
            linear.y = self.Y.update(position.y, position.y, time)
            # Control in Z vel
            linear.z = self.Z.update(position.z, position.z, time)
        else:
            # Control in X vel
            linear.x = self.X.update(self.target.position.x, position.x, time)
            # Control in Y vel
            linear.y = self.Y.update(self.target.position.y, position.y, time)
            # Control in Z vel
            linear.z = self.Z.update(self.target.position.z, position.z, time)
        # Control yaw (no x, y angular)

        delta = 0.3
        #if((abs(linear.y) > delta) and (abs(linear.x) > delta)) or (self.psi_target == None):
        output.twist = Twist()
        #if(self.distance(self.target,state.pose)>delta):
        phi_target = 0.0
        theta_target = 0.0
        psi_target = self.heading_target  #np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x)
        #psi_target = np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x)
        #psi_target = math.atan2(linear.y,linear.x)

        #angular.x = self.PHI.update(phi_target, att[0],time)
        #angular.y = self.THETA.update(theta_target, att[1],time)
        angular.z = self.PSI.update(psi_target, att[2], time)
        output.twist.angular = angular

        # TODO

        output.twist.linear = linear
        if output == None:
            print "output", output

        return output
def callback(cmdVelocity):
    baseVelocity = TwistStamped()
    baseVelocity.twist = cmdVelocity
    now = rospy.get_rostime()
    baseVelocity.header.stamp.secs = now.secs
    baseVelocity.header.stamp.nsecs = now.nsecs
    baseVelocityPub = rospy.Publisher('base_velocity',
                                      TwistStamped,
                                      queue_size=10)
    baseVelocityPub.publish(baseVelocity)
    rospy.loginfo(rospy.get_caller_id() + "I heard %s", baseVelocity)
def publishCmdVel(cmdvel, velPublisher, velPublisherStamped):
    
     velPublisher.publish(cmdvel)
     
     baseVelocity = TwistStamped()

     baseVelocity.twist = cmdvel
     
     now = rospy.get_rostime()
     baseVelocity.header.stamp.secs = now.secs
     baseVelocity.header.stamp.nsecs = now.nsecs
     velPublisherStamped.publish(baseVelocity)
def callback(cmd_velocity):
    move_stamped = TwistStamped()

    move_stamped.twist = cmd_velocity

    now = rospy.get_rostime()
    move_stamped.header.stamp.secs = now.secs
    move_stamped.header.stamp.nsecs = now.nsecs

    move_stampedPub = rospy.Publisher(
        '/hummingbird/copilot/manual_desired_velocity',
        TwistStamped,
        queue_size=10)
    move_stampedPub.publish(move_stamped)
Esempio n. 21
0
def callback(cmdVelocity):

    baseVelocity = TwistStamped()

    baseVelocity.twist = cmdVelocity

    now = rospy.get_rostime()
    baseVelocity.header.stamp.secs = now.secs
    baseVelocity.header.stamp.nsecs = now.nsecs
    baseVelocity.header.frame_id = "map"

    baseVelocityPub = rospy.Publisher('base_velocity',
                                      TwistStamped,
                                      queue_size=10)
    baseVelocityPub.publish(baseVelocity)
    def callback(self, cmdVelocity):
        self.baseVelocity.twist = cmdVelocity
        self.flag = 1

        if self.flag:
            # Publish Updated TwistStamped

            baseVelocity = TwistStamped()

            baseVelocity.twist = cmdVelocity

            now = rospy.get_rostime()
            baseVelocity.header.stamp.secs = now.secs
            baseVelocity.header.stamp.nsecs = now.nsecs
            self.baseVelocityPub.publish(baseVelocity)
Esempio n. 23
0
 def parse_twist_stamped(twist_stamped):
     rospy.loginfo('twist_stamped')
     rospy.logdebug('parse_twist_stamped: parsing ' + str(twist_stamped))
     f_from = YamlPars0r.get_dict_value(twist_stamped, 'from')
     f_pose = YamlPars0r.get_dict_value(twist_stamped, 'pose')
     twist_msg = YamlPars0r.parse_twist(f_pose)
     header_msg = Header()
     if f_from is not None:
         header_msg.frame_id = f_from
     twist_stamped_msg = TwistStamped(
         header=header_msg
     )
     if twist_msg is not None:
         twist_stamped_msg.twist = twist_msg
     return twist_stamped_msg
Esempio n. 24
0
    def getTwistInBaseFrame(self, twistWC, header):
        """Returns a TwistWithCovarianceStamped in base frame"""
        # Create the stamped object
        twistS = TwistStamped()
        twistS.header = header
        twistS.twist = twistWC.twist

        twistS_base = self.transformTwist(self.base_frame_id, twistS)

        twistWC_out = TwistWithCovarianceStamped()
        twistWC_out.header = twistS_base.header
        twistWC_out.twist.twist = twistS_base.twist
        twistWC_out.twist.covariance = twistWC.covariance

        return twistWC_out
Esempio n. 25
0
    def cmdVel_publish(self, cmdVelocity):

        # Publish Twist
        self.cmdVel_pub.publish(cmdVelocity)

        # Publish TwistStamped
        self.baseVelocity.twist = cmdVelocity

        baseVelocity = TwistStamped()

        baseVelocity.twist = cmdVelocity

        now = rospy.get_rostime()
        baseVelocity.header.stamp.secs = now.secs
        baseVelocity.header.stamp.nsecs = now.nsecs
        self.cmdVelStamped_pub.publish(baseVelocity)
Esempio n. 26
0
 def create_waypoint_message(self, waypoint, reachability_estimator):
     lin = Vector3()
     angular = Vector3()
     # item converts from numpy float type to python float type
     lin.x = float(waypoint[0].item()) / 4
     lin.y = 0.0
     # msg.z = reachability_estimator.item()
     lin.z = 0.0
     angular.x = 0.0
     angular.y = 0.0
     angular.z = float(waypoint[1].item()) / 1
     msg = TwistStamped()
     msg_twist = Twist()
     msg_twist.linear = lin
     msg_twist.angular = angular
     msg.twist = msg_twist
     return msg
    def update_prop(self, state, gain):
        output = TwistStamped()
        output.header = state.header
        position = state.pose.position

        linear = Vector3()
        angular = Vector3()
        linear.x = gain * (self.target.position.x - position.x)
        # Control in Y vel
        linear.y = gain * (self.target.position.y - position.y)
        # Control in Z vel
        linear.z = gain * (self.target.position.z - position.z)
        # output velocities

        output.twist = Twist()
        output.twist.linear = linear
        return output
Esempio n. 28
0
def tf_broadcaster(msg):

    global vel_pub
    global t

    br = tf.TransformBroadcaster()
    br.sendTransform(
        (msg.pose.position.x, msg.pose.position.y, msg.pose.position.z),
        (msg.pose.orientation.x, msg.pose.orientation.y,
         msg.pose.orientation.z, msg.pose.orientation.w), rospy.Time.now(),
        "base_link", msg.header.frame_id)

    br2 = tf.TransformBroadcaster()
    br2.sendTransform((0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), "map",
                      msg.header.frame_id)

    try:

        m = TransformStamped()
        m.header.frame_id = "map"
        m.header.stamp = rospy.Time.now()
        m.child_frame_id = "base_link"
        m.transform.translation.x = msg.pose.position.x
        m.transform.translation.y = msg.pose.position.y
        m.transform.translation.z = msg.pose.position.z

        m.transform.rotation.x = msg.pose.orientation.x
        m.transform.rotation.y = msg.pose.orientation.y
        m.transform.rotation.z = msg.pose.orientation.z
        m.transform.rotation.w = msg.pose.orientation.w

        t.setTransform(m)

        [linear, angular] = t.lookupTwist("base_link", "map", rospy.Time(0.0),
                                          rospy.Duration(1.001))

        twist = TwistStamped()
        twist.header.stamp = rospy.Time.now()
        twist.twist = Twist(Vector3(linear[0], linear[1], linear[2]),
                            Vector3(angular[0], angular[1], angular[2]))

        vel_pub.publish(twist)

    except Exception as e:
        print 'vel pub error'
Esempio n. 29
0
def CreateTwistMessage(linear, angular, gt_time, sequence):
    msg_av = Vector3()
    msg_lv = Vector3()
    msg_twist = Twist()
    msg_twist_stamped = TwistStamped()
    msg_twist_stamped.header.frame_id = "world"
    msg_twist_stamped.header.stamp = gt_time
    msg_twist_stamped.header.seq = sequence
    msg_lv.x = linear.x_val
    msg_lv.y = linear.y_val
    msg_lv.z = linear.z_val
    msg_twist.linear = msg_lv
    msg_av.x = angular.x_val
    msg_av.y = angular.y_val
    msg_av.z = angular.z_val
    msg_twist.angular = msg_av
    msg_twist_stamped.twist = msg_twist
    return msg_twist_stamped
def fix_data(bag_file):
    """
	Repairs data in the turtlebot velocity topic by converting to a stamped message.
	Both topics (turtlebot velocity and vicon data) are written to a new file with -fixed appended to the name.

	Parameters:
		bag_file - file to be repaired
	Returns:
		repaired_file - name of the repaired bag file
	"""

    # setup repaired file name
    repaired_file = bag_file.replace(".bag", "_fixed.bag")  # Output bag path

    # the two topics to put in the new file
    topic1 = '/mobile_base/commands/velocity'  # Target topic you want to repair
    topic2 = '/vicon/turtlebot_traj_track/turtlebot_traj_track'

    # Open bags
    bag = rosbag.Bag(bag_file, 'r')
    fix_bag = rosbag.Bag(repaired_file, "w")  # Create a repaired bag

    # Iterate through bag
    for topic, msg, t in bag.read_messages():

        # Add time back to the target message header
        if topic == topic1:
            m = TwistStamped()
            m.twist = msg
            m.header.stamp.secs = t.secs
            m.header.stamp.nsecs = t.nsecs

            # Write message to bag
            fix_bag.write(topic + "_FIXED", m, t)

        if topic == topic2:

            # Write message to bag
            fix_bag.write(topic2, msg, t)

    # Close bag - Very important else you'll have to reindex it
    fix_bag.close()

    return repaired_file
Esempio n. 31
0
    def update(self, state):
        if (self.target is None):
            rospy.logwarn("Target position for velocity controller is none.")
            return None

        # simplify variables a bit
        time = state.header.stamp.to_sec()
        position = state.pose.position
        orientation = state.pose.orientation

        # create output structure
        output = TwistStamped()
        output.header = state.header
        # output velocities
        linear = Vector3()
        angular = Vector3()

        # Control in X vel
        linear.x = self.X.update(self.target.position.x, position.x, time)
        # Control in Y vel
        linear.y = self.Y.update(self.target.position.y, position.y, time)
        # Control in Z vel
        #Front sensor
        linear.z = self.Z.update(self.target.position.z, position.z, time)
        # Control yaw (no x, y angular)

        (DesRoll, DesPitch,
         DesYaw) = tf.transformations.euler_from_quaternion([
             self.target.orientation.x, self.target.orientation.y,
             self.target.orientation.z, self.target.orientation.w
         ])
        (CurrRoll, CurrPitch,
         CurrYaw) = tf.transformations.euler_from_quaternion(
             [orientation.x, orientation.y, orientation.z, orientation.w])

        angular.z = self.yaw.update(DesYaw, CurrYaw, time)

        # TODO
        output.twist = Twist()
        output.twist.linear = linear
        output.twist.angular = angular
        return output
Esempio n. 32
0
 def update(self, state, velocity):
     self.velocity = velocity
     if (self.target is None):
         rospy.logwarn("Target position for landing controller is none.")
         return None
     # simplify variables a bit
     time = state.header.stamp.to_sec()
     position = state.pose.position
     orientation = state.pose.orientation
     # create output structure
     output = TwistStamped()
     output.header = state.header
     # check if we're at target altitude to initiate descent
     distToTarget = math.pow(
         position.x - self.target.position.x, 2) + math.pow(
             position.y - self.target.position.y, 2) + math.pow(
                 position.z - self.target.position.z, 2)
     if (not self.landingDescent and distToTarget < self.distTolerance):
         self.landingDescent = True
         print "Started landing descent"
     # output velocities
     linear = Vector3()
     angular = Vector3()
     # Control in X vel
     linear.x = self.X.update(self.target.position.x, position.x, time)
     # Control in Y vel
     linear.y = self.Y.update(self.target.position.y, position.y, time)
     # Control in Z vel
     if (self.landingDescent):
         linear.z = self.descentVelocity
         if (not self.descending and velocity.twist.linear.z <
             (1 - self.velTolerance) * self.descentVelocity):
             self.descending = True
             print "Descending"
     else:
         linear.z = self.Z.update(self.target.position.z, position.z, time)
     # Control yaw (no x, y angular)
     # TODO
     output.twist = Twist()
     output.twist.linear = linear
     return output
Esempio n. 33
0
def publish_arm_state():
    rospy.wait_for_service('/gazebo/get_link_state')
    try:
        get_link_state = rospy.ServiceProxy('/gazebo/get_link_state',
                                            GetLinkState)

        # Publising arm link poses and twist
        ii = 0
        for pub in arm_pose_pub_dic:
            ii += 1
            if ii == 1:
                response = get_link_state('Link_1', 'link_chassis')
            else:
                response = get_link_state('Link_' + str(ii),
                                          'Link_' + str(ii - 1))
            link_pose = response.link_state.pose

            stamped_link_pose = PoseStamped()
            stamped_link_pose.pose = response.link_state.pose
            stamped_link_pose.header.stamp = rospy.Time.now(
            )  # Add a time stamp to the message
            pub.publish(stamped_link_pose)

        # Publish arm link twists
        ii = 0
        for pub in arm_twist_pub_dic:
            ii += 1
            if ii == 1:
                response = get_link_state('Link_1', 'link_chassis')
            else:
                response = get_link_state('Link_' + str(ii),
                                          'Link_' + str(ii - 1))

            stamped_link_twist = TwistStamped()
            stamped_link_twist.twist = response.link_state.twist
            stamped_link_twist.header.stamp = rospy.Time.now(
            )  # Add a time stamp to the message
            pub.publish(stamped_link_twist)

    except rospy.ServiceException as e:
        print("Service call failed: %s" % e)
Esempio n. 34
0
    def twist_cb(self, twist):
        # get teleop twist message
        twist_stamped = TwistStamped()
        twist_stamped.twist = twist

        if not (
            twist.linear.x == 0
            and twist.linear.y == 0
            and twist.linear.z == 0
            and twist.angular.x == 0
            and twist.angular.y == 0
            and twist.angular.z == 0
        ):

            # give it a header
            header = Header()
            header.stamp = rospy.Time.now()
            twist_stamped.header = header

            # publish the command
            self.teleop_stamped_pub.publish(twist_stamped)
Esempio n. 35
0
def main():
    global sub, pub, velo_unity_pub, cur_velo, finger_unity_pub, open
    rospy.init_node("prime_kinova_unity_control")
    #init Subscriber and Publisher
    sub = rospy.Subscriber("GlovesData", GlovesData, callBack)
    velo_unity_pub = rospy.Publisher(
        '/servo_server/delta_twist_cmds', TwistStamped,
        queue_size=1)  #j2n6s300_driver/in/cartesian_velocity
    finger_unity_pub = rospy.Publisher('/j2n6s300/fingers',
                                       std_msgs.msg.Float32,
                                       queue_size=1)

    # rospy.spin()
    r = rospy.Rate(110)
    while not rospy.is_shutdown():
        cur_velo = TwistStamped()
        cur_velo.twist = twist
        velo_unity_pub.publish(cur_velo)
        x = std_msgs.msg.Float32()
        x.data = open  # 0为close, 1为open
        finger_unity_pub.publish(x)
        r.sleep()
Esempio n. 36
0
def process_joy_data_callback(data):
	linear_x = data.axes[1] * max_linear_velocity
	linear_y = data.axes[0] * max_linear_velocity
	angular_z = data.axes[3] * max_angular_velocity
	seq = data.header.seq

	twist_stamped_msg = TwistStamped()
	twist_stamped_msg.twist = Twist()
	twist_stamped_msg.twist.linear = Vector3()
	twist_stamped_msg.twist.angular = Vector3()

	twist_stamped_msg.twist.linear.x = linear_x
	twist_stamped_msg.twist.linear.y = linear_y
	twist_stamped_msg.twist.linear.z = 0
	
	twist_stamped_msg.twist.angular.x = 0
	twist_stamped_msg.twist.angular.y = 0
	twist_stamped_msg.twist.angular.z = angular_z

	twist_stamped_msg.header.seq = seq
	twist_stamped_msg.header.frame_id = '/base_link'
	twist_stamped_msg.header.stamp = data.header.stamp
	pub.publish(twist_stamped_msg)
Esempio n. 37
0
    def timer_cb(self, event):
        enable = False

        twist = Twist()

        fx = self.cur_wrench.force.x
        fy = self.cur_wrench.force.y
        fz = self.cur_wrench.force.z

        if fx > self.dz_x_max:
            twist.linear.x = self.k_f * (fx - self.dz_x_max)
            enable = True
        elif fx < self.dz_x_min:
            twist.linear.x = self.k_f * (fx - self.dz_x_min)
            enable = True
        if fy > self.dz_y_max:
            twist.linear.y = self.k_f * (fy - self.dz_y_max)
            enable = True
        elif fy < self.dz_y_min:
            twist.linear.y = self.k_f * (fy - self.dz_y_min)
            enable = True
        if fz > self.dz_z_max:
            twist.linear.z = self.k_f * (fz - self.dz_z_max)
            enable = True
        elif fz < self.dz_z_min:
            twist.linear.z = self.k_f * (fz - self.dz_z_min)
            enable = True

        if enable:
            twist.linear.x = self.saturate_vel(twist.linear.x)
            twist.linear.y = self.saturate_vel(twist.linear.y)
            twist.linear.z = self.saturate_vel(twist.linear.z)

            twist_s = TwistStamped()
            twist_s.header.stamp = rospy.get_rostime()
            twist_s.twist = twist
            self.pub_cmd_vel.publish(twist_s)
	def get_velocity(self, pose):
		velocity = TwistStamped()
		velocity.header.stamp = pose.header.stamp
		velocity.header.frame_id = pose.header.frame_id
		velocity.twist = pose.twist.twist
		return velocity
def callback(data):
	data_new = TwistStamped()
	data_new.header.stamp = rospy.Time.now()
	data_new.twist = data
	pub2.publish(data_new)
Esempio n. 40
0
def ProcessXlateImage( data ):
    global PrevDiameter
    global p_x
    global p_y
    global p_z
    global p_z_ang
    global FwdCam
    global prev_key
    global FWD_TAG_DIAMETER
    global DWN_TAG_DIAMETER
    global TagFound
    global NavTwist
    global LastTwist
        
    InputTags = data
    NewTwist = TwistStamped()
    
    if InputTags.image_width != 320 and FwdCam and prev_key != 'switch':
        print "Switch to Down Cam"
        p_x.ReInit('vel_x', DWN_X_VEL_SCALAR, DWN_X_INT_SCALAR, DWN_X_DERIVATIVE_SCALAR, DWN_X_MAX_VELOCITY, 0, DWN_X_DEAD_BAND)
        p_y.ReInit('vel_y', DWN_Y_VEL_SCALAR, DWN_Y_INT_SCALAR, DWN_Y_DERIVATIVE_SCALAR, DWN_Y_MAX_VELOCITY, 0, DWN_Y_DEAD_BAND)
        p_z.ReInit('vel_z', DWN_Z_VEL_SCALAR, DWN_Z_INT_SCALAR, DWN_Z_DERIVATIVE_SCALAR, DWN_Z_MAX_VELOCITY, 0, DWN_Z_DEAD_BAND)
        p_z_ang.ReInit('ang_z', DWN_Z_ANG_VEL_SCALAR, DWN_Z_ANG_INT_SCALAR, DWN_Z_ANG_DERIVATIVE_SCALAR, DWN_Z_ANG_MAX_VELOCITY, 0, DWN_Z_ANG_DEAD_BAND)
        FwdCam = False

    if InputTags.tag_count > 0:
        TagFound = True
        if InputTags.tags[0].id == 0:
            NewTwist.header.frame_id = 'switch'
        else:
            NewTwist.header.frame_id = 'move'
            
        if FwdCam:
            # Forward Camera
            # +Z_fwd_cam = +Z_base - points up
            # +Y_fwd_cam = +Y_base - points left
            # +X_fwd_cam = +X_base - points forward
            NewTwist.twist.linear.x = InputTags.tags[0].diameter - FWD_TAG_DIAMETER
            NewTwist.twist.linear.y = InputTags.tags[0].x - ( FWD_IMAGE_WIDTH/2 ) #( IMAGE_WIDTH/2 ) - InputTags.tags[0].y
            NewTwist.twist.linear.z = InputTags.tags[0].y - ( FWD_IMAGE_HEIGHT/2 ) #( IMAGE_HEIGHT/2 ) - InputTags.tags[0].x
            NewTwist.twist.angular.z = 0 # No rotation on fwd cam
            #PrevDiameter = InputTags.tags[0].diameter
        else:
            # Downward Camera
            # NOTE: the downward camera 
            # +Z_down_cam = -Z_base - points down, 
            # +Y_down_cam = +X_base - points forward
            # +X_down_cam = +Y_base - points left
            NewTwist.twist.linear.x = InputTags.tags[0].y - ( DWN_IMAGE_HEIGHT/2 )
            NewTwist.twist.linear.y = InputTags.tags[0].x - ( DWN_IMAGE_WIDTH/2 )
            NewTwist.twist.linear.z = DWN_TAG_DIAMETER - InputTags.tags[0].diameter
            print InputTags.tags[0].zRot
            NewTwist.twist.angular.z = 2*math.pi - InputTags.tags[0].zRot
            
        #rospy.Publisher("image_pos", NewTwist )
        ProcessImagePosition( NewTwist )

        # Keep some history for when the tag disappears
        while len(PrevVector) >= MAX_HISTORY:
            PrevVector.pop(0)
        PrevVector.append(NewTwist.twist)
    else:
        NewTwist.header.frame_id = 'lost'
        # Extrapolate history
        try:
            NewTwist.twist = PrevVector.pop(0)
            print "Use History %d" % len(PrevVector)
            NewTwist.twist = LastTwist
            # Save off some history
        except IndexError, e:
            
            # Ran out of history
            NewTwist.twist.linear.x = 0
            NewTwist.twist.linear.y = 0
            NewTwist.twist.linear.z = NavTwist.linear.z
            NewTwist.twist.angular.z = 0
        if TagFound:
            p_x.Reset()
            p_y.Reset()
            p_z.Reset()
            p_z_ang.Reset()
            prev_key = 'foobar'
        TagFound = False    
        pub = rospy.Publisher("cmd_vel", Twist )
        pub.publish( NewTwist.twist )
Esempio n. 41
0
	def onTwistStamped(self,msg):
		out_msg = TwistStamped()
		out_msg.header = rospy.time.now()
		out_msg.twist = msg
		self.twist_pub.publish(out_msg)
Esempio n. 42
-1
def main():
    if not len(sys.argv) == 2:
        print "Usage: ./post_process.py <input.bag>"
        sys.exit(1)

    topics=["/bebop/states/ARDrone3/PilotingState/AttitudeChanged", "/bebop/cmd_vel", "/vicon/bebop_blue_en/bebop_blue_en", "/bebop/states/ARDrone3/PilotingState/SpeedChanged", "/vservo/cmd_vel"]

    input_bag = sys.argv[1]
    output_bag = os.path.splitext(os.path.basename(input_bag))[0] + "_processed.bag"

    cmd_vel_stamped = TwistStamped()
    cmd_vel_stamped.header.frame_id = "base_link"

    print "Output: ", output_bag

    i = 0
    try:
        i_bag = rosbag.Bag(input_bag)
        o_bag = rosbag.Bag(output_bag, "w")
        total_msgs =  i_bag.get_message_count(topics)
        for topic, msg, t in i_bag.read_messages(topics):
            o_bag.write(topic, msg, t)
            if topic == "/bebop/cmd_vel":
                cmd_vel_stamped.header.stamp = t
                cmd_vel_stamped.twist = msg
                o_bag.write("/bebop/cmd_vel_stamped", cmd_vel_stamped, t)
            i += 1
            progress(i, total_msgs, output_bag)
    finally:
        o_bag.close()
        i_bag.close()