def __init__(self, target_link): rospy.init_node('GetJointPose') tf_listener = TransformListener() base_link = "base_link" print "waiting for transform" tf_listener.waitForTransform (target_link, base_link, rospy.Time(), rospy.Duration(4.0)) print "done waiting" if not tf_listener.frameExists("base_link"): print "ERROR NO FRAME base_link" return elif not tf_listener.frameExists(target_link): print "ERROR NO FRAME" + target_link return else: t = tf_listener.getLatestCommonTime("/base_link", target_link) joint_pose = geometry_msgs.msg.PoseStamped() joint_pose.header.frame_id = target_link joint_pose.pose.orientation.w = 1.0 # Neutral orientation pose_from_base = tf_listener.transformPose("/base_link", joint_pose) print "Position from " + base_link + " to " + target_link print pose_from_base
class FTSCalibSampler: def __init__(self, filename='poses.txt'): print('init') self.tf = TransformListener() self.base_link = 'base_link' self.tool_link = 'fts_toolside' self.pose_cnt = 0 self.file = open(filename, 'w+') try: self.tf.waitForTransform(self.base_link, self.tool_link, rospy.Time(), rospy.Duration(5.0)) except tf.Exception: # likely a timeout print( "Timeout while waiting for the TF transformation with the map!" " Is someone publishing TF tansforms?\n ROS positions won't be available." ) self.tf_running = False def update(self): if self.tf.frameExists(self.base_link) and self.tf.frameExists( self.tool_link): t = self.tf.getLatestCommonTime(self.base_link, self.tool_link) position, quaternion = self.tf.lookupTransform( self.base_link, self.tool_link, t) self.new_pose = position + list(euler_from_quaternion(quaternion)) print(self.new_pose) def write(self): self.file.write('pose{}: {}\n'.format(self.pose_cnt, str(self.new_pose))) self.pose_cnt += 1
class myNode: def __init__(self, *args): self.tf = TransformListener() # rospy.Subscriber(...) # ... def some_method(self): if self.tf.frameExists( "left_fingertip_sensor_s0") and self.tf.frameExists("world"): t = self.tf.getLatestCommonTime("left_fingertip_sensor_s0", "world") position, quaternion = self.tf.lookupTransform( "left_fingertip_sensor_s0", "world", t) print position, quaternion else: print "not found frame" def fury(self): # now = rospy.Time.now() self.tf.waitForTransform("pbase_link", "world", rospy.Time(0), rospy.Duration(10.0)) print "waited" (trans, rot) = self.tf.lookupTransform("left_fingertip_sensor_s0", "world", rospy.Time(0)) print trans, rot return trans def locations(self, frame1, frame2): self.tf.waitForTransform(frame1, frame2, rospy.Time(0), rospy.Duration(10.0)) print "waited" (trans, rot) = self.tf.lookupTransform(frame1, frame2, rospy.Time(0)) print trans, rot
class SLAM(object): ''' Adapted from L310 coursework ''' def __init__(self, robot_id): self.robot_id = robot_id self._occupancy_grid = None rospy.Subscriber('/robot{}/map'.format(self.robot_id), OccupancyGrid_msg, self.callback) self._tf = TransformListener() # self._occupancy_grid = None # NOTE why was this defined after calling callback? self._pose = np.array([np.nan, np.nan, np.nan], dtype=np.float32) def callback(self, msg): values = np.array(msg.data, dtype=np.int8).reshape( (msg.info.width, msg.info.height)) processed = np.empty_like(values) processed[:] = FREE processed[values < 0] = UNKNOWN processed[values > 50] = OCCUPIED processed = processed.T origin = [msg.info.origin.position.x, msg.info.origin.position.y, 0.] resolution = msg.info.resolution # if self._occupancy_grid is not None: # then merge new map with old map self._occupancy_grid = OccupancyGrid(processed, origin, resolution, msg) def update(self): # Get pose w.r.t. map. a = 'robot{}/occupancy_grid'.format(self.robot_id) b = 'robot{}/base_link'.format(self.robot_id) if self._tf.frameExists(a) and self._tf.frameExists(b): try: t = rospy.Time(0) position, orientation = self._tf.lookupTransform( '/' + a, '/' + b, t) self._pose[X] = position[X] self._pose[Y] = position[Y] _, _, self._pose[YAW] = euler_from_quaternion(orientation) except Exception as e: print(e) else: print('Unable to find:', self._tf.frameExists(a), self._tf.frameExists(b)) pass @property def ready(self): return self._occupancy_grid is not None and not np.isnan(self._pose[0]) @property def pose(self): return self._pose @property def occupancy_grid(self): return self._occupancy_grid
class ChallengeProblemLogger(object): _knownObstacles = {} _placedObstacle = False _lastgzlog = 0.0 _tf_listener = None def __init__(self,name): self._name = name; self._experiment_started = False self._tf_listener = TransformListener() # Subscribe to robot pose ground truth from gazebo rospy.Subscriber("/gazebo/model_states", ModelStates, callback=self.gazebo_model_monitor, queue_size=1) # Whenever we get a report from Gazebo, map the gazebo coordinate to map coordinates and # log this # Only do this every second - this should be accurate enough # TODO: model is assumed to be the third in the list. Need to make this based # on the array to account for obstacles (maybe) def gazebo_model_monitor(self, data): if (len(data.pose))<=2: return data_time = rospy.get_rostime().to_sec() if ((self._lastgzlog == 0.0) | (data_time - self._lastgzlog >= 1)): # Only do this every second # Get the turtlebot model state information (assumed to be indexed at 2) tb_pose = data.pose[2] tb_position = tb_pose.position self._lastgzlog = data_time # Do this only if the transform exists if self._tf_listener.frameExists("/base_link") and self._tf_listener.frameExists("/map"): self._tf_listener.waitForTransform("/map", "/base_link", rospy.Time(0), rospy.Duration(1)) (trans,rot) = self._tf_listener.lookupTransform("/map", "/base_link",rospy.Time(0)) rospy.loginfo("BRASS | Turtlebot | {},{}".format(trans[0], trans[1])) # Log any obstacle information, but do it only once. This currently assumes one obstacle # TODO: test this if len(data.name) > 3: addedObstacles = {} removedObstacles = self._knownObstacles.copy() for obs in range(3, len(data.name)-1): if (data.name[obs] not in self._knownObstacles): addedObstacles[data.name[obs]] = obs else: self._knownObstacles[data.name[obs]] = obs del removedObstacles[data.name[obs]] for key, value in removedObstacles.iteritems(): rospy.logInfo("BRASS | Obstacle {} | removed".format(key)) del self._knownObstacles[key] for key, value in addedObstacles.iteritems(): obs_pos = data.pose[value].position rospy.logInfo ("BRASS | Obstacle {} | {},{}".format(key, obs_pos.x, obs_pos.y)) self._knownObstacles[key] = value
class TfTest(RosTestCase): def setUpEnv(self): robot = ATRV() odometry = Odometry() robot.append(odometry) odometry.add_stream('ros') motion = Teleport() robot.append(motion) motion.add_stream('socket') robot2 = ATRV() robot2.translate(0,1,0) odometry2 = Odometry() robot2.append(odometry2) odometry2.add_stream('ros', frame_id="map", child_frame_id="robot2") env = Environment('empty', fastmode = True) env.add_service('socket') def _check_pose(self, frame1, frame2, position, quaternion, precision = 0.01): t = self.tf.getLatestCommonTime(frame1, frame2) observed_position, observed_quaternion = self.tf.lookupTransform(frame1, frame2, t) for a,b in zip(position, observed_position): self.assertAlmostEqual(a, b, delta = precision) for a,b in zip(quaternion, observed_quaternion): self.assertAlmostEqual(a, b, delta = precision) def test_tf(self): rospy.init_node('morse_ros_tf_test') self.tf = TransformListener() sleep(1) self.assertTrue(self.tf.frameExists("/odom")) self.assertTrue(self.tf.frameExists("/base_footprint")) self.assertTrue(self.tf.frameExists("/map")) self.assertTrue(self.tf.frameExists("/robot2")) self._check_pose("odom", "base_footprint", [0,0,0], [0,0,0,1]) self._check_pose("map", "robot2", [0,0,0], [0,0,0,1]) with Morse() as morse: teleport = morse.robot.motion teleport.publish({'x' : 2, 'y' : 0, 'z' : 0, \ 'yaw' : 0, 'pitch' : 0.0, 'roll' : 0.0}) morse.sleep(0.1) self._check_pose("odom", "base_footprint", [2,0,-0.1], [0,0,0,1])
class TransformNode: def __init__(self, *args): self.tf = TransformListener() rospy.Subscriber("/tf", TFMessage, transform, queue_size=1) def transform(self, msg): if self.tf.frameExists("/base_link") and self.tf.frameExists("/map"): t = self.tf.getLatestCommonTime("/base_link", "/map") position, quaternion = self.tf.lookupTransform("/base_link", "/map", t) print position, quaternion
class SLAM(object): def __init__(self): rospy.Subscriber('/map', OccupancyGrid, self.callback) self._tf = TransformListener() self._occupancy_grid = None self._pose = np.array([np.nan, np.nan, np.nan], dtype=np.float32) def callback(self, msg): values = np.array(msg.data, dtype=np.int8).reshape( (msg.info.width, msg.info.height)) processed = np.empty_like(values) processed[:] = 0 processed[values < 0] = 1 processed[values > 50] = 2 processed = processed.T origin = [msg.info.origin.position.x, msg.info.origin.position.y, 0.] resolution = msg.info.resolution # self._occupancy_grid = rrt.OccupancyGrid(processed, origin, resolution) def update(self): # Get pose w.r.t. map. a = 'occupancy_grid' # a = 'map' b = LEADER + '/base_link' if self._tf.frameExists(a) and self._tf.frameExists(b): try: t = rospy.Time(0) position, orientation = self._tf.lookupTransform( '/' + a, '/' + b, t) self._pose[X] = position[X] self._pose[Y] = position[Y] _, _, self._pose[YAW] = euler_from_quaternion(orientation) print('pose:', self._pose) except Exception as e: print(e) else: print('Unable to finddd:', self._tf.frameExists(a), self._tf.frameExists(b)) pass @property def ready(self): return self._occupancy_grid is not None and not np.isnan(self._pose[0]) @property def pose(self): return self._pose @property def occupancy_grid(self): return self._occupancy_grid
def run(): tt = tf.Transformer(True, rospy.Duration(10.0)) tfl = TransformListener() #print tf.allFramesAsString() print tfl.getFrameStrings() if tfl.frameExists("/base_link") and tfl.frameExists("/base_footprint"): t = tfl.getLatestCommonTime("/base_link", "/base_footprint") position, quaternion = tfl.lookupTransform("/base_link", "/base_footprint", t) print position, quaternion else: print "no"
def get_object_pose(object_frame, reference_frame): pose = None tf_l = TransformListener() try: print object_frame, reference_frame tf_l.waitForTransform(object_frame, reference_frame, rospy.Time(0), rospy.Duration(5.0)) if (tf_l.frameExists(object_frame) and tf_l.frameExists(reference_frame)): p, q = tf_l.lookupTransform(reference_frame, object_frame, rospy.Time(0)) pose = Pose(Point(*p), Quaternion(*q)) except (tf.LookupException, tf.ConnectivityException): print "Unable to retrieve object pose!" del tf_l # kill listener to prevent annoying warnings return pose
class KinectNiteHelp: def __init__(self, name="kinect_listener", user=1): # rospy.init_node(name, anonymous=True) self.tf = TransformListener() self.user = user def getLeftHandPos(self): if self.tf.frameExists(BASE_FRAME) and self.tf.frameExists("left_hand_1"): print " Inside TF IF Get LEft HAnd POS " t = self.tf.getLatestCommonTime(BASE_FRAME, "left_hand_1") (leftHandPos, quaternion) = self.tf.lookupTransform(BASE_FRAME, "left_hand_1", t) return leftHandPos def getRightHandPos(self): if self.tf.frameExists(BASE_FRAME) and self.tf.frameExists("right_hand_1"): t = self.tf.getLatestCommonTime(BASE_FRAME, "right_hand_1") (rightHandPos, quaternion) = self.tf.lookupTransform(BASE_FRAME, "right_hand_1", t) return rightHandPos
class Transformation(): def __init__(self): pub = 0 self.tf = TransformListener() self.tf1 = TransformerROS() self.fdata = geometry_msgs.msg.TransformStamped() self.fdata_base = geometry_msgs.msg.TransformStamped() self.transform = tf.TransformBroadcaster() self.wrench = WrenchStamped() self.wrench_bl = WrenchStamped() def wrench_cb(self,msg): self.wrench = msg.wrench self.transform.sendTransform((0,0,-0.025),quaternion_from_euler(3.14, 0, 3.665195102, 'rxyz'),rospy.Time.now(),'/ft_debug_link','/arm_7_link') self.fdata.transform.translation.x = self.wrench.force.x self.fdata.transform.translation.y = self.wrench.force.y self.fdata.transform.translation.z = self.wrench.force.z try: if self.tf.frameExists("/dummy_link") and self.tf.frameExists("ft_debug_link"): t = self.tf.getLatestCommonTime("/dummy_link", "/ft_debug_link") (transform_ee_base_position,transform_ee_base_quaternion) = self.tf.lookupTransform("/dummy_link", '/ft_debug_link', t) #print transform_ee_base_position #print transform_ee_base_quaternion #print self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion) except(tf.LookupException,tf.ConnectivityException): print("TRANSFORMATION ERROR") sss.say(["error"]) #return 'failed' self.fdata_base.transform.translation.x =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[0,3])) self.fdata_base.transform.translation.y =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[1,3])) self.fdata_base.transform.translation.z =((self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,0] * self.fdata.transform.translation.x)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,1] * self.fdata.transform.translation.y)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,2] * self.fdata.transform.translation.z)+ (self.tf1.fromTranslationRotation(transform_ee_base_position,transform_ee_base_quaternion)[2,3])) self.wrench_bl.wrench.force.y = self.fdata_base.transform.translation.y self.wrench_bl.wrench.force.x = self.fdata_base.transform.translation.x self.wrench_bl.wrench.force.z = self.fdata_base.transform.translation.z self.wrench_bl.header.stamp = rospy.Time.now(); self.pub.publish(self.wrench_bl)
class TfNode: def __init__(self, *args): self.tf = TransformListener() self.sub = rospy.Subscriber( "/detectedObjs_primitive", thesis_visualization.msg.objectLocalization, self.__subCb, queue_size=1) self.msg = thesis_visualization.msg.objectLocalization() self.thread_sub = Thread(target=self.start_sub) def __subCb(self, msg): self.msg = msg print self.msg.modelList def start_sub(self): rospy.spin() def subscribe(self): self.thread_sub.start() def unregister_sub(self): self.sub.unregister() rospy.signal_shutdown("close subcriber") def example_function(self): if self.tf.frameExists("/world") and self.tf.frameExists( "/kinect2_depth"): t = self.tf.getLatestCommonTime("/world", "/kinect2_depth") p1 = geometry_msgs.msg.PoseStamped() p1.header.frame_id = "kinect2_depth" p1.pose.orientation.w = 1.0 # Neutral orientation p_in_base = self.tf.transformPose("/world", p1_) print "Position of the fingertip in the robot base:" print p_in_base def pcd2worldFrame(self, pcd): self.tf.transformPointCloud("world", pcd)
class GoToHuman(): global lastknown_human global lastknown_RAS lastknown_human = (1.0, 1.0) lastknown_RAS = (0.0, 0.0) def __init__(self): rospy.init_node('nav_test_human', anonymous=False) #Listeners # rospy.Subscriber("tf", tf, setRAS(tf.lookupTransform("/base_link")) self.tf = TransformListener() self.setRAS() # rospy.Subscriber("human/human/human", Human, setHuman(data)) locations = self.getLocation() human = locations[0] RAS = locations[1] if (human==RAS): print("Incorrect") #Shouldn't be same location, means it wasn't updated from listener msg = self.doMath(human[0], human[1], RAS[0], RAS[1]) #log stuff rospy.loginfo("Beginning goto_human service") rospy.spin() #what to do if shut down (e.g. ctrl + C or failure) rospy.on_shutdown(self.shutdown) return msg #message from moving RAS def doMath(self, h_x, h_y, r_x, r_y): #Do math, given location A and B, straight line between the two with 2ft gap between rospy.wait_for_service('goto_xy') #wait for service to start, need to start yourself (run file) goto_human = rospy.ServiceProxy('goto_xy', Goto_xy) rospy.wait_for_service('rotate') #wait for service to start, need to start yourself (run file) rotate_to_human = rospy.ServiceProxy('rotate', Rotate) if ((h_x == r_x) or (h_y == r_y)): # Simple -- Just keep it two feet away if (h_x == r_x): #horizontal match, change this to be more forgiving with in 5 degrees? #rosservice call goto_xy h_x, h_y-0.6096 horizontal = goto_human(h_x, h_y-0.6096) rotate_to_human(r_x, r_y, h_x, h_y) return horizontal.response else: goto(h_x-0.6096, h_y) #vertical match, change this to be more forgiving with in 5 degrees? #rosservice call goto_xy h_x-0.6096, h_y vert = goto_human(h_x-0.6096, h_y) rotate_to_human(r_x, r_y, h_x, h_y) return triangle.response else: # triangle -- equalatiral triangle with 0.6096m (2ft) longest size # and the legs would be 0.431m (1.41ft) each. #rosservice call goto_xy h_x-0.431, h_y-0.431 triangle = goto_human(h_x-0.431, h_y-0.431) rotate_to_human(r_x, r_y, h_x, h_y) return triangle.response def getLocation(self): return [lastknown_human, lastknown_RAS] def setRAS(self): #get data from tf listener #wiki.ros.org/tf/TfUsingPython if self.tf.frameExists("/base_link") and self.tf.frameExists("/map"): t = self.tf.getLatestCommonTime("/base_link", "/map") position, quaternion = self.tf.lookupTransform("/base_link", "/map", t) #position is tuple (x, y, z), quat (x, y, z, w) lastknown_RAS = (position[0], position[1]) def setHuman(data): #get data from Human listener, currently incorrect # lastknown_human = (data.x, data.y) lastknown_human = (2.0, 2.0) #testing with human 2m away from origin def shutdown(self): rospy.loginfo("Editing Service")
class GripperMarkers: _offset = 0.09 def __init__(self, side_prefix): self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers_' + self.side_prefix) self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.r_joint_names = ['r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'] self.l_joint_names = ['l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'] # Create a trajectory action client r_traj_contoller_name = None l_traj_contoller_name = None if self.side_prefix == 'r': r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() else: l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.ik = IK(side_prefix) self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker_' + self.side_prefix) self._im_server.applyChanges() print self.ik def get_ee_pose(self): from_frame = 'base_link' to_frame = self.side_prefix + '_wrist_roll_link' try: if self._tf_listener.frameExists(from_frame) and self._tf_listener.frameExists(to_frame): t = self._tf_listener.getLatestCommonTime(from_frame, to_frame) # t = rospy.Time.now() (pos, rot) = self._tf_listener.lookupTransform(to_frame, from_frame, t) # Note argument order :( else: rospy.logerr('TF frames do not exist; could not get end effector pose.') except Exception as err: rospy.logerr('Could not get end effector pose through TF.') rospy.logerr(err) pos = [1.0, 0.0, 1.0] rot = [0.0, 0.0, 0.0, 1.0] return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def move_to_pose_cb(self, feedback): rospy.loginfo('You pressed the `Move arm here` button from the menu.') '''Moves the arm to the desired joints''' print feedback time_to_joint = 2.0 positions = self.ik.get_ik_for_ee(feedback.pose) velocities = [0] * len(positions) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.points.append(JointTrajectoryPoint(positions=positions, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) if (self.side_prefix == 'r'): traj_goal.trajectory.joint_names = self.r_joint_names self.r_traj_action_client.send_goal(traj_goal) else: traj_goal.trajectory.joint_names = self.l_joint_names self.l_traj_action_client.send_goal(traj_goal) pass def marker_clicked_cb(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.ee_pose = feedback.pose self.update_viz() self._menu_handler.reApply(self._im_server) self._im_server.applyChanges() elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo('Changing visibility of the pose controls.') if (self.is_control_visible): self.is_control_visible = False else: self.is_control_visible = True else: rospy.loginfo('Unhandled event: ' + str(feedback.event_type)) def update_viz(self): menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = 'base_link' pose = self.ee_pose menu_control = self._add_gripper_marker(menu_control) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 text = 'x=' + str(pose.position.x) + ' y=' + str(pose.position.y) + ' x=' + str(pose.position.z) menu_control.markers.append(Marker(type=Marker.TEXT_VIEW_FACING, id=0, scale=Vector3(0, 0, 0.03), text=text, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = 'ik_target_marker_' + self.side_prefix int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker) int_marker.controls.append(menu_control) self._im_server.insert(int_marker, self.marker_clicked_cb) def _add_gripper_marker(self, control): is_hand_open=False if is_hand_open: angle = 28 * numpy.pi / 180.0 else: angle = 0 transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1, transform2) mesh1 = self._make_mesh_marker() mesh1.mesh_resource = ('package://pr2_description/meshes/gripper_v0/gripper_palm.dae') mesh1.pose.position.x = -GripperMarkers._offset mesh1.pose.orientation.w = 1 mesh2 = self._make_mesh_marker() mesh2.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh3 = self._make_mesh_marker() mesh3.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal) quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(numpy.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle)) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices(transform1,transform2) mesh4 = self._make_mesh_marker() mesh4.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh5 = self._make_mesh_marker() mesh5.mesh_resource = ('package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal) control.markers.append(mesh1) control.markers.append(mesh2) control.markers.append(mesh3) control.markers.append(mesh4) control.markers.append(mesh5) return control @staticmethod def get_pose_from_transform(transform): pos = transform[:3,3].copy() rot = tf.transformations.quaternion_from_matrix(transform) return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 print self ik_solution = self.ik.get_ik_for_ee(self.ee_pose) if (ik_solution is None): mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) return mesh def _add_6dof_marker(self, int_marker): is_fixed = True control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed) int_marker.controls.append(control) def _make_6dof_control(self, name, orientation, is_move, is_fixed): control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if (self.is_control_visible): if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control
#!/usr/bin/env python import rospy from tf import TransformListener import time rospy.init_node('tf_tester', anonymous=True) time.sleep(1) tf_listener = TransformListener() while True: frames = tf_listener.allFramesAsString() if frames: #1/0 pass if tf_listener.frameExists( "base_link"): #and tf_listener.frameExists("map"): t = tf_listener.getLatestCommonTime("odom", "map") position, quaternion = tf_listener.lookupTransform("odom", "map", t) print "odom->map", position, quaternion t = tf_listener.getLatestCommonTime("base_link", "odom") position, quaternion = tf_listener.lookupTransform( "base_link", "odom", t) print "base_link->odom", position, quaternion position, quaternion = tf_listener.lookupTransform( "base_link", "map", rospy.Time.now()) # Problems with time in the past print "base_link->map", position, quaternion break # time.sleep(1)
class AccurateDocking(RComponent): """ A class used to make a simple docking process """ def __init__(self): self.dock_action_name = "/rb1_base/pp_docker" self.move_action_name = "/rb1_base/move" self.robot_link = "rb1_base_base_footprint" self.pregoal_link = "laser_docking_pregoal_footprint" self.goal_link = "laser_docking_goal_footprint" RComponent.__init__(self) def ros_read_params(self): """Gets params from param server""" RComponent.ros_read_params(self) #self.command_name = rospy.get_param('~command_name', self.command_name) def ros_setup(self): """Creates and inits ROS components""" RComponent.ros_setup(self) self.tf = TransformListener() self.move_action_client = actionlib.SimpleActionClient( self.move_action_name, MoveAction) self.move_action_client.wait_for_server() self.dock_action_client = actionlib.SimpleActionClient( self.dock_action_name, DockAction) self.dock_action_client.wait_for_server() return 0 def ready_state(self): """Actions performed in ready state""" # Docking dock_goal = MoveActionGoal() dock_goal.goal.dock_frame = self.pregoal_link dock_goal.goal.robot_dock_frame = self.robot_link #dock_goal.goal.dock_frame = self.goal_link #dock_goal.goal.dock_offset.x = -0.5 self.dock_action_client.send_goal(dock_goal) self.dock_action_client.wait_for_result() if self.dock_action_client.get_result() == True: # Execute move if self.tf.frameExists(self.robot_link) and self.tf.frameExists( self.goal_link): # Get relative goal position t = self.tf.getLatestCommonTime(self.robot_link, self.goal_link) (position, quaternion) = self.tf.lookupTransform(self.robot_link, self.goal_link, t) (_, _, orientation) = euler_from_quaternion(quaternion) # Orientate robot move_goal = MoveActionGoal() move_goal.goal.goal.theta = orientation self.move_action_client.send_goal(move_goal) self.move_action_client.wait_for_result() if self.move_action_client.get_result() == True: # Get relative goal position t = self.tf.getLatestCommonTime(self.robot_link, self.goal_link) (position, quaternion) = self.tf.lookupTransform( self.robot_link, self.goal_link, t) (_, _, orientation) = euler_from_quaternion(quaternion) # Move forward move_goal = MoveActionGoal() move_goal.goal.goal.x = position[0] self.move_action_client.send_goal(move_goal) self.move_action_client.wait_for_result() else: rospy.ERROR("Move failed. TF are not available") else: rospy.ERROR("Docking failed") switch_to_state(State.SHUTDOWN_STATE) def shutdown(self): return RComponent.shutdown(self) def switch_to_state(self, new_state): """Performs the change of state""" return RComponent.switch_to_state(self, new_state)
class ttsconvert(): def __init__(self): rospy.init_node('google', anonymous=False) rospy.loginfo('Initializing') #soundhandle = SoundClient(blocking=True) self.goal_sent = False self.tf = TransformListener() # 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("Wait 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)) def speak(self): sound_client = SoundClient() G = nx.read_graphml( "/opt/ros/indigo/share/pocketsphinx/demo/Ourstuff/gSpeech.graphml", unicode) root = 'n1' current = root tracker = {'task': None, 'room': None} counter = 0 output = None sound_client.playWave('/home/turtlebot/wavFiles/beep.wav', blocking=False) while True: #counter = counter +1 # THE USER SPEAKS subprocess.call('./speech-rec.sh') # ANALYZE WHAT THE USER SAID f = open("words.log", "r+") line = f.readline() #output = None if line: if line[0] in 't': speech = line[12:].rstrip().lower() print speech if speech == "stop": break see = -1 for e in G.edges(current, data=True): expected = str(e[2].values()[0]) match = re.search(expected, speech) if match or expected == 'room': see = 1 current = e[1] output = G.node[current]['dialogue'] if (expected == 'guide'): tracker['task'] = 'guide' break #out of for loop elif (expected == 'deliver'): tracker['task'] = 'deliver' tts = gTTS( text= 'Please place your item on top of my shell.', lang='en') tts.save('/home/turtlebot/wavFiles/item.wav') rospy.loginfo( 'Turtlebot says place you item...') sound_client.playWave( '/home/turtlebot/wavFiles/item.wav', blocking=True) break #out of for loop elif (expected == 'task'): if (tracker['task'] == 'deliver'): tracker['task'] = 'guide' elif (tracker['task'] == 'guide'): tracker['task'] = 'deliver' tts = gTTS( text= 'Please place your item on top of my shell.', lang='en') tts.save( '/home/turtlebot/wavFiles/item.wav') rospy.loginfo( 'Turtlebot says place you item...') sound_client.playWave( '/home/turtlebot/wavFiles/item.wav', blocking=True) tracker['room'] = str(room_number[0]) output = output.replace( '*', tracker['room'], 1) #if (tracker['task'] is not None): output = output.replace( '#', tracker['task'], 1) elif (expected == 'room'): if any(char.isdigit() for char in speech): room_number = [ int(s) for s in speech.split() if s.isdigit() ] if (room_number[0] < 201 or room_number[0] > 207): tts = gTTS( text= 'The room is out of range. I can only go to room 201 to 207', lang='en') tts.save( '/home/turtlebot/wavFiles/oor.wav') rospy.loginfo( 'Turtlebot says room out of range...' ) sound_client.playWave( '/home/turtlebot/wavFiles/oor.wav', blocking=True) current = e[0] output = G.node[current]['dialogue'] else: tracker['room'] = str(room_number[0]) output = output.replace( '*', tracker['room'], 1) #if (tracker['task'] is not None): output = output.replace( '#', tracker['task'], 1) print "out of the loop" if see < 0: #tts = gTTS(text = 'Sorry, I did not understand that.', lang = 'en') #tts.save('/home/turtlebot/wavFiles/pardon.wav') rospy.loginfo('Turtlebot says I do not understand...') sound_client.playWave( '/home/turtlebot/wavFiles/pardon.wav', blocking=True) if output: tts = gTTS(text=output, lang='en') tts.save('/home/turtlebot/wavFiles/output.wav') rospy.loginfo('Turtlebot says I understood what you said...') sound_client.playWave('/home/turtlebot/wavFiles/output.wav', blocking=True) '''else: #tts = gTTS(text = 'Sorry, I did not understand that.', lang = 'en') #tts.save('/home/turtlebot/wavFiles/pardon.wav') rospy.loginfo('Turtlebot says I do not understand...') sound_client.playWave('/home/turtlebot/wavFiles/pardon.wav')''' # CHECK IF YOU HAVE TO GO OUT OF LOOP if (output == "Hold on! Let me calculate the path. Here I go!"): tts = gTTS(text='Byeeee', lang='en') tts.save('/home/turtlebot/wavFiles/bye.wav') rospy.loginfo('Turtlebot says end of conversation...') sound_client.playWave('/home/turtlebot/wavFiles/bye.wav', blocking=True) break return tracker def goto(self, pos, quat): print "goto started" sound_client = SoundClient() #wait for sound_play to connect to publishers otherwise, it will miss first published psg rospy.sleep(3) #3 seconds? tts = gTTS(text='Going to destination', lang='en') tts.save("/home/turtlebot/wavFiles/goingTo.wav") self.move_base.max_vel_x = 0.25 # Send a goal self.goal_sent = True if self.tf.frameExists("/base_link") and self.tf.frameExists("/map"): t = self.tf.getLatestCommonTime("/base_link", "/map") position, quaternion = self.tf.lookupTransform( "/base_link", "/map", t) rospy.loginfo(position[0]) goal = MoveBaseGoal() #if(os.path.isfile("/home/wavFiles/goingTo.wav") == False) #the path to gtts-cli.py might have to be changed based on setup #play the wav file rospy.loginfo("Playing goingTo.wav") sound_client.playWave("/home/turtlebot/wavFiles/goingTo.wav") goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose = Pose( Point(pos['x'], pos['y'], 0.000), Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4'])) # Start moving self.move_base.send_goal(goal) # Allow TurtleBot up to 60 seconds to complete task success = self.move_base.wait_for_result(rospy.Duration(120)) state = self.move_base.get_state() result = False if success and state == GoalStatus.SUCCEEDED: # We made it! result = True tts = gTTS(text='Yay, reached destination', lang='en') tts.save("/home/turtlebot/wavFiles/reachedTo.wav") sound_client.playWave("/home/turtlebot/wavFiles/reachedTo.wav") else: self.move_base.cancel_goal() self.goal_sent = False return result def shutdown(self): if self.goal_sent: self.move_base.cancel_goal() rospy.loginfo("Stop") rospy.sleep(1)
class MapWhiteout: def map_callback(self, map): print("received map!") self.map = map if self.pose is None: # uninitialized pose print("pose not received yet, passing on map...") else: grid = np.array(map.data) grid = np.resize(grid, (map.info.height, map.info.width)) res = map.info.resolution origin = np.array( [map.info.origin.position.x, map.info.origin.position.y]) m_center = self.pose #print m_center m_center_adj = m_center - origin #print m_center_adj cell_center = (m_center_adj / res) m_width = 0.4 cell_width = (m_width / res) y_start = int(cell_center[0] - cell_width / 2) y_stop = int(cell_center[0] + cell_width / 2) x_start = int(cell_center[1] - cell_width / 2) x_stop = int(cell_center[1] + cell_width / 2) grid[x_start:x_stop, y_start:y_stop].fill(0) #print "grid value: " + str(any(grid)) # print type(np.resize(grid, (1, map.info.height*map.info.width)).astype(int).tolist()[0]) map.data = np.resize(grid, (1, map.info.height * map.info.width)).astype( np.int8).tolist()[0] def __init__(self): rospy.init_node("map_whiteout") self.map = None self.pose = None self.pub = rospy.Publisher("/turtlebot/map", OccupancyGrid, latch=True, queue_size=1) self.sub1 = rospy.Subscriber("/map", OccupancyGrid, self.map_callback) self.tf = TransformListener() print("running map whiteout node...") r = rospy.Rate(1) while not rospy.is_shutdown(): try: if self.tf.frameExists("/map") and self.tf.frameExists( "/turtlebot/base_link"): t = self.tf.getLatestCommonTime("/map", "/turtlebot/base_link") pos, quat = self.tf.lookupTransform( "/map", "/turtlebot/base_link", t) print("got map to base link transform") self.pose = np.array([pos[0], pos[1]]) except: pass if self.map is not None: self.pub.publish(self.map) r.sleep()
class ttsconvert(): def __init__(self): tracker = {'room': None} # initiliaze rospy.init_node('ttsconvert', anonymous=False) self.tf = TransformListener() #rospy.loginfo('Example: Playing sounds in *blocking* mode.') soundhandle = SoundClient(blocking=True) self.goal_sent = False #rospy.loginfo('Good Morning.') #soundhandle.playWave('/home/turtlebot/wavFiles/start.wav') self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Wait 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)) # What to do you ctrl + c rospy.on_shutdown(self.shutdown) # Import the graphml to be read later G = nx.read_graphml("2nd_demo.graphml", unicode) root = 'n1' current = root # Clear words.log f = open("words.log", "r+") f.seek(0) f.truncate() # Clear clean.log with open("clean.log", "w"): pass # Constantly read from words.log for new lines while True: line = f.readline() # If the user said something if line: # The lines with dialogue all begin with a numerical value if line[0][:1] in '0123456789': # remove 9 numbers, colon, and space from the beginning, and any whitespace from the end of the line speech = line[11:].rstrip() print speech count = 0 # Search through all edges connected to the current node for e in G.edges(current, data=True): # If what was spoken matches the 'spoken' attribute of the edge if str(speech) == str(e[2].values()[0]): # Switch the current node to be the node the edge goes to current = e[1] # find '*' symbol in output string and and replace it with what user said stored in speech # Get the dialogue stored in the new node and save it in 'output' output = G.node[current]['dialogue'] if current == 'n7': output = output.replace('*', str(speech)) tracker['room'] = str(speech).lower() print 'OUTPUT: %s' % output tts = gTTS(text=output, lang='en') tts.save('/home/raeesa/Desktop/line.wav') subprocess.Popen([ 'gnome-terminal', '-x', 'bash', '-c', 'pacmd set-source-mute 2 1' ]) #rospy.loginfo('playing output') soundhandle.playWave( '/home/raeesa/Desktop/line.wav', blocking=True) #soundhandle.say(output) subprocess.Popen([ 'gnome-terminal', '-x', 'bash', '-c', 'pacmd set-source-mute 2 0' ]) # Add 'output' to the top of clean.log with open("clean.log", "r+") as g: # Read everything from clean.log into 'content' content = g.read() # Go to the top of the file g.seek(0, 0) # Write 'output' with 'content' appended to the end back to clean.log g.write(output.rstrip('\r\n') + '\n' + content) g.close() if current == 'n9': room = tracker.get('room') position = { 'x': goals.get(room, "201")[0], 'y': goals.get(room, "201")[1] } # Customize the following values so they are appropriate for your location #position = {'x': 5.12, 'y' : 2.72} quaternion = { 'r1': 0.000, 'r2': 0.000, 'r3': 0.000, 'r4': 1.000 } rospy.loginfo("Go to (%s, %s) pose", position['x'], position['y']) self.goto(position, quaternion) # If there are no outgoing edges from the current node, go back to the root if G.out_degree(current) == 0: current = root def goto(self, pos, quat): print "goto started" sound_client = SoundClient() #wait for sound_play to connect to publishers otherwise, it will miss first published psg rospy.sleep(3) #3 seconds? tts = gTTS(text='Going to destination', lang='en') tts.save("/home/turtlebot/wavFiles/goingTo.wav") # Send a goal self.goal_sent = True if self.tf.frameExists("/base_link") and self.tf.frameExists("/map"): t = self.tf.getLatestCommonTime("/base_link", "/map") position, quaternion = self.tf.lookupTransform( "/base_link", "/map", t) rospy.loginfo(position[0]) goal = MoveBaseGoal() #if(os.path.isfile("/home/wavFiles/goingTo.wav") == False) #the path to gtts-cli.py might have to be changed based on setup #play the wav file rospy.loginfo("Playing goingTo.wav") sound_client.playWave("/home/turtlebot/wavFiles/goingTo.wav") goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose = Pose( Point(pos['x'], pos['y'], 0.000), Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4'])) # Start moving self.move_base.send_goal(goal) # Allow TurtleBot up to 60 seconds to complete task success = self.move_base.wait_for_result(rospy.Duration(120)) state = self.move_base.get_state() result = False if success and state == GoalStatus.SUCCEEDED: # We made it! result = True tts = gTTS(text='Yay, reached destination', lang='en') tts.save("/home/turtlebot/wavFiles/reachedTo.wav") sound_client.playWave("/home/turtlebot/wavFiles/reachedTo.wav") else: self.move_base.cancel_goal() self.goal_sent = False return result def shutdown(self): if self.goal_sent: self.move_base.cancel_goal() rospy.loginfo("Stop") rospy.sleep(1)
class i2oNode(object): def __init__(self, *args, **kwargs): #pylint: disable=unused-argument rospy.init_node('i2oNode', anonymous=True) self.tf = TransformListener() # print str(dir(self.tf)) self.imu_to_baselink = None # PUB/SUB Setup self.pub = rospy.Publisher('imu2odom', Odometry, queue_size=10) self.init_sub = rospy.Subscriber('initialize_localization', Bool, queue_size=10) self.sub = rospy.Subscriber('imu', Imu, self.imu_callback) # state model self.baselink_model = State2D(0,0,0) self.imu_model = State2D(0,0,0) self.imu_zero = State2D(0,0,0) self.init_state = False # MAIN FUNCTION def listal(self): # listen-talk # run the node rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): rate.sleep() return 0 # === TRANSITION FUNCTION === def update_state_imu(self, state, imu_msg): # update state with new imu_information # Renamed state vars # (time - time) => duration dt = (imu_msg.header.stamp - state.timestamp()).to_sec() final_orientation = self.quat_to_euler(imu_msg.orientation) # jerk twerk = self.twerk(state.theta(), final_orientation, state.omega(), state.alpha(), dt) # intermediate vars avg_heading = self.theta_avg(twerk, state.theta(), state.omega(), state.alpha(), dt) ang_acc = self.ang_acc(twerk, state.alpha(), dt) # lin_vel((a_measured, v0, dt)) lin_vel = self.lin_vel(imu_msg.linear_acceleration.x, state.v(), dt) # assign final state state.stamp = imu_msg.header.timestamp state.point = self.linear_shift(state.x(), state.y(), avg_heading, state.v(), imu_msg.linear_acceleration.x, dt) # includes x, y state.heading = final_orientation # includes heading state.ang_acc = ang_acc # includes alpha state.ang_vel = Vector.from_Vector3(imu_msg.angular_velocity) # includes omega state.lin_vel = lin_vel # includes v state.lin_acc = Vector.from_Vector3(imu_msg.linear_acceleration) # includes a return state def update_state_init(self, state, imu_msg): # average state with new imu_information # Renamed state vars # (time - time) => duration dt = (imu_msg.header.stamp - state.timestamp()).to_sec() final_orientation = self.quat_to_euler(imu_msg.orientation) # jerk twerk = self.twerk(state.theta(), final_orientation, state.omega(), state.alpha(), dt) # intermediate vars avg_heading = self.theta_avg(twerk, state.theta(), state.omega(), state.alpha(), dt) ang_acc = self.ang_acc(twerk, state.alpha(), dt) # lin_vel((a_measured, v0, dt)) lin_vel = self.lin_vel(imu_msg.linear_acceleration.x, state.v(), dt) # average final state # so the values collected during the init are a moving average # this all should be pretty stable around 0, because the robot should be stopped # also, all state models should use this to zero-out their models state.point = Vector.average(state.point, self.linear_shift(state.x(), state.y(), avg_heading, state.v(), imu_msg.linear_acceleration.x, dt)) # includes x, y state.heading = state.heading/2 + final_orientation/2 # includes heading state.ang_acc = Vector.average(state.ang_acc, ang_acc) # includes alpha state.ang_vel = Vector.average(state.ang_vel, Vector.from_Vector3(imu_msg.angular_velocity)) # includes omega state.lin_vel = Vector.average(state.lin_vel, lin_vel) # includes v state.lin_acc = Vector.average(state.lin_acc, Vector.from_Vector3(imu_msg.linear_acceleration)) # includes a return state # DATA CALLBACK def imu_callback(self, imu_msg): # TODO(buckbaskin): implement initialization/zeroing, not necessarily here if self.init_state: # do initializations self.imu_zero = self.update_state_init(self.imu_zero, imu_msg) else: # lookup Transformation here because of unknown imu frame until callback if self.tf.frameExists('/base_link') and self.tf.frameExists(imu_msg.header.frame_id): #pylint: disable=no-member try: t = self.tf.getLatestCommonTime('/baselink', imu_msg.header.frame_id) #pylint: disable=no-member position, quaternion = self.tf.lookupTransform('/baselink', imu_msg.header.frame_id, t) #pylint: disable=no-member self.imu_to_baselink = [position, quaternion] rospy.loginfo(' >>> Transform Found') except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo(' <<< Error finding transform') # update state models if self.imu_to_baselink and len(self.imu_to_baselink) == 2: # the imu first updates the statemodel for the imu frame self.imu_model = self.update_state_imu(self.imu_model, imu_msg) # baselink_imu is imu data in the baselink frame baselink_imu = self.convert_to_baselink(imu_msg, self.imu_model, self.imu_to_baselink) # then the baselink (robot) model is updated using the transformed data self.baselink_model = self.update_state_imu(self.baselink_model, baselink_imu) # immediately publish data self.pub.publish(self.baselink_model.to_msg()) else: rospy.loginfo(' <<< Imu data not used. Saved transform not valid') def init_cb(self, b): if b.data: self.init_state = True else: self.init_state = False # DATA TRANSFORMATION def convert_to_baselink(self, imu_msg, imu_model, transform): # convert imu_msg from its frame to baselink frame # Assumption! TODO: I'm going to assume that the axis are aligned between frames to start # come back to this later, and rotate to align axis first # proposed path: # -> rorate-transform data (orientation, angular velocity, linear acceleration) to align with base_link # -> run the same code below ''' [sensor_msgs/Imu]: std_msgs/Header header - DONE uint32 seq time stamp string frame_id geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w float64[9] orientation_covariance geometry_msgs/Vector3 angular_velocity float64 x float64 y float64 z float64[9] angular_velocity_covariance geometry_msgs/Vector3 linear_acceleration - DONE float64 x float64 y float64 z float64[9] linear_acceleration_covariance - DONE ''' new_msg = Imu() # Header new_msg.header = imu_msg.header new_msg.header.frame_id = '/base_link' # Orientation (same based on Assumption! above) new_msg.orientation = imu_msg.orientation # including covariance, because same. will likely drop for rotation new_msg.orientation_covariance = imu_msg.orientation_covariance # Angular Velocity (same based on Assumption! above) new_msg.angular_velocity = imu_msg.angular_velocity # including covariance, because same. will likely drop for rotation new_msg.angular_velocity_covariance = imu_msg.angular_velocity_covariance # Linear Acceleration (not the same, even with assumption) new_msg.linear_acceleration = self.solid_body_translate_lin_acc(imu_msg.linear_acceleration, imu_model, transform) return new_msg def solid_body_translate_lin_acc(self, lin_acc, state, transform): # TODO(buckbaskin): check dynamics notes # Free Moving Rigid Body Kinematics # A-p = A-o + w-dot x R_prime-p + w x (w x R_prime-p) # We have A-p (the linear acceleration from the Imu) # We are solving for A-o (the acceleration for the baselink frame) # angular velocity is consistent across the rigid body (measured at Imu) # angular acceleration (w-dot) is interpolated from velocity data # R-p is the vector between the two (transform) # known: A-p, w-dot, w, R-p # solving for: A-o # A-o = A-p - w-dot x R-p - w x ( w x R-p ) Ap = Vector.from_Vector3(lin_acc) # v is a state_model.Vector position = transform[0] r = Vector.from_tf_position(position) Ap = lin_acc w_dot = state.alpha() w = state.omega() Ao = (Ap .minus(w_dot.cross(r)) .minus(w.cross(w.cross(r)))) return Ao.to_Vector3() # HELPER FUNCTIONS def theta_avg(self, twerk, theta0, w0, a0, dt): # twerk aka twisting jerk return twerk*pow(dt,3)/24 + a0*pow(dt,2)/6 + w0*dt/2 + theta0 def twerk(self, theta0, theta1, w0, a0, dt): twerk = ((((((theta1-theta0) / dt) - w0) / (dt / 2)) - a0) / (dt / 6)) return twerk def ang_acc(self, twerk, a0, dt): a1 = a0 + twerk*dt return Vector(0,0,a1) def lin_vel(self, a_measured, v0, dt): v1 = v0 + a_measured*dt return Vector(v1, 0, 0) def linear_shift(self, x0, y0, theta, v0, a, dt): v_avg = v0 + (v0 + a*dt) dx = v_avg*math.cos(theta) dy = v_avg*math.sin(theta) return Vector(x0+dx, y0+dy, 0) def quat_to_euler(self, quaternion): # returns the heading from a given quaternion q = quaternion q_array = [q.x, q.y, q.z, q.w] return euler_from_quaternion(q_array)[2] def euler_to_quat(self, euler_heading): # returns Quaternion ros message q = quaternion_from_euler(0,0,euler_heading) return Quaternion(w=q[3], x=q[0], y=q[1], z=q[2])
class KeyPointManager(object): def __init__(self): self.tf = TransformListener() self.keyPointList = list() def add(self, marker): for i in range(len(self.keyPointList)): if (self.keyPointList[i].id==marker.id): return position = self.transformMarkerToWorld(marker) k = KeyPoint(marker.id, Point(position[0], position[1], position[2]), Quaternion(0., 0., 0., 1.)) self.keyPointList.append(k) self.addWaypointMarker(k) rospy.loginfo('Marker is added to following position') rospy.loginfo(k.pose) pass def getWaypoints(self): waypoints = list() for i in range(len(self.keyPointList)): waypoints.append(self.keyPointList[i].pose); return waypoints def keyPointListComplete(self): if (len(self.keyPointList)==5): self.keyPointList.sort(key=lambda x: x.id, reverse=True) return True return False def markerHasValidId(self, marker): if (marker.id>=61) and (marker.id<=65): return True return False def transformMarkerToWorld(self, marker): markerTag = "ar_marker_"+str(marker.id ) rospy.loginfo(markerTag); if self.tf.frameExists("map") and self.tf.frameExists(markerTag): self.tf.waitForTransform("map", markerTag, rospy.Time(), rospy.Duration(4.0)) t = self.tf.getLatestCommonTime("map", markerTag) position, quaternion = self.tf.lookupTransform("map", markerTag, t) return self.shiftPoint( position, quaternion) def shiftPoint(self, position, quaternion): try: euler = euler_from_quaternion(quaternion) yaw = euler[2] tf_mat = np.array([[np.cos(yaw), -np.sin(yaw), 0, position[0]],[np.sin(yaw), np.cos(yaw), 0, position[1]],[0, 0, 1, 0],[0, 0, 0, 1]]) displacement = np.array([[1, 0, 0, 0],[0, 1, 0, 0.35],[0, 0, 1, 0],[0, 0, 0, 1]]) point_map = np.dot(tf_mat, displacement) position = (point_map[0,3], point_map[1,3], 0) except Exception as inst: print type(inst) # the exception instance print inst.args # arguments stored in .args print inst return position def addWaypointMarker(self, keyPoint): rospy.loginfo('Publishing marker') # Set up our waypoint markers marker_scale = 0.2 marker_lifetime = 0 # 0 is forever marker_ns = 'waypoints' marker_id = keyPoint.id marker_color = {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0} # Initialize the marker points list. self.waypoint_markers = Marker() self.waypoint_markers.ns = marker_ns self.waypoint_markers.id = marker_id self.waypoint_markers.type = Marker.CUBE_LIST self.waypoint_markers.action = Marker.ADD self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime) self.waypoint_markers.scale.x = marker_scale self.waypoint_markers.scale.y = marker_scale self.waypoint_markers.color.r = marker_color['r'] self.waypoint_markers.color.g = marker_color['g'] self.waypoint_markers.color.b = marker_color['b'] self.waypoint_markers.color.a = marker_color['a'] self.waypoint_markers.header.frame_id = 'map' self.waypoint_markers.header.stamp = rospy.Time.now() self.waypoint_markers.points = list() p = Point(keyPoint.pose.position.x, keyPoint.pose.position.y, keyPoint.pose.position.z) self.waypoint_markers.points.append(p) # Publish the waypoint markers self.marker_pub = rospy.Publisher('waypoint_markers', Marker) self.marker_pub.publish(self.waypoint_markers)
class Alexa: """Alexa Voice Control Module This ROS node connects to AWS IoT and sends commands to ROS. """ # set up constants def __init__(self): # create a new node rospy.init_node('alexa', anonymous=True) self.action_client = actionlib.SimpleActionClient( 'move_base', MoveBaseAction) self.tf = TransformListener() self.tag_id = rospy.get_param("~user_tag_id", '1001') self.FEET_TO_M = 0.3048 self.DEGREES_TO_RAD = math.pi / 180 root_dir = os.path.dirname(os.path.abspath(__file__)) # set up AWS constants self.host = rospy.get_param( "~host", 'a1vgqh9vgvjzyh.iot.us-east-1.amazonaws.com') self.rootCAPath = root_dir + rospy.get_param( "~rootCAPath", '/Certificates/root-CA.crt') self.certificatePath = root_dir + rospy.get_param( "~certificatePath", '/Certificates/Pi.cert.pem') self.privateKeyPath = root_dir + rospy.get_param( "~privateKeyPath", '/Certificates/Pi.private.key') self.clientId = rospy.get_param("~clientId", "Pi") self.topic = rospy.get_param("~topic", '/Transnavigators/Pi') # callback for receiving AWS message def _callback(self, client, userdata, message): """This callback receives a JSON message and outputs a ROS move_base command """ # extract data data_string = message.payload.decode("utf8").replace("'", '"') rospy.loginfo(data_string) data = json.loads(data_string) # publish data goal = MoveBaseGoal() goal.target_pose.header.frame_id = "base_footprint" goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.z = 0 goal.target_pose.pose.position = Point(0, 0, 0) goal.target_pose.pose.orientation = Quaternion(0, 0, 0, 1) if 'type' not in data: return # Move forward if data['type'] == 'forward': if 'distance' in data and 'distanceUnit' in data: goal.target_pose.pose.position.x = float(data['distance']) if data['distanceUnit'] == 'feet': goal.target_pose.pose.position.x *= self.FEET_TO_M else: return # Remove this if it works #goal.target_pose.pose.position.x = 100000.0 # Turn the wheelchair elif data['type'] == 'turn': if 'angle' in data: angle = float(data['angle']) if data['angleUnit'] == 'degrees': angle *= self.DEGREES_TO_RAD else: angle = 90 if 'direction' in data and data['direction'] == 'right': angle = -angle goal.target_pose.pose.orientation = Quaternion( 0, 0, math.sin(angle / 2), math.cos(angle / 2)) # Stop the wheelchair elif data['type'] == 'stop': goal.target_pose.pose.position = Point(0, 0, 0) goal.target_pose.pose.orientation = Quaternion(0, 0, 0, 1) # Go to the localino tag elif data['type'] == 'locateme' and self.tf.frameExists( "/base_link") and self.tf.frameExists("/tag_" + self.tag_id): t = self.tf.getLatestCommonTime("/base_link", '/tag_' + self.tag_id) pos, quat = self.tf.lookupTransform("/base_link", "/tag_" + self.tag_id, t) # Go to the target, rotation is the vector from here to the tag goal.target_pose.pose.position = pos dist = math.sqrt(pos.x**2 + pos.y**2) goal.target_pose.pose.orientation = Quaternion( 0, 0, pos.y / dist, pos.x / dist) # Go to the static landmark elif data['type'] == 'moveto' and self.tf.frameExists( "/map") and self.tf.frameExists("/%s" % str(data['location'])): t = self.tf.getLatestCommonTime("/base_link", "/%s" % str(data['location'])) pos, quat = self.tf.lookupTransform("/base_link", "/%s" % str(data['location']), t) # Go to the target, rotation is the vector from here to the location goal.target_pose.pose.position = pos dist = math.sqrt(pos.x**2 + pos.y**2) goal.target_pose.pose.orientation = Quaternion( 0, 0, pos.y / dist, pos.x / dist) else: rospy.logerr("Could not find transform from base_link to map") return self.action_client.send_goal(goal) def begin(self): """This function sets up the AWS IoT MQTT client, connects, and waits until ROS closes """ rospy.loginfo("Connecting to AWS") # Configure logging logger = logging.getLogger("AWSIoTPythonSDK.core") logger.setLevel(logging.WARN) stream_handler = logging.StreamHandler() formatter = logging.Formatter( '%(asctime)s - %(name)s - %(levelname)s - %(message)s') stream_handler.setFormatter(formatter) logger.addHandler(stream_handler) # MQTT Client documaentation: https://s3.amazonaws.com/aws-iot-device-sdk-python-docs/sphinx/html/index.html aws_iot_mqtt_client = AWSIoTMQTTClient(self.clientId) aws_iot_mqtt_client.configureEndpoint(self.host, 8883) aws_iot_mqtt_client.configureCredentials(self.rootCAPath, self.privateKeyPath, self.certificatePath) # AWSIoTMQTTClient connection configuration aws_iot_mqtt_client.configureAutoReconnectBackoffTime(1, 32, 20) aws_iot_mqtt_client.configureOfflinePublishQueueing( -1) # Infinite offline Publish queueing aws_iot_mqtt_client.configureDrainingFrequency(2) # Draining: 2 Hz aws_iot_mqtt_client.configureConnectDisconnectTimeout(10) # 10 sec aws_iot_mqtt_client.configureMQTTOperationTimeout(5) # 5 sec # Connect and subscribe to AWS IoT aws_iot_mqtt_client.connect() aws_iot_mqtt_client.subscribe(self.topic, 0, self._callback) rospy.loginfo("Connected to " + self.topic) self.action_client.wait_for_server() rospy.loginfo("Connected to the action server") # wait rospy.spin()
class pcScan_to_pcWorld(): def __init__(self): self.pcSub = rospy.Subscriber('/slam_cloud', pc, self.callback) self._tf = TransformListener() self.laser_pc = None self.mat_laser2world = None self.world_pc = None self.laser_pc_sync = None def callback(self, data): self.laser_pc = data def update(self): target_frame = "world" src_frame = "laser0_frame" if self._tf.frameExists(target_frame) and self._tf.frameExists( src_frame): try: self.mat_laser2world = self._tf.asMatrix( target_frame, self.laser_pc.header) except Exception as e: print(e) else: print('Unable to find:', self._tf.frameExists(target_frame), self._tf.frameExists(src_frame)) if not isinstance(self.laser_pc, pc) or not isinstance( self.mat_laser2world, np.ndarray): return try: lpc = copy.deepcopy(self.laser_pc) mat = copy.deepcopy(self.mat_laser2world) wpc = copy.deepcopy(self.laser_pc) for idx, p in enumerate(np.array(lpc.points)): col = np.array([[float(p.x), float(p.y), float(p.z), 1.0]]).T tran_p = np.matmul(mat, col).reshape((4)) wpc.points[idx].x = tran_p[X] wpc.points[idx].y = tran_p[Y] wpc.points[idx].z = tran_p[Z] self.world_pc = wpc self.laser_pc_sync = lpc except Exception as e: print(e) def get_Cell(self): thres = 0.05 #threshold to be considered as occupied pt = copy.deepcopy(self.world_pc.points) val = copy.deepcopy(self.world_pc.channels[0].values) #'intensities' mat = copy.deepcopy(self.mat_laser2world) pt = list(pt) val = np.array(val) trans_pt = [[p.x, p.y, p.z] for p in pt] trans_pt = np.array(trans_pt) occupied_world = trans_pt[val < thres] laser_pt = copy.deepcopy(self.laser_pc_sync.points) laser_pt = list(laser_pt) trans_laser_pt = [[p.x, p.y, p.z] for p in laser_pt] trans_laser_pt = np.array(trans_laser_pt) occupied_laser = trans_laser_pt[val < thres] # free = trans_pt[val >=thres] #Higher intensity laser means obstacle doesnt exist #Calculate free space based on distance from obstacles free_world = [] distance = 0.5 for pt in occupied_laser: a = abs(pt[X]) / distance b = abs(pt[Y]) / distance c = abs(pt[Z]) / distance num_of_sample = max(a, b, c) x = np.linspace(0, pt[X], num_of_sample) y = np.linspace(0, pt[Y], num_of_sample) z = np.linspace(0, pt[Z], num_of_sample) coords = zip(x, y, z) for c in coords: #Translate back to World Coordinate col = np.array([[float(c[X]), float(c[Y]), float(c[Z]), 1.0]]).T tran_p = np.matmul(mat, col).reshape((4)) wx = tran_p[X] wy = tran_p[Y] wz = tran_p[Z] free_world.append((wx, wy, wz)) print("Occupied", len(occupied_world), "Free", len(free_world)) return occupied_world, free_world def ready(self): return isinstance(self.world_pc, pc)
class calib: def __init__(self, *args): self.tf = TransformListener() self.detector_service = rospy.ServiceProxy("/fiducials/get_fiducials", DetectObjects) self.frame_camera_mount = rospy.get_param('~frame_camera_mount') self.frame_marker_mount = rospy.get_param('~frame_marker_mount') self.frame_marker = rospy.get_param('~frame_marker', "/marker") def compute(self): x_values = [] y_values = [] z_values = [] roll_values = [] pitch_values = [] yaw_values = [] count_success = 0 count_failed = 0 while count_success <= 15 and count_failed <= 100: try: rospy.wait_for_service("/fiducials/get_fiducials", 3.0) res = self.detector_service(DetectObjectsRequest()) if self.tf.frameExists(self.frame_camera_mount) and self.tf.frameExists(self.frame_marker_mount): t = self.tf.getLatestCommonTime(self.frame_camera_mount, self.frame_marker_mount) position, quaternion = self.tf.lookupTransform(self.frame_camera_mount, self.frame_marker_mount, t) euler = tf.transformations.euler_from_quaternion(quaternion) print '<origin xyz="'+str(position[0])+" "+str(position[1])+" "+str(position[2])+'" rpy="'+str(euler[0])+" "+str(euler[1])+" "+str(euler[2])+'" />' x_values.append(position[0]) y_values.append(position[1]) z_values.append(position[2]) roll_values.append(euler[0]) pitch_values.append(euler[1]) yaw_values.append(euler[2]) else: print "tf does not exist!", self.tf.frameExists(self.frame_camera_mount), self.tf.frameExists(self.frame_marker_mount) except: print "did not detect marker." print "count_success = ", count_success print "count_failed = ", count_failed count_failed += 1 continue count_success += 1 if len(x_values) < 5: print "to few detections, aborting" return x_avg_value = (float)(sum(x_values))/len(x_values) y_avg_value = (float)(sum(y_values))/len(y_values) z_avg_value = (float)(sum(z_values))/len(z_values) roll_avg_value = (float)(sum(roll_values))/len(roll_values) pitch_avg_value = (float)(sum(pitch_values))/len(pitch_values) yaw_avg_value = (float)(sum(yaw_values))/len(yaw_values) print "The average values (without hardcoding) <xyz> are : \n\n" + '<origin xyz="' + str(x_avg_value) +" "+ str(-y_avg_value) +" "+ str(z_avg_value) + '" rpy="' + str(roll_avg_value) + " " + str(pitch_avg_value) + " " + str(yaw_avg_value) + '" />\n' # HACK set some DOf to fixed values z_avg_value = 1.80 roll_avg_value = -3.1415926 pitch_avg_value = 0 yaw_avg_value = -3.1415926 print "The average values<xyz> are : \n\n" + '<origin xyz="' + str(x_avg_value) +" "+ str(-y_avg_value) +" "+ str(z_avg_value) + '" rpy="' + str(roll_avg_value) + " " + str(pitch_avg_value) + " " + str(yaw_avg_value) + '" />\n'
class FetchClient(object): def __init__(self): self.bfp = True self.robot = robot_interface.Robot_Interface() self.url = 'http://172.31.76.30:80/ThinkingQ/' self.joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] self.tf_listener = TransformListener() trfm = Transformer() self.r2b = trfm.transform_matrix_of_frames( 'head_camera_rgb_optical_frame', 'base_link') self.model = load_model("./model/0.988.h5") self.br = TransformBroadcaster() self.move_group = MoveGroupInterface("arm", "base_link") # self.pose_group = moveit_commander.MoveGroupCommander("arm") self.cam = camera.RGBD() self.position_cloud = None while True: try: cam_info = self.cam.read_info_data() if cam_info is not None: break except: rospy.logerr('camera info not recieved') self.pcm = PCM() self.pcm.fromCameraInfo(cam_info) def transform_matrix_of_frames(self, source_frame, target_frame): if self.tf_listener.frameExists(source_frame) and self.tf_listener.frameExists(target_frame): t = self.tf_listener.getLatestCommonTime( source_frame, target_frame) # Compute the transform from source to target position, quaternion = self.tf_listener.lookupTransform( target_frame, source_frame, t) transfrom_matrix = transformations.quaternion_matrix(quaternion) transfrom_matrix[0:3, 3] = position return transfrom_matrix else: return None def get_corner(self): print "get_corner" html = "" while html=="": image_time = time.time() img = self.cam.read_color_data() while img is None: img = self.cam.read_color_data() cv2.imwrite("image/fetch.jpg", img) files = {'IMAGE': (str(image_time) + '.jpg', open('image/fetch.jpg', 'rb'), 'image/png', {})} response = requests.request("POST", url=self.url, files=files, timeout=15000) html = response.text print html exit(0) a = html.split("\n")[1:-1] # print a pl = np.array([int(i) for i in a]) # print pl re = [] i = 0 while i < len(a): point = [] # print "pl i", pl[i] point.append(pl[i]) point.append(pl[i+1]) re.append(point) i = i+2 # print re return re def convert_camera_position_to_base_position(self, array): print 'r2b', self.r2b ret = np.dot(self.r2b[0:3, 0:3], array) # Rotate ret = ret + self.r2b[0:3, 3] return ret def update_position_cloud(self): pc = self.robot.camera.read_point_cloud() arr = pypcd.numpy_pc2.pointcloud2_to_array(pc, split_rgb=True) self.position_cloud = pypcd.numpy_pc2.get_xyz_points(arr, remove_nans=False) def get_position_by_pix(self, x, y): pc = self.robot.camera.read_point_cloud() arr = pypcd.numpy_pc2.pointcloud2_to_array(pc, split_rgb=True) po = pypcd.numpy_pc2.get_xyz_points(arr, remove_nans=False) return po[x, y] def broadcast_position(self, position, name): # while self.bfp: # print "111" a = transformations.quaternion_from_euler( ai=-2.355, aj=-3.14, ak=0.0) b = transformations.quaternion_from_euler( ai=0.0, aj=0.0, ak=1.57) base_rot = transformations.quaternion_multiply(a, b) self.br.sendTransform(position, base_rot, rospy.Time.now(), name, 'head_camera_rgb_optical_frame') def get_position_of_baselink(self, name): listener = TransformListener() rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: fetch_client.broadcast_position(cam_position, name) pose, rot = listener.lookupTransform('base_link', name, rospy.Time(0)) except Exception as e: print e continue rate.sleep() if pose: return pose def show_image(self, im): plt.imshow(im) pylab.show() def move_to_position(self, position): position[2] -= 0.24 x = np.round(np.array([position]), 4) * 100 print self.model.predict(x) joints = np.round(self.model.predict(x)[0], 4) / 100 print joints joint_3 = 0 - joints[1] - joints[2] + 1.5 disco_poses = [ [joints[0], joints[1], 0.0, joints[2], 0.0, joint_3, 1.5+joints[0]],# 1 close gripper ] for pose in disco_poses: if rospy.is_shutdown(): break print pose # Plans the joints in joint_names to angles in pose self.move_group.moveToJointPosition(self.joint_names, pose, wait=False) # Since we passed in wait=False above we need to wait here self.move_group.get_move_action().wait_for_result() result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Reach success!") # position = self.pose_group.get_current_pose("wrist_roll_link").pose.position # print(position) else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Reach fail") else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals # It should be called when a program is exiting so movement stops # self.move_group.get_move_action().cancel_all_goals() def move_to_position_up(self, position): position[2] -= 0.24 x = np.round(np.array([position]), 4) * 100 print self.model.predict(x) joints = np.round(self.model.predict(x)[0], 4) / 100 print joints joint_3 = 0 - joints[1] - joints[2] + 1.5 disco_poses = [ [joints[0], joints[1], 0.0, joints[2], 0.0, joint_3 , 0-joints[1]-joints[2]], #up ] for pose in disco_poses: if rospy.is_shutdown(): break print pose # Plans the joints in joint_names to angles in pose self.move_group.moveToJointPosition(self.joint_names, pose, wait=False) # Since we passed in wait=False above we need to wait here self.move_group.get_move_action().wait_for_result() result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Reach success!") # position = self.pose_group.get_current_pose("wrist_roll_link").pose.position # print(position) else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Reach fail") else: rospy.logerr("MoveIt! failure no result returned.") def move_to_position_with_up(self, position): position[2] -= 0.24 x = np.round(np.array([position]), 4) * 100 print self.model.predict(x) joints = np.round(self.model.predict(x)[0], 4) / 100 print joints joint_3 = 0 - joints[1] - joints[2] + 1.5 disco_poses = [ [joints[0], joints[1], 0.0, joints[2] - 0.6, 0.0, joint_3 + 0.6, 1.5+joints[0]], #up [joints[0], joints[1], 0.0, joints[2], 0.0, joint_3, 1.5+joints[0]],# 1 close gripper ] for pose in disco_poses: if rospy.is_shutdown(): break print pose # Plans the joints in joint_names to angles in pose self.move_group.moveToJointPosition(self.joint_names, pose, wait=False) # Since we passed in wait=False above we need to wait here self.move_group.get_move_action().wait_for_result() result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Reach success!") # position = self.pose_group.get_current_pose("wrist_roll_link").pose.position # print(position) else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Reach fail") else: rospy.logerr("MoveIt! failure no result returned.") def stow(self): disco_poses = [ [-0.0, -0.7, 0.0, 0.5, 0.0, 1.5, 1.3], [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]#stow ] for pose in disco_poses: if rospy.is_shutdown(): break print pose # Plans the joints in joint_names to angles in pose self.move_group.moveToJointPosition(self.joint_names, pose, wait=False) # Since we passed in wait=False above we need to wait here self.move_group.get_move_action().wait_for_result() result = self.move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Reach success!") if disco_poses.index(pose) == 1: self.robot.open_gripper() # position = self.pose_group.get_current_pose("wrist_roll_link").pose.position # print(position) else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Reach fail") else: rospy.logerr("MoveIt! failure no result returned.")
class Bag2UosConverter: def __init__(self, rootdir, scale_factor, transform_pitch_angle): rospy.init_node('bad_to_uos_convert', anonymous=False) rospy.Subscriber('velotime_points', PointCloud2, self.processMsg) self.counter = 0 self.rootdir = rootdir self.scale_factor = scale_factor self.tf = TransformListener() self.init = True self.transform_pitch_angle = math.radians(transform_pitch_angle) pass def transformPoint(self,x ,y , z): yy = y xx = math.cos(self.transform_pitch_angle) * x + math.sin(self.transform_pitch_angle) * z zz = -math.sin(self.transform_pitch_angle) * x + math.cos(self.transform_pitch_angle) * z return xx, yy, zz def run(self): rospy.spin() pass def processMsg(self, msg): print 'data arrived' # print list(cloud) # print self.tf.frameExists("velodyne") , self.tf.frameExists("odom") if self.tf.frameExists("world") and self.tf.frameExists("odom"): t = self.tf.getLatestCommonTime("world", "odom") position, quat= self.tf.lookupTransform("world", "odom", t) print position, quat euler = tf.transformations.euler_from_quaternion(quat) # heading_rad = math.degrees(euler[2]) heading_rad = euler[2] print heading_rad cloud = pc2.read_points(msg, skip_nans=True) self.saveData(list(cloud)) self.savePose(position[0],position[1],0,0,0,heading_rad) self.counter = self.counter + 1 if self.init: self.init = False cloud = pc2.read_points(msg, skip_nans=True) self.saveData(list(cloud)) self.savePose(0,0,0,0,0,0) self.counter = self.counter + 1 def saveData(self, cloud): file_string = 'scan'+format(self.counter,'03d') fullfilename_data = self.rootdir + file_string + '.3d' fh_data = open(fullfilename_data, 'w') for (x,y,z,intensity,ring) in cloud: # print x,y,z x, y, z = self.transformPoint(x, y, z) x = x*self.scale_factor y = y*self.scale_factor z = z*self.scale_factor str_ = format(x, '.1f') + ' ' + format(z, '.1f') + ' ' + format(y, '.1f') + '\n' fh_data.write(str_) fh_data.close() def savePose(self, x, y, z, roll, pitch, yaw): file_string = 'scan'+format(self.counter,'03d') fullfilename_pose = self.rootdir + file_string + '.pose' fh_pose = open(fullfilename_pose, 'w') x = x*self.scale_factor y = y*self.scale_factor z = z*self.scale_factor roll = roll*180/math.pi yaw = yaw*180/math.pi pitch = pitch*180/math.pi str_first_line = format(x, '.1f') + ' ' + format(-z, '.1f') + ' ' + format(y, '.1f') + '\n' str_2nd_line = format(roll, '.1f') + ' ' + format(-yaw, '.1f') + ' ' + format(pitch, '.1f') + '\n' print str_first_line+str_2nd_line fh_pose.writelines([str_first_line, str_2nd_line]) fh_pose.close()
class Movement: def __init__(self, main=None): self.main = main self.map_ = Map() self.tf = TransformListener() # Last n visited locations self.visited = deque(self.map_.locations, 3) # MoveBaseGoal self.pos = MoveBaseGoal() self.pos.target_pose.pose.orientation.w = 1.0 self.pos.target_pose.header.frame_id = '/map' # SimpleActionClient self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction) self.client.wait_for_server() self.longest_waiting_time = rospy.Duration(10) # Clear costmaps rospy.wait_for_service('/move_base/clear_costmaps') self.clear_costmaps = rospy.ServiceProxy('/move_base/clear_costmaps', Empty) # Register listener for motor availability and odometry. self.disabled = False rospy.wait_for_message("/mobile_base/commands/motor_power", MotorPower) rospy.Subscriber("/mobile_base/commands/motor_power", MotorPower, self._get_motor_info) self.odom = 0.0 rospy.wait_for_message("/odom", Odometry) rospy.Subscriber('/odom', Odometry, self._get_odom_data) # Teleop self.teleop = rospy.Publisher('/cmd_vel_mux/input/teleop', Twist, queue_size=1) self.TELEOP_X_MAX = 0.4 self.TELEOP_X_SCALE = 1.0 self.TELEOP_Z_MAX = 1.3 self.TELEOP_Z_SCALE = 0.6 self.TELEOP_SPEED = 0.8 def abort(self): self.client.cancel_all_goals() # goal: Point() def _move_to(self, goal): self.clear_costmaps() self.pos.target_pose.pose.position = goal self.pos.target_pose.header.stamp = rospy.Time.now() self.client.send_goal(self.pos) # portal: key of Map.points def _move_to_portal(self, portal): goal = self.map_.points[portal] self._move_to(goal) def explore(self): portal = random.choice(self.map_.locations) while portal in self.visited: portal = random.choice(self.map_.locations) print("Explore: " + portal) self._move_to_portal(portal) while self.client.get_state() == GoalStatus.PENDING or \ self.client.get_state() == GoalStatus.ACTIVE: time.sleep(0.05) if self.client.get_state() != GoalStatus.SUCCEEDED: print("Failed to explore " + portal) else: self.visited.append(portal) # Do a 360-degree rotation. teleop_cmd = Twist() teleop_cmd.angular.z = 2.0 self.teleop.publish(teleop_cmd) def follow(self, pos): teleop_cmd = Twist() # No rotate if tag is in the middle. if abs(pos.x) < 0.1: teleop_cmd.angular.z = 0.0 else: teleop_cmd.angular.z = -1 * pos.x / self.TELEOP_X_MAX * \ self.TELEOP_X_SCALE * self.TELEOP_SPEED # Break if too close. if pos.z < 0.4: teleop_cmd.linear.x = 0.0 else: teleop_cmd.linear.x = pos.z / self.TELEOP_Z_MAX * \ self.TELEOP_Z_SCALE * self.TELEOP_SPEED print("Follow: distance = " + str(pos.z)) self.teleop.publish(teleop_cmd) def rotate_to(self, target, direction): print("Now at: " + str(self.odom) + ". Rotate to: " + str(target)) teleop_cmd = Twist() teleop_cmd.angular.z = direction * 0.5 while not self.main.detector.detected and abs(self.odom - target) > 0.01: time.sleep(0.2) self.teleop.publish(teleop_cmd) def get_position(self): if self.tf.frameExists("/base_link") and self.tf.frameExists("/map"): t = rospy.Time(0) self.tf.waitForTransform("/base_link", "/map", t, rospy.Duration(4.0)) position, rotation = self.tf.lookupTransform("/base_link", "/map", t) return position else: print("Frame doesn't exist.") def _get_motor_info(self, data): """Callback function of subscriber. Args: data (MotorPower) """ # print("Got motor info!") if data.state == MotorPower.ON: self.disabled = False else: self.disabled = True # print(data) def _get_odom_data(self, data): self.odom = data.pose.pose.orientation.z
def run(self, aut_path=None): """Intialize all NL components.""" # pylint: disable=W0603 global WORLD_KNOWLEDGE # Start the NL pipeline if not aut_path: print "Starting NL pipeline..." init_pipes() else: print "Skipping loading nlpipeline because an automaton was loaded" # Start the ROS node print "Initializing ROS node..." rospy.init_node('nlproxy') # Set up the state mgr and its callbacks print "Starting state manager..." self.state_mgr = StateManager(self) self.state_mgr.set_basedir(LTLGEN_BASE_DIR) # Load the automaton if needed if aut_path: self.state_mgr.load_test_automaton(aut_path, False) # Create world knowledge print "Starting knowledge..." WORLD_KNOWLEDGE = Knowledge(self) self.state_mgr.world_knowledge = WORLD_KNOWLEDGE # Wait a little for ROS to avoid timing startup issues. print "Waiting for ROS node to settle..." time.sleep(1) # Set up ROS listening print "Subscribing to notifications..." # Set up state manager's sending and listening pub = add_ltl_publisher() self.state_mgr.set_publisher(pub) add_subscriber(LTL_ENVIRONMENT_TOPIC, self.state_mgr.process_sensor_data) add_subscriber(LTL_ENVIRONMENT_TOPIC, WORLD_KNOWLEDGE.process_sensor_data) # Create the input comm_proxy and iPad connections print "Starting comm_proxy..." self.comm_proxy = CallbackSocket(self.text_port) self.comm_proxy.register_callback(self.process_text) self.ipad_conn = iPadConnection(self.map_port) self.ipad_conn.register_rename_callback(rename_entity) self.ipad_conn.register_text_callback(self.process_text) # TODO Add highlight callback self.map_proxy = MapProxy(self.ipad_conn) add_subscriber(LTL_ENVIRONMENT_TOPIC, self.ipad_conn.add_icons) WORLD_KNOWLEDGE.map_proxy = self.map_proxy # Set up odometry forwarding to the ipad tf = TransformListener() while not tf.frameExists("/map"): rospy.logwarn("Not ready for transforms yet") rospy.sleep(1.0) position_proxy = RobotPositionProxy(self.ipad_conn, tf) rospy.Subscriber(ODOM_TOPIC, Odometry, position_proxy.forward_position) # Set up getting directions direction_proxy = DirectionProxy() rospy.Subscriber(LOCATION_TOPIC, String, direction_proxy.set_location) WORLD_KNOWLEDGE.direction_proxy = direction_proxy print "NLMaster startup complete!" print "*" * 80 # Finally, wait for input before quitting try: while True: text = raw_input("").strip() if text == "q": break except (KeyboardInterrupt, EOFError): pass except: raise finally: # Only shutdown the pipeline if we actually were taking language input if not aut_path: print "Shutting down NL pipeline..." close_pipes() self.comm_proxy.shutdown() self.ipad_conn.shutdown() sys.exit()
class ObjectMap: def __init__(self): self.result_pub_ = rospy.Publisher("object_map", MarkerArray, queue_size=10) # subscribe object detections from current frame self.convex_hull_sub_ = rospy.Subscriber("object_convex_hull", MarkerArray, self.convex_hull_callback, queue_size=10) self.people_sub_ = rospy.Subscriber("/hdl_people_tracking_nodelet/markers", MarkerArray, self.people_callback, queue_size=10) ''' On the /hdl_people_tracking_nodelet/markers topic - the first marker is a CUBE_LIST - people poses are in marker_array.markers[0].points - following markers in marker_array are text ''' self.ground_plane_height_ = -0.35 self.object_map_ = [] self.iou_thresh_ = 0.05 # iou threshold for data association self.dist_thresh_ = 0.6 # the distance from a person to an object for the object to be considered as occupied self.need_clean_thresh_ = 80 # how many frame an object is occupied make it needs to be cleaned self.duplicate_iou_thresh_ = 0.8 # threshold to view two detections as duplicate in the same frame self.tf_listener_ = TransformListener() def publish_convex_hull_marker(self): ''' Publish the object map ''' marker_array = MarkerArray() for i in range(len(self.object_map_)): line_strip = Marker() line_strip.header.frame_id = "map" line_strip.header.stamp = rospy.Time.now() line_strip.ns = "object_map" line_strip.action = Marker.ADD line_strip.pose.orientation.w = 1.0 line_strip.id = i line_strip.type = Marker.LINE_STRIP line_strip.scale.x = 0.05 line_strip.color.g = 1.0 if self.object_map_[i].need_clean_ == True: line_strip.color.r = 1.0 elif self.object_map_[i].occupied_ > 0: line_strip.color.r = self.object_map_[i].occupied_ / float(self.need_clean_thresh_) else: line_strip.color.b = 1.0 line_strip.color.a = 1.0 for j in range(self.object_map_[i].points_.shape[0]): point = Point() point.x = self.object_map_[i].points_[j, 0] point.y = self.object_map_[i].points_[j, 1] point.z = 0 line_strip.points.append(point) point = Point() point.x = self.object_map_[i].points_[0, 0] point.y = self.object_map_[i].points_[0, 1] point.z = 0 line_strip.points.append(point) line_strip.lifetime = rospy.Duration(0) marker_array.markers.append(line_strip) # publish class tag class_tag = copy.deepcopy(self.object_map_[i].class_text_) class_tag.header = copy.deepcopy(line_strip.header) class_tag.ns = "class_tag" class_tag.id = copy.deepcopy(line_strip.id) class_tag.lifetime = rospy.Duration(0) marker_array.markers.append(class_tag) # Add 'Need Disinfection' tag if self.object_map_[i].need_clean_ == True: text_marker = Marker() text_marker.header = copy.deepcopy(line_strip.header) text_marker.action = Marker.ADD text_marker.ns = "disinfection_tag" text_marker.id = copy.deepcopy(line_strip.id) text_marker.lifetime = rospy.Duration(0) text_marker.type = Marker.TEXT_VIEW_FACING text_marker.scale.z = 0.2 text_marker.pose.position = copy.deepcopy(line_strip.points[0]) text_marker.pose.position.z += 0.5 text_marker.color.r = 1.0 text_marker.color.g = 1.0 text_marker.color.a = 1.0 text_marker.text = "Need Disinfection" marker_array.markers.append(text_marker) self.result_pub_.publish(marker_array) def calculate_iou(self, polygon_1, polygon_2): ''' Calculate IoU of two polygons Args: polygon_1/polygon_2: polygons as 2D numpy array ''' poly_1 = shapely.geometry.Polygon(polygon_1) poly_2 = shapely.geometry.Polygon(polygon_2) iou = poly_1.intersection(poly_2).area / poly_1.union(poly_2).area return iou def data_association(self, current_frame_detection_tuple): ''' Implement the data association pipeline ''' # create a matrix of IoU scores # rows are detection list, columns are map list ''' Example: Detection\Map Object 0 Object 1 Object 2 Detection A IoU = 0 0 0 Detection B 0.56 0 0 Detection C 0 0.77 0 Object 2 in the map is not matches Detection A is a new object ''' iou_matrix = np.zeros((len(current_frame_detection_tuple), len(self.object_map_))) for i in range(iou_matrix.shape[0]): for j in range(iou_matrix.shape[1]): # calculate iou and fill the matrix iou = self.calculate_iou(current_frame_detection_tuple[i][0], self.object_map_[j].points_) # it can be a new detection is iou is below the threshold if iou <= self.iou_thresh_: iou = 0 iou_matrix[i, j] = -iou # fill with negative value as cost new_detection_idx = [] for i in range(iou_matrix.shape[0]): # find new detections if np.sum(iou_matrix[i, :]) == 0: # this is a new detection self.object_map_.append(ObjectMarker(current_frame_detection_tuple[i][0], \ current_frame_detection_tuple[i][1])) new_detection_idx.append(i) not_matched_objects = [] for j in range(iou_matrix.shape[1]): # find not matched objects if np.sum(iou_matrix[:, j]) == 0: not_matched_objects.append(j) # run the hungarian algorithm row_ind, col_ind = linear_sum_assignment(iou_matrix) for idx in range(row_ind.shape[0]): if (row_ind[idx] in new_detection_idx) or (col_ind[idx] in not_matched_objects): # either no match or new detection case continue matched_detection = row_ind[idx] matched_object_in_map = col_ind[idx] # if the match iou is too small, do nothing # caveat: add negative sign here if -iou_matrix[matched_detection, matched_object_in_map] < self.iou_thresh_: continue # otherwise, get a new convex hull self.object_map_[matched_object_in_map].add_associated_times() new_points = np.append(self.object_map_[matched_object_in_map].points_, \ current_frame_detection_tuple[matched_detection][0],\ axis = 0) new_convex_hull = ConvexHull(new_points) # update the object self.object_map_[matched_object_in_map].update_points(new_points[new_convex_hull.vertices, :]) def check_duplicate(self, polygon_1, polygon_2): ''' Check if two detections are duplicate by checking whether one convex hull is (almost) completely inside the other one Args: polygon_1/polygon_2: polygons as 2D numpy array ''' poly_1 = shapely.geometry.Polygon(polygon_1) poly_2 = shapely.geometry.Polygon(polygon_2) intersection = poly_1.intersection(poly_2).area if (intersection / poly_1.area) > self.duplicate_iou_thresh_ \ or (intersection / poly_2.area) > self.duplicate_iou_thresh_: return True else: return False def remove_duplication_detection(self, current_frame_detection_tuple): ''' Remove duplicate detection and combine them into one convex hull ''' output = [] for i in range(len(current_frame_detection_tuple)): no_duplicate = True for j in range(i + 1, len(current_frame_detection_tuple)): if self.check_duplicate(current_frame_detection_tuple[i][0], current_frame_detection_tuple[j][0]): no_duplicate = False new_points = np.append(current_frame_detection_tuple[i][0], \ current_frame_detection_tuple[j][0],\ axis = 0) new_convex_hull = ConvexHull(new_points) # update the new convex hull to the latter one # it will then be added to the output when the loop traverse to there current_frame_detection_tuple[j][0] = new_points[new_convex_hull.vertices, :] if no_duplicate: output.append(current_frame_detection_tuple[i]) return output def convex_hull_callback(self, data): # subcribe to all the objects (convex hull) in current frame current_frame_detection_tuple = [] # stores all the objects in this frame, a list of numpy arrays data_idx = 0 while data_idx < len(data.markers): object_convex_hull = np.empty((0,2), float) for point in data.markers[data_idx].points: # add each point into the list object_convex_hull = np.append(object_convex_hull, [[point.x, point.y]], axis=0) current_frame_detection_tuple.append([object_convex_hull, data.markers[data_idx + 1]]) data_idx += 2 if (len(current_frame_detection_tuple) == 0): return current_frame_detection_tuple = self.remove_duplication_detection(current_frame_detection_tuple) # do data association using the Hungarian algorithm if len(self.object_map_) == 0: # initailize the map for det in current_frame_detection_tuple: self.object_map_.append(ObjectMarker(det[0], det[1])) return else: self.data_association(current_frame_detection_tuple) def people_callback(self, data): # traverse all the objects in the map # if a person is within a specific distance, add 1 to occupied counting # if hasn't initialized yet, return if len(self.object_map_) == 0: return if self.tf_listener_.frameExists("map") and self.tf_listener_.frameExists("velodyne"): t = self.tf_listener_.getLatestCommonTime("map", "velodyne") # store all the person person_list = [] for person_marker in data.markers[0].points: # transform to map frame pos_in_velodyne = PoseStamped() pos_in_velodyne.pose.position.x = person_marker.x pos_in_velodyne.pose.position.y = person_marker.y pos_in_velodyne.header.frame_id = "velodyne" pos_in_velodyne.pose.orientation.w = 1.0 pos_in_map = self.tf_listener_.transformPose("map", pos_in_velodyne) person = shapely.geometry.Point(pos_in_map.pose.position.x,\ pos_in_map.pose.position.y) person_list.append(person) for obj_idx in range(len(self.object_map_)): polygon = shapely.geometry.Polygon(self.object_map_[obj_idx].points_) if_occupied = False for person in person_list: distance = person.distance(polygon) if distance < self.dist_thresh_: if_occupied = True if if_occupied == True: self.object_map_[obj_idx].add_occupied_times() else: self.object_map_[obj_idx].reset_occupied_times() for idx in range(len(self.object_map_)): # if an object has been occupied for more than a certain number of frames # set need clean to be true if self.object_map_[idx].occupied_ > self.need_clean_thresh_: self.object_map_[idx].set_need_clean()
class SLAM(object): def __init__(self): rospy.Subscriber('/map', OccupancyGrid, self.callback) self._tf = TransformListener() self._occupancy_grid = None self._pose = np.array([np.nan, np.nan, np.nan], dtype=np.float32) def callback(self, msg): pass def update(self): # Get pose w.r.t. map. r0_pose = self.get_pose(LEADER) r1_pose = self.get_pose(FOLLOWER_1) r2_pose = self.get_pose(FOLLOWER_2) print('leader') print('\t X: ', r0_pose[X]) print('\t Y: ', r0_pose[Y]) print('\t YAW: ', r0_pose[YAW]) print() print('follower 1') print('\t X: ', r1_pose[X]) print('\t Y: ', r1_pose[Y]) print('\t YAW: ', r1_pose[YAW]) print() print('follower 2') print('\t X: ', r2_pose[X]) print('\t Y: ', r2_pose[Y]) print('\t YAW: ', r2_pose[YAW]) print() print() print() def get_pose(self, robot): a = 'occupancy_grid' b = robot + '/base_link' if self._tf.frameExists(a) and self._tf.frameExists(b): try: t = rospy.Time(0) position, orientation = self._tf.lookupTransform('/' + a, '/' + b, t) pose = np.zeros(3, dtype=np.float32) pose[X] = position[X] pose[Y] = position[Y] _, _, pose[YAW] = euler_from_quaternion(orientation) return pose except Exception as e: print(e) else: print('Unable to find:', self._tf.frameExists(a), self._tf.frameExists(b)) @property def ready(self): return self._occupancy_grid is not None and not np.isnan(self._pose[0]) @property def pose(self): return self._pose @property def occupancy_grid(self): return self._occupancy_grid
class Inchworm(object): """ Default class to subscribe and publish to orthosis robots """ def __init__(self, namespace="/", timestep=0.01): """ Initialize the class with the namespace and timestep as params """ super(Inchworm, self).__init__() self._default_pub_rate = 10000 self._namespace = namespace self._timestep = timestep self.tf = TransformListener() self._is_set_point_ctrl = bool( rospy.get_param(namespace + "set_point_enable")) self._is_set_point_ctrl = True self._walk = bool(rospy.get_param(namespace + "default_conf")) self._n_joints = int(rospy.get_param(namespace + "n_joints")) # Movable joints self._m_joints = rospy.get_param(namespace + "parameters/control/joints") # self._n_joints = len(self._m_joints) self._m_joints_dict = { typ: rospy.get_param(namespace + "all_joints/{}".format(typ)) for typ in urdf.Joint.TYPES } self._fixed_joint_names = [] self._floating_and_unknown_joint_names = [] for k, v in self._m_joints_dict.items(): if len(v) < 1: del self._m_joints_dict[k] else: if k == 'fixed': self._fixed_joint_names = v del self._m_joints_dict[k] elif k == 'floating' or k == 'unknown': self._floating_and_unknown_joint_names = v del self._m_joints_dict[k] # del self._floating_and_unknown_joint_names[0] self._links = {} for typ, joints in self._m_joints_dict.items(): for joint_i, joint in enumerate(joints): if joint not in self._links.keys(): self._links[joint] = rospy.get_param( namespace + "joints/{}/{}".format(typ, joint_i)) self._fixed_links_and_joints = { joint: rospy.get_param(namespace + "joints/fixed/{}".format(joint_i)) for joint_i, joint in enumerate(self._fixed_joint_names) } self.initializeRBDLModel() self._M = np.zeros([self._n_joints, self._n_joints]) self._C = np.zeros([self._n_joints, self._n_joints]) self._scale = rospy.get_param(namespace + "scale") # Init joint states to zero self._joint_states = { joint: [1.0, 2.0, 3.0] for joint_i, joint in enumerate(self._m_joints) } self._joint_limits = { joint: rospy.get_param(namespace + "joint/limits/{}".format(joint)) for joint in self._m_joints } # print self._joint_limits self.subToJointStates() # Setup effort publishers self._effort_flag = bool( rospy.get_param(namespace + "is_effort_enabled")) if self._effort_flag: self._joint_effort_pubs = { joint: rospy.Publisher( (namespace + "control/config/joint_effort_controller_joint_{}/command". format(joint_i)), Float64, queue_size=10) for joint_i, joint in enumerate(self._m_joints) } else: self._joint_effort_pubs = { joint: rospy.Publisher( (namespace + "control/config/joint_position_controller_joint{}/command" .format(joint_i)), Float64, queue_size=10) for joint_i, joint in enumerate(self._m_joints) } # Publish initial efforts to reach home position? for k, v in self._joint_effort_pubs.items(): cmd = 0 self._joint_effort_pubs[k].publish(cmd) return def jointStatesCb(self, data): """ Joint States Callback """ for joint_i, joint in enumerate(self._m_joints): index = data.name.index(joint) msg = [ data.position[index], data.velocity[index], data.effort[index] ] self._joint_states[joint] = msg return def subToJointStates(self): """ Subscribe to joint state messages if published """ self._joint_states_sub = rospy.Subscriber( self._namespace + "joint_states", JointState, self.jointStatesCb) return def publishJointEfforts(self, cmd=None, effort=True): """ Publish cmd[arr] values to joints """ # if effort: if cmd is None: cmd = [0 for i, (k, v) in enumerate(self._joint_effort_pubs)] for i, (k, v) in enumerate(self._joint_effort_pubs.items()): cmd[i] = 0.01 * np.sin(rospy.get_time() * np.pi / 2) * 1 # print str(i) + " : " + str(cmd[i]) self._joint_effort_pubs[k].publish(cmd[i]) else: for i, c in enumerate(cmd): k = self._states_map.keys()[self._states_map.values().index(i)] self._joint_effort_pubs[k].publish(cmd[i]) return def initializeRBDLModel(self): """ Load the URDF model using RBDL """ rospack = rospkg.RosPack() rospack.list() root_path = rospack.get_path('inchworm_description') model_path = root_path + "/urdf/inchworm_description.urdf" # Create a new model self._model = rbdl.loadModel(model_path) self._q = np.zeros(self._model.q_size) self._qdot = np.zeros(self._model.qdot_size) self._qddot = np.zeros(self._model.qdot_size) self._tau = np.zeros(self._model.qdot_size) self._G = np.zeros(self._model.qdot_size) self._states_map = { joint: self._model.mJoints[self._model.GetBodyId(link[1])].q_index for joint, link in self._links.items() } self._unique_links = list() for link in self._links.values(): if len(self._unique_links) == 0: self._unique_links.append(link[0]) else: for i in range(2): if link[i] in self._unique_links: continue else: self._unique_links.append(link[i]) flajv = self._fixed_links_and_joints.values() flajv_ = [] for i in flajv: flajv_.append(i[1]) for i, link in enumerate(self._unique_links): if link in flajv_: del self._unique_links[i] self._bodies_map = { link: (self._model.GetBodyId(link), self._model.mBodies[self._model.GetBodyId(link)]) for link in self._unique_links } self._end_effector_positions = [] for frames in self._fixed_links_and_joints.values(): if self.tf.frameExists(frames[0]) and self.tf.frameExists( frames[1]): t = self.tf.getLatestCommonTime(frames[0], frames[1]) position, quaternion = self.tf.lookupTransform( frames[0], frames[1], t) self._end_effector_positions.extend(position) # print self._end_effector_positions return
class yoboticsOdometry: def __init__(self): rospy.Subscriber("/yobotics/gazebo/contacts", Contacts, self.contacts_callback) self.odom_publisher = rospy.Publisher("odom/raw", Odometry, queue_size=50) self.odom_broadcaster = tf.TransformBroadcaster() self.tf = TransformListener() self.foot_links = [ "lf_foot_link", "rf_foot_link", "lh_foot_link", "rh_foot_link" ] self.nominal_foot_positions = [[0, 0], [0, 0], [0, 0], [0, 0]] self.prev_foot_positions = [[0, 0], [0, 0], [0, 0], [0, 0]] self.prev_theta = [0, 0, 0, 0] self.prev_stance_angles = [0, 0, 0, 0] self.prev_time = 0 self.pos_x = 0 self.pos_y = 0 self.theta = 0 self.publish_odom_tf(0, 0, 0, 0) rospy.sleep(1) for i in range(4): self.nominal_foot_positions[i] = self.get_foot_position(i) self.prev_foot_positions[i] = self.nominal_foot_positions[i] self.prev_theta[i] = math.atan2(self.prev_foot_positions[i][1], self.prev_foot_positions[i][0]) def publish_odom(self, x, y, z, theta, vx, vy, vth): current_time = rospy.Time.now() odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = "odom" odom_quat = tf.transformations.quaternion_from_euler(0, 0, theta) odom.pose.pose = Pose(Point(x, y, z), Quaternion(*odom_quat)) odom.child_frame_id = "base_footprint" odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth)) # publish the message self.odom_publisher.publish(odom) def publish_odom_tf(self, x, y, z, theta): current_time = rospy.Time.now() odom_quat = quaternion_from_euler(0, 0, theta) self.odom_broadcaster.sendTransform((x, y, z), odom_quat, current_time, "base_footprint", "odom") def get_foot_position(self, leg_id): if self.tf.frameExists("base_link" and self.foot_links[leg_id]): t = self.tf.getLatestCommonTime("base_link", self.foot_links[leg_id]) position, quaternion = self.tf.lookupTransform( "base_link", self.foot_links[leg_id], t) return position else: return 0, 0, 0 def contacts_callback(self, data): self.leg_contact_states = data.contacts def is_almost_equal(self, a, b, max_rel_diff): diff = abs(a - b) a = abs(a) b = abs(b) largest = 0 if b > a: largest = b else: larget = a if diff <= (largest * max_rel_diff): return True return False def run(self): while not rospy.is_shutdown(): leg_contact_states = self.leg_contact_states current_foot_position = [[0, 0], [0, 0], [0, 0], [0, 0]] stance_angles = [0, 0, 0, 0] current_theta = [0, 0, 0, 0] delta_theta = 0 in_xy = False total_contact = sum(leg_contact_states) x_sum = 0 y_sum = 0 theta_sum = 0 for i in range(4): current_foot_position[i] = self.get_foot_position(i) for i in range(4): current_theta[i] = math.atan2(current_foot_position[i][1], current_foot_position[i][0]) from_nominal_x = self.nominal_foot_positions[i][ 0] - current_foot_position[i][0] from_nominal_y = self.nominal_foot_positions[i][ 1] - current_foot_position[i][1] stance_angles[i] = math.atan2(from_nominal_y, from_nominal_x) # print stance_angles #check if it's moving in the x or y axis if self.is_almost_equal(stance_angles[i], abs(1.5708), 0.001) or self.is_almost_equal( stance_angles[i], abs(3.1416), 0.001): in_xy = True if current_foot_position[i] != None and leg_contact_states[ i] == True and total_contact == 2: delta_x = (self.prev_foot_positions[i][0] - current_foot_position[i][0]) / 2 delta_y = (self.prev_foot_positions[i][1] - current_foot_position[i][1]) / 2 x = delta_x * math.cos(self.theta) - delta_y * math.sin( self.theta) y = delta_x * math.sin(self.theta) + delta_y * math.cos( self.theta) x_sum += delta_x y_sum += delta_y self.pos_x += x self.pos_y += y if not in_xy: delta_theta = self.prev_theta[i] - current_theta[i] theta_sum += delta_theta self.theta += delta_theta / 2 now = rospy.Time.now().to_sec() dt = now - self.prev_time vx = x_sum / dt vy = y_sum / dt vth = theta_sum / dt self.publish_odom(self.pos_x, self.pos_y, 0, self.theta, vx, vy, vth) # self.publish_odom_tf(self.pos_x, self.pos_y, 0, self.theta) self.prev_foot_positions = current_foot_position self.prev_theta = current_theta self.prev_stance_angles = stance_angles self.prev_time = now rospy.sleep(0.01)
def run(self, aut_path=None): """Intialize all NL components.""" # pylint: disable=W0603 global WORLD_KNOWLEDGE # Start the NL pipeline if not aut_path: print "Starting NL pipeline..." init_pipes() else: print "Skipping loading nlpipeline because an automaton was loaded" # Start the ROS node print "Initializing ROS node..." rospy.init_node('nlproxy') # Set up the state mgr and its callbacks print "Starting state manager..." self.state_mgr = StateManager(self) self.state_mgr.set_basedir(LTLGEN_BASE_DIR) # Load the automaton if needed if aut_path: self.state_mgr.load_test_automaton(aut_path, False) # Create world knowledge print "Starting knowledge..." WORLD_KNOWLEDGE = Knowledge.Knowledge(self) self.state_mgr.world_knowledge = WORLD_KNOWLEDGE # Wait a little for ROS to avoid timing startup issues. print "Waiting for ROS node to settle..." time.sleep(1) # Set up ROS listening print "Subscribing to notifications..." # Set up state manager's sending and listening pub = add_ltl_publisher() self.state_mgr.set_publisher(pub) add_subscriber(LTL_ENVIRONMENT_TOPIC, self.state_mgr.process_sensor_data) add_subscriber(LTL_ENVIRONMENT_TOPIC, WORLD_KNOWLEDGE.process_sensor_data) # Create the input comm_proxy and iPad connections print "Starting comm_proxy..." self.comm_proxy = CallbackSocket(self.text_port) self.comm_proxy.register_callback(self.process_text) self.ipad_conn = iPadConnection(self.map_port) self.ipad_conn.register_rename_callback(Knowledge.rename_entity) self.ipad_conn.register_text_callback(self.process_text) # TODO Add highlight callback self.map_proxy = MapProxy(self.ipad_conn) add_subscriber(LTL_ENVIRONMENT_TOPIC, self.ipad_conn.add_icons) WORLD_KNOWLEDGE.map_proxy = self.map_proxy # Set up odometry forwarding to the ipad tf = TransformListener() while not tf.frameExists("/map"): rospy.logwarn("Not ready for transforms yet") rospy.sleep(1.0) position_proxy = RobotPositionProxy(self.ipad_conn,tf) rospy.Subscriber(ODOM_TOPIC, Odometry, position_proxy.forward_position) # Set up getting directions direction_proxy = DirectionProxy() rospy.Subscriber(LOCATION_TOPIC, String, direction_proxy.set_location) WORLD_KNOWLEDGE.direction_proxy = direction_proxy print "NLMaster startup complete!" print "*" * 80 # Finally, wait for input before quitting try: while True: text = raw_input("").strip() if text == "q": break except (KeyboardInterrupt, EOFError): pass except: raise finally: # Only shutdown the pipeline if we actually were taking language input if not aut_path: print "Shutting down NL pipeline..." close_pipes() self.comm_proxy.shutdown() self.ipad_conn.shutdown() sys.exit()
class KeyPointManager(object): def __init__(self): self.tf = TransformListener() self.keyPointList = list() def add(self, marker): for i in range(len(self.keyPointList)): if (self.keyPointList[i].id == marker.id): return position = self.transformMarkerToWorld(marker) k = KeyPoint(marker.id, Point(position[0], position[1], position[2]), Quaternion(0., 0., 0., 1.)) self.keyPointList.append(k) self.addWaypointMarker(k) rospy.loginfo('Marker is added to following position') rospy.loginfo(k.pose) pass def getWaypoints(self): waypoints = list() for i in range(len(self.keyPointList)): waypoints.append(self.keyPointList[i].pose) return waypoints def keyPointListComplete(self): if (len(self.keyPointList) == 5): self.keyPointList.sort(key=lambda x: x.id, reverse=True) return True return False def markerHasValidId(self, marker): if (marker.id >= 61) and (marker.id <= 65): return True return False def transformMarkerToWorld(self, marker): markerTag = "ar_marker_" + str(marker.id) rospy.loginfo(markerTag) if self.tf.frameExists("map") and self.tf.frameExists(markerTag): self.tf.waitForTransform("map", markerTag, rospy.Time(), rospy.Duration(4.0)) t = self.tf.getLatestCommonTime("map", markerTag) position, quaternion = self.tf.lookupTransform("map", markerTag, t) return self.shiftPoint(position, quaternion) def shiftPoint(self, position, quaternion): try: euler = euler_from_quaternion(quaternion) yaw = euler[2] tf_mat = np.array([[np.cos(yaw), -np.sin(yaw), 0, position[0]], [np.sin(yaw), np.cos(yaw), 0, position[1]], [0, 0, 1, 0], [0, 0, 0, 1]]) displacement = np.array([[1, 0, 0, 0], [0, 1, 0, 0.35], [0, 0, 1, 0], [0, 0, 0, 1]]) point_map = np.dot(tf_mat, displacement) position = (point_map[0, 3], point_map[1, 3], 0) except Exception as inst: print type(inst) # the exception instance print inst.args # arguments stored in .args print inst return position def addWaypointMarker(self, keyPoint): rospy.loginfo('Publishing marker') # Set up our waypoint markers marker_scale = 0.2 marker_lifetime = 0 # 0 is forever marker_ns = 'waypoints' marker_id = keyPoint.id marker_color = {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0} # Initialize the marker points list. self.waypoint_markers = Marker() self.waypoint_markers.ns = marker_ns self.waypoint_markers.id = marker_id self.waypoint_markers.type = Marker.CUBE_LIST self.waypoint_markers.action = Marker.ADD self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime) self.waypoint_markers.scale.x = marker_scale self.waypoint_markers.scale.y = marker_scale self.waypoint_markers.color.r = marker_color['r'] self.waypoint_markers.color.g = marker_color['g'] self.waypoint_markers.color.b = marker_color['b'] self.waypoint_markers.color.a = marker_color['a'] self.waypoint_markers.header.frame_id = 'map' self.waypoint_markers.header.stamp = rospy.Time.now() self.waypoint_markers.points = list() p = Point(keyPoint.pose.position.x, keyPoint.pose.position.y, keyPoint.pose.position.z) self.waypoint_markers.points.append(p) # Publish the waypoint markers self.marker_pub = rospy.Publisher('waypoint_markers', Marker) self.marker_pub.publish(self.waypoint_markers)
class GripperMarkers: _offset = 0.09 def __init__(self, side_prefix): self.side_prefix = side_prefix self._im_server = InteractiveMarkerServer('ik_request_markers_' + self.side_prefix) self._tf_listener = TransformListener() self._menu_handler = MenuHandler() self._menu_handler.insert('Move arm here', callback=self.move_to_pose_cb) self.r_joint_names = [ 'r_shoulder_pan_joint', 'r_shoulder_lift_joint', 'r_upper_arm_roll_joint', 'r_elbow_flex_joint', 'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint' ] self.l_joint_names = [ 'l_shoulder_pan_joint', 'l_shoulder_lift_joint', 'l_upper_arm_roll_joint', 'l_elbow_flex_joint', 'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint' ] # Create a trajectory action client r_traj_contoller_name = None l_traj_contoller_name = None if self.side_prefix == 'r': r_traj_controller_name = '/r_arm_controller/joint_trajectory_action' self.r_traj_action_client = SimpleActionClient( r_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for RIGHT arm...') self.r_traj_action_client.wait_for_server() else: l_traj_controller_name = '/l_arm_controller/joint_trajectory_action' self.l_traj_action_client = SimpleActionClient( l_traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory ' + 'action server for LEFT arm...') self.l_traj_action_client.wait_for_server() self.is_control_visible = False self.ee_pose = self.get_ee_pose() self.ik = IK(side_prefix) self.update_viz() self._menu_handler.apply(self._im_server, 'ik_target_marker_' + self.side_prefix) self._im_server.applyChanges() print self.ik def get_ee_pose(self): from_frame = 'base_link' to_frame = self.side_prefix + '_wrist_roll_link' try: if self._tf_listener.frameExists( from_frame) and self._tf_listener.frameExists(to_frame): t = self._tf_listener.getLatestCommonTime(from_frame, to_frame) # t = rospy.Time.now() (pos, rot) = self._tf_listener.lookupTransform( to_frame, from_frame, t) # Note argument order :( else: rospy.logerr( 'TF frames do not exist; could not get end effector pose.') except Exception as err: rospy.logerr('Could not get end effector pose through TF.') rospy.logerr(err) pos = [1.0, 0.0, 1.0] rot = [0.0, 0.0, 0.0, 1.0] return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def move_to_pose_cb(self, feedback): rospy.loginfo('You pressed the `Move arm here` button from the menu.') '''Moves the arm to the desired joints''' print feedback time_to_joint = 2.0 positions = self.ik.get_ik_for_ee(feedback.pose) velocities = [0] * len(positions) traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.points.append( JointTrajectoryPoint( positions=positions, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) if (self.side_prefix == 'r'): traj_goal.trajectory.joint_names = self.r_joint_names self.r_traj_action_client.send_goal(traj_goal) else: traj_goal.trajectory.joint_names = self.l_joint_names self.l_traj_action_client.send_goal(traj_goal) pass def marker_clicked_cb(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: self.ee_pose = feedback.pose self.update_viz() self._menu_handler.reApply(self._im_server) self._im_server.applyChanges() elif feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: rospy.loginfo('Changing visibility of the pose controls.') if (self.is_control_visible): self.is_control_visible = False else: self.is_control_visible = True else: rospy.loginfo('Unhandled event: ' + str(feedback.event_type)) def update_viz(self): menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True frame_id = 'base_link' pose = self.ee_pose menu_control = self._add_gripper_marker(menu_control) text_pos = Point() text_pos.x = pose.position.x text_pos.y = pose.position.y text_pos.z = pose.position.z + 0.1 text = 'x=' + str(pose.position.x) + ' y=' + str( pose.position.y) + ' x=' + str(pose.position.z) menu_control.markers.append( Marker(type=Marker.TEXT_VIEW_FACING, id=0, scale=Vector3(0, 0, 0.03), text=text, color=ColorRGBA(0.0, 0.0, 0.0, 0.5), header=Header(frame_id=frame_id), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) int_marker = InteractiveMarker() int_marker.name = 'ik_target_marker_' + self.side_prefix int_marker.header.frame_id = frame_id int_marker.pose = pose int_marker.scale = 0.2 self._add_6dof_marker(int_marker) int_marker.controls.append(menu_control) self._im_server.insert(int_marker, self.marker_clicked_cb) def _add_gripper_marker(self, control): is_hand_open = False if is_hand_open: angle = 28 * numpy.pi / 180.0 else: angle = 0 transform1 = tf.transformations.euler_matrix(0, 0, angle) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, 0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices( transform1, transform2) mesh1 = self._make_mesh_marker() mesh1.mesh_resource = ( 'package://pr2_description/meshes/gripper_v0/gripper_palm.dae') mesh1.pose.position.x = -GripperMarkers._offset mesh1.pose.orientation.w = 1 mesh2 = self._make_mesh_marker() mesh2.mesh_resource = ( 'package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh2.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh3 = self._make_mesh_marker() mesh3.mesh_resource = ( 'package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh3.pose = GripperMarkers.get_pose_from_transform(t_distal) quat = tf.transformations.quaternion_multiply( tf.transformations.quaternion_from_euler(numpy.pi, 0, 0), tf.transformations.quaternion_from_euler(0, 0, angle)) transform1 = tf.transformations.quaternion_matrix(quat) transform1[:3, 3] = [0.07691 - GripperMarkers._offset, -0.01, 0] transform2 = tf.transformations.euler_matrix(0, 0, -angle) transform2[:3, 3] = [0.09137, 0.00495, 0] t_proximal = transform1 t_distal = tf.transformations.concatenate_matrices( transform1, transform2) mesh4 = self._make_mesh_marker() mesh4.mesh_resource = ( 'package://pr2_description/meshes/gripper_v0/l_finger.dae') mesh4.pose = GripperMarkers.get_pose_from_transform(t_proximal) mesh5 = self._make_mesh_marker() mesh5.mesh_resource = ( 'package://pr2_description/meshes/gripper_v0/l_finger_tip.dae') mesh5.pose = GripperMarkers.get_pose_from_transform(t_distal) control.markers.append(mesh1) control.markers.append(mesh2) control.markers.append(mesh3) control.markers.append(mesh4) control.markers.append(mesh5) return control @staticmethod def get_pose_from_transform(transform): pos = transform[:3, 3].copy() rot = tf.transformations.quaternion_from_matrix(transform) return Pose(Point(pos[0], pos[1], pos[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) def _make_mesh_marker(self): mesh = Marker() mesh.mesh_use_embedded_materials = False mesh.type = Marker.MESH_RESOURCE mesh.scale.x = 1.0 mesh.scale.y = 1.0 mesh.scale.z = 1.0 print self ik_solution = self.ik.get_ik_for_ee(self.ee_pose) if (ik_solution is None): mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6) else: mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6) return mesh def _add_6dof_marker(self, int_marker): is_fixed = True control = self._make_6dof_control('rotate_x', Quaternion(1, 0, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_x', Quaternion(1, 0, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_z', Quaternion(0, 1, 0, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_z', Quaternion(0, 1, 0, 1), True, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('rotate_y', Quaternion(0, 0, 1, 1), False, is_fixed) int_marker.controls.append(control) control = self._make_6dof_control('move_y', Quaternion(0, 0, 1, 1), True, is_fixed) int_marker.controls.append(control) def _make_6dof_control(self, name, orientation, is_move, is_fixed): control = InteractiveMarkerControl() control.name = name control.orientation = orientation control.always_visible = False if (self.is_control_visible): if is_move: control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS else: control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS else: control.interaction_mode = InteractiveMarkerControl.NONE if is_fixed: control.orientation_mode = InteractiveMarkerControl.FIXED return control
class MapRelay(object): def __init__(self, master_relay, name, map_topic_name, relay_map_topic_suffix, max_dist): self._name = name self._namespace = '/' + name + '/' self.subscribe_topic = self._namespace + map_topic_name rospy.Subscriber(self.subscribe_topic, OccupancyGrid, self.callback) self.publish_topic = self._namespace + relay_map_topic_suffix self.publisher = rospy.Publisher(self.publish_topic, OccupancyGrid, queue_size = 5) self.master_relay = master_relay self.max_dist = max_dist self._tf = TransformListener() self._occupancy_grid = None self._occupancy_grid_pub = None self._pose = np.array([np.nan, np.nan, np.nan], dtype=np.float32) def callback(self, msg): # self.update_pose() self._occupancy_grid = msg # if self.master_relay is None: # self._occupancy_grid_pub = msg # else: # self.master_relay.update_pose() # p1 = self.master_relay.pose # # p2 = self.master_relay.get_pose(self._name) # p2 = self.pose # dist = np.linalg.norm([p1[X] - p2[X], p1[Y] - p2[Y]]) # print("Distance ", self._name, self.master_relay._name, dist) # if dist <= self.max_dist: # print("Map updated ", dist, self.max_dist) # self._occupancy_grid = msg def publish_map(self): if self.master_relay is None: self._occupancy_grid_pub = self._occupancy_grid else: dist = self.get_distance(self.master_relay._name) if dist is not None and dist <= self.max_dist: # print("AAAAAAAAAAAAAAAA Map updated ", dist, self.max_dist) self._occupancy_grid_pub = self._occupancy_grid # self.master_relay.update_pose() # self.update_pose() # p1 = self.master_relay.pose # # p2 = self.master_relay.get_pose(self._name) # p2 = self.pose # if not np.isnan(p2[X]) and not np.isnan(p1[X]): # dist = np.linalg.norm([p1[X] - p2[X], p1[Y] - p2[Y]]) # print("AAAAAAAAAAAAAAA Distance ", self.master_relay._name, self._name, p1, p2, dist) # if dist <= self.max_dist: # print("AAAAAAAAAAAAAAAA Map updated ", dist, self.max_dist) # self._occupancy_grid_pub = self._occupancy_grid # else: # print("AAAAAAAAAAAAAAAAA Nan ", self.master_relay._name, self._name, p1, p2) if self._occupancy_grid_pub is not None: self.publisher.publish(self._occupancy_grid_pub) # print("Published") def get_distance(self, robot_name): a = self._name + '/base_link' b = robot_name + '/base_link' dist = None try: t = rospy.Time(0) position, orientation = self._tf.lookupTransform(a, b, t) dist = np.linalg.norm([position[X], position[Y]]) # print(position, dist) except Exception as e: print(e) return dist def update_pose(self): # Get pose w.r.t. map. a = self._name + '/map' b = self._name + '/base_link' if True: # self._tf.frameExists(a) and self._tf.frameExists(b): try: t = rospy.Time(0) position, orientation = self._tf.lookupTransform(a, b, t) self._pose[X] = position[X] self._pose[Y] = position[Y] _, _, self._pose[YAW] = euler_from_quaternion(orientation) # print('pose updated') # print(self._pose) except Exception as e: print(e) print('Unable to find:', a, self._tf.frameExists(a), b, self._tf.frameExists(b)) else: print('Unable to find:', a, self._tf.frameExists(a), b, self._tf.frameExists(b)) pass def get_pose(self, robot_name): # Get pose w.r.t. map. a = self._name + '/map' b = robot_name + '/base_link' if self._tf.frameExists(a) and self._tf.frameExists(b): try: t = rospy.Time(0) position, orientation = self._tf.lookupTransform(a, b, t) self._pose[X] = position[X] self._pose[Y] = position[Y] _, _, self._pose[YAW] = euler_from_quaternion(orientation) # print('pose updated') # print(self._pose) except Exception as e: print(e) else: print('Unable to find:', a, self._tf.frameExists(a), b, self._tf.frameExists(b)) pass @property def ready(self): return self._occupancy_grid is not None # and not np.isnan(self._pose[0]) @property def pose(self): return self._pose @property def occupancy_grid(self): return self._occupancy_grid
class MergePoses: def __init__(self): self.avg_pose = None self.tl = TransformListener() self.topics = rospy.get_param('~poses',[]) print self.topics if len(self.topics) == 0: rospy.logerr('Please provide a poses list as input parameter') return self.output_pose = rospy.get_param('~output_pose','ar_avg_pose') self.output_frame = rospy.get_param('~output_frame', 'rf_map') self.subscribers = [] for i in self.topics: subi = rospy.Subscriber(i, PoseStamped, self.callback, queue_size=1) self.subscribers.append(subi) self.pub = rospy.Publisher(self.output_pose, PoseStamped, queue_size=1) self.mutex_avg = threading.Lock() self.mutex_t = threading.Lock() self.transformations = {} def callback(self, pose_msg): # get transformation to common frame position = None quaternion = None if self.transformations.has_key(pose_msg.header.frame_id): position, quaternion = self.transformations[pose_msg.header.frame_id] else: if self.tl.frameExists(pose_msg.header.frame_id) and \ self.tl.frameExists(self.output_frame): t = self.tl.getLatestCommonTime(self.output_frame, pose_msg.header.frame_id) position, quaternion = self.tl.lookupTransform(self.output_frame, pose_msg.header.frame_id, t) self.mutex_t.acquire() self.transformations[pose_msg.header.frame_id] = (position, quaternion) self.mutex_t.release() rospy.loginfo("Got static transform for %s" % pose_msg.header.frame_id) # transform pose framemat = self.tl.fromTranslationRotation(position, quaternion) posemat = self.tl.fromTranslationRotation([pose_msg.pose.position.x, pose_msg.pose.position.y, pose_msg.pose.position.z], [pose_msg.pose.orientation.x, pose_msg.pose.orientation.y, pose_msg.pose.orientation.z, pose_msg.pose.orientation.w]) newmat = numpy.dot(framemat,posemat) xyz = tuple(translation_from_matrix(newmat))[:3] quat = tuple(quaternion_from_matrix(newmat)) tmp_pose = PoseStamped() tmp_pose.header.stamp = pose_msg.header.stamp tmp_pose.header.frame_id = self.output_frame tmp_pose.pose = Pose(Point(*xyz),Quaternion(*quat)) tmp_angles = euler_from_quaternion([tmp_pose.pose.orientation.x, tmp_pose.pose.orientation.y, tmp_pose.pose.orientation.z, tmp_pose.pose.orientation.w]) # compute average self.mutex_avg.acquire() if self.avg_pose == None or (pose_msg.header.stamp - self.avg_pose.header.stamp).to_sec() > 0.5: self.avg_pose = tmp_pose else: self.avg_pose.header.stamp = pose_msg.header.stamp a = 0.7 b = 0.3 self.avg_pose.pose.position.x = a*self.avg_pose.pose.position.x + b*tmp_pose.pose.position.x self.avg_pose.pose.position.y = a*self.avg_pose.pose.position.y + b*tmp_pose.pose.position.y self.avg_pose.pose.position.z = a*self.avg_pose.pose.position.z + b*tmp_pose.pose.position.z angles = euler_from_quaternion([self.avg_pose.pose.orientation.x, self.avg_pose.pose.orientation.y, self.avg_pose.pose.orientation.z, self.avg_pose.pose.orientation.w]) angles = list(angles) angles[0] = avgAngles(angles[0], tmp_angles[0], 0.7, 0.3) angles[1] = avgAngles(angles[1], tmp_angles[1], 0.7, 0.3) angles[2] = avgAngles(angles[2], tmp_angles[2], 0.7, 0.3) q = quaternion_from_euler(angles[0], angles[1], angles[2]) self.avg_pose.pose.orientation.x = q[0] self.avg_pose.pose.orientation.y = q[1] self.avg_pose.pose.orientation.z = q[2] self.avg_pose.pose.orientation.w = q[3] self.pub.publish(self.avg_pose) self.mutex_avg.release()
class Rotate(smach.State): #class handles the rotation until program is stopped def __init__(self): self.tf = TransformListener() smach.State.__init__(self, outcomes=['finished','failed'], input_keys=['base_pose','stop_rotating','id'], output_keys=['detected']) rospy.Subscriber("/cob_people_detection/detection_tracker/face_position_array",DetectionArray, self.callback) self.stop_rotating=False self.detections= list() self.false_detections=list() def callback(self,msg): # go through list of detections and append them to detection list if len(msg.detections) >0: #clear detection list del self.detections[:] for i in xrange( len(msg.detections)): self.detections.append(msg.detections[i].label) return def execute(self, userdata): sss.say(["I am going to take a look around now."]) # get position from tf if self.tf.frameExists("/base_link") and self.tf.frameExists("/map"): t = self.tf.getLatestCommonTime("/base_link", "/map") position, quaternion = self.tf.lookupTransform("/base_link", "/map", t) # calculate angles from quaternion [r,p,y]=euler_from_quaternion(quaternion) #print r #print p #print y #print position else: print "No transform available" return "failed" time.sleep(1) self.stop_rotating=False # create relative pose - x,y,theta curr_pose=list() curr_pose.append(0) curr_pose.append(0) curr_pose.append(0.1) while not rospy.is_shutdown() and self.stop_rotating==False and curr_pose[2]< 3.14: handle_base = sss.move_base_rel("base", curr_pose) #check in detection and react appropriately for det in self.detections: # right person is detected if det == userdata.id: self.stop_rotating=True sss.say(['I have found you, %s! Nice to see you.'%str(det)]) elif det in self.false_detections: # false person is detected print "Already in false detections" # person detected is unknown - only react the first time elif det == "Unknown": print "Unknown face detected" sss.say(['Hi! Nice to meet you, but I am still searching for %s.'%str(userdata.id)]) self.false_detections.append("Unknown") # wrong face is detected the first time else: self.false_detections.append(det) print "known - wrong face detected" sss.say(['Hello %s! Have you seen %s.'%(str(det),str(userdata.id))]) #clear detection list, so it is not checked twice del self.detections[:] time.sleep(2) print "-->stop rotating" return 'finished'
class Laser2PCWorld(): #translate laser to pc and publish point cloud to /cloud_in def __init__(self): self.laserSub = rospy.Subscriber('/scan', LaserScan, self.laserCallBack) self.laser = [float('inf')] self.pc_laser = None self.pc_world = None self._tf = TransformListener() self.occupied_cells = [] self.free_cells = [] self.mat_laser2world = None def laserCallBack(self, data): self.laser = data def update(self): # Get the transformation matrix for laser to world target_frame = "world" src_frame = "laser0_frame" if self._tf.frameExists(target_frame) and self._tf.frameExists( src_frame): try: self.mat_laser2world = self._tf.asMatrix( target_frame, self.laser.header) except Exception as e: print(e) else: print('Unable to find:', self._tf.frameExists(target_frame), self._tf.frameExists(src_frame)) # Transform laser data to world coordinate if not isinstance(self.laser, LaserScan) or not isinstance( self.mat_laser2world, np.ndarray): return try: l = copy.deepcopy(self.laser) mat = copy.deepcopy(self.mat_laser2world) free = [] #in world coord occuiped = [] #in world coord pc_laser = [] pc_world = [] for r in range(len(l.ranges)): radius = 0 encountered_obs = True if l.ranges[r] == float('inf'): #append free cells till max range radius = l.range_max encountered_obs = False theta = 180 - (l.angle_min + r * l.angle_increment ) #due to differences in ros corrdinate frame x = l.ranges[radius] * np.cos(theta) y = l.ranges[radius] * np.sin(theta) z = 0 pc_laser.append([x, y, z]) col = np.array([[x, y, z, 1]]).T tran = np.matmul(mat, col).reshape((4)) pc_world.append([tran[X], tran[Y], tran[X]]) discretized_pt = self.append_free_space( [tran[X], tran[Y], tran[X]]) # print("discretized_pt", len(discretized_pt)) if encountered_obs: # print("before", len(occuiped)) occuiped.extend(discretized_pt[-10:]) # print("pt", discretized_pt[-10:]) free.extend(discretized_pt[:-10]) # print("after", len(occuiped)) else: free.extend(discretized_pt) # print("encoutered obs", encountered_obs) # print("occuipied", len(occuiped)) # print("free", len(free)) self.pc_laser = pc_laser self.pc_world = pc_world self.occupied_cells = occuiped self.free_cells = free except Exception as e: print(e) def append_free_space(self, pt): distance = 0.05 a = abs(pt[X]) / distance b = abs(pt[Y]) / distance c = abs(pt[Z]) / distance num_of_sample = max(a, b, c) x = np.linspace(0, pt[X], num_of_sample) y = np.linspace(0, pt[Y], num_of_sample) z = np.linspace(0, pt[Z], num_of_sample) coords = zip(x, y, z) return coords @property def ready(self): laser_ready = isinstance(self.laser, LaserScan) mat_ready = isinstance(self.mat_laser2world, np.ndarray) return laser_ready and mat_ready @property def measurements(self): return self._measurements
class controller: def __init__(self): # self.kp=1.0*1 self.moving=True self.robot_L = None self.robot_R= None self.theta= None self.goal =np.array([0.0 , 0.0 , 0.0]) self.ball_pos =np.array([0.0 , 0.0 , 0.0]) self.ball_vel =np.array([0.0 , 0.0 , 0.0]) self.kp = 0.3 self.kp_z = 1.0*0.8 self.kd = 1.0*0 # self.kd=1.0*0 self.robot_R = [0.0,0.0,0.0,0.0,0.0,0.0] self.robot_L = [0.0,0.0,0.0,0.0,0.0,0.0] self.robot_pose = np.array([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]) self.robot_vel = np.array([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]) # message order is right then left # self.goal = np.array([0.28637367486953735, -0.23328398168087006, 0.09796947240829468, 0.9996191263198853, 0.027586041018366814, -0.0007667283643968403 / # 0.16970492899417877, 0.3836527466773987, 0.16571415960788727, 0.8278752565383911, -0.37586238980293274, -0.4163532853126526]) # Subscribing to join_state_topic. It should contain message from YuMi Paul node # [right arm left arm - joints angles] self.tf = TransformListener() self.operational_pub = rospy.Publisher("/operational_velocity_command",Float32MultiArray,queue_size=1) self.operational_sub = rospy.Subscriber("/operational_position_R",Float32MultiArray,self.callback_operational_R,buff_size=1) # Subscribing to simulation from Sean with ball's position and velocity self.camera_coords_sub = rospy.Subscriber("/camera_coords",Float32MultiArray,self.callback_camera_coords,buff_size=1) self.ball_sub = rospy.Subscriber("/ball_position",Float32MultiArray,self.callback_ball,buff_size=1) self.ball_vel_sub = rospy.Subscriber("/ball_velocity",Float32MultiArray,self.callback_vel_ball,buff_size=1) self.joint_states_sub = rospy.Subscriber("/joint_state_R",Float32MultiArray,self.callback_joint_state_R,buff_size=1) self.joints_R =[0.0,0.0,0.0,0.0,0.0,0.0,0.0] # Publishing to joint command which receives a message from Yumi Paul node # What we actually sending to the robot to do # Use Jacobian matrix to find joint velocities for each joint self.joint_command_pub = rospy.Publisher("/joint_command_topic",Float32MultiArray,queue_size=10) # Publishing to yumi hand topic with velocities message # end effector position (need to add rocket length) -[x y z] - get it from transform from 0 to 7 # Try to get this message from already existing function getVel self.yumi_hand_pub = rospy.Publisher("/yumi_hand_topic",Float32MultiArray,queue_size=10) def callback_joint_state_R(self,msg): if any(np.isnan(msg.data)): return self.joints_R = msg.data[:7] def callback_ball(self,msg): self.ball_pos[0] = msg.data[0] self.ball_pos[1] = msg.data[1] self.ball_pos[2] = msg.data[2] def callback_vel_ball(self,msg): self.ball_vel[0] = msg.data[0] self.ball_vel[1] = msg.data[1] self.ball_vel[2] = msg.data[2] def callback_camera_coords(self,msg): if self.tf.frameExists("yumi_body") and self.tf.frameExists("camera_depth_optical_frame"): t = self.tf.getLatestCommonTime("yumi_body", "camera_depth_optical_frame") #position, quaternion = self.tf.lookupTransform("yumi_body", "camera_depth_optical_frame", t) p1 = PoseStamped() p1.pose.position.x =msg.data[0] p1.pose.position.y =msg.data[1] p1.pose.position.z =msg.data[2] p1.header.frame_id = "camera_depth_optical_frame" p_in_base = self.tf.transformPose("yumi_body", p1) self.goal[0] = p_in_base.pose.position.x self.goal[1] = p_in_base.pose.position.y #print(p1) def callback_operational_R(self,msg): self.robot_L = [0.0,0.0,0.0,0.0,0.0,0.0] self.robot_R=list(msg.data) self.theta=self.robot_R[0:8] for i in range(0,len(self.robot_L)): self.robot_R.append(self.robot_L[i]) #combines two arrays self.robot_pose= np.array(self.robot_R) if self.moving==True: vel_msg=self.getVelMsg() self.operational_pub.publish(vel_msg) #velocity message for joints (14 values for each joint. Left can be 0). # self.joint_command_pub.publish(vel_Msg) # self.yumi_hand_pub.publish(hand_msg) def callback_goal(self,msg): self.goal = np.array(msg.data) self.moving=True def getVelMsg(self): #self.goal = np.array([0.88, 0.343, 0.3]) self.ball_vel= self.ball_vel try: serv= rospy.ServiceProxy('getLastTransformation', getM) resp = serv(self.joints_R) except rospy.ServiceException, e: print("Service call failed: %s"%e) return self.robot_vel Te = np.matrix(np.zeros((4,4))) Te[0:3,:] = np.reshape(resp.M,(3,4)) Te[3,3] = 1.0 #Te = np.linalg.inv(Te) Tep = np.matrix([[1.0, 0.0,0.0, 0.0],[0.0, 0.0, -1.0, 0.0],[0.0, 1.0, 0.0, 0.25],[0.0, 0.0, 0.0 ,1.0]]) #Tep = np.matrix([[0.0, 0.0,1.0, 0.0],[0.0, 1.0, 0.0, 0.0],[-1.0, 0.0, 0.0, 0.05],[0.0, 0.0, 0.0 ,1.0]]) #Tep = np.matrix([[0.0, -1.0 ,0.0, 0.0],[1.0, 0.0, 0.0, 0.0],[0.0, 0.0, 1.0, 0.05],[0.0, 0.0, 0.0 ,1.0]]) # Tep = np.matrix([[1.0, 0.0 ,0.0, 0.0],[0.0, 1.0, 0.0, 0.0],[0.0, 0.0, 1.0, 0.05],[0.0, 0.0, 0.0 ,1.0]]) #T0p = np.linalg.inv(np.matmul(Te,Tep)) #Tep = np.linalg.inv(Tep) T0p = np.matmul(Te,Tep) tmp = T0p[0:3,3] self.ball_pos[2] = 0.25 p = (self.ball_pos[0:3]-np.array(tmp.transpose()))*2 normal = np.matmul(T0p[0:3,0:3],np.array([1.0,0.0,0.0])) tmp[2] = 0.0 self.ball_pos[2] = 0.0 #save = np.array([0.0,0.0,1.0])-0.5*(self.ball_pos-np.array([0.2,-0.5,0.0])) save = np.array([0.0,0.0,1.0])-0.5*(self.robot_pose[0:3]-np.array([0.0,-0.2,0.0])) #save[1]*=-1 save =save/np.linalg.norm(save) rot = np.cross(normal,np.array(save)) rot = np.matrix(rot).transpose() tmp = normal tmp =tmp/np.linalg.norm(normal) an1 = np.arccos(np.clip(np.dot(tmp,np.array([-0.0,0.0,1.0])), -1.0, 1.0)) #Tpe = np.linalg.inv(Tep) #rot = np.matmul(Tpe[0:3,0:3],np.matrix(rot).transpose()) # d = self.goal[0:3]*0-self.robot_vel[0:3] direction = p/np.linalg.norm(p) val = direction*np.linalg.norm(p)*self.kp #+direction*d*self.kd val = p for i in range(0,2): #val[i] = np.sign(val[i]) self.robot_vel[i] =12.0*val[0,i]+self.ball_vel[i] self.robot_vel[2] = 4.0*val[0,2] # p_z = self.goal[2]-self.robot_pose[2] # direction_z = p_z/np.linalg.norm(p_z) # val_z = -direction_z*np.linalg.norm(p_z)*self.kp_z #val_z = 0.0 #if (self.ball_vel[2]<0.0): self.robot_vel[2] += -1*0.4*self.ball_vel[2]/(2*np.abs(val[0,2])+0.2) #else: # self.robot_vel[2] += 0.0 tmp = self.robot_pose[3:6] tmp =tmp/np.linalg.norm(self.robot_pose[3:6]) rot2 =np.cross(tmp,np.array([1.0,0.0,0.0])) an2 = np.arccos(np.clip(np.dot(tmp,np.array([1.0,0.0,0.0])), -1.0, 1.0)) # np.arccos(np.dot(self.robot_pose[3:6],np.array([1.0,0.0,0.0]))) self.robot_vel[3:6] = (rot2*0.8*an2+an1*rot.transpose())*300 #self.robot_vel[1]=0.0 #self.robot_vel[0]=0.1 vel_msg = Float32MultiArray() vel_msg.data = self.robot_vel.tolist() return vel_msg
class DynamicTFBroadcaster: def __init__(self): self.pub_tf = rospy.Publisher("/denso_cube_tf", tf.msg.tfMessage, queue_size=10) self.tf = TransformListener() self.t = geometry_msgs.msg.TransformStamped() self.translation_array = np.zeros((3), dtype=np.float64) self.mainloop() """ def median(self,array): norma = np.sqrt(np.power(array, 2)) median_array.pop(0) median_array.append(norma) median_array.sort() print(array) median = median_array(6) return median """ def mainloop(self): values_array = np.zeros((7, 4), dtype=np.float64) i = 0 while not rospy.is_shutdown(): # Run this loop at about 10Hz rospy.sleep(0.5) if self.tf.frameExists("denso") and self.tf.frameExists( "ar_marker_0"): #t = self.tf.getLatestCommonTime("world", "ar_marker_7") position, quaternion = self.tf.lookupTransform( "denso", "ar_marker_0", rospy.Time(0)) self.t.header.frame_id = "denso" self.t.header.stamp = rospy.Time.now() self.t.child_frame_id = "ar_marker_0" self.t.transform.translation.x = position[0] self.t.transform.translation.y = position[1] self.t.transform.translation.z = position[2] self.t.transform.rotation.x = quaternion[0] self.t.transform.rotation.y = quaternion[1] self.t.transform.rotation.z = quaternion[2] self.t.transform.rotation.w = quaternion[3] tfm = tf.msg.tfMessage([self.t]) self.pub_tf.publish(tfm) values_array[0][i] = position[0] values_array[1][i] = position[1] values_array[2][i] = position[2] values_array[3][i] = quaternion[0] values_array[4][i] = quaternion[1] values_array[5][i] = quaternion[2] values_array[6][i] = quaternion[3] #translation_array = [position[0], position[1], position[2] if (i == 3): i = 0 translation_x = self.mean(values_array[0]) translation_y = self.mean(values_array[1]) translation_z = self.mean(values_array[2]) #rotation_x = self.madian(values_array[3]) #rotation_y = self.median(values_array[4]) #rotation_z = self.median(values_array[5]) #rotation_w = self.median(values_array[6]) print("Translation x: %.3f " % (translation_x)) print("Translation y: %.3f " % (translation_y)) print("Translation z: %.3f " % (translation_z)) #print("Rotation x: %.3f " % (rotation_x)) #print("Rotation y: %.3f " % (rotation_y)) #print("Rotation z: %.3f " % (rotation_z)) #print("Rotation w: %.3f " % (rotation_w)) else: i = i + 1 #print(values_array[0]) rospy.loginfo(tfm) def mean(self, array): mean = array[0] + array[1] + array[2] + array[3] return mean / 4
class TurnTableOperator(object): def __init__(self, serial_port, marker_id, contactdb_recorder, hand_pose_recorder, step=3, motor_speed=150, motor_acceleration=200, marker_stable_thresh=30): """" :param serial_port: :param marker_id: :param {contactdb,hand_pose}_recorder: Object of the RosbagRecord class :param step: :param motor_speed: :param motor_acceleration: :param marker_stable_thresh: no. of times marker needs to be seen continuously """ self.step = step self.marker_id = marker_id self.contactdb_recorder = contactdb_recorder self.hand_pose_recorder = hand_pose_recorder self.tt_base_t = [] self.tt_base_pose = TransformStamped() self.tf_listener = TransformListener() self.marker_seen_count = 0 self.marker_stable_thresh = marker_stable_thresh self.marker_pose_ready = False self.tt_angle_bcaster = tf2_ros.TransformBroadcaster() self.tt_center_bcaster = tf2_ros.TransformBroadcaster() # logging for the Arduino driver ch = logging.StreamHandler() ch.setLevel(logging.DEBUG) board.logger.addHandler(ch) # init and connect the driver self.arduino = board.Board() self.arduino.serial_name = serial_port try: self.arduino.connect() except Exception as e: rospy.logerr('Could not connect to turntable: {:s}'.format(e)) raise e # init and configure the motor self.arduino.motor_invert(True) self.arduino.motor_enable() self.arduino.motor_reset_origin() self.arduino.motor_speed(motor_speed) self.arduino.motor_acceleration(motor_acceleration) # OpenCV window for clicking self.im_size = 50 self.button_win_name = 'Marker Ready' self.button_im = self._make_im([0, 0, 255]) cv2.imshow(self.button_win_name, self.button_im) cv2.waitKey(1) rospy.sleep(0.5) assert self.tf_listener.frameExists('turntable_base_tmp') assert self.tf_listener.frameExists('boson_frame') def _make_im(self, pixel): """ :param pixel: (3, ) :return: """ im = np.asarray(pixel, dtype=np.uint8)[np.newaxis, np.newaxis, :] im = np.repeat(np.repeat(im, self.im_size, axis=0), self.im_size, axis=1) return im def publish_tfs(self, tt_angle): t = TransformStamped() # pose of turntable_frame w.r.t. turntable_base t.header.stamp = rospy.Time.now() t.header.frame_id = 'turntable_base' t.child_frame_id = 'turntable_frame' t.transform.translation.x = 0 t.transform.translation.y = 0 t.transform.translation.z = 0 q = tx.quaternion_from_euler(0, 0, np.deg2rad(tt_angle)) t.transform.rotation.x = q[0] t.transform.rotation.y = q[1] t.transform.rotation.z = q[2] t.transform.rotation.w = q[3] self.tt_angle_bcaster.sendTransform(t) t.header.frame_id = 'kinect2_rgb_optical_frame' t.child_frame_id = 'turntable_base' tm = np.mean(self.tt_base_t, axis=0) t.transform.translation.x = tm[0] t.transform.translation.y = tm[1] t.transform.translation.z = tm[2] t.transform.rotation = self.tt_base_pose.transform.rotation self.tt_center_bcaster.sendTransform(t) def record_hand_pose(self, object_name): self.hand_pose_recorder.start_recording(object_name) def run_turntable(self, object_name): # first stop the hand pose recorder node if it is active self.hand_pose_recorder.stop_recording_handler() # start recording for contactdb angle = 0 self.contactdb_recorder.start_recording(object_name) rospy.sleep(1.5) while angle < 360: rospy.loginfo('Angle = {:d}'.format(angle)) self.publish_tfs(tt_angle=angle) rospy.sleep(0.5) self.arduino.motor_move(self.step) angle += self.step rospy.sleep(0.5) cv2.waitKey(1) self.contactdb_recorder.stop_recording_handler() def disconnect(self): self.arduino.disconnect() rospy.loginfo('Disconnected from Arduino') self.contactdb_recorder.stop_recording_handler() self.hand_pose_recorder.stop_recording_handler() def ar_tag_callback(self, markers): cv2.imshow(self.button_win_name, self.button_im) cv2.waitKey(1) if self.marker_pose_ready: return for m in markers.markers: if m.id != self.marker_id: continue if self.marker_seen_count > self.marker_stable_thresh: self.button_im = self._make_im([0, 255, 0]) self.marker_pose_ready = True self.marker_seen_count = self.marker_stable_thresh else: self.button_im = self._make_im([0, 0, 255]) pos, rotquat = self.tf_listener.lookupTransform( 'kinect2_rgb_optical_frame', 'turntable_base_tmp', rospy.Time(0)) if np.isnan(pos).any() or np.isnan(rotquat).any(): rospy.logwarn('NaN pose') break self.marker_seen_count += 1 self.tt_base_t.append(pos) self.tt_base_pose.transform.rotation.x = rotquat[0] self.tt_base_pose.transform.rotation.y = rotquat[1] self.tt_base_pose.transform.rotation.z = rotquat[2] self.tt_base_pose.transform.rotation.w = rotquat[3] rospy.loginfo('Marker {:d} seen {:d} times'. format(self.marker_id, self.marker_seen_count)) rospy.loginfo('Current pose of TT base w.r.t. Kinect =' '[{:4.3f}, {:4.3f}, {:4.3f}]'. format(self.tt_base_pose.transform.translation.x, self.tt_base_pose.transform.translation.y, self.tt_base_pose.transform.translation.z)) break else: rospy.logwarn('Marker {:d} not seen, resetting counter'. format(self.marker_id))