def rotate_and_publish(pose_stamped): # Transform position position = Vector3Stamped() position.vector.x = pose_stamped.pose.position.x position.vector.y = pose_stamped.pose.position.y position.vector.z = pose_stamped.pose.position.z position_frd = tf2_geometry_msgs.do_transform_vector3( position, transform_frd) # Transform rotation quaternion_xyz = Vector3Stamped() quaternion_xyz.vector.x = pose_stamped.pose.orientation.x quaternion_xyz.vector.y = pose_stamped.pose.orientation.y quaternion_xyz.vector.z = pose_stamped.pose.orientation.z quaternion_xyz_frd = tf2_geometry_msgs.do_transform_vector3( quaternion_xyz, transform_frd) # Construct and publish FRD position pose_stamped_frd = PoseStamped() pose_stamped_frd.header = pose_stamped.header pose_stamped_frd.pose.position.x = position_frd.vector.x pose_stamped_frd.pose.position.y = position_frd.vector.y pose_stamped_frd.pose.position.z = position_frd.vector.z pose_stamped_frd.pose.orientation.x = quaternion_xyz_frd.vector.x pose_stamped_frd.pose.orientation.y = quaternion_xyz_frd.vector.y pose_stamped_frd.pose.orientation.z = quaternion_xyz_frd.vector.z pose_stamped_frd.pose.orientation.w = pose_stamped.pose.orientation.w # WTF vision_pose_pub.publish(pose_stamped_frd) return pose_stamped_frd
def update_R_err(self): e_R = np.dot(self.desired_R_.transpose(), self.R_) - \ np.dot(self.R_.transpose(), self.desired_R_) self.e_R_ = self.vee(e_R) / 2.0 vec3_e_R = Vector3Stamped() vec3_e_R.header.stamp = rospy.Time.now() euler_des = self.rotationMatrixToEulerAngles(self.desired_R_) vec3_e_R.vector = Vector3(euler_des[0], euler_des[1], euler_des[2]) self.publisher_euler_des.publish(vec3_e_R) vec3_e_R = Vector3Stamped() vec3_e_R.header.stamp = rospy.Time.now() vec3_e_R.vector = Vector3(self.e_R_[0, 0], self.e_R_[1, 0], self.e_R_[2, 0]) self.publisher_err_R.publish(vec3_e_R) pub_vec3 = Vector3Stamped() pub_vec3.header.stamp = rospy.Time.now() pub_vec3.vector = Vector3(self.euler_quads[0, 0], self.euler_quads[1, 0], self.euler_quads[2, 0]) self.publisher_eulers.publish(pub_vec3) pub_vec3 = Vector3Stamped() pub_vec3.header.stamp = rospy.Time.now() pub_vec3.vector = Vector3(self.e_R_[0, 0], self.e_R_[1, 0], self.e_R_[2, 0]) self.publisher_err_angles.publish(pub_vec3)
def do_transform_imu(imu_msg, transform): """ Transforms the given Imu message into the given target frame :param imu_msg: :type imu_msg: Imu :param transform: :type transform: TransformStamped :return: Imu message transformed into the target frame :rtype: Imu """ src_l = Vector3Stamped() src_l.header = imu_msg.header src_l.vector = imu_msg.linear_acceleration src_a = Vector3Stamped() src_a.header = imu_msg.header src_a.vector = imu_msg.angular_velocity src_o = QuaternionStamped() src_o.header = imu_msg.header src_o.quaternion = imu_msg.orientation tgt_l = do_transform_vector3(src_l, transform) tgt_a = do_transform_vector3(src_a, transform) tgt_o = do_transform_quaternion(src_o, transform) tgt_imu = copy.deepcopy(imu_msg) tgt_imu.header.frame_id = transform.child_frame_id tgt_imu.linear_acceleration = tgt_l tgt_imu.angular_velocity = tgt_a tgt_imu.orientation = tgt_o return tgt_imu
def __init__(self): self.p = Vector3Stamped() self.p.header.frame_id = 'odom' self.g = PointStamped() self.g.point.x = -5 self.g.point.y = 5 self.g.point.z = 0 self.g.header.frame_id = 'odom' self.a1 = Vector3Stamped() self.a2 = Vector3Stamped() self.a3 = Vector3Stamped() self.a4 = Vector3Stamped() self.a1.vector.x = 0.18 self.a1.vector.y = 0 self.a1.vector.z = 0 self.a1.header.frame_id = 'base_link' self.a2.vector.x = 0 self.a2.vector.y = 0.18 self.a2.vector.z = 0 self.a2.header.frame_id = 'base_link' self.a3.vector.x = -0.18 self.a3.vector.y = 0 self.a3.vector.z = 0 self.a3.header.frame_id = 'base_link' self.a4.vector.x = 0 self.a4.vector.y = -0.18 self.a4.vector.z = 0 self.a4.header.frame_id = 'base_link' self.listener = tf.TransformListener() self.odom_msg = Odometry() self.msg_rfid = EpcInfo() self.base_EPC = rospy.get_param('RFIDrx/base1') self.sub_odom = rospy.Subscriber('odom', Odometry, self.odometry_cb) self.epc_publisher = rospy.Publisher('epcs', EpcInfo, queue_size=1)
def timer_callback(self): now_msg = self.get_clock().now().to_msg() info = {} self.PS.write(b"A") out = self.PS.readline() try: info = json.loads(out) except: pass if len(info) > 0: msg = Feet() msg.header.stamp = now_msg msg.right_front_force = info["RF"]["F"] msg.right_front_valid = info["RF"]["S"] == 1 msg.right_back_force = info["RB"]["F"] msg.right_back_valid = info["RB"]["S"] == 1 msg.left_front_force = info["LF"]["F"] msg.left_front_valid = info["LF"]["S"] == 1 msg.left_back_force = info["LB"]["F"] msg.left_back_valid = info["LB"]["S"] == 1 self.feetPublisher.publish(msg) msg = Vector3Stamped() msg.header.stamp = now_msg msg.vector.x = info["ANG"]["X"] msg.vector.y = info["ANG"]["Y"] msg.vector.z = info["ANG"]["Z"] self.posePublisher.publish(msg) msg = Vector3Stamped() msg.header.stamp = now_msg msg.vector.x = info["GYR"]["X"] msg.vector.y = info["GYR"]["Y"] msg.vector.z = info["GYR"]["Z"] self.twistPublisher.publish(msg)
def check_camera(name): try: img = rospy.wait_for_message(name + '/image_raw', Image, timeout=1) except rospy.ROSException: failure('%s: no images (is the camera connected properly?)', name) return try: camera_info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1) except rospy.ROSException: failure('%s: no calibration info', name) return if img.width != camera_info.width: failure('%s: calibration width doesn\'t match image width (%d != %d)', name, camera_info.width, img.width) if img.height != camera_info.height: failure('%s: calibration height doesn\'t match image height (%d != %d))', name, camera_info.height, img.height) try: optical = Vector3Stamped() optical.header.frame_id = img.header.frame_id optical.vector.z = 1 cable = Vector3Stamped() cable.header.frame_id = img.header.frame_id cable.vector.y = 1 optical = describe_direction(tf_buffer.transform(optical, 'base_link').vector) cable = describe_direction(tf_buffer.transform(cable, 'base_link').vector) if not optical or not cable: info('%s: custom camera orientation detected', name) else: info('camera is oriented %s, cable from camera goes %s', optical, cable) except tf2_ros.TransformException: failure('cannot transform from base_link to camera frame')
def execute(self): """this function gets called from pybullet ros main update loop""" if not self.cmd_vel_msg: return # check if timestamp is recent if (rospy.Time.now() - rospy.Duration(0.5)) > self.received_cmd_vel_time: return # transform Twist from base_link to odom (pybullet allows to set vel only on world ref frame) # NOTE: we would normally use tf for this, but there are issues currently between python 2 and 3 in ROS 1 lin_vec = Vector3Stamped() lin_vec.header.frame_id = 'base_link' lin_vec.vector.x = self.cmd_vel_msg.linear.x lin_vec.vector.y = self.cmd_vel_msg.linear.y lin_vec.vector.z = self.cmd_vel_msg.linear.z ang_vec = Vector3Stamped() ang_vec.header.frame_id = 'base_link' ang_vec.vector.x = self.cmd_vel_msg.angular.x ang_vec.vector.y = self.cmd_vel_msg.angular.y ang_vec.vector.z = self.cmd_vel_msg.angular.z lin_cmd_vel_in_odom = self.transformVector3('odom', lin_vec) ang_cmd_vel_in_odom = self.transformVector3('odom', ang_vec) # set vel directly on robot model self.pb.resetBaseVelocity(self.robot, [ lin_cmd_vel_in_odom.vector.x, lin_cmd_vel_in_odom.vector.y, lin_cmd_vel_in_odom.vector.z ], [ ang_cmd_vel_in_odom.vector.x, ang_cmd_vel_in_odom.vector.y, ang_cmd_vel_in_odom.vector.z ])
def test_attach_existing_box_non_fixed(self, better_pose): """ :type zero_pose: Donbot """ pocky = u'box' p = PoseStamped() p.header.frame_id = u'refills_finger' p.pose.position.y = -0.075 p.pose.orientation = Quaternion(0, 0, 0, 1) better_pose.add_box(pocky, [0.05, 0.2, 0.03], p) better_pose.attach_existing(pocky, frame_id=u'refills_finger') tip_normal = Vector3Stamped() tip_normal.header.frame_id = pocky tip_normal.vector.y = 1 root_normal = Vector3Stamped() root_normal.header.frame_id = u'base_footprint' root_normal.vector.z = 1 better_pose.align_planes(pocky, tip_normal, u'base_footprint', root_normal) pocky_goal = PoseStamped() pocky_goal.header.frame_id = pocky pocky_goal.pose.position.y = -.5 pocky_goal.pose.position.x = .3 pocky_goal.pose.position.z = -.2 pocky_goal.pose.orientation.w = 1 better_pose.allow_self_collision() better_pose.set_translation_goal(pocky_goal, pocky, u'base_footprint') better_pose.send_and_check_goal()
def __init__(self): self.g = PointStamped() self.listener = tf.TransformListener() rospy.Subscriber('odom', Odometry, self.odometry_cb) self.odom_msg = Odometry() self.a1 = Vector3Stamped() self.a2 = Vector3Stamped() self.a3 = Vector3Stamped() self.a4 = Vector3Stamped() self.a1.vector.x = 0.18 self.a1.vector.y = 0 self.a1.vector.z = 0 self.a1.header.frame_id = 'base_link' self.a2.vector.x = 0 self.a2.vector.y = 0.18 self.a2.vector.z = 0 self.a2.header.frame_id = 'base_link' self.a3.vector.x = -0.18 self.a3.vector.y = 0 self.a3.vector.z = 0 self.a3.header.frame_id = 'base_link' self.a4.vector.x = 0 self.a4.vector.y = -0.18 self.a4.vector.z = 0 self.a4.header.frame_id = 'base_link' self.p = Vector3Stamped() self.p.header.frame_id = 'odom' self.yaw = [0, np.pi / 2, np.pi, 3 * np.pi / 2] self.pose = PoseStamped() self.pose.header.frame_id = 'base_link' self.rate = rospy.Rate(1) self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction) self.goal_pose = MoveBaseGoal()
def __init__(self): self.euler_msg = Vector3Stamped() self.euler_deg_msg = Vector3Stamped() # Create subscribers and publishers. self.pub_imu_euler = rospy.Publisher("imu_euler", Vector3Stamped, queue_size=1) self.pub_imu_euler_deg = rospy.Publisher("imu_euler_deg", Vector3Stamped, queue_size=1) self.pub_odom_euler = rospy.Publisher("odom_euler", Vector3Stamped, queue_size=1) self.pub_odom_euler_deg = rospy.Publisher("odom_euler_deg", Vector3Stamped, queue_size=1) self.pub_pose_euler = rospy.Publisher("pose_euler", Vector3Stamped, queue_size=1) self.pub_pose_euler_deg = rospy.Publisher("pose_euler_deg", Vector3Stamped, queue_size=1) sub_imu = rospy.Subscriber("imu", Imu, self.imu_callback) sub_odom = rospy.Subscriber("odom", Odometry, self.odom_callback) sub_pose = rospy.Subscriber("pose", Pose, self.pose_callback)
def simpleTrajectory2PPTrajectory(traj): t = Trajectory() for i in range(len(traj.t)): p = Vector3Stamped() v = Vector3Stamped() a = Vector3Stamped() j = Vector3Stamped() p.header.stamp = rospy.Time(traj.t[i]) p.vector.x = traj.px[i] p.vector.y = traj.py[i] p.vector.z = traj.pz[i] v.header.stamp = rospy.Time(traj.t[i]) v.vector.x = traj.vx[i] v.vector.y = traj.vy[i] v.vector.z = traj.vz[i] a.header.stamp = rospy.Time(traj.t[i]) a.vector.x = traj.ax[i] a.vector.y = traj.ay[i] a.vector.z = traj.az[i] j.header.stamp = rospy.Time(traj.t[i]) j.vector.x = traj.jx[i] j.vector.y = traj.jy[i] j.vector.z = traj.jz[i] t.pos.append(p) t.vel.append(v) t.acc.append(a) t.jerk.append(j) return t
def createTrajMsg(): data = readFromFile('trajectory.txt') traj = Trajectory() z_default = 0.5 z_velDefault = 0 z_accDefault = 0 for data_line in data: #timestamp #time at data_line[0] #traj.header.stamp.sec = data_line[0] posLine = Vector3Stamped() posLine.header.stamp = rospy.Time(data_line[0]) posLine.vector.x = data_line[1] posLine.vector.y = data_line[2] posLine.vector.z = z_default traj.pos.append(posLine) velLine = Vector3Stamped() velLine.header = posLine.header velLine.vector.x = data_line[3] velLine.vector.y = data_line[4] velLine.vector.z = z_velDefault traj.vel.append(velLine) accLine = Vector3Stamped() accLine.header = posLine.header accLine.vector.x = data_line[5] accLine.vector.y = data_line[6] accLine.vector.z = z_accDefault traj.acc.append(accLine) print traj return traj
def _calc_accel(self): """ Calculate Acceleration Setpoint from Twist Setpoint PID """ if self._latest_vel_sp is None: rospy.logwarn_throttle( 10.0, "{} | No vel setpoint.".format(rospy.get_name())) return False if self._latest_odom_fb is None: rospy.logwarn_throttle( 10.0, "{} | No vel feedback.".format(rospy.get_name())) return False # Convert setpoint velocity into the correct frame if self._latest_vel_sp.header.frame_id != self._latest_odom_fb.child_frame_id and len( self._latest_vel_sp.header.frame_id) > 0: if self._tf_buff.can_transform(self._latest_odom_fb.child_frame_id, self._latest_vel_sp.header.frame_id, rospy.Time.from_sec(0.0)): cmd_vel = TwistStamped() header = Header(self._latest_vel_sp.header.seq, rospy.Time.from_sec(0.0), self._latest_vel_sp.header.frame_id) cmd_vel.twist.linear = self._tf_buff.transform( Vector3Stamped(header, self._latest_vel_sp.twist.linear), self._latest_odom_fb.child_frame_id, rospy.Duration(5)).vector cmd_vel.twist.angular = self._tf_buff.transform( Vector3Stamped(header, self._latest_vel_sp.twist.angular), self._latest_odom_fb.child_frame_id, rospy.Duration(5)).vector cmd_vel.header.stamp = rospy.Time.now() else: rospy.logwarn_throttle( 5, "{} | Cannot TF Velocity SP frame_id: {}".format( rospy.get_name(), self._latest_vel_sp.header.frame_id)) return False else: cmd_vel = self._latest_vel_sp clean_linear_vel = self._enforce_deadband( self._latest_odom_fb.twist.twist.linear) clean_angular_vel = self._enforce_deadband( self._latest_odom_fb.twist.twist.angular) vel_err = np.array([[cmd_vel.twist.linear.x - clean_linear_vel.x], [cmd_vel.twist.linear.y - clean_linear_vel.y], [cmd_vel.twist.linear.z - clean_linear_vel.z], [ cmd_vel.twist.angular.z - self._latest_odom_fb.twist.twist.angular.z ]], dtype=float) accel_sp_msg = AccelStamped() accel_sp_msg.header = cmd_vel.header accel_sp_msg.accel.linear.x, accel_sp_msg.accel.linear.y, accel_sp_msg.accel.linear.z, accel_sp_msg.accel.angular.z = self._vel_pids( vel_err, self._latest_odom_fb.header.stamp.to_sec()) accel_sp_msg.accel.linear.y = 0 if not self._control_sway else accel_sp_msg.accel.linear.y accel_sp_msg.accel.linear.z = 0 if not self._control_depth else accel_sp_msg.accel.linear.z accel_sp_msg.accel.angular.z = 0 if not self._control_yaw else accel_sp_msg.accel.angular.z self._latest_accel_sp = accel_sp_msg return True
def __init__(self): rospy.init_node("imu_orient") # Subscribers and frames for Arduino data rospy.Subscriber("imu/data_array", ImuArray, self.imu_array_callback, queue_size=1) rospy.Subscriber("imu/mag_array", ImuMag, self.imu_mag_callback, queue_size=1) rospy.Subscriber("imu/calibration", ImuCalibration, self.imu_calib_callback, queue_size=1) self.odom_frame_id = "odom" self.base_frame_id = "base_link" self.imu_frame_id = "imu_link" # Messages and publishers for conversion self.imu_msg = Imu() self.imu_base_link_msg = Imu() self.imu_msg.header.frame_id = self.imu_frame_id self.imu_base_link_msg.header.frame_id = self.base_frame_id self.imu_pub = rospy.Publisher("imu/data", Imu, queue_size=10) self.imu_rpy_msg = Vector3Stamped() self.imu_rpy_pub = rospy.Publisher("imu/rpy", Vector3Stamped, queue_size=10) self.imu_mag_msg = Vector3Stamped() self.imu_mag_pub = rospy.Publisher("imu/mag", Vector3Stamped, queue_size=10) # Add transform to visualize orientation #self.odom_broadcaster = TransformBroadcaster() # Listener for base_link to imu_link transform to convert imu message into base_link frame # Parameters for checking and storing calibration data self.last_save_time = time() self.calibration = ImuCalibration() # Obtain the filepath for the IMU package (currently in 'sensors') rospack = rospkg.RosPack() self.package_path = rospack.get_path('sensors') self.imu_calib_filename = rospy.get_param( "/imu/calib_filename", self.package_path + "/config/imu_calibration.yaml") # Procedure # -save initial time of startup for when the calibration parameters were set # -save calibration status and values when messages are received # -during update function, check whether the calibration status is all 3's # -if the status is all 3's then get the time since that last saved file # -if time since last save has been one minute or more, save the new # parameters in a yaml file and update the last save time # Loop rate for repeated code self.rate = rospy.Rate(30)
def test_place_in_shelf(self, shelf_setup): """ :type shelf_setup: Donbot """ box = u'box' box_pose = PoseStamped() box_pose.header.frame_id = u'map' box_pose.pose.orientation.w = 1 box_pose.pose.position.y = -0.5 box_pose.pose.position.z = .1 shelf_setup.add_box(box, [0.05, 0.05, 0.2], box_pose) grasp_pose = deepcopy(box_pose) grasp_pose.pose.position.z += 0.05 grasp_pose.pose.orientation = Quaternion(*quaternion_from_matrix([[-1, 0, 0, 0], [0, 0, 1, 0], [0, 1, 0, 0], [0, 0, 0, 1]])) shelf_setup.allow_collision([CollisionEntry.ALL], box, [CollisionEntry.ALL]) # shelf_setup.allow_all_collisions() shelf_setup.set_and_check_cart_goal(grasp_pose, u'refills_finger') shelf_setup.attach_existing(box, u'refills_finger') box_goal = PoseStamped() box_goal.header.frame_id = u'map' box_goal.pose.position.z = 1.12 box_goal.pose.position.y = -.9 grasp_pose.pose.orientation.w = 1 shelf_setup.set_translation_goal(box_goal, box) tip_normal = Vector3Stamped() tip_normal.header.frame_id = box tip_normal.vector.z = 1 root_normal = Vector3Stamped() root_normal.header.frame_id = u'base_footprint' root_normal.vector.z = 1 shelf_setup.align_planes(box, tip_normal, u'base_footprint', root_normal) shelf_setup.send_and_check_goal() box_goal = PoseStamped() box_goal.header.frame_id = box box_goal.pose.position.y = -0.2 grasp_pose.pose.orientation.w = 1 shelf_setup.set_translation_goal(box_goal, box) tip_normal = Vector3Stamped() tip_normal.header.frame_id = box tip_normal.vector.z = 1 root_normal = Vector3Stamped() root_normal.header.frame_id = u'base_footprint' root_normal.vector.z = 1 shelf_setup.align_planes(box, tip_normal, u'base_footprint', root_normal) shelf_setup.send_and_check_goal()
def do_transform_wrench(wrench, transform): force = Vector3Stamped() torque = Vector3Stamped() force.vector = wrench.wrench.force torque.vector = wrench.wrench.torque res = WrenchStamped() res.wrench.force = do_transform_vector3(force, transform).vector res.wrench.torque = do_transform_vector3(torque, transform).vector res.header = transform.header return res
def twist_in_frame(tf_buffer, twist_s, frame_id): t = get_transform(tf_buffer, frame_id, twist_s.header.frame_id) if not t: return None h = Header(frame_id=frame_id, stamp=twist_s.header.stamp) vs_l = Vector3Stamped(header=h, vector=twist_s.twist.linear) vs_a = Vector3Stamped(header=h, vector=twist_s.twist.angular) msg = TwistStamped(header=h) msg.twist.linear = tf2_geometry_msgs.do_transform_vector3(vs_l, t).vector msg.twist.angular = tf2_geometry_msgs.do_transform_vector3(vs_a, t).vector return msg
def transform_twist(listener, base_frame, target_frame, twist): lin = Vector3Stamped() ang = Vector3Stamped() lin.vector = twist.linear ang.vector = twist.angular lin.header.frame_id = base_frame ang.header.frame_id = base_frame twist_tf = Twist() twist_tf.linear = listener.transformVector3(target_frame, lin).vector twist_tf.angular = listener.transformVector3(target_frame, ang).vector return twist_tf
def callback(self, qtc, ppl): vels = [] try: t = self.tf.getLatestCommonTime("base_link", ppl.header.frame_id) vs = Vector3Stamped(header=ppl.header) vs.header.stamp = t for v in ppl.velocities: vs.vector = v vels.append(self.tf.transformVector3("base_link", vs).vector) except Exception as e: rospy.logwarn(e) return data_buffer = { e.uuid: { "qtc": json.loads(e.qtc_serialised), "angle": ppl.angles[ppl.uuids.index(e.uuid)], "velocity": vels[ppl.uuids.index(e.uuid)] } for e in qtc.qtc } try: element = data_buffer[ppl.uuids[ppl.distances.index( ppl.min_distance)]] # Only taking the closest person for now self.publish_closest_person_marker( ppl.poses[ppl.distances.index(ppl.min_distance)], ppl.header.frame_id) except KeyError: return qtc = element["qtc"].split(',') self.cc.publish(angle=element["angle"], qtc_symbol=','.join([qtc[0]] if len(qtc) == 2 else [qtc[0], qtc[2]]), velocity=element["velocity"])
def getState(self, posData): # odomEstimate = self.getOdomEstimate() # velxEstimate = odomEstimate.twist.twist.linear.x # velyEstimate = odomEstimate.twist.twist.linear.y pose = posData.pose[1] position = pose.position orientation = pose.orientation t = tf.TransformerROS(True, rospy.Duration(10.0)) m = TransformStamped() m.header.frame_id = 'world' m.child_frame_id = 'base_link' m.transform.translation.x = position.x m.transform.translation.y = position.y m.transform.translation.z = position.z m.transform.rotation.x = orientation.x m.transform.rotation.y = orientation.y m.transform.rotation.z = orientation.z m.transform.rotation.w = orientation.w (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w]) t.setTransform(m) velx = posData.twist[1].linear.x vely = posData.twist[1].linear.y velz = posData.twist[1].linear.z # Transform to body frame velocities velVector = Vector3Stamped() velVector.vector.x = velx velVector.vector.y = vely velVector.vector.z = velz velVector.header.frame_id = "world" # velVectorTransformed = self.tl.transformVector3("base_link", velVector) velVectorTransformed = t.transformVector3("base_link", velVector) velxBody = velVectorTransformed.vector.x velyBody = velVectorTransformed.vector.y velzBody = velVectorTransformed.vector.z carTangentialSpeed = math.sqrt(velx ** 2 + vely ** 2) carAngularVel = posData.twist[1].angular.z stateInfo = { "x": position.x, "y": position.y, "i": orientation.x, "j": orientation.y, "k": orientation.z, "w": orientation.w, "xdot": velx, "ydot": vely, "thetadot": carAngularVel, "s": carTangentialSpeed, "xdotbodyframe": velxBody, "ydotbodyframe": velyBody } state = [] for key in self.state_info: state.append(stateInfo[key]) #rewardState = [stateInfo["thetadot"], stateInfo["xdotbodyframe"], stateInfo["ydotbodyframe"]] return np.array(state)
def write_sub(self, vector, parameter): output = Vector3Stamped() output.header.frame_id = parameter output.vector.x = vector[0] output.vector.y = vector[1] output.vector.z = vector[2] return output
def set_accel(self): plc = self.ros_data["plc"] rx, ry, rz, r, p, y = self.parse_local_position("e") local_pos = np.array([rx, ry, rz]) self.path_rec.append(local_pos.copy()) loc_goal = self.goal - local_pos ax, ay, az = self.parse_linear_acc() vx, vy, vz, _, _, _ = self.parse_velocity() state = np.array([rx, ry, rz, vx, vy, vz, ax, ay, az]) accel, self.loc_goal_old, self.f_angle = control_method.start( plc, state, loc_goal, r, p, y, self.loc_goal_old, self.f_angle, self.path_rec) acc = Vector3Stamped() acc.header = Header() acc.header.frame_id = "map" acc.header.stamp = rospy.Time.now() acc.vector.x = accel[0] acc.vector.y = accel[1] acc.vector.z = -accel[2] - self.g # acc.accel.angular.x = 0 # acc.accel.angular.y = 0 # acc.accel.angular.z = 0 # rospy.Publisher( # 'mavros/setpoint_accel/send_force', True, queue_size=10) self.pos_setacc_pub.publish(acc)
def my_callback(event): global mov_x global mov_y global abs_x global abs_y vec = Vector3Stamped() vec.header.stamp = rospy.Time.now() vec.vector = Vector3(x=mov_x*scaling, y=mov_y*scaling, z=0) pub.publish(vec) print(vec) #odom = Odometry() #odom.header.stamp = rospy.Time.now() #try: # transf = tfBuffer.lookup_transform(BASE_FRAME, 'mouse_front', rospy.Time()) # vec = tf2_geometry_msgs.do_transform_vector3(vec, transf) # abs_x += vec.vector.x # abs_y += vec.vector.y #except Exception as ex: # print(ex) # abs_x = abs_x + mov_x*scaling # abs_y = abs_y + mov_y*scaling #odom.header.frame_id = "mouse_front" # since all odometry is 6DOF we'll need a quaternion created from yaw #odom_quat = tf.transformations.quaternion_from_euler(0, 0, 0) #odom.pose.pose = Pose(Point(abs_x, abs_y, 0.), Quaternion(*odom_quat)) #odom.child_frame_id = "base_link" #odom.twist.twist = Twist(Vector3(mov_x, mov_y, 0), Vector3(0, 0, 0)) #odom_pub.publish(odom) mov_x = 0 mov_y = 0
def create_vector3_stamped(vector, frame_id='base_link'): m = Vector3Stamped() m.header.frame_id = frame_id m.header.stamp = rospy.Time() #m.header.stamp = rospy.get_rostime() m.vector = Vector3(*vector) return m
def get_lines(self, fmt): t = Time() t.secs = 0 # MESSAGE_TO_TUM_SHORT line = ROSMsg2CSVLine.to(fmt, Point(), t, ROSMessageTypes.GEOMETRY_MSGS_VECTOR3) line = ROSMsg2CSVLine.to(fmt, PointStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_POINTSTAMPED) line = ROSMsg2CSVLine.to(fmt, Vector3(), t, ROSMessageTypes.GEOMETRY_MSGS_VECTOR3) line = ROSMsg2CSVLine.to(fmt, Vector3Stamped(), t, ROSMessageTypes.GEOMETRY_MSGS_VECTOR3STAMPED) line = ROSMsg2CSVLine.to( fmt, PoseWithCovarianceStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED) line = ROSMsg2CSVLine.to( fmt, PoseWithCovariance(), t, ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCE) line = ROSMsg2CSVLine.to(fmt, PoseStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_POSESTAMPED) line = ROSMsg2CSVLine.to(fmt, Pose(), t, ROSMessageTypes.GEOMETRY_MSGS_POSE) line = ROSMsg2CSVLine.to(fmt, Quaternion(), t, ROSMessageTypes.GEOMETRY_MSGS_QUATERNION) line = ROSMsg2CSVLine.to( fmt, QuaternionStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_QUATERNIONSTAMPED) line = ROSMsg2CSVLine.to(fmt, Transform(), t, ROSMessageTypes.GEOMETRY_MSGS_TRANSFORM) line = ROSMsg2CSVLine.to( fmt, TransformStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_TRANSFORMSTAMPED) return line
def pub_rew(self, rx, ry=0, rz=0): reward = Vector3Stamped() reward.vector.x = rx reward.vector.y = ry reward.vector.z = rz reward.header.stamp = rospy.Time.now() self._ros_reward_pub.publish(reward)
def __init__(self): # Create a publisher for acceleration data self.pub = rospy.Publisher('/master/control/error', Vector3Stamped, queue_size=10) # Create variable for use self.error_vector = Vector3Stamped() # The offset of the quad from were it wants to be self.setpoint_x = 0 # Creating these values here in the master controller self.setpoint_y = 0 self.setpoint_z = 0 self.measured_value_x = 0 # Subscribe to these, x and y are the roomba location self.measured_value_y = 0 self.measured_value_z = 0 # Subscribe to this, the altitude self.rate = rospy.Rate(10) # 10hz self.state = 1 # starts in the take off state self.at_target_location = 0 # 1. need to produce this self.look_for_roomba = 0 # self.obstacle_in_the_way = 0 # 4. Subscribe to this? self.need_to_land = 0 # 5. calculate here? self.main() # Runs the main def
def __init__(self): self.listener = TransformListener() # Subscribe to the ROS topic that contains the grasps. self.sub = rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, self.grasp_callback) self.pub = rospy.Publisher('/object_pose', Pose, queue_size=1) self.grasp_acc = [] self.tempPoint = PointStamped() #self.msg = PointStamped() self.transformedPoint = PointStamped() self.orient = Vector3Stamped() ## initiate a RobotCommander object. This object is an interface to the ## robot as a whole self.robot = moveit_commander.RobotCommander() ## Instantiate a PlanningSceneInterface object. This object is an interface ## to the world surrounding the robot. self.scene = moveit_commander.PlanningSceneInterface() ## Instantiate a MoveGroupCommander object. This object is an interface ## to one group of joints. self.group = moveit_commander.MoveGroupCommander("manipulator") ## We create this DisplayTrajectory publisher which is used below to publish ## trajectories for RVIZ to visualize. self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
def __init__(self, god_map, tip, goal_point, root=None, pointing_axis=None, weight=HIGH_WEIGHT): super(Pointing, self).__init__(god_map) if root is None: self.root = self.get_robot().get_root() else: self.root = root self.tip = tip goal_point = convert_dictionary_to_ros_message(u'geometry_msgs/PointStamped', goal_point) goal_point = transform_point(self.root, goal_point) if pointing_axis is not None: pointing_axis = convert_dictionary_to_ros_message(u'geometry_msgs/Vector3Stamped', pointing_axis) pointing_axis = transform_vector(self.tip, pointing_axis) else: pointing_axis = Vector3Stamped() pointing_axis.header.frame_id = self.tip pointing_axis.vector.z = 1 tmp = np.array([pointing_axis.vector.x, pointing_axis.vector.y, pointing_axis.vector.z]) tmp = tmp / np.linalg.norm(tmp) # TODO possible /0 pointing_axis.vector = Vector3(*tmp) params = {self.goal_point: goal_point, self.pointing_axis: pointing_axis, self.weight: weight} self.save_params_on_god_map(params)
def __init__(self): # Create a publisher for acceleration data self.pub = rospy.Publisher('/roomba/location_meters', Vector3Stamped, queue_size=10) self.rate = rospy.Rate(10) # 10hz # For finding radius self.a = 391.3 self.b = -0.9371 self.radius = 0 # Altitude self.alt = 0.8128 # 32 inches ~ as high as the countertop # NEEDS TO CHANGE FROM HARD CODE TO SUBSCRIPTION self.location_new = Vector3Stamped() self.location_old = Vector3Stamped() self.cap = cv2.VideoCapture(0) self.loop_search()