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)
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()
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")
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()
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")
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)
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
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
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)
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)
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)
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)
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
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
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)
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
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'
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
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
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
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)
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)
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()
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)
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)
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 )
def onTwistStamped(self,msg): out_msg = TwistStamped() out_msg.header = rospy.time.now() out_msg.twist = msg self.twist_pub.publish(out_msg)
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()