我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用rospy.Duration()。
def _add_point(self, positions, side, time): """ Appends trajectory with new point @param positions: joint positions @param side: limb to command point @param time: time from start for point in seconds """ #creates a point in trajectory with time_from_start and positions point = JointTrajectoryPoint() point.positions = copy(positions) point.time_from_start = rospy.Duration(time) if side == self.limb: self.goal.trajectory.points.append(point) elif self.gripper and side == self.gripper_name: self.grip.trajectory.points.append(point)
def __init__(self, limb, joint_names): self._joint_names = joint_names ns = 'robot/limb/' + limb + '/' self._client = actionlib.SimpleActionClient( ns + "follow_joint_trajectory", FollowJointTrajectoryAction, ) self._goal = FollowJointTrajectoryGoal() self._goal_time_tolerance = rospy.Time(0.1) self._goal.goal_time_tolerance = self._goal_time_tolerance server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0)) if not server_up: rospy.logerr("Timed out waiting for Joint Trajectory" " Action Server to connect. Start the action server" " before running example.") rospy.signal_shutdown("Timed out waiting for Action Server") sys.exit(1) self.clear(limb)
def wait(self): """ Waits for and verifies trajectory execution result """ #create a timeout for our trajectory execution #total time trajectory expected for trajectory execution plus a buffer last_time = self.goal.trajectory.points[-1].time_from_start.to_sec() time_buffer = rospy.get_param(self._param_ns + 'goal_time', 0.0) + 1.5 timeout = rospy.Duration(self._slow_move_offset + last_time + time_buffer) finish = self._limb_client.wait_for_result(timeout) result = (self._limb_client.get_result().error_code == 0) #verify result if all([finish, result]): return True else: msg = ("Trajectory action failed or did not finish before " "timeout/interrupt.") rospy.logwarn(msg) return False
def __init__(self, limb="right"): """ @param limb: Limb to run CalibrateArm on arm side. """ self._limb=limb self._client = actionlib.SimpleActionClient('/calibration_command', CalibrationCommandAction) # Waits until the action server has started up and started # listening for goals. server_up = self._client.wait_for_server(rospy.Duration(10.0)) if not server_up: rospy.logerr("Timed out waiting for Calibration" " Server to connect. Check your ROS networking" " and/or reboot the robot.") rospy.signal_shutdown("Timed out waiting for Action Server") sys.exit(1)
def _get_minjerk_point(self, m_matrix, idx, t, cmd_time, dimensions_dict): pnt = JointTrajectoryPoint() pnt.time_from_start = rospy.Duration(cmd_time) num_joints = m_matrix.shape[0] pnt.positions = [0.0] * num_joints if dimensions_dict['velocities']: pnt.velocities = [0.0] * num_joints if dimensions_dict['accelerations']: pnt.accelerations = [0.0] * num_joints for jnt in range(num_joints): m_point = minjerk.minjerk_point(m_matrix[jnt, :, :, :], idx, t) # Positions at specified time pnt.positions[jnt] = m_point[0] # Velocities at specified time if dimensions_dict['velocities']: pnt.velocities[jnt] = m_point[1] # Accelerations at specified time if dimensions_dict['accelerations']: pnt.accelerations[jnt] = m_point[-1] return pnt
def revalidate(self, timeout, invalidate_state=True, invalidate_config=True): """ invalidate the state and config topics, then wait up to timeout seconds for them to become valid again. return true if both the state and config topic data are valid """ if invalidate_state: self.invalidate_state() if invalidate_config: self.invalidate_config() timeout_time = rospy.Time.now() + rospy.Duration(timeout) while not self.is_state_valid() and not rospy.is_shutdown(): rospy.sleep(0.1) if timeout_time < rospy.Time.now(): rospy.logwarn("Timed out waiting for node interface valid...") return False return True
def move(self, goal): # Send the goal pose to the MoveBaseAction server self.move_base.send_goal(goal) # Allow 1 minute to get there finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) # If we don't get there in time, abort the goal if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: # We made it! state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!")
def moveToJointAngles(self, jointStates, timeout=1, wait=False, rightArm=True): # Create and send trajectory message for new joint angles trajMsg = JointTrajectoryGoal() trajPoint = JointTrajectoryPoint() trajPoint.positions = jointStates trajPoint.time_from_start = rospy.Duration(timeout) trajMsg.trajectory.points.append(trajPoint) trajMsg.trajectory.joint_names = self.rightJointNames if rightArm else self.leftJointNames if not wait: if rightArm: self.rightArmClient.send_goal(trajMsg) else: self.leftArmClient.send_goal(trajMsg) else: if rightArm: self.rightArmClient.send_goal_and_wait(trajMsg) else: self.leftArmClient.send_goal_and_wait(trajMsg)
def lookAt(self, pos, sim=False): goal = PointHeadGoal() point = PointStamped() point.header.frame_id = self.frame point.point.x = pos[0] point.point.y = pos[1] point.point.z = pos[2] goal.target = point # Point using kinect frame goal.pointing_frame = 'head_mount_kinect_rgb_link' if sim: goal.pointing_frame = 'high_def_frame' goal.pointing_axis.x = 1 goal.pointing_axis.y = 0 goal.pointing_axis.z = 0 goal.min_duration = rospy.Duration(1.0) goal.max_velocity = 1.0 self.pointHeadClient.send_goal_and_wait(goal)
def printJointStates(self): try: # now = rospy.Time.now() # self.tf.waitForTransform(self.frame, 'r_gripper_tool_frame', now, rospy.Duration(10.0)) self.tf.waitForTransform(self.frame, 'r_gripper_tool_frame', rospy.Time(), rospy.Duration(10.0)) currentRightPos, currentRightOrient = self.tf.lookupTransform(self.frame, 'r_gripper_tool_frame', rospy.Time(0)) msg = rospy.wait_for_message('/r_arm_controller/state', JointTrajectoryControllerState) currentRightJointPositions = msg.actual.positions print 'Right positions:', currentRightPos, currentRightOrient print 'Right joint positions:', currentRightJointPositions # now = rospy.Time.now() # self.tf.waitForTransform(self.frame, 'l_gripper_tool_frame', now, rospy.Duration(10.0)) currentLeftPos, currentLeftOrient = self.tf.lookupTransform(self.frame, 'l_gripper_tool_frame', rospy.Time(0)) msg = rospy.wait_for_message('/l_arm_controller/state', JointTrajectoryControllerState) currentLeftJointPositions = msg.actual.positions print 'Left positions:', currentLeftPos, currentLeftOrient print 'Left joint positions:', currentLeftJointPositions # print 'Right gripper state:', rospy.wait_for_message('/r_gripper_controller/state', JointControllerState) except tf.ExtrapolationException: print 'No transformation available! Failing to record this time step.'
def __init__(self): # ROS initialization: rospy.init_node('pose_manager') self.poseLibrary = dict() self.readInPoses() self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction, execute_cb=self.executeBodyPose, auto_start=False) self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction) if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)): try: rospy.wait_for_service("stop_walk_srv", timeout=2.0) self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty) except: rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. " +"This is normal if there is no nao_walker running.") self.stopWalkSrv = None self.poseServer.start() rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys()); else: rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?"); rospy.signal_shutdown("Required component missing");
def parseXapPoses(self, xaplibrary): try: poses = xapparser.getpostures(xaplibrary) except RuntimeError as re: rospy.logwarn("Error while parsing the XAP file: %s" % str(re)) return for name, pose in poses.items(): trajectory = JointTrajectory() trajectory.joint_names = pose.keys() joint_values = pose.values() point = JointTrajectoryPoint() point.time_from_start = Duration(2.0) # hardcoded duration! point.positions = pose.values() trajectory.points = [point] self.poseLibrary[name] = trajectory
def update_step_timer(self, step): """ Helper method that shuts the current step timer down, then recreates the step timer for the next step in the task. If the step timeout is zero, do not create a new timeout; otherwise create a new timer using the timeout value. Args: step (dict): the dictionary object representing the task step. """ if self.step_timer and self.step_timer.is_alive(): self.step_timer.shutdown() if not self.active: return if step and step.timeout > 0: self.step_timer = rospy.Timer( rospy.Duration(step.timeout), self.step_to_handler, oneshot=True )
def init_transforms(self): """ Initialize the tf2 transforms """ rospy.loginfo("Start transform calibration.") self.tf_buffer = tf2_ros.Buffer(rospy.Duration(300.0)) self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) while True: try: now = rospy.get_rostime() self.tf_buffer.lookup_transform('head', 'left_camera_optical_frame', now - rospy.Duration(0.1)) except: self.rate.sleep() continue break rospy.loginfo('transform calibration finished.')
def move(x, y): """ Moves the robot to a place defined by coordinates x and y. :type x: float :type y: int :rtype: NoneType """ client = actionlib.SimpleActionClient("move_base", move_base_msgs.msg.MoveBaseAction) client.wait_for_server(rospy.Duration(10)) goal = move_base_msgs.msg.MoveBaseGoal() goal.target_pose.header.frame_id = "map" goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = x goal.target_pose.pose.position.y = y goal.target_pose.pose.orientation.w = 1 client.send_goal(goal) success = client.wait_for_result(rospy.Duration(60)) loginfo(success)
def _add_point(self, positions, side, time): """ Appends trajectory with new point @param positions: joint positions @param side: limb to command point @param time: time from start for point in seconds """ #creates a point in trajectory with time_from_start and positions point = JointTrajectoryPoint() point.positions = copy(positions) point.time_from_start = rospy.Duration(time) if side == 'left': self._l_goal.trajectory.points.append(point) elif side == 'right': self._r_goal.trajectory.points.append(point) elif side == 'left_gripper': self._l_grip.trajectory.points.append(point) elif side == 'right_gripper': self._r_grip.trajectory.points.append(point)
def wait(self): """ Waits for and verifies trajectory execution result """ #create a timeout for our trajectory execution #total time trajectory expected for trajectory execution plus a buffer last_time = self._r_goal.trajectory.points[-1].time_from_start.to_sec() time_buffer = rospy.get_param(self._param_ns + 'goal_time', 0.0) + 1.5 timeout = rospy.Duration(self._slow_move_offset + last_time + time_buffer) l_finish = self._left_client.wait_for_result(timeout) r_finish = self._right_client.wait_for_result(timeout) l_result = (self._left_client.get_result().error_code == 0) r_result = (self._right_client.get_result().error_code == 0) #verify result if all([l_finish, r_finish, l_result, r_result]): return True else: msg = ("Trajectory action failed or did not finish before " "timeout/interrupt.") rospy.logwarn(msg) return False
def cartesian_pose_client(position, orientation): """Send a cartesian goal to the action server.""" action_address = '/j2n6s300_driver/pose_action/tool_pose' client = actionlib.SimpleActionClient(action_address, kinova_msgs.msg.ArmPoseAction) client.wait_for_server() goal = kinova_msgs.msg.ArmPoseGoal() goal.pose.header = std_msgs.msg.Header(frame_id='j2n6s300_link_base') goal.pose.pose.position = geometry_msgs.msg.Point( x=position[0], y=position[1], z=position[2]) goal.pose.pose.orientation = geometry_msgs.msg.Quaternion( x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3]) # print('goal.pose in client 1: {}'.format(goal.pose.pose)) # debug client.send_goal(goal) if client.wait_for_result(rospy.Duration(15.0)): return client.get_result() else: client.cancel_all_goals() print(' the cartesian action timed-out') return None
def gripper_client(finger_positions): """Send a gripper goal to the action server.""" action_address = '/' + prefix + 'driver/fingers_action/finger_positions' client = actionlib.SimpleActionClient(action_address, kinova_msgs.msg.SetFingersPositionAction) client.wait_for_server() goal = kinova_msgs.msg.SetFingersPositionGoal() goal.fingers.finger1 = float(finger_positions[0]) goal.fingers.finger2 = float(finger_positions[1]) # The MICO arm has only two fingers, but the same action definition is used if len(finger_positions) < 3: goal.fingers.finger3 = 0.0 else: goal.fingers.finger3 = float(finger_positions[2]) client.send_goal(goal) if client.wait_for_result(rospy.Duration(5.0)): return client.get_result() else: client.cancel_all_goals() rospy.WARN(' the gripper action timed-out') return None
def joint_angle_client(angle_set): """Send a joint angle goal to the action server.""" action_address = '/j2n6s300_driver/joints_action/joint_angles' client = actionlib.SimpleActionClient(action_address, kinova_msgs.msg.ArmJointAnglesAction) client.wait_for_server() goal = kinova_msgs.msg.ArmJointAnglesGoal() goal.angles.joint1 = angle_set[0] goal.angles.joint2 = angle_set[1] goal.angles.joint3 = angle_set[2] goal.angles.joint4 = angle_set[3] goal.angles.joint5 = angle_set[4] goal.angles.joint6 = angle_set[5] goal.angles.joint7 = angle_set[6] client.send_goal(goal) if client.wait_for_result(rospy.Duration(20.0)): return client.get_result() else: print(' the joint angle action timed-out') client.cancel_all_goals() return None
def search_USBlight(self): if self.listen.frameExists("/j2n6a300_end_effector") and self.listen.frameExists("/root"): # print ("we are in the search spoon fucntion") self.listen.waitForTransform('/j2n6a300_end_effector','/root',rospy.Time(),rospy.Duration(100.0)) t = self.listen.getLatestCommonTime("/j2n6a300_end_effector","/root") translation, quaternion = self.listen.lookupTransform("/j2n6a300_end_effector","/root",t) matrix1=self.listen.fromTranslationRotation(translation,quaternion) counter=0 rate=rospy.Rate(100) while not self.obj_det: counter = counter + 1 if(counter < 200): cart_velocities = np.dot(matrix1[:3,:3],np.array([0,0,0.05])[np.newaxis].T) #change in y->x, z->y, x->z cart_velocities = cart_velocities.T[0].tolist() self.cmmnd_CartesianVelocity(cart_velocities + [0,0,0,1]) else: cart_velocities = np.dot(matrix1[:3,:3],np.array([0,0,-0.05])[np.newaxis].T) cart_velocities = cart_velocities.T[0].tolist() self.cmmnd_CartesianVelocity(cart_velocities + [0,0,0,1]) rate.sleep() if(counter >400): counter=0
def search_straw(self): if self.listen.frameExists("/j2n6a300_end_effector") and self.listen.frameExists("/root"): # print ("we are in the search spoon fucntion") self.listen.waitForTransform('/j2n6a300_end_effector','/root',rospy.Time(),rospy.Duration(100.0)) t = self.listen.getLatestCommonTime("/j2n6a300_end_effector","/root") translation, quaternion = self.listen.lookupTransform("/j2n6a300_end_effector","/root",t) matrix1=self.listen.fromTranslationRotation(translation,quaternion) counter=0 rate=rospy.Rate(100) while not self.obj_det: counter = counter + 1 if(counter < 200): cart_velocities = np.dot(matrix1[:3,:3],np.array([00,0,0.05])[np.newaxis].T) #change in y->x, z->y, x->z cart_velocities = cart_velocities.T[0].tolist() self.cmmnd_CartesianVelocity(cart_velocities + [0,0,0,1]) else: cart_velocities = np.dot(matrix1[:3,:3],np.array([0.0,0,-.05])[np.newaxis].T) cart_velocities = cart_velocities.T[0].tolist() self.cmmnd_CartesianVelocity(cart_velocities + [0,0,0,1]) rate.sleep() if(counter >400): counter=0
def goto_plate(self): if self.listen.frameExists("/root") and self.listen.frameExists("/plate_position"): self.listen.waitForTransform('/root','/plate_position',rospy.Time(),rospy.Duration(100.0)) # t1 = self.listen.getLatestCommonTime("/root", "bowl_position") translation, quaternion = self.listen.lookupTransform("/root", "/plate_position", rospy.Time(0)) translation = list(translation) quaternion = list(quaternion) #quaternion = [0.8678189045198146, 0.0003956789257977804, -0.4968799802988633, 0.0006910675928639343] #quaternion = [0]*4 pose_value = translation + quaternion #second arg=0 (absolute movement), arg = '-r' (relative movement) self.cmmnd_CartesianPosition(pose_value, 0) else: print ("we DONT have the bowl frame")
def searchSpoon(self): if self.listen.frameExists("/j2n6a300_end_effector") and self.listen.frameExists("/root"): # print ("we are in the search spoon fucntion") self.listen.waitForTransform('/j2n6a300_end_effector','/root',rospy.Time(),rospy.Duration(100.0)) t = self.listen.getLatestCommonTime("/j2n6a300_end_effector","/root") translation, quaternion = self.listen.lookupTransform("/j2n6a300_end_effector","/root",t) matrix1=self.listen.fromTranslationRotation(translation,quaternion) counter=0 rate=rospy.Rate(100) while not self.obj_det: counter = counter + 1 if(counter < 200): cart_velocities = np.dot(matrix1[:3,:3],np.array([0,0,0.05])[np.newaxis].T) #change in y->x, z->y, x->z cart_velocities = cart_velocities.T[0].tolist() self.cmmnd_CartesianVelocity(cart_velocities + [0,0,0,1]) else: cart_velocities = np.dot(matrix1[:3,:3],np.array([0,0,-0.05])[np.newaxis].T) cart_velocities = cart_velocities.T[0].tolist() self.cmmnd_CartesianVelocity(cart_velocities + [0,0,0,1]) rate.sleep() if(counter >400): counter=0
def scoopSpoon(self): while not (self.listen.frameExists("/j2n6a300_end_effector") and self.listen.frameExists("/j2n6a300_link_finger_tip_3")): pass print ("Publishing transform of figner wrt endEffector frame") p.listen.waitForTransform('/j2n6a300_end_effector','/j2n6a300_link_finger_tip_3',rospy.Time(),rospy.Duration(100.0)) t = self.listen.getLatestCommonTime("/j2n6a300_end_effector", "/j2n6a300_link_finger_tip_3") translation, quaternion = self.listen.lookupTransform("/j2n6a300_end_effector", "/j2n6a300_link_finger_tip_3", t) # print (translation) matrix2 = self.listen.fromTranslationRotation(translation, quaternion) # print (matrix2) # required_cartvelo = [0,0,0,0.1,0,0] quaternion = pose_action_client.EulerXYZ2Quaternion([0.15,0,0]) # set rot angle HERE (deg) matrix1 = self.listen.fromTranslationRotation([0,0,0], quaternion) # print (matrix1) matrix3 = np.dot(matrix2,matrix1) scale, shear, rpy_angles, trans, perps = tf.transformations.decompose_matrix(matrix3) trans = list(trans.tolist()) rpy_angles = list(rpy_angles) # euler = pose_action_client.Quaternion2EulerXYZ(quat_1) # pose = trans + rpy_angles return pose # return translation, quaternion
def searchdriver(self): if self.listen.frameExists("/j2n6a300_end_effector") and self.listen.frameExists("/root"): # print ("we are in the search spoon fucntion") self.listen.waitForTransform('/j2n6a300_end_effector','/root',rospy.Time(),rospy.Duration(100.0)) t = self.listen.getLatestCommonTime("/j2n6a300_end_effector","/root") translation, quaternion = self.listen.lookupTransform("/j2n6a300_end_effector","/root",t) matrix1=self.listen.fromTranslationRotation(translation,quaternion) counter=0 rate=rospy.Rate(100) while not self.obj_det: counter = counter + 1 if(counter < 200): cart_velocities = np.dot(matrix1[:3,:3],np.array([0,0,0.05])[np.newaxis].T) #change in y->x, z->y, x->z cart_velocities = cart_velocities.T[0].tolist() self.cmmnd_CartesianVelocity(cart_velocities + [0,0,0,1]) else: cart_velocities = np.dot(matrix1[:3,:3],np.array([0,0,-0.05])[np.newaxis].T) cart_velocities = cart_velocities.T[0].tolist() self.cmmnd_CartesianVelocity(cart_velocities + [0,0,0,1]) rate.sleep() if(counter >400): counter=0
def create_pose_velocity_msg(self,cart_velo,transform=False): if transform: if self.listen.frameExists("/root") and self.listen.frameExists("/j2n6a300_end_effector"): self.listen.waitForTransform('/root',"/j2n6a300_end_effector",rospy.Time(),rospy.Duration(100.0)) t = self.listen.getLatestCommonTime("/root", "/j2n6a300_end_effector") translation, quaternion = self.listen.lookupTransform("/root", "/j2n6a300_end_effector", t) transform = self.transformer.fromTranslationRotation(translation, quaternion) transformed_vel = np.dot(transform,(cart_velo[0:3]+[1.0])) print(transformed_vel) cart_velo[0:3] = transformed_vel[0:3] msg = PoseVelocity( twist_linear_x=cart_velo[0], twist_linear_y=cart_velo[1], twist_linear_z=cart_velo[2], twist_angular_x=cart_velo[3], twist_angular_y=cart_velo[4], twist_angular_z=cart_velo[5]) return msg
def __init__(self): # rospy.init_node('nav_test', anonymous=False) #what to do if shut down (e.g. ctrl + C or failure) rospy.on_shutdown(self._shutdown) #tell the action client that we want to spin a thread by default self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("waiting for the action server to come up...") #allow up to 5 seconds for the action server to come up self.move_base.wait_for_server(rospy.Duration(5)) #we'll send a goal to the robot to tell it to move to a pose that's near the docking station self.goal = MoveBaseGoal() self.goal.target_pose.header.frame_id = 'odom' self.goal.target_pose.header.stamp = rospy.Time.now()
def move_to_pose(self, x1, y1): # Goal self.goal.target_pose.pose.position.x = x1 self.goal.target_pose.pose.position.y = y1 self.goal.target_pose.pose.position.z = 0.0 self.goal.target_pose.pose.orientation.x = 0.0 self.goal.target_pose.pose.orientation.y = 0.0 self.goal.target_pose.pose.orientation.z = -0.5 self.goal.target_pose.pose.orientation.w = 0.1 #start moving self.move_base.send_goal(self.goal) rospy.loginfo("Moving to desired position...") #allow TurtleBot up to 60 seconds to complete task self.success = self.move_base.wait_for_result(rospy.Duration(60)) if not self.success: self.move_base.cancel_goal() rospy.loginfo("The base failed to reach the desired position :(") else: # We made it! state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Destination reached!")
def servo_axis_rotation(self, x): if abs(x) > self.params['sensitivity_joy']: x = x if abs(x) > self.params['sensitivity_joy'] else 0 min_x = self.params['bounds'][0][0] + self.params['bounds'][3][0] max_x = self.params['bounds'][0][1] + self.params['bounds'][3][1] self.goal = min(max(min_x, self.goal + self.params['speed']*x*self.delta_t), max_x) if self.goal > self.params['bounds'][0][1]: new_x_m3 = self.goal - self.params['bounds'][0][1] new_x = self.params['bounds'][0][1] elif self.goal < self.params['bounds'][0][0]: new_x_m3 = self.goal - self.params['bounds'][0][0] new_x = self.params['bounds'][0][0] else: new_x = self.goal new_x_m3 = 0 new_x_m3 = max(min(new_x_m3, self.params['bounds'][3][1]), self.params['bounds'][3][0]) self.reach({'m1': new_x, 'm4': new_x_m3}, 0) # Duration = 0 means joint teleportation
def execute(self, motion, duration): """ Blocking function executing a motion :param motion: [{"motor_name": value_in_degrees}, {}...] :param duration: duration in secs """ request = ExecuteTrajectoryRequest() request.trajectory.joint_names = motion.keys request.trajectory.time = rospy.Time.now() motion_length = len(duration) for point_id, point in enumerate(motion.values()): point = JointTrajectoryPoint(positions=point, time_from_start=rospy.Duration(float(point_id*duration)/motion_length)) request.points.append(point) return self.execute_proxy(request)
def _init_params(self): self.port = rospy.get_param('~port', self.default_port) self.robot_type = rospy.get_param('~robot_type', 'create') #self.baudrate = rospy.get_param('~baudrate', self.default_baudrate) self.update_rate = rospy.get_param('~update_rate', self.default_update_rate) self.drive_mode = rospy.get_param('~drive_mode', 'twist') self.has_gyro = rospy.get_param('~has_gyro', True) self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0) self.odom_linear_scale_correction = rospy.get_param('~odom_linear_scale_correction', 1.0) self.cmd_vel_timeout = rospy.Duration(rospy.get_param('~cmd_vel_timeout', 0.6)) self.stop_motors_on_bump = rospy.get_param('~stop_motors_on_bump', True) self.min_abs_yaw_vel = rospy.get_param('~min_abs_yaw_vel', None) self.max_abs_yaw_vel = rospy.get_param('~max_abs_yaw_vel', None) self.publish_tf = rospy.get_param('~publish_tf', False) self.odom_frame = rospy.get_param('~odom_frame', 'odom') self.base_frame = rospy.get_param('~base_frame', 'base_footprint') self.operate_mode = rospy.get_param('~operation_mode', 3) rospy.loginfo("serial port: %s"%(self.port)) rospy.loginfo("update_rate: %s"%(self.update_rate)) rospy.loginfo("drive mode: %s"%(self.drive_mode)) rospy.loginfo("has gyro: %s"%(self.has_gyro))
def publishShape(self, position, rx, ry, tag, namespace): marker = Marker() marker.header.frame_id = '/map' marker.header.stamp = rospy.Time.now() marker.ns = namespace marker.id = int(tag) marker.action = Marker.ADD marker.type = self.shape self.setPosition(marker, position) self.setRadius(marker, rx, ry) self.setColor(marker, tag) marker.lifetime = rospy.Duration(5) self.publisher.publish(marker)
def publishShape(self, position, measurement, tag, namespace): marker = Marker() marker.header.frame_id = '/map' marker.header.stamp = rospy.Time.now() marker.ns = namespace marker.id = int(tag) marker.action = Marker.ADD marker.type = self.shape self.setPosition(marker, position) self.setRadius(marker, measurement) self.setColor(marker, tag) marker.lifetime = rospy.Duration(5) self.publisher.publish(marker)
def __init__(self,dis=0.0): State.__init__(self, outcomes=['succeeded','preempted','aborted'],input_keys=['target']) rospy.on_shutdown(self.shutdown) self.test_distance = target.y-dis self.rate = 200 self.r = rospy.Rate(self.rate) self.speed = rospy.get_param('~speed',0.08) self.tolerance = rospy.get_param('~tolerance', 0.01) self.cmd_vel = rospy.Publisher('/mobile_base_controller/smooth_cmd_vel',Twist,queue_size=5) self.base_frame = rospy.get_param('~base_frame', '/base_link') self.odom_frame = rospy.get_param('~odom_frame', '/odom') self.tf_listener = tf.TransformListener() rospy.sleep(1) self.tf_listener.waitForTransform(self.odom_frame,self.base_frame,rospy.Time(),rospy.Duration(60.0)) #define a bianliang self.flag = rospy.get_param('~flag', True) rospy.loginfo(self.test_distance) #tf get position #publish cmd_vel
def __init__(self,angle=0): State.__init__(self, outcomes=['succeeded','aborted','preempted'],input_keys=["angle_in"]) self.speed = rospy.get_param('~speed', 0.10) self.secretindigal = 1.0 self.tolerance = rospy.get_param('tolerance', math.radians(5)) self.start = True self.cmd_vel = rospy.Publisher('mobile_base_controller/smooth_cmd_vel', Twist, queue_size=5) self.base_frame = rospy.get_param('~base_frame', '/base_link') self.odom_frame = rospy.get_param('~odom_frame', '/odom') rospy.on_shutdown(self.shutdown) self.rate = 30 self.start_test = True self.r = rospy.Rate(self.rate) self.tf_listener = tf.TransformListener() rospy.sleep(0.5) self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60))
def __init__(self, angle=0): State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted']) self.speed = rospy.get_param('~speed', 0.03) self.secretindigal = 1.0 self.tolerance = rospy.get_param('tolerance', math.radians(5)) self.start = True self.cmd_vel = rospy.Publisher('mobile_base_controller/smooth_cmd_vel', Twist, queue_size=5) self.base_frame = rospy.get_param('~base_frame', '/base_link') self.odom_frame = rospy.get_param('~odom_frame', '/odom') rospy.on_shutdown(self.shutdown) self.rate = 30 self.start_test = True self.r = rospy.Rate(self.rate) self.angle=angle self.tf_listener = tf.TransformListener() rospy.sleep(0.5) self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60))
def __init__(self,dis=0): State.__init__(self, outcomes=['succeeded','preempted','aborted']) rospy.on_shutdown(self.shutdown) self.test_distance = dis self.rate = 100 self.r = rospy.Rate(self.rate) self.speed = rospy.get_param('~speed',0.08) self.tolerance = rospy.get_param('~tolerance', 0.01) self.cmd_vel = rospy.Publisher('/mobile_base_controller/smooth_cmd_vel',Twist,queue_size=5) self.base_frame = rospy.get_param('~base_frame', '/base_link') self.odom_frame = rospy.get_param('~odom_frame', '/odom') self.tf_listener = tf.TransformListener() rospy.sleep(1) self.tf_listener.waitForTransform(self.odom_frame,self.base_frame,rospy.Time(),rospy.Duration(60.0)) self.flag = rospy.get_param('~flag', True) #tf get position self.position = Point() self.position = self.get_position() self.y_start = self.position.y self.x_start = self.position.x #publish cmd_vel
def __init__(self,angle=0): State.__init__(self, outcomes=['succeeded','aborted','preempted']) self.speed = rospy.get_param('~speed', 0.20) self.secretindigal = 1.0 self.tolerance = rospy.get_param('tolerance', math.radians(5)) self.start = True self.cmd_vel = rospy.Publisher('mobile_base_controller/smooth_cmd_vel', Twist, queue_size=5) self.base_frame = rospy.get_param('~base_frame', '/base_link') self.odom_frame = rospy.get_param('~odom_frame', '/odom') rospy.on_shutdown(self.shutdown) self.rate = 30 self.start_test = True self.r = rospy.Rate(self.rate) self.angle = angle self.tf_listener = tf.TransformListener() rospy.sleep(0.5) self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60))
def __init__(self, limb): ns = 'robot/limb/' + limb + '/' self._client = actionlib.SimpleActionClient( ns + "follow_joint_trajectory", FollowJointTrajectoryAction, ) self._goal = FollowJointTrajectoryGoal() self._goal_time_tolerance = rospy.Time(0.1) self._goal.goal_time_tolerance = self._goal_time_tolerance server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0)) if not server_up: rospy.logerr("Timed out waiting for Joint Trajectory" " Action Server to connect. Start the action server" " before running example.") rospy.signal_shutdown("Timed out waiting for Action Server") sys.exit(1) self.clear(limb)
def blink(self,color, timeout=0.0, frequency=2.0, blocking=True): #timeout <= 0 blinks endlessly """ Blinks a led for a specific time and frequency (blocking) :param color: Color of the led (red, green, orange) :type color: str :param timeout: Duration to let the led blinking. A value <= 0.0 means infinite time :type timeout: float :param frequency: Rate, how often a led blinks in one second :type frequency: float """ if blocking is False: self.post.blink(color,timeout,frequency,True) else : end = rospy.Time.now()+ rospy.Duration(timeout) self.led_state[name] = 1 while not rospy.is_shutdown(): start = rospy.Time.now() if (start > end and timeout > 0) or self.led_state[name] == 0: self.setLed(color, 0) break self.setLed(color, 100) rospy.Rate(frequency*2).sleep() self.setLed(color, 0) rospy.Rate(frequency*2).sleep()
def blink(self,name, timeout=0.0, frequency=2.0): #timeout <= 0 blinks endlessly """ Blinks a led for a specific time and frequency (blocking) :param name: Name of the led :type name: str :param timeout: Duration to let the led blinking. A value <= 0.0 means infinite time :type timeout: float :param frequency: Rate, how often a led blinks in one second :type frequency: float """ end = rospy.Time.now()+ rospy.Duration(timeout) self.led_state[name] = 1 try: while not rospy.is_shutdown(): start = rospy.Time.now() if (start > end and timeout > 0) or self.led_state[name] == 0: self.led_handle[name].set_output(False) break self.led_handle[name].set_output(True) rospy.Rate(frequency*2).sleep() self.led_handle[name].set_output(False) rospy.Rate(frequency*2).sleep() except: pass
def execute(self, userdata): self.goal.target_pose.pose = userdata.waypoint_in # Send the goal pose to the MoveBaseAction server self.move_base.send_goal(self.goal) if self.preempt_requested(): self.move_base.cancel_goal() self.service_preempt() return 'preempted' # Allow 1 minute to get there finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) # If we don't get there in time, abort the goal if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") return 'aborted' else: # We made it! state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!") return 'succeeded'
def IK(self, T_goal): req = moveit_msgs.srv.GetPositionIKRequest() req.ik_request.group_name = self.group_name req.ik_request.robot_state = moveit_msgs.msg.RobotState() req.ik_request.robot_state.joint_state = self.joint_state req.ik_request.avoid_collisions = True req.ik_request.pose_stamped = geometry_msgs.msg.PoseStamped() req.ik_request.pose_stamped.header.frame_id = "world_link" req.ik_request.pose_stamped.header.stamp = rospy.get_rostime() req.ik_request.pose_stamped.pose = convert_to_message(T_goal) req.ik_request.timeout = rospy.Duration(3.0) res = self.ik_service(req) q = [] if res.error_code.val == res.error_code.SUCCESS: q = self.q_from_joint_state(res.solution.joint_state) return q
def __init__(self): maki_body = actionlib.SimpleActionClient("/CoRDial/Behavior", BehaviorAction) # the action server of maki_cordial_server rospy.sleep(2.0) global loop_count global loop_time global flag self.new_behavior_time = rospy.Time.now() self.eye_gaze_time = rospy.Time.now() self.blink_time = rospy.Time.now() + rospy.Duration(10) while not loop_count == 0 or flag == 1: # while number of times the actions are repeated print loop_count loop_count -= 1 if rospy.Time.now() >= self.new_behavior_time: behav = maki_body_act self.new_behavior_time = rospy.Time.now() + rospy.Duration(loop_time) maki_body.send_goal(BehaviorGoal(behavior=behav)) # sending the behavior to maki, name changed to behavior by xuan, March 22th print "sent behavior: ", behav sleep(loop_time)
def __init__(self, limb): ns = 'robot/limb/' + limb + '/' self._client = actionlib.SimpleActionClient( ns + "follow_joint_trajectory", FollowJointTrajectoryAction, ) self._goal = FollowJointTrajectoryGoal() self._goal_time_tolerance = rospy.Time(0.1) self._goal.goal_time_tolerance = self._goal_time_tolerance server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0)) if not server_up: rospy.logerr("Timed out waiting for Joint Trajectory" " Action Server to connect. Start the action server" " before running example.") rospy.signal_shutdown("Timed out waiting for Action Server") sys.exit(1) self._sub_path = rospy.Subscriber('joint_path', numpy_msg(Float32MultiArray), self.path_cb) self._sub_traj = rospy.Subscriber('joint_traj', numpy_msg(Float32MultiArray), self.traj_cb) self._limb_interface = baxter_interface.limb.Limb('right') self._q_start = np.array([-0.22281071, -0.36470393, 0.36163597, 1.71920897, -0.82719914, -1.16889336, -0.90888362]) self.clear(limb)
def publish_cloud_markers(pub_ns, _arr, c='b', stamp=None, flip_rb=False, frame_id='map'): """ Publish point cloud on: pub_ns: Namespace on which the cloud will be published arr: numpy array (N x 3) for point cloud data c: Option 1: 'c', 'b', 'r' etc colors accepted by matplotlibs color Option 2: float ranging from 0 to 1 via matplotlib's jet colormap Option 3: numpy array (N x 3) with r,g,b vals ranging from 0 to 1 s: supported only by matplotlib plotting alpha: supported only by matplotlib plotting """ arr, carr = copy_pointcloud_data(_arr, c, flip_rb=flip_rb) marker = vis_msg.Marker(type=vis_msg.Marker.SPHERE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD) marker.header.frame_id = frame_id marker.header.stamp = stamp if stamp is not None else rospy.Time.now() marker.scale.x = 0.01 marker.scale.y = 0.01 # XYZ inds, = np.where(~np.isnan(arr).any(axis=1)) marker.points = [geom_msg.Point(arr[j,0], arr[j,1], arr[j,2]) for j in inds] # RGB (optionally alpha) N, D = arr.shape[:2] if D == 3: marker.colors = [std_msg.ColorRGBA(carr[j,0], carr[j,1], carr[j,2], 1.0) for j in inds] elif D == 4: marker.colors = [std_msg.ColorRGBA(carr[j,0], carr[j,1], carr[j,2], carr[j,3]) for j in inds] marker.lifetime = rospy.Duration() _publish_marker(marker) print 'Publishing marker', N
