def odometry_callback(self, msg): """Handle updated measured velocity callback.""" if not bool(self.config): return q = msg.orientation q = numpy.array([q.x, q.y, q.z, q.w]) if not self.initialized: # If this is the first callback: Store and hold latest pose. self.quat_des = q self.initialized = True # Compute control output: t = time_in_float_sec_from_msg(msg.header.stamp) # Error quaternion wrt body frame e_rot_quat = transf.quaternion_multiply(transf.quaternion_conjugate(q), self.quat_des) # Error angles e_rot = numpy.array(transf.euler_from_quaternion(e_rot_quat)) v_angular = self.pid_rot.regulate(e_rot, t) # Convert and publish vel. command: cmd_vel = geometry_msgs.Twist() cmd_vel.angular = geometry_msgs.Vector3(x=v_angular[0], y=v_angular[1], z=v_angular[2]) self.pub_cmd_vel.publish(cmd_vel)
def odometry_callback(msg, name): global vehicle_pub if name not in vehicle_pub: return x = msg.pose.pose.position.x y = msg.pose.pose.position.y orientation = np.array([ msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w ]) yaw = euler_from_quaternion(orientation)[2] new_marker = deepcopy(marker) points = list() for i in range(new_marker.shape[0]): new_marker[i, :] = np.dot(rot(yaw - np.pi / 2), new_marker[i, :]) new_marker[i, 0] += x new_marker[i, 1] += y p = Point32() p.x = new_marker[i, 0] p.y = new_marker[i, 1] points.append(p) new_poly = PolygonStamped() new_poly.header.stamp = rospy.Time.now() new_poly.header.frame_id = 'world' new_poly.polygon.points = points vehicle_pub[name].publish(new_poly)
def set_waypoints(self, waypoints, init_rot=(0, 0, 0, 1)): """Initializes the waypoint interpolator with a set of waypoints.""" self._logger.info('Initial rotation vector (quat)=%s', str(init_rot)) self._logger.info('Initial rotation vector (rpy)=%s', str(euler_from_quaternion(init_rot))) if self._wp_interp.init_waypoints(waypoints, init_rot): self._wp_interp_on = True return True else: return False
def odometry_callback(self, msg): """Handle updated measured velocity callback.""" if not bool(self.config): return p = msg.pose.pose.position q = msg.pose.pose.orientation p = numpy.array([p.x, p.y, p.z]) q = numpy.array([q.x, q.y, q.z, q.w]) if not self.initialized: # If this is the first callback: Store and hold latest pose. self.pos_des = p self.quat_des = q self.initialized = True # Compute control output: t = time_in_float_sec_from_msg(msg.header.stamp) # Position error e_pos_world = self.pos_des - p e_pos_body = transf.quaternion_matrix(q).transpose()[0:3, 0:3].dot( e_pos_world) # Error quaternion wrt body frame e_rot_quat = transf.quaternion_multiply(transf.quaternion_conjugate(q), self.quat_des) if numpy.linalg.norm(e_pos_world[0:2]) > 5.0: # special case if we are far away from goal: # ignore desired heading, look towards goal position heading = math.atan2(e_pos_world[1], e_pos_world[0]) quat_des = numpy.array( [0, 0, math.sin(0.5 * heading), math.cos(0.5 * heading)]) e_rot_quat = transf.quaternion_multiply( transf.quaternion_conjugate(q), quat_des) # Error angles e_rot = numpy.array(transf.euler_from_quaternion(e_rot_quat)) v_linear = self.pid_pos.regulate(e_pos_body, t) v_angular = self.pid_rot.regulate(e_rot, t) # Convert and publish vel. command: cmd_vel = geometry_msgs.Twist() cmd_vel.linear = geometry_msgs.Vector3(x=v_linear[0], y=v_linear[1], z=v_linear[2]) cmd_vel.angular = geometry_msgs.Vector3(x=v_angular[0], y=v_angular[1], z=v_angular[2]) self.pub_cmd_vel.publish(cmd_vel)
def odometry_callback(self, msg): x = msg.pose.pose.position.x y = msg.pose.pose.position.y orientation = np.array([ msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w ]) yaw = euler_from_quaternion(orientation)[2] # Generating the vehicle footprint marker new_marker = self._scale_footprint * deepcopy(self.MARKER) points = list() for i in range(new_marker.shape[0]): new_marker[i, :] = np.dot(self.rot(yaw - np.pi / 2), new_marker[i, :]) new_marker[i, 0] += x new_marker[i, 1] += y p = Point32() p.x = new_marker[i, 0] p.y = new_marker[i, 1] points.append(p) new_poly = PolygonStamped() new_poly.header.stamp = self.get_clock().now() #rospy.Time.now() new_poly.header.frame_id = 'world' new_poly.polygon.points = points self._footprint_pub.publish(new_poly) # Generating the label marker self._label_marker.pose.position.x = msg.pose.pose.position.x + self._label_x_offset self._label_marker.pose.position.y = msg.pose.pose.position.y self._label_marker.pose.position.z = msg.pose.pose.position.z self._label_pub.publish(self._label_marker)
def rot(self): """`numpy.array`: `roll`, `pitch` and `yaw` angles""" rpy = euler_from_quaternion(self._rot) return np.array([rpy[0], rpy[1], rpy[2]])
def odometry_callback(self, msg): """Handle odometry callback: The actual control loop.""" # Update local planner's vehicle position and orientation pos = [ msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z ] quat = [ msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w ] self.local_planner.update_vehicle_pose(pos, quat) # Compute the desired position t = time_in_float_sec(self.get_clock().now()) des = self.local_planner.interpolate(t) # Publish the reference ref_msg = TrajectoryPoint() ref_msg.header.stamp = self.get_clock().now().to_msg() ref_msg.header.frame_id = self.local_planner.inertial_frame_id ref_msg.pose.position = geometry_msgs.Point(**self.to_dict_vect3( *des.p)) ref_msg.pose.orientation = geometry_msgs.Quaternion( **self.to_dict_quat(*des.q)) ref_msg.velocity.linear = geometry_msgs.Vector3(**self.to_dict_vect3( *des.vel[0:3])) ref_msg.velocity.angular = geometry_msgs.Vector3(**self.to_dict_vect3( *des.vel[3::])) self.reference_pub.publish(ref_msg) p = self.vector_to_np(msg.pose.pose.position) forward_vel = self.vector_to_np(msg.twist.twist.linear) ref_vel = des.vel[0:3] q = self.quaternion_to_np(msg.pose.pose.orientation) rpy = euler_from_quaternion(q, axes='sxyz') # Compute tracking errors wrt world frame: e_p = des.p - p abs_pos_error = np.linalg.norm(e_p) abs_vel_error = np.linalg.norm(ref_vel - forward_vel) # Generate error message error_msg = TrajectoryPoint() error_msg.header.stamp = self.get_clock().now().to_msg() error_msg.header.frame_id = self.local_planner.inertial_frame_id error_msg.pose.position = geometry_msgs.Point(**self.to_dict_vect3( *e_p)) error_msg.pose.orientation = geometry_msgs.Quaternion( **self.to_dict_quat( *quaternion_multiply(quaternion_inverse(q), des.q))) error_msg.velocity.linear = geometry_msgs.Vector3(**self.to_dict_vect3( *(des.vel[0:3] - self.vector_to_np(msg.twist.twist.linear)))) error_msg.velocity.angular = geometry_msgs.Vector3( **self.to_dict_vect3( *(des.vel[3::] - self.vector_to_np(msg.twist.twist.angular)))) # Based on position tracking error: Compute desired orientation pitch_des = -math.atan2(e_p[2], np.linalg.norm(e_p[0:2])) # Limit desired pitch angle: pitch_des = max(-self.desired_pitch_limit, min(pitch_des, self.desired_pitch_limit)) yaw_des = math.atan2(e_p[1], e_p[0]) yaw_err = self.unwrap_angle(yaw_des - rpy[2]) # Limit yaw effort yaw_err = min(self.yaw_error_limit, max(-self.yaw_error_limit, yaw_err)) # Roll: P controller to keep roll == 0 roll_control = self.p_roll * rpy[0] # Pitch: P controller to reach desired pitch angle pitch_control = self.p_pitch * self.unwrap_angle( pitch_des - rpy[1]) + self.d_pitch * (des.vel[4] - msg.twist.twist.angular.y) # Yaw: P controller to reach desired yaw angle yaw_control = self.p_yaw * yaw_err + self.d_yaw * ( des.vel[5] - msg.twist.twist.angular.z) # Limit thrust thrust = min( self.max_thrust, self.p_gain_thrust * np.linalg.norm(abs_pos_error) + self.d_gain_thrust * abs_vel_error) thrust = max(self.min_thrust, thrust) rpy = np.array([roll_control, pitch_control, yaw_control]) # In case the world_ned reference frame is used, the convert it back # to the ENU convention to generate the reference fin angles rtf = deepcopy(self.rpy_to_fins) if self.local_planner.inertial_frame_id == 'world_ned': rtf[:, 1] *= -1 rtf[:, 2] *= -1 # Transform orientation command into fin angle set points fins = rtf.dot(rpy) # Check for saturation max_angle = max(np.abs(fins)) if max_angle >= self.max_fin_angle: fins = fins * self.max_fin_angle / max_angle thrust_force = self.thruster.tam_column * thrust self.thruster.publish_command(thrust_force[0]) cmd = FloatStamped() for i in range(self.n_fins): cmd.header.stamp = self.get_clock().now().to_msg() cmd.header.frame_id = '%s/fin%d' % (self.namespace, i) cmd.data = min(fins[i], self.max_fin_angle) cmd.data = max(cmd.data, -self.max_fin_angle) self.pub_cmd[i].publish(cmd) self.error_pub.publish(error_msg)