def odometry_callback(self, msg): """Handle updated measured velocity callback.""" if not bool(self.config): return linear = msg.twist.twist.linear angular = msg.twist.twist.angular v_linear = numpy.array([linear.x, linear.y, linear.z]) v_angular = numpy.array([angular.x, angular.y, angular.z]) if self.config['odom_vel_in_world']: # This is a temp. workaround for gazebo's pos3d plugin not behaving properly: # Twist should be provided wrt child_frame, gazebo provides it wrt world frame # see http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html xyzw_array = lambda o: numpy.array([o.x, o.y, o.z, o.w]) q_wb = xyzw_array(msg.pose.pose.orientation) R_bw = transf.quaternion_matrix(q_wb)[0:3, 0:3].transpose() v_linear = R_bw.dot(v_linear) v_angular = R_bw.dot(v_angular) # Compute compute control output: t = time_in_float_sec_from_msg(msg.header.stamp) e_v_linear = (self.v_linear_des - v_linear) e_v_angular = (self.v_angular_des - v_angular) a_linear = self.pid_linear.regulate(e_v_linear, t) a_angular = self.pid_angular.regulate(e_v_angular, t) # Convert and publish accel. command: cmd_accel = geometry_msgs.Accel() cmd_accel.linear = geometry_msgs.Vector3(x=a_linear[0], y=a_linear[1], z=a_linear[2]) cmd_accel.angular = geometry_msgs.Vector3(x=a_angular[0], y=a_angular[1], z=a_angular[2]) self.pub_cmd_accel.publish(cmd_accel)
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(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 from_message(self, msg): """Parse a trajectory point message of type `uuv_control_msgs/TrajectoryPoint` into the `uuv_trajectory_generator/TrajectoryPoint`. > *Input arguments* * `msg` (*type:* `uuv_control_msgs/TrajectoryPoint`): Input trajectory message """ t = time_in_float_sec_from_msg(msg.header.stamp) p = msg.pose.position q = msg.pose.orientation v = msg.velocity.linear w = msg.velocity.angular a = msg.acceleration.linear al = msg.acceleration.angular self._t = t self._pos = np.array([p.x, p.y, p.z]) self._rot = np.array([q.x, q.y, q.z, q.w]) self._vel = np.array([v.x, v.y, v.z, w.x, w.y, w.z]) self._acc = np.array([a.x, a.y, a.z, al.x, al.y, al.z]) return True