Esempio n. 1
0
    def _msg(self, quaternion, x_dist, y_dist, linear_speed, angular_speed, now, use_pose_ekf=False):
        # next, we will publish the odometry message over ROS
        msg = Odometry()
        msg.header.frame_id = "odom"
        msg.header.stamp = now
        msg.pose.pose = Pose(Point(x_dist, y_dist, 0.0), quaternion)

        msg.child_frame_id = "base_link"
        msg.twist.twist = Twist(Vector3(linear_speed, 0, 0), Vector3(0, 0, angular_speed))

        if use_pose_ekf:
            # robot_pose_ekf needs these covariances and we may need to adjust them.
            # From: ~/turtlebot/src/turtlebot_create/create_node/src/create_node/covariances.py
            # However, this is not needed because we are not using robot_pose_ekf
            # odometry.pose.covariance = [1e-3, 0, 0, 0, 0, 0,
            # 0, 1e-3, 0, 0, 0, 0,
            #                         0, 0, 1e6, 0, 0, 0,
            #                         0, 0, 0, 1e6, 0, 0,
            #                         0, 0, 0, 0, 1e6, 0,
            #                         0, 0, 0, 0, 0, 1e3]
            #
            # odometry.twist.covariance = [1e-3, 0, 0, 0, 0, 0,
            #                          0, 1e-3, 0, 0, 0, 0,
            #                          0, 0, 1e6, 0, 0, 0,
            #                          0, 0, 0, 1e6, 0, 0,
            #                          0, 0, 0, 0, 1e6, 0,
            #                          0, 0, 0, 0, 0, 1e3]
            pass

        return msg
Esempio n. 2
0
        def tf_and_odom(self, zumo_msg):
            """Broadcast tf transform and publish odometry.  It seems a little
            redundant to do both, but both are needed if we want to use the ROS
            navigation stack."""
    
            trans = (zumo_msg.x, zumo_msg.y, 0)
            rot = tf.transformations.quaternion_from_euler(0, 0, zumo_msg.theta)

            self.broadcaster.sendTransform(trans, rot, \
               rospy.Time.now(), \
               "base_link", \
               "odom")

            odom = Odometry()
            odom.header.frame_id = 'odom'
            odom.header.stamp = rospy.Time.now()
            odom.pose.pose.position.x = trans[0]
            odom.pose.pose.position.y = trans[1]
            odom.pose.pose.position.z = trans[2]
            odom.pose.pose.orientation.x = rot[0]
            odom.pose.pose.orientation.y = rot[1]
            odom.pose.pose.orientation.z = rot[2]
            odom.pose.pose.orientation.w = rot[3]
            odom.child_frame_id = 'base_link'
            odom.twist.twist = self.stored_twist

            self.odomPublisher.publish(odom)
def getOdometry(odometry_line):
    # Timestamp [seconds.microseconds]
    # Rolling Counter [signed 16bit integer]
    # TicksRight [ticks]
    # TicksLeft [ticks]
    # X [m]*
    # Y [m]*
    # Theta [rad]*
    # => 1235603339.32339, 4103, 2464, 2559, 1.063, 0.008, -0.028
    str_x, str_y, str_th = odometry_line.split(', ')[4:7]
    current_time = rospy.Time.now()
    th = float(str_th)
    x = float(str_x)
    y = float(str_y)
    z = 0.0
    roll = 0
    pitch = 0
    yaw = th
    qx, qy, qz, qw = tf.transformations.quaternion_from_euler(roll, pitch, yaw)

    # BUILDING ODOMETRY
    msg = Odometry()
    msg.header.stamp = current_time
    msg.header.frame_id = 'odom'
    msg.child_frame_id = 'base_link'
    msg.pose.pose.position = Point(x, y, z)
    msg.pose.pose.orientation = Quaternion(qx, qy, qz, qw)

    return msg
Esempio n. 4
0
 def pub_ekf_odom(self, msg):
     odom = Odometry()
     odom.header = msg.header
     odom.child_frame_id = 'base_footprint'
     odom.pose = msg.pose
     
     self.ekf_pub.publish(odom)
Esempio n. 5
0
    def publish_odom(self, cur_x, cur_y, cur_theta, vx, vth):
        quat = tf.transformations.quaternion_from_euler(0, 0, cur_theta)
        current_time = rospy.Time.now()

        br = tf.TransformBroadcaster()
        br.sendTransform((cur_x, cur_y, 0),
                         tf.transformations.quaternion_from_euler(0, 0, cur_theta),
                         current_time,
                         "base_footprint",
                         "odom")

        odom = Odometry()
        odom.header.stamp = current_time
        odom.header.frame_id = 'odom'

        odom.pose.pose.position.x = cur_x
        odom.pose.pose.position.y = cur_y
        odom.pose.pose.position.z = 0.0
        odom.pose.pose.orientation = Quaternion(*quat)

        odom.pose.covariance[0] = 0.01
        odom.pose.covariance[7] = 0.01
        odom.pose.covariance[14] = 99999
        odom.pose.covariance[21] = 99999
        odom.pose.covariance[28] = 99999
        odom.pose.covariance[35] = 0.01

        odom.child_frame_id = 'base_footprint'
        odom.twist.twist.linear.x = vx
        odom.twist.twist.linear.y = 0
        odom.twist.twist.angular.z = vth
        odom.twist.covariance = odom.pose.covariance

        self.odom_pub.publish(odom)
Esempio n. 6
0
def publish_odom(event):
    global motor_controller, od
    pos = motor_controller.position
    covariance = Config.get_covariance_matrix()

    odom = Odometry()
    odom.header.frame_id = "odom"
    odom.child_frame_id = "base_link"

    if rospy.get_time() - pos.position_time > 50.0/1000:
        odom.header.stamp = rospy.Time.now()
    else:
        odom.header.stamp = rospy.Time.from_sec(pos.position_time)

    odom.pose.pose.position.x = pos.x
    odom.pose.pose.position.y = pos.y

    odom.pose.pose.orientation.z = math.sin(pos.yaw / 2)
    odom.pose.pose.orientation.w = math.cos(pos.yaw / 2)
    odom.pose.covariance = covariance

    odom.twist.twist.linear.x = pos.linear_vel
    odom.twist.twist.angular.z = pos.angular_vel
    odom.twist.covariance = covariance

    odom_publisher.publish(odom)
Esempio n. 7
0
    def publish_odometry(self, odom_combined):
        """
            use current pose, along with delta from last pose to publish 
            the current Odometry on the /odom topic
        """

        if not self.last_time:
            # set up initial times and pose
            rospy.loginfo("Setting up initial position")
            self.last_time, self.last_x, y, self.last_theta = self.current_pose(odom_combined)
            return

        # publish to the /odom topic
        odom = Odometry()
        odom.header.stamp = rospy.Time.now()
        odom.header.frame_id = "/base_link"
        odom.pose = odom_combined.pose

        current_time, x, y, theta = self.current_pose(odom_combined)
        dt = current_time - self.last_time
        dt = dt.to_sec()
        d_x = x - self.last_x
        d_theta = theta - self.last_theta
        odom.twist.twist = Twist(Vector3(d_x/dt, 0, 0), Vector3(0, 0, d_theta/dt))

        self.odom_publisher.publish(odom)

        self.last_time, self.last_x, self.last_theta = current_time, x, theta
	def Publish_Odom_Tf(self):
		
		
	#quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
		quaternion = Quaternion()
		quaternion.x = 0 
		quaternion.y = 0
		quaternion.z = math.sin(self.Heading / 2.0)
		quaternion.w = math.cos(self.Heading / 2.0)
			
		rosNow = rospy.Time.now()
		self._tf_broad_caster.sendTransform(
				(self.X_Pos, self.Y_Pos, 0), 
					(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
					rosNow,
					"base_footprint",
					"odom"
					)

		# next, we'll publish the odometry message over ROS
		odometry = Odometry()
		odometry.header.frame_id = "odom"
		odometry.header.stamp = rosNow
		odometry.pose.pose.position.x = self.X_Pos
		odometry.pose.pose.position.y = self.Y_Pos
		odometry.pose.pose.position.z = 0
		odometry.pose.pose.orientation = quaternion


		odometry.child_frame_id = "base_link"
		odometry.twist.twist.linear.x = self.Velocity
		odometry.twist.twist.linear.y = 0
		odometry.twist.twist.angular.z = self.Omega
		self._odom_publisher.publish(odometry)
    def odom_transform_2d(self, data, new_frame, trans, rot):
        # NOTES: only in 2d rotation
        # also, it removes covariance, etc information
        odom_new = Odometry()
        odom_new.header = data.header
        odom_new.header.frame_id = new_frame

        odom_new.pose.pose.position.x = data.pose.pose.position.x + trans[0]
        odom_new.pose.pose.position.y = data.pose.pose.position.y + trans[1]
        odom_new.pose.pose.position.z = data.pose.pose.position.z + trans[2]
        # rospy.loginfo('td: %s tr: %s' % (str(type(data)), str(type(rot))))
        q = data.pose.pose.orientation
        q_tuple = (q.x, q.y, q.z, q.w,)
        # Quaternion(*(tft quaternion)) converts a tf-style quaternion to a ROS msg quaternion
        odom_new.pose.pose.orientation = Quaternion(*tft.quaternion_multiply(q_tuple, rot))

        heading_change = quaternion_to_heading(rot)

        odom_new.twist.twist.linear.x = data.twist.twist.linear.x*math.cos(heading_change) - data.twist.twist.linear.y*math.sin(heading_change)
        odom_new.twist.twist.linear.y = data.twist.twist.linear.y*math.cos(heading_change) + data.twist.twist.linear.x*math.sin(heading_change)
        odom_new.twist.twist.linear.z = 0

        odom_new.twist.twist.angular.x = 0
        odom_new.twist.twist.angular.y = 0
        odom_new.twist.twist.angular.z = data.twist.twist.angular.z

        return odom_new
    def poll(self):
        (x, y, theta) = self.arduino.arbot_read_odometry()

        quaternion = Quaternion()
        quaternion.x = 0.0 
        quaternion.y = 0.0
        quaternion.z = sin(theta / 2.0)
        quaternion.w = cos(theta / 2.0)
    
        # Create the odometry transform frame broadcaster.
        now = rospy.Time.now()
        self.odomBroadcaster.sendTransform(
            (x, y, 0), 
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            now,
            "base_link",
            "odom"
            )
    
        odom = Odometry()
        odom.header.frame_id = "odom"
        odom.child_frame_id = "base_link"
        odom.header.stamp = now
        odom.pose.pose.position.x = x
        odom.pose.pose.position.y = y
        odom.pose.pose.position.z = 0
        odom.pose.pose.orientation = quaternion
        
        odom.twist.twist.linear.x = self.forwardSpeed
        odom.twist.twist.linear.y = 0
        odom.twist.twist.angular.z = self.angularSpeed

        self.arduino.arbot_set_velocity(self.forwardSpeed, self.angularSpeed)

        self.odomPub.publish(odom)
Esempio n. 11
0
    def Talk(self, time):
        #print self.orientation.get_euler()['yaw'], self.position.yaw, self.sensors['gps'].position.yaw
        msgs = Odometry( )
        msgs.header.stamp = rospy.Time.now()
        msgs.header.frame_id = "/nav"

        msgs.child_frame_id = "/drone_local"

        msgs.pose.pose.position.x = self.position.x
        msgs.pose.pose.position.y = self.position.y
        msgs.pose.pose.position.z = self.position.z
        
        msgs.pose.pose.orientation.x = self.orientation.x
        msgs.pose.pose.orientation.y = self.orientation.y
        msgs.pose.pose.orientation.z = self.orientation.z
        msgs.pose.pose.orientation.w = self.orientation.w
        
        msgs.twist.twist.linear.x = self.velocity.x
        msgs.twist.twist.linear.y = self.velocity.y
        msgs.twist.twist.linear.z = self.velocity.z

        #rospy.loginfo(msgs)
        
        self.publisher['state'].publish(msgs)

        self.tf_broadcaster['drone_local_tf'].sendTransform( (msgs.pose.pose.position.x, msgs.pose.pose.position.y, msgs.pose.pose.position.z) , 
            self.orientation.get_quaternion( ), 
            msgs.header.stamp,
            msgs.child_frame_id, 
            msgs.header.frame_id)
Esempio n. 12
0
 def publish_odom(self, robot):
     # Transform etc
     x = robot.x
     y = robot.y
     th = robot.th
     br = tf.TransformBroadcaster()
     quat = tf.transformations.quaternion_from_euler(0,0,th)
     odom_trans = TransformStamped()
     odom_trans.header.stamp = rospy.Time.now()
     odom_trans.header.frame_id = "/map"
     odom_trans.child_frame_id = "/base_link"
     odom_trans.transform.translation.x = x
     odom_trans.transform.translation.y = y
     odom_trans.transform.translation.z = 0.0
     odom_trans.transform.rotation = quat;
     #tf.TransformBroadcaster().SendTransform(R,t,time,child,parent)
     br.sendTransform((x,y,0.0), quat, odom_trans.header.stamp, odom_trans.child_frame_id, odom_trans.header.frame_id)
     
     # Publish Odom msg
     odom_msg = Odometry()
     odom_msg.header.stamp = rospy.Time.now()
     odom_msg.header.frame_id = "/map"
     odom_msg.child_frame_id = "/base_link"
     odom_msg.pose.pose.position.x = x
     odom_msg.pose.pose.position.y = y
     odom_msg.pose.pose.position.z = 0.0
     
     odom_msg.pose.pose.orientation.x = quat[0]
     odom_msg.pose.pose.orientation.y = quat[1]
     odom_msg.pose.pose.orientation.z = quat[2]
     odom_msg.pose.pose.orientation.w = quat[3]
     
     # publish
     self.odomPub.publish(odom_msg)
Esempio n. 13
0
def callback(msg):
    pub = rospy.Publisher("odom_covar", Odometry)

    corrected = Odometry()
    corrected.header.seq = msg.header.seq
    corrected.header.stamp = rospy.Time.now()
    corrected.header.frame_id = msg.header.frame_id
    corrected.child_frame_id = "base_footprint"
    corrected.pose.pose = msg.pose.pose
    corrected.pose.covariance = [.01, 0, 0, 0, 0, 0,
                                 0, .01, 0, 0, 0, 0,
                                 0, 0, .01, 0, 0, 0,
                                 0, 0, 0, .01, 0, 0,
                                 0, 0, 0, 0, .01, 0,
                                 0, 0, 0, 0, 0, .01]

    corrected.twist.twist = msg.twist.twist
    corrected.twist.covariance = [.01, 0, 0, 0, 0, 0,
                                 0, .01, 0, 0, 0, 0,
                                 0, 0, .01, 0, 0, 0,
                                 0, 0, 0, .01, 0, 0,
                                 0, 0, 0, 0, .01, 0,
                                 0, 0, 0, 0, 0, .01]

    pub.publish(corrected)
Esempio n. 14
0
def update_vel():
    "Main loop"""
    global g_last_loop_time, g_vel_x, g_vel_y, g_vel_dt, x_pos, y_pos

    #while not rospy.is_shutdown():
    current_time = rospy.Time.now()

    linear_velocity_x = g_vel_x
    linear_velocity_y = g_vel_y

    # Calculate current position of the robot
    x_pos += linear_velocity_x * g_vel_dt
    y_pos += linear_velocity_y * g_vel_dt

    # All odometry is 6DOF, so need a quaternion created from yaw
    odom_quat = tf.transformations.quaternion_from_euler(0, 0, 0)

    # next, we'll publish the odometry message over ROS
    odom = Odometry()
    odom.header.stamp = current_time
    odom.header.frame_id = "odom"

    # set the position
    odom.pose.pose = Pose(Point(x_pos, y_pos, 0.), Quaternion(*odom_quat))

    # set the velocity
    odom.child_frame_id = "base_footprint"
    odom.twist.twist = Twist(
        Vector3(linear_velocity_x, linear_velocity_y, 0), Vector3(0, 0, 0))

    # publish the message
    odom_pub.publish(odom)

    g_last_loop_time = current_time
def handle_harlie_pose(msg, push_casters):
	pose = changePoseCoordFrame(msg)
	current_time = rospy.Time.now()
	theta = pose.theta + pi if push_casters else pose.theta
	quaternion = tf.transformations.quaternion_about_axis(theta, (0,0,1))
	tf_br.sendTransform(translation = (pose.x, pose.y, 0), 
			rotation = tuple(quaternion),
			time = current_time,
			child = 'base_link',
			parent = 'odom')
	odom_msg = Odometry()
	odom_msg.header.stamp = current_time
	odom_msg.header.frame_id = 'odom'

	odom_msg.pose.pose.position.x = pose.x
	odom_msg.pose.pose.position.y = pose.y
	odom_msg.pose.pose.position.z = 0.0
	odom_msg.pose.pose.orientation = Quaternion(*quaternion)

	odom_msg.pose.covariance[0] = pose.x_var
	odom_msg.pose.covariance[7] = pose.y_var
	odom_msg.pose.covariance[35] = pose.theta_var

	odom_msg.child_frame_id = 'base_link'
	odom_msg.twist.twist.linear.x = pose.vel
	odom_msg.twist.twist.angular.z = pose.omega

	odom_msg.twist.covariance[0] = pose.vel_var
	odom_msg.twist.covariance[35] = pose.omega_var

	odom_pub.publish(odom_msg)
Esempio n. 16
0
def main():
	rospy.init_node("pose2d_to_odometry")

	own_num = rospy.get_param('~own_num', 0)
	
	global odom, head
	head = Header()
	head.stamp = rospy.Time.now()
	head.frame_id = "robot_{}/odom_combined".format(own_num)

	odom = Odometry()
	odom.child_frame_id = "robot_{}/base_footprint".format(own_num)
	o = 10**-9 # zero
	odom.pose.covariance = [1, 0, 0, 0, 0, 0,
							0, 1, 0, 0, 0, 0,
							0, 0, o, 0, 0, 0, # z doesn't change
							0, 0, 0, o, 0, 0, # constant roll
							0, 0, 0, 0, o, 0, # constant pitch
							0, 0, 0, 0, 0, o] # guess - .3 radians rotation cov
	odom.twist.covariance = [100, 0, 0, 0, 0, 0,
							 0, 100, 0, 0, 0, 0,
							 0, 0, 100, 0, 0, 0, # large covariances - no data
							 0, 0, 0, 100, 0, 0,
							 0, 0, 0, 0, 100, 0,
							 0, 0, 0, 0, 0, 100]



	global odom_pub
	odom_topic = rospy.get_param('~odometry_publish_topic', 'vo')
	pose2D_topic = rospy.get_param('~pose2D_topic', 'pose2D')
	odom_pub = rospy.Publisher(odom_topic, Odometry)
	rospy.Subscriber(pose2D_topic, Pose2D, on_pose)

	rospy.spin()
Esempio n. 17
0
 def onNavSts(self,data):   
     q = tf.transformations.quaternion_from_euler(math.pi, 0, math.pi/2)
     self.broadcaster.sendTransform((0,0,0), q.conjugate(), rospy.Time.now(), "uwsim_frame", "local");
     
     try:
         (trans,rot) = self.listener.lookupTransform("uwsim_frame", self.base_frame, rospy.Time(0))
         
         odom = Odometry();  
         odom.twist.twist.linear.x = data.body_velocity.x;
         odom.twist.twist.linear.y = data.body_velocity.y;
         odom.twist.twist.linear.z = data.body_velocity.z;
         odom.twist.twist.angular.x = data.orientation_rate.roll;
         odom.twist.twist.angular.y = data.orientation_rate.pitch;
         odom.twist.twist.angular.z = data.orientation_rate.yaw;
         odom.child_frame_id = self.base_frame; 
         odom.pose.pose.orientation.x = rot[0];
         odom.pose.pose.orientation.y = rot[1];
         odom.pose.pose.orientation.z = rot[2];
         odom.pose.pose.orientation.w = rot[3];
         odom.pose.pose.position.x = trans[0];
         odom.pose.pose.position.y = trans[1];
         odom.pose.pose.position.z = trans[2];
         odom.header.stamp = rospy.Time.now();
         odom.header.frame_id = "local";
         
         self.pub.publish(odom);
         
     except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
         None
Esempio n. 18
0
	def __update_odometry(self, linear_offset, angular_offset, tf_delay):
		self.__heading = (self.__heading + angular_offset) % 360
		
		q = tf.transformations.quaternion_from_euler(0, 0, math.radians(self.__heading))
		self.__pose.position.x += math.cos(math.radians(self.__heading)) * self.__distance_per_cycle * self.__twist.linear.x
		self.__pose.position.y += math.sin(math.radians(self.__heading)) * self.__distance_per_cycle * self.__twist.linear.x
		self.__pose.position.z = 0.33
		self.__pose.orientation = Quaternion(q[0], q[1], q[2], q[3])

		now = rospy.Time.now() + rospy.Duration(tf_delay)

		self.__tfb.sendTransform(
			(self.__pose.position.x, self.__pose.position.y, self.__pose.position.z),
			q,
			now,
			'base_link',
			'odom')

		o = Odometry()
		o.header.stamp = now
		o.header.frame_id = 'odom'
		o.child_frame_id = 'base_link'
		o.pose = PoseWithCovariance(self.__pose, None)

		o.twist = TwistWithCovariance()
		o.twist.twist.linear.x = self.__distance_per_second * self.__twist.linear.x
		o.twist.twist.angular.z = math.radians(self.__rotation_per_second) * self.__twist.angular.z
Esempio n. 19
0
def publish_Tfs():
    global tf_rot,tf_trans,tf_tda,tf_rda,tf_tbc,tf_rbc,scale
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(50.0)
    while not rospy.is_shutdown():
        rate.sleep()
        camOdom = Odometry()
        camOdom.header.stamp = rospy.Time.now()
        camOdom.header.frame_id = "/odom_combined"
        camOdom.pose.pose.position.x =0
        camOdom.pose.pose.position.y =0
        camOdom.pose.pose.position.z =0. #Tbc[2]
        M=np.identity(4)
        q=tf.transformations.quaternion_from_matrix(M)
        camOdom.pose.pose.orientation.x = q[0]
        camOdom.pose.pose.orientation.y = q[1]
        camOdom.pose.pose.orientation.z = q[2]
        camOdom.pose.pose.orientation.w = q[3]
        camOdom.child_frame_id = "/base_footprint"
        camOdom.twist.twist.linear.x = 0
        camOdom.twist.twist.linear.y = 0
        camOdom.twist.twist.angular.z = 0
        T=np.array([0.,0.,0.])
        M=np.identity(4)
        q=tf.transformations.quaternion_from_matrix(M)
        br.sendTransform(T,q,
            rospy.Time.now(),
            "/base_footprint",
            "/odom_combined")
        camOdomPub.publish(camOdom)
Esempio n. 20
0
def talker(message):
    corrected = Odometry()
    corrected.header.seq = message.header.seq
    corrected.header.stamp = rospy.Time.now()
    corrected.header.frame_id = message.header.frame_id #base_footprint #message.header.frame_id
    corrected.child_frame_id = message.child_frame_id #"base_footprint"
    corrected.pose.pose = message.pose.pose

    corrected.pose.covariance = [0.05, 0, 0, 0, 0, 0,
                                 0, 0.05, 0, 0, 0, 0,
                                 0, 0, 0.05, 0, 0, 0,
                                 0, 0, 0, 0.001, 0, 0,
                                 0, 0, 0, 0, 0.001, 0,
                                 0, 0, 0, 0, 0, .5]

    corrected.twist.twist = message.twist.twist

    corrected.twist.covariance = [0.05, 0, 0, 0, 0, 0,
                                 0, 0.05, 0, 0, 0, 0,
                                 0, 0, 0.05, 0, 0, 0,
                                 0, 0, 0, 0.001, 0, 0,
                                 0, 0, 0, 0, 0.001, 0,
                                 0, 0, 0, 0, 0, .5]

    pub = rospy.Publisher('odom_w_cov', Odometry)
    pub.publish(corrected)
Esempio n. 21
0
 def pubOdometry(self, event):
     odom = Odometry()
     odom.header.stamp = rospy.Time.now()
     odom.header.frame_id = str(self.frame_id)
     odom.child_frame_id = "real_girona500"
     
     odom.pose.pose.position.x = self.p[0]
     odom.pose.pose.position.y = self.p[1]
     odom.pose.pose.position.z = self.p[2]
     
     orientation = tf.transformations.quaternion_from_euler(self.p[3], self.p[4], self.p[5], 'sxyz')
     odom.pose.pose.orientation.x = orientation[0]
     odom.pose.pose.orientation.y = orientation[1]
     odom.pose.pose.orientation.z = orientation[2]
     odom.pose.pose.orientation.w = orientation[3]
         
     #Haig de comentar les velocitats sino l'UWSim no va 
     odom.twist.twist.linear.x = 0.0 #v_[0]
     odom.twist.twist.linear.y = 0.0 #v_[1]
     odom.twist.twist.linear.z = 0.0 #v_[2]
     odom.twist.twist.angular.x = 0.0 #v_[3]
     odom.twist.twist.angular.y = 0.0 #v_[4]
     odom.twist.twist.angular.z = 0.0 #v_[5]
 
     self.pub_odom.publish(odom)
  
     # Broadcast transform
     br = tf.TransformBroadcaster()
     br.sendTransform((self.p[0], self.p[1], self.p[2]), orientation, 
     odom.header.stamp, odom.header.frame_id, "world")
Esempio n. 22
0
def post_odometry(self, component_instance):
    """ Publish the data of the Pose as a Odometry message for fake localization 
    """
    parent_name = component_instance.robot_parent.blender_obj.name
    component_name = component_instance.blender_obj.name
    frame_id = self._properties[component_name]['frame_id']
    child_frame_id = self._properties[component_name]['child_frame_id']

    odometry = Odometry()
    odometry.header.stamp = rospy.Time.now()
    odometry.header.frame_id = frame_id
    odometry.child_frame_id = child_frame_id

    odometry.pose.pose.position.x = component_instance.local_data['x']
    odometry.pose.pose.position.y = component_instance.local_data['y']
    odometry.pose.pose.position.z = component_instance.local_data['z']

    euler = mathutils.Euler((component_instance.local_data['roll'],
                             component_instance.local_data['pitch'],
                             component_instance.local_data['yaw']))
    quaternion = euler.to_quaternion()
    odometry.pose.pose.orientation = quaternion

    for topic in self._topics:
        # publish the message on the correct topic    
        if str(topic.name) == str("/" + parent_name + "/" + component_name):
            topic.publish(odometry)
Esempio n. 23
0
    def Talk(self):
        msgs = Odometry()
        msgs.header.stamp = rospy.Time.now()
        msgs.header.frame_id = '/nav'
        msgs.child_frame_id = self.name 


        msgs.pose.pose.position.x = self.position.x
        msgs.pose.pose.position.y = self.position.y
        msgs.pose.pose.position.z = self.position.z
        
        msgs.pose.pose.orientation.x = self.orientation.x
        msgs.pose.pose.orientation.y = self.orientation.y
        msgs.pose.pose.orientation.z = self.orientation.z
        msgs.pose.pose.orientation.w = self.orientation.w 
        
        msgs.twist.twist.linear.x = self.velocity.x
        msgs.twist.twist.linear.y = self.velocity.y
        msgs.twist.twist.linear.z = self.velocity.z

        #rospy.loginfo(msgs)
        
        self.publisher['state'].publish(msgs)

        self.tf_broadcaster['local_tf'].sendTransform( (msgs.pose.pose.position.x, msgs.pose.pose.position.y, msgs.pose.pose.position.z) , 
            (msgs.pose.pose.orientation.x, msgs.pose.pose.orientation.y, msgs.pose.pose.orientation.z, msgs.pose.pose.orientation.w), 
            msgs.header.stamp, 
            msgs.child_frame_id, 
            msgs.header.frame_id)
 def handle_imu(self, imu_data):
     
     if not self.imu_recv:
         
         rospy.loginfo('IMU data received')
         self.imu_recv = True
 
     if self.pub.get_num_connections() != 0:
 
       msg = Odometry()
       
       #msg.header.frame_id = imu_data.header.frame_id
       msg.header.frame_id = 'imu_frame'
       msg.header.stamp = imu_data.header.stamp
       
       msg.child_frame_id = 'imu_base_link'
       
       msg.pose.pose.position.x = 0.0 # here, we can maybe try to integrate accelerations...
       msg.pose.pose.position.y = 0.0
       msg.pose.pose.position.z = 0.0
       
       msg.pose.pose.orientation = imu_data.orientation # should we transform this orientation into base_footprint frame????
       
       msg.pose.covariance = [99999, 0,     0,     0,     0,     0, # large covariance on x, y, z
                              0,     99999, 0,     0,     0,     0,
                              0,     0,     99999, 0,     0,     0, 
                              0,     0,     0,     1e-06, 0,     0, # we trust in rpy
                              0,     0,     0,     0,     1e-06, 0,
                              0,     0,     0,     0,     0,     1e-06]
       
       # does robot_pose_ekf care about twist?
       
       self.pub.publish(msg)
Esempio n. 25
0
def callback(data):
    print data
    odom = Odometry()
    odom.pose = data.pose
    odom.header.stamp = rospy.Time.now()
    odom.header.frame_id = "/odom"
    global pub
    pub.publish(odom)
def listener():

    rospy.init_node('listener', anonymous=True)

    rospy.Subscriber("/wiimote1/imu", Imu, wiimote1Callback)
    rospy.Subscriber("/wiimote2/imu", Imu, wiimote2Callback)

    odomPub = rospy.Publisher("/base_controller/odom", Odometry)

    odom = Odometry()

    while 1:
        br = tf.TransformBroadcaster()
        time.sleep(.01)

        robot.updateState()
        if robot.readyToPublish:
	    quaternion = tf.transformations.quaternion_from_euler(0, 0, robot.theta)
            #tf msg
            #br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),"/odom", "/map")
            br.sendTransform((robot.x, robot.y, 0), tf.transformations.quaternion_from_euler(0, 0, robot.theta),rospy.Time.now(),"/base_link", "/odom")
            br.sendTransform((0, 0, .7), tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),"/base_laser_link", "/base_link")
            br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),"/camera1_link", "/base_laser_link")
	    br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 3.14),rospy.Time.now(),"/camera2_link", "/base_laser_link")

            robot.readyToPublish = False

            #odometry msg
            
            odom.header.stamp = robot.lastTime
            odom.child_frame_id = "base_link"
            odom.pose.pose.position.x = robot.x
            odom.pose.pose.position.y = robot.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation.x = quaternion[0]
	    odom.pose.pose.orientation.y = quaternion[1]
	    odom.pose.pose.orientation.z = quaternion[2]
	    odom.pose.pose.orientation.w = quaternion[3]
	    odom.twist.twist.linear.x = robot.xV
	    odom.twist.twist.linear.y = robot.yV 
	    # the x angular velocity corresponds to left wheel and y to right wheel
	    odom.twist.twist.angular.x = robot.leftV
	    odom.twist.twist.angular.y = robot.rightV
            odom.twist.twist.angular.z = robot.thetaV
	    # place holder covariance matrix
	    P = [.0001, 0, 0, 0, 0, 0,
                 0, .0001, 0, 0, 0, 0,
                 0, 0, .0001, 0, 0, 0,
		 0, 0, 0, .0001, 0, 0,
                 0, 0, 0, 0, .0001, 0,
                 0, 0, 0, 0, 0, .0001]
	    odom.pose.covariance = P
            odomPub.publish(odom)

            

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()
Esempio n. 27
0
    def inspvax_handler(self, inspvax):
        # Convert the latlong to x,y coordinates and publish an Odometry
        try:
            utm_pos = geodesy.utm.fromLatLong(inspvax.latitude, inspvax.longitude)
        except ValueError:
            # Probably coordinates out of range for UTM conversion.
            return

        if not self.init and self.zero_start:
            self.origin.x = utm_pos.easting
            self.origin.y = utm_pos.northing
            self.origin.z = inspvax.altitude
            self.pub_origin.publish(position=self.origin)

        odom = Odometry()
        odom.header.stamp = rospy.Time.now()
        odom.header.frame_id = self.odom_frame
        odom.child_frame_id = self.base_frame
        odom.pose.pose.position.x = utm_pos.easting - self.origin.x
        odom.pose.pose.position.y = utm_pos.northing - self.origin.y
        odom.pose.pose.position.z = inspvax.altitude - self.origin.z

        # Orientation
        # Save this on an instance variable, so that it can be published
        # with the IMU message as well.
        self.orientation = tf.transformations.quaternion_from_euler(
            radians(inspvax.pitch), radians(inspvax.roll), radians(90 - inspvax.azimuth)
        )
        print inspvax.azimuth
        odom.pose.pose.orientation = Quaternion(*self.orientation)
        odom.pose.covariance[21] = self.orientation_covariance[0] = pow(2, inspvax.pitch_std)
        odom.pose.covariance[28] = self.orientation_covariance[4] = pow(2, inspvax.roll_std)
        odom.pose.covariance[35] = self.orientation_covariance[8] = pow(2, inspvax.azimuth_std)

        # Twist is relative to vehicle frame
        odom.twist.twist.linear.x = inspvax.east_velocity
        odom.twist.twist.linear.y = inspvax.north_velocity
        odom.twist.twist.linear.z = inspvax.up_velocity
        TWIST_COVAR[0] = pow(2, inspvax.east_velocity_std)
        TWIST_COVAR[7] = pow(2, inspvax.north_velocity_std)
        TWIST_COVAR[14] = pow(2, inspvax.up_velocity_std)
        # odom.twist.twist.angular = imu.angular_velocity
        odom.twist.covariance = TWIST_COVAR

        self.pub_odom.publish(odom)

        # Odometry transform (if required)
        if self.publish_tf:
            self.tf_broadcast.sendTransform(
                (odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z),
                self.orientation,
                odom.header.stamp,
                odom.child_frame_id,
                odom.header.frame_id,
            )

        # Mark that we've received our first fix, and set origin if necessary.
        self.init = True
Esempio n. 28
0
    def publish(self, data):
        q = Quaternion()
        q.x = 0
        q.y = 0
        q.z = data[6]
        q.w = data[7]
        odomMsg = Odometry()
        odomMsg.header.frame_id = self._odom
        odomMsg.header.stamp = rospy.get_rostime()
        odomMsg.child_frame_id = self._baseLink
        odomMsg.pose.pose.position.x = data[0]
        odomMsg.pose.pose.position.y = data[1]
        odomMsg.pose.pose.position.z = 0
        odomMsg.pose.pose.orientation = q
        if self._firstTimePub:
            self._prevOdom = odomMsg
            self._firstTimePub = False
            return

        velocity = Twist()

        deltaTime = odomMsg.header.stamp.to_sec() - self._prevOdom.header.stamp.to_sec()
        yaw, pitch, roll = euler_from_quaternion(
            [odomMsg.pose.pose.orientation.w, odomMsg.pose.pose.orientation.x, odomMsg.pose.pose.orientation.y,
             odomMsg.pose.pose.orientation.z])
        prevYaw, prevPitch, prevRollprevYaw = euler_from_quaternion(
            [self._prevOdom.pose.pose.orientation.w, self._prevOdom.pose.pose.orientation.x,
             self._prevOdom.pose.pose.orientation.y, self._prevOdom.pose.pose.orientation.z])
        if deltaTime > 0:
            velocity.linear.x = (data[8] / deltaTime)

        deltaYaw = yaw - prevYaw

        # rospy.loginfo("yaw: %f\t\tpevYaw: %f\t\tdeltaYaw: %f" % (yaw,prevYaw,deltaYaw))

        if deltaYaw > math.pi: deltaYaw -= 2 * math.pi
        elif deltaYaw < -math.pi: deltaYaw += 2 * math.pi

        if deltaTime > 0:
            velocity.angular.z = -(deltaYaw / deltaTime)

        # rospy.loginfo("deltaYaw after check: %f\t\t angular: %f" % (deltaYaw, velocity.angular.z))

        odomMsg.twist.twist = velocity

        self._prevOdom = odomMsg

        traMsg = TransformStamped()
        traMsg.header.frame_id = self._odom
        traMsg.header.stamp = rospy.get_rostime()
        traMsg.child_frame_id = self._baseLink
        traMsg.transform.translation.x = data[0]
        traMsg.transform.translation.y = data[1]
        traMsg.transform.translation.z = 0
        traMsg.transform.rotation = q

        self._pub.publish(odomMsg)
        self._broadCase.sendTransformMessage(traMsg)
Esempio n. 29
0
def callback(pose):
    # cov_east = navsatfix.position_covariance[0]
    # cov_north = navsatfix.position_covariance[4]
    # if cov_east>cov_thresh or cov_north>cov_thresh:
    #     return
    odom = Odometry()
    odom.pose.pose = pose.pose
    odom.header = pose.header
    pub.publish(odom)
 def transition(self, state, twist, dt):
     desired = state[1]
     current = state[0]
     # do something
     n = Odometry()
     n.header = current.header
     n.twist.twist = self.update_twist(current.twist.twist, twist)
     n.pose.pose = self.update_pose(current.pose.pose, current.twist.twist, n.twist.twist, dt)
     return (n, desired, )
    def trajectory_in_camera_fov(self, data):
        """generates trajecory in camera workspace"""
        r = 9.5
        self.end_point = np.array([
            r * np.sin((self.RHP_time - 5) / 10), -r * np.cos(
                (self.RHP_time - 5) / 10), 2
        ])
        #self.end_point = self.end_point + np.array([self.target_velocity*0.05, 0, 0])
        start = time.time()
        odom_msg = Odometry()
        odom_msg.pose.pose.position.x = self.end_point[0]
        odom_msg.pose.pose.position.y = self.end_point[1]
        odom_msg.pose.pose.position.z = self.end_point[2]
        odom_msg.header.stamp = rospy.Time.now()
        odom_msg.header.frame_id = 'world'

        self.target_odom.publish(odom_msg)

        if self.traj_gen_counter == 0 or self.RHP_time >= self.tracking_in_global_time - 0.1:

            t1 = time.time() - start
            #this bit of code is to be commented out to generate the cloud seperately

            start_point = np.zeros((0, 3))
            if self.traj_gen_counter != 0:
                start_point = np.concatenate((start_point, self.p_eoe), 0)
            else:
                start_point = np.concatenate(
                    (start_point, self.Pglobal[np.newaxis]), 0)

            #if self.traj_gen_counter != 0:
            #    #start_point = np.concatenate((start_point, self.p_eoe), 0)
            #    self.Pglobal = np.dot(self.Rglobal.T, self.Rcg_vibase[0:3, 3:4]) + np.dot(self.Rcg_vibase[0:3, 0:3], self.Pglobal[np.newaxis].T)
            #    start_point = np.concatenate((start_point, self.Pglobal.T), 0) # was self.p_eoe
            #else:
            #    # need to get a point corresponds to self.Pglobal at the apex of the grid
            #    # keep on adding newaxis and .T because you do not maintain uniformity
            #    self.Pglobal = -self.Rcg_vibase[0:3, 3:4] + np.dot(self.Rcg_vibase[0:3, 0:3], self.Pglobal[np.newaxis].T)
            #    start_point = np.concatenate((start_point, self.Pglobal.T), 0) # new axis was needed because you dont maintain uniformity

            #start_point = np.concatenate((start_point, self.Pglobal[np.newaxis]), 0)
            points_in_cone = self.generate_regular_pyramid_grid()
            occupied_points = ros_numpy.numpify(data)

            if len(occupied_points) != 0:
                #print occupied_points
                points_in_cone = self.generate_final_grid(
                    points_in_cone, occupied_points)

            p1 = [
                np.dot(self.Rcdoc_cdl[0:3, 0:3], x[np.newaxis].T)
                for x in points_in_cone
            ]
            p2 = [
                self.Rcg_vibase[0:3, 3:4] +
                np.dot(np.linalg.inv(self.Rcg_vibase[0:3, 0:3]), x) for x in p1
            ]
            points_in_cone = [
                self.Pglobal + np.dot(self.Rglobal, x).ravel() for x in p2
            ]
            points_in_cone = [x for x in points_in_cone if not x[2] <= 0]

            #kdt_points_in_cone = cKDTree(points_in_cone)

            #if len(occupied_points) == 0:
            #print start_point, len(points_in_cone), len(occupied_points)
            #if self.traj_gen_counter != 0:
            points_in_cone = np.concatenate((start_point, points_in_cone), 0)

            # publish generated pointcloud
            header = std_msgs.msg.Header()
            header.stamp = data.header.stamp  #rospy.Time.now()
            header.frame_id = 'world'
            p = pc2.create_cloud_xyz32(header, points_in_cone)
            self.pcl_pub.publish(p)

            #this bit of code is to be commented out to generate the cloud seperately

            #should be uncommented if cloud is genereted here
            #points = ros_numpy.numpify(data)
            #p = np.zeros((points.shape[0],3))
            #p[:,0] = points['x']
            #p[:,1] = points['y']
            #p[:,2] = points['z']
            #points_in_cone = p

            kdt_points_in_cone = cKDTree(points_in_cone)
            closest_to_end = kdt_points_in_cone.query(self.end_point, 1)

            #closest_to_end_index = points_in_cone[closest_to_end[1]]

            end_point_index = closest_to_end[1]
            #print closest_to_end, points_in_cone[end_point_index]
            planning_horizon = 4  # planning horizon
            execution_horizon = 1  # execution horizon

            dist_to_goal = np.linalg.norm(self.end_point - self.Pglobal)
            ttt = self.RHP_time - 5.0
            timefactor = 1 / (1 + np.exp(-1 * (ttt)))
            distancefactor = erf(0.25 * (dist_to_goal - self.resolution))
            smoothing_factor = timefactor * distancefactor
            f0 = open('velocity.txt', 'a')
            f0.write('%s, %s, %s, %s\n' %
                     (ttt, timefactor, distancefactor,
                      self.uav_velocity * smoothing_factor))
            t2 = time.time() - start

            #one dimension simplicial complex which is basically a graph
            rips = gudhi.RipsComplex(points=points_in_cone,
                                     max_edge_length=1.5 * self.resolution)
            f = rips.create_simplex_tree(max_dimension=1)

            # make graph
            G = nx.Graph()
            G.add_nodes_from(range(f.num_vertices()))
            edge_list = [(simplex[0][0], simplex[0][1], {
                'weight': simplex[1]
            }) if len(simplex[0]) == 2 else None
                         for simplex in f.get_skeleton(1)]
            edge_list = [k for k in edge_list if k is not None]
            G.add_edges_from(edge_list)

            try:
                print 'i m here.................', points_in_cone[
                    0], points_in_cone[end_point_index]
                shortest_path = nx.shortest_path(G,
                                                 source=0,
                                                 target=end_point_index,
                                                 weight='weight',
                                                 method='dijkstra')
                path = np.array([points_in_cone[j] for j in shortest_path])
                print 'length of the path is:', len(path) - 1
                t3 = time.time() - start
                if dist_to_goal < self.resolution:
                    self.reached_goal = True

                if self.reached_goal == False:
                    if len(path) - 1 >= planning_horizon:
                        no_of_segments = planning_horizon
                        no_of_segments_to_track = execution_horizon
                        path = path[:no_of_segments + 1]
                    elif execution_horizon <= len(path) - 1 < planning_horizon:
                        no_of_segments = len(path) - 1
                        no_of_segments_to_track = execution_horizon
                        path = path[:no_of_segments + 1]
                    elif len(path) - 1 < execution_horizon:
                        no_of_segments = len(path) - 1
                        no_of_segments_to_track = no_of_segments
                        path = path[:no_of_segments + 1]
                else:
                    if self.static_goal == True:
                        no_of_segments = len(path) - 1
                        no_of_segments_to_track = no_of_segments
                        path = path[:no_of_segments + 1]
                    else:
                        if len(path) - 1 >= planning_horizon:
                            no_of_segments = planning_horizon
                            no_of_segments_to_track = execution_horizon
                            path = path[:no_of_segments + 1]
                        elif execution_horizon <= len(
                                path) - 1 < planning_horizon:
                            no_of_segments = len(path) - 1
                            no_of_segments_to_track = execution_horizon
                            path = path[:no_of_segments + 1]
                        elif len(path) - 1 < execution_horizon:
                            no_of_segments = len(path) - 1
                            no_of_segments_to_track = no_of_segments
                            path = path[:no_of_segments + 1]

                #print '-----len of path is---', len(path)-1
                path = zip(*path)

                f0 = open('path.txt', 'a')
                f0.write('%s\n' % (zip(*path)))
                # now construct the minimum snap trajectory on the minimum path
                waypoint_specified = True
                waypoint_bc = False
                t4 = time.time() - start

                #erf1 = erf(1000*dist_to_goal)
                #erf2 = erf(-1000*(dist_to_goal-self.initial_dist_to_goal))

                velocity = self.uav_velocity * smoothing_factor
                print '----------average velocity is--------', velocity
                #f1 = open('velocity.txt', 'a')
                #f1.write('%s, %s, %s, %s, %s, %s\n' %(self.initial_dist_to_goal, dist_to_goal, ttt, timefactor, distancefactor, velocity))
                T, _p1, _p2, _p3 = Minimum_snap_trajetory(
                    self.replanning_started, velocity, path,
                    waypoint_specified, waypoint_bc, self.v_eoe, self.a_eoe,
                    self.j_eoe).construct_polynomial_trajectory()
                t5 = time.time() - start

                #self.plot_graph_and_trajectory(points_in_cone, occupied_points, G, path, T, p1, p2, p3)

                N = 8
                _pp1 = [_p1[i:i + N] for i in range(0, len(_p1), N)]
                [i.reverse() for i in _pp1]

                _pp2 = [_p2[i:i + N] for i in range(0, len(_p2), N)]
                [i.reverse() for i in _pp2]

                _pp3 = [_p3[i:i + N] for i in range(0, len(_p3), N)]
                [i.reverse() for i in _pp3]

                xx = np.poly1d(_pp1[no_of_segments_to_track - 1])
                vx = np.polyder(xx, 1)
                accx = np.polyder(xx, 2)
                jx = np.polyder(xx, 3)
                sx = np.polyder(xx, 4)

                yy = np.poly1d(_pp2[no_of_segments_to_track - 1])
                vy = np.polyder(yy, 1)
                accy = np.polyder(yy, 2)
                jy = np.polyder(yy, 3)
                sy = np.polyder(yy, 4)

                zz = np.poly1d(_pp3[no_of_segments_to_track - 1])
                vz = np.polyder(zz, 1)
                accz = np.polyder(zz, 2)
                jz = np.polyder(zz, 3)
                sz = np.polyder(zz, 4)

                xdes = xx(T[no_of_segments_to_track])
                ydes = yy(T[no_of_segments_to_track])
                zdes = zz(T[no_of_segments_to_track])
                vxdes = vx(T[no_of_segments_to_track])
                vydes = vy(T[no_of_segments_to_track])
                vzdes = vz(T[no_of_segments_to_track])
                axdes = accx(T[no_of_segments_to_track])
                aydes = accy(T[no_of_segments_to_track])
                azdes = accz(T[no_of_segments_to_track])
                jxdes = jx(T[no_of_segments_to_track])
                jydes = jy(T[no_of_segments_to_track])
                jzdes = jz(T[no_of_segments_to_track])

                self.p_eoe = np.array([[xdes, ydes, zdes]])
                # needed to transform the trajectory back to cog
                #self.p_eoe = (-self.Rcg_vibase[0:3, 3:4] + np.dot(self.Rcg_vibase[0:3, 0:3], self.p_eoe.T)).T
                self.v_eoe = np.array([[vxdes, vydes, vzdes]])
                self.a_eoe = np.array([[axdes, aydes, azdes]])
                self.j_eoe = np.array([[jxdes, jydes, jzdes]])

                eoe_msg = Odometry()
                eoe_msg.pose.pose.position.x = self.p_eoe[0][0]
                eoe_msg.pose.pose.position.y = self.p_eoe[0][1]
                eoe_msg.pose.pose.position.z = self.p_eoe[0][2]
                eoe_msg.header.stamp = rospy.Time.now()
                eoe_msg.header.frame_id = 'world'

                self.eoe_odom.publish(eoe_msg)

                #f1 = open('eoe_points.txt', 'a')
                #f1.write('%s, %s\n' %(self.Pglobal, self.p_eoe))

                #vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]])
                vector = np.array([
                    self.end_point[0] - self.p_eoe[0][0],
                    self.end_point[1] - self.p_eoe[0][1],
                    self.end_point[2] - self.p_eoe[0][2]
                ])
                #vector = np.array([1,0,0])#self.v_eoe
                direction = vector / np.linalg.norm(vector)
                t6 = time.time() - start
                msg = PolynomialCoefficients()
                header = std_msgs.msg.Header()
                header.stamp = data.header.stamp  #rospy.Time.now()
                header.frame_id = 'world'
                msg.header = header
                msg.polynomial_order = N - 1
                for i in range(len(_p1)):
                    msg.poly_x.append(_p1[i])
                    msg.poly_y.append(_p2[i])
                    msg.poly_z.append(_p3[i])
                msg.number_of_segments = no_of_segments_to_track
                msg.planning_start_time = data.header.stamp.to_sec()
                for j in T[:no_of_segments_to_track + 1]:
                    msg.segment_end_times.append(j)
                msg.desired_direction.x = direction[0]
                msg.desired_direction.y = direction[1]
                msg.desired_direction.z = direction[2]
                #msg.desired_direction.x = direction[0][0]; msg.desired_direction.y = direction[0][1]; msg.desired_direction.z = direction[0][2]
                msg.trajectory_mode = 'planning_in_camera_fov'
                self.previous_coefficients = msg
                self.traj_polycoeff.publish(msg)
                self.republish_trajectory = True
                f1 = open('total_time_taken.txt', 'a')
                f1.write("%s, %s, %s, %s, %s, %s\n" % (t1, t2, t3, t4, t5, t6))

            except:
                print 'No path between start and end'
                #vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]])
                #direction = vector/np.linalg.norm(vector)
                #self.previous_coefficients.desired_direction.x = direction[0]
                #self.previous_coefficients.desired_direction.y = direction[1]
                #self.previous_coefficients.desired_direction.z = direction[1]
                self.traj_polycoeff.publish(self.previous_coefficients)
                self.republish_trajectory = False

        else:  #elif self.RHP_time <= self.tracking_in_global_time - 0.1:
            self.traj_polycoeff.publish(self.previous_coefficients)
            self.republish_trajectory = False

        time_taken = time.time() - start
        #time.sleep(T[no_of_segments_to_track]-0.1-time_taken)
        self.RHP_time = data.header.stamp.to_sec() - self.hover_start_time

        #print 'total time taken to execute the callbacak is:', time_taken

        self.traj_gen_counter += 1
Esempio n. 32
0
    def timerOdomCB(self, event):
        # Serial read & publish
        try:
            myData = self.serial.readline(8).strip()
            if len(myData) > 0:
                WL = int(myData[1]) * 100 + int(myData[2]) * 10 + int(
                    myData[3])
                WR = int(myData[5]) * 100 + int(myData[6]) * 10 + int(
                    myData[7])
                WL = float(WL) / 100.0
                WR = float(WR) / 100.0
                if int(myData[0]) < 1:
                    WL = WL * (-1)
                if int(myData[4]) < 1:
                    WR = WR * (-1)
                rospy.loginfo(myData)
            #print "serialRead success~"
            # Twist
            VL = WL * self.wheelRad * self.scale  # V = omega * radius, unit: m/s
            VR = WR * self.wheelRad * self.scale
            Vyaw = (VR - VL) / self.wheelSep
            Vx = (VR + VL) / 2.0
            #print "Twist success~"

            # Pose
            self.current_time = rospy.Time.now()
            dt = (self.current_time - self.previous_time).to_sec()
            self.previous_time = self.current_time
            self.pose_x = self.pose_x + Vx * math.cos(self.pose_yaw) * dt
            self.pose_y = self.pose_y + Vx * math.sin(self.pose_yaw) * dt
            self.pose_yaw = self.pose_yaw + Vyaw * dt
            pose_quat = tf.transformations.quaternion_from_euler(
                0, 0, self.pose_yaw)

            #print "Pose success~"

            # Publish Odometry
            msg = Odometry()
            msg.header.stamp = self.current_time
            msg.header.frame_id = self.odomId
            msg.child_frame_id = self.baseId
            msg.pose.pose.position.x = self.pose_x
            msg.pose.pose.position.y = self.pose_y
            msg.pose.pose.position.z = 0.0
            msg.pose.pose.orientation.x = pose_quat[0]
            msg.pose.pose.orientation.y = pose_quat[1]
            msg.pose.pose.orientation.z = pose_quat[2]
            msg.pose.pose.orientation.w = pose_quat[3]
            msg.twist.twist.linear.x = Vx
            msg.twist.twist.angular.z = Vyaw

            #print "Odometry pub success~"

            for i in range(36):
                msg.twist.covariance[i] = 0
            msg.twist.covariance[0] = self.VxCov
            msg.twist.covariance[35] = self.VyawCov
            msg.pose.covariance = msg.twist.covariance
            self.pub.publish(msg)

            #print "pub success~"

            # TF Broadcaster
            if self.pub_tf:
                self.tf_broadcaster.sendTransform(
                    (self.pose_x, self.pose_y, 0.0), pose_quat,
                    self.current_time, self.baseId, self.odomId)

        except:
            rospy.loginfo("Error in encoder value !")
            pass
Esempio n. 33
0
    def poll(self):
        now = rospy.Time.now()
        if now > self.t_next:
            if self.debugPID:
                rospy.logdebug("poll start------------------------")
                try:
                    left_pidin, right_pidin = self.arduino.get_pidin()
                    self.lEncoderPub.publish(left_pidin)
                    self.rEncoderPub.publish(right_pidin)
                    rospy.logdebug("left_pidin: " + str(left_pidin))
                    rospy.logdebug("right_pidin: " + str(right_pidin))
                except:
                    rospy.logerr("getpidout exception count: ")
                    return

                try:
                    left_pidout, right_pidout = self.arduino.get_pidout()
                    self.lPidoutPub.publish(left_pidout)
                    self.rPidoutPub.publish(right_pidout)
                    rospy.logdebug("left_pidout: " + str(left_pidout))
                    rospy.logdebug("right_pidout: " + str(right_pidout))
                except:
                    rospy.logerr("getpidout exception count: ")
                    return

            # Read the encoders
            try:
                left_enc, right_enc = self.arduino.get_encoder_counts()
            except:
                self.bad_encoder_count += 1
                rospy.logerr("Encoder exception count: " +
                             str(self.bad_encoder_count))
                return

            dt = now - self.then
            self.then = now
            dt = dt.to_sec()

            # Calculate odometry
            if self.enc_left == None:
                dright = 0
                dleft = 0
            else:
                dright = (right_enc - self.enc_right) / self.ticks_per_meter
                dleft = (left_enc - self.enc_left) / self.ticks_per_meter

            self.enc_right = right_enc
            self.enc_left = left_enc

            dxy_ave = (dright + dleft) / 2.0
            dth = (dright - dleft) / self.wheel_track
            vxy = dxy_ave / dt
            vth = dth / dt

            if (dxy_ave != 0):
                dx = cos(dth) * dxy_ave
                dy = -sin(dth) * dxy_ave
                self.x += (cos(self.th) * dx - sin(self.th) * dy)
                self.y += (sin(self.th) * dx + cos(self.th) * dy)

            if (dth != 0):
                self.th += dth

            quaternion = Quaternion()
            quaternion.x = 0.0
            quaternion.y = 0.0
            quaternion.z = sin(self.th / 2.0)
            quaternion.w = cos(self.th / 2.0)

            # Create the odometry transform frame broadcaster.
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0),
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                rospy.Time.now(), self.base_frame, "odom")

            odom = Odometry()
            odom.header.frame_id = "odom"
            odom.child_frame_id = self.base_frame
            odom.header.stamp = now
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = vxy
            odom.twist.twist.linear.y = 0
            odom.twist.twist.angular.z = vth

            self.odomPub.publish(odom)

            if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
                self.v_des_left = 0
                self.v_des_right = 0

            if self.v_left < self.v_des_left:
                self.v_left += self.max_accel
                if self.v_left > self.v_des_left:
                    self.v_left = self.v_des_left
            else:
                self.v_left -= self.max_accel
                if self.v_left < self.v_des_left:
                    self.v_left = self.v_des_left

            if self.v_right < self.v_des_right:
                self.v_right += self.max_accel
                if self.v_right > self.v_des_right:
                    self.v_right = self.v_des_right
            else:
                self.v_right -= self.max_accel
                if self.v_right < self.v_des_right:
                    self.v_right = self.v_des_right

            # Set motor speeds in encoder ticks per PID loop
            if not self.stopped:
                if self.debugPID:
                    self.lVelPub.publish(self.v_left)
                    self.rVelPub.publish(self.v_right)

                self.arduino.drive(self.v_left, self.v_right)

            self.t_next = now + self.t_delta
Esempio n. 34
0
#!/usr/bin/env python
# license removed for brevity

import rospy
import math
import tf
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion

initial_pose = Odometry()
target_pose = Odometry()
target_distance = 0
actuator_vel = 15
Ianterior = 0
rate_value = 10
Kp = 10
Ki = 2


def get_pose(initial_pose_tmp):
    global initial_pose 
    initial_pose = initial_pose_tmp

def get_target(target_pose_tmp):
    global target_pose 
    target_pose = target_pose_tmp

def thruster_ctrl_msg():
    global actuator_vel
Esempio n. 35
0
 def pose(self):
     last_odom_msg = self._odom_sub.get_last_message()
     if self.test_mode:
         last_odom_msg = Odometry()  # All 0's
     pose = pose_editor.PoseEditor.from_Odometry(last_odom_msg)
     return pose
Esempio n. 36
0
	def __init__(self):
		rospy.loginfo(rospy.get_name() + ": Start")
		# defines
		self.count = 0

		# static parameters
		self.update_rate = 50 # set update frequency [Hz]
		self.deadman_tout_duration = 0.2 # [s]
		self.cmd_vel_tout_duration = 0.2 # [s]

		# get parameters
		self.w_dist = rospy.get_param("/diff_steer_wheel_distance", 0.2) # [m]
		self.ticks_per_meter_left = rospy.get_param("/ticks_per_meter_left", 500)
		self.ticks_per_meter_right = rospy.get_param("/ticks_per_meter_right", 500)

		acc_lin_max = rospy.get_param("~max_linear_acceleration", 1.0) # [m/s^2]
		acc_ang_max = rospy.get_param("~max_angular_acceleration", 1.8) # [rad/s^2]
		self.wheel_speed_variance = 0.01
		self.wheel_speed_delay = 0.1 # [s]
		self.wheel_speed_delay_variance = 0.05
		self.wheel_speed_error = 0.25 # [m/s]
		self.wheel_speed_minimum = 0.07 

		pub_fb_rate = rospy.get_param("~publish_wheel_feedback_rate", 0)
		if pub_fb_rate != 0:
			self.pub_fb_interval = int(self.update_rate/pub_fb_rate)
		else:
			self.pub_fb_interval = 0

		# get topic names
		self.topic_deadman = rospy.get_param("~deadman_sub",'/fmCommand/deadman')
		self.topic_cmd_vel = rospy.get_param("~cmd_vel_sub",'/fmCommand/cmd_vel')
		self.topic_odom_reset = rospy.get_param("~odom_reset_sub",'/fmInformation/odom_reset')
		self.topic_pose = rospy.get_param("~pose_pub",'/fmKnowledge/pose')
		self.topic_w_fb_left_pub = rospy.get_param("~wheel_feedback_left_pub",'/fmInformation/wheel_feedback_left')
		self.topic_w_fb_right_pub = rospy.get_param("~wheel_feedback_right_pub",'/fmInformation/wheel_feedback_right')

		# initialize internal variables
		self.update_interval = 1/(self.update_rate * 1.0)
		self.pi2 = 2*pi
		self.cmd_vel_tout_active = True
		self.pose = [0.0, 0.0, 0.0]
		self.deadman_tout = 0.0
		self.cmd_vel_msgs = []
		self.vel_lin_desired = 0.0
		self.vel_ang_desired = 0.0
		self.acc_lin_max_step = acc_lin_max*self.update_interval		
		self.acc_ang_max_step = acc_ang_max*self.update_interval		
		self.vel_lin = 0.0
		self.vel_ang = 0.0
		self.ref_vel_left = 0.0
		self.ref_vel_right = 0.0
		self.sim_vel_left = 0.0
		self.sim_vel_right = 0.0
		self.wheel_speed_err_left = 0.0	
		self.wheel_speed_err_right = 0.0

		self.dk = differential_kinematics(self.w_dist)

		# setup topic publishers
		self.pose_pub = rospy.Publisher(self.topic_pose, Odometry)
		self.pose_msg = Odometry()

		# setup wheel feedback topic publisher
		if self.pub_fb_interval > 0:
			self.wl_fb_vel_set = 0.0
			self.wr_fb_vel_set = 0.0
			self.w_fb_left_pub = rospy.Publisher(self.topic_w_fb_left_pub, PropulsionModuleFeedback)
			self.w_fb_right_pub = rospy.Publisher(self.topic_w_fb_right_pub, PropulsionModuleFeedback)
			self.w_fb = PropulsionModuleFeedback()

		# setup subscription topic callbacks
		rospy.Subscriber(self.topic_deadman, Bool, self.on_deadman_message)
		rospy.Subscriber(self.topic_cmd_vel, TwistStamped, self.on_cmd_vel_message)
		rospy.Subscriber(self.topic_odom_reset, FloatArrayStamped, self.on_odom_reset_message)

		# call updater function
		self.r = rospy.Rate(self.update_rate)
		self.updater()
Esempio n. 37
0
import matplotlib.pyplot as plt
from scipy import interpolate

from autominy_msgs.msg import NormalizedSpeedCommand, NormalizedSteeringCommand, SteeringFeedback
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseWithCovariance, Pose, Point, PointStamped
from visualization_msgs.msg import Marker
from std_msgs.msg import Header, Int32
from sensor_msgs.msg import LaserScan

data = np.load('lane1.npy')
datab = np.load('lane2.npy')
print len(data[:, 0])
lane = 1
timecounter = 0
aktuellerpunkt = Odometry()
isactiv = False

f = interpolate.CubicSpline(data[:, 0], data[:, 1])
f2 = interpolate.CubicSpline(data[:, 0], data[:, 2])

fb = interpolate.CubicSpline(datab[:, 0], datab[:, 1])
f2b = interpolate.CubicSpline(datab[:, 0], datab[:, 2])

xnew = np.arange(0, 12.8, 0.1)
ynew = f(xnew)

xnewb = np.arange(0, 14.8, 0.1)
ynewb = fb(xnewb)
#plt.plot(data[:,0], data[:,1], 'o', xnew, ynew, '-')
#plt.show()
Esempio n. 38
0
root = Tk()
root.title("Bebop Interface")

mainframe = ttk.Frame(root, padding="3 3 12 12")
mainframe.grid(column=0, row=0, sticky=(N, W, E, S))
mainframe.columnconfigure(0, weight=1)

rospy.init_node('BebopGUI', anonymous=False)

takeoff_pub = rospy.Publisher('/bebop/takeoff', Empty, queue_size=1)

x_odom = StringVar()
y_odom = StringVar()
z_odom = StringVar()
yaw_odom = StringVar()
odom_var = Odometry()


def receive_height(data):
    global height
    height = data.altitude
    z_odom.set("{0:.2f}".format(height))


def receive_attitude(data):
    global att_p
    att_p = math.degrees(data.yaw)
    if (att_p >= -179.99 and att_p <= -0.1):
        #att_p = 360 + att_p
        att_p = att_p * -1
    else:
Esempio n. 39
0
#! /usr/bin/env python

import rospy
import math
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Vector3
from std_msgs.msg import Float64
from nav_msgs.msg import Odometry
import roslib
import actionlib

import tbug.msg

robotpos = Odometry()


def robotposcheck(updatedState):
    global robotpos
    robotpos = updatedState


class GoalServer(object):

    global robotpos

    _feedback = tbug.msg.goalStatusFeedback()
    _result = tbug.msg.goalStatusResult()

    def __init__(self):
        self._as = actionlib.SimpleActionServer('check_goals',
                                                tbug.msg.goalStatusAction,
Esempio n. 40
0
    def spin(self):
        encoders = [0, 0]

        self.x = 0  # position in xy plane
        self.y = 0
        self.th = 0
        then = rospy.Time.now() + rospy.Duration(1.0)

        # things that don't ever change
        #       scan_link = rospy.get_param('~frame_id', 'base_laser_link')
        #       scan = LaserScan(header=rospy.Header(frame_id=scan_link))

        #       scan.angle_min =0.0
        #       scan.angle_max =359.0*pi/180.0
        #       scan.angle_increment =pi/180.0
        #       scan.range_min = 0.020
        #       scan.range_max = 5.0

        odom = Odometry(header=rospy.Header(frame_id=self.odom_frame),
                        child_frame_id=self.base_frame)

        dist = Vector3Stamped(header=rospy.Header(frame_id=self.odom_frame))

        #        button = Button()
        #        sensor = Sensor()
        self.robot.setBacklight(1)
        self.robot.setLED("Green")
        # main loop of driver
        r = rospy.Rate(10)
        cmd_rate = self.CMD_RATE

        self.prevRanges = []

        while not rospy.is_shutdown():
            # notify if low batt
            #if self.robot.getCharger() < 10:
            #    print "battery low " + str(self.robot.getCharger()) + "%"
            # get motor encoder values
            left, right = self.robot.getMotors()

            cmd_rate = cmd_rate - 1
            if cmd_rate == 0:
                # send updated movement commands
                #if self.cmd_vel != self.old_vel or self.cmd_vel == [0,0]:
                # max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1])))
                #self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], (abs(self.cmd_vel[0])+abs(self.cmd_vel[1]))/2)
                self.robot.setMotors(
                    self.cmd_vel[0], self.cmd_vel[1],
                    max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1])))
                cmd_rate = self.CMD_RATE

            self.old_vel = self.cmd_vel

            # prepare laser scan
            #           scan.header.stamp = rospy.Time.now()

            #           self.robot.requestScan()
            #           scan.ranges = self.robot.getScanRanges()

            #            # now update position information
            #            dt = (scan.header.stamp - then).to_sec()
            #            then = scan.header.stamp

            # compute delta time
            dt = (rospy.Time.now() - then).to_sec()
            then = rospy.Time.now()

            d_left = (left - encoders[0]) / 1000.0
            d_right = (right - encoders[1]) / 1000.0
            encoders = [left, right]
            #print d_left, d_right, encoders

            dx = (d_left + d_right) / 2
            dth = (d_right - d_left) / (self.robot.base_width / 1000.0)

            x = cos(dth) * dx
            y = -sin(dth) * dx
            self.x += cos(self.th) * x - sin(self.th) * y
            self.y += sin(self.th) * x + cos(self.th) * y
            self.th += dth
            #print self.x,self.y,self.th

            # prepare tf from base_link to odom
            quaternion = Quaternion()
            quaternion.z = sin(self.th / 2.0)
            quaternion.w = cos(self.th / 2.0)

            # prepare odometry
            # synchronize /odom and /scan in time
            #odom.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
            # no need to synchronize /odom and /scan
            odom.header.stamp = rospy.Time.now()
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = dx / dt
            odom.twist.twist.angular.z = dth / dt

            dist.header.stamp = odom.header.stamp
            dist.vector.x = encoders[0] / 1000
            dist.vector.y = encoders[1] / 1000

            # sensors
            #            lsb, rsb, lfb, rfb = self.robot.getDigitalSensors()
            #
            #            # buttons
            #            btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons()
            #
            #
            # publish everything
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0),
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w), then,
                self.base_frame, self.odom_frame)

            #            if self.prevRanges != scan.ranges:
            #                self.scanPub.publish(scan)
            #                self.prevRanges = scan.ranges

            self.odomPub.publish(odom)
            self.distPub.publish(dist)

            #            button_enum = ("Soft_Button", "Up_Button", "Start_Button", "Back_Button", "Down_Button")
            #            sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper", "Right_Bumper")
            #            for idx, b in enumerate((btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)):
            #                if b == 1:
            #                    button.value = b
            #                    button.name = button_enum[idx]
            #                    self.buttonPub.publish(button)
            #
            #            for idx, b in enumerate((lsb, rsb, lfb, rfb)):
            #                if b == 1:
            #                    sensor.value = b
            #                    sensor.name = sensor_enum[idx]
            #                    self.sensorPub.publish(sensor)

            # wait, then do it again
            r.sleep()

        # shut down
        self.robot.setBacklight(0)
        self.robot.setLED("Off")
        self.robot.setLDS("off")
        self.robot.setTestMode("off")
Esempio n. 41
0
print 'Warming up the IMU...'
rospy.init_node('imu_node')
rate = rospy.Rate(100)

br = tf.TransformBroadcaster()
pubTime = rospy.Publisher('/imu/timestamp', UInt32, queue_size=10)
pubAccAng = rospy.Publisher('/imu/AccAndAng', Twist, queue_size=10)
pubMag = rospy.Publisher('/imu/mag', Vector3, queue_size=10)
pubOdom = rospy.Publisher('/odom', Odometry, queue_size=10)
ser = serial.Serial('/dev/razor')
time.sleep(30)  #Time for the IMU to warm up
print 'Done! Starting to publish...'
msgT = UInt32()
msgAA = Twist()
msgM = Vector3()
msgO = Odometry()
x = 0  #0.2475
y = 0  #-0.057
msgO.header.frame_id = 'odom'
msgO.child_frame_id = 'base_frame'

scale = 1

fixScale = True

while (True):
    ser.flushInput()
    string_list = (ser.read(150))

    values_string = string_list
    values_list = values_string.split(', ')
Esempio n. 42
0
def ros():

    rospy.init_node('simulator_gui_node')
    a = rospy.Service('simulator_robot_step', simulator_robot_step,
                      handle_robot_step)
    b = rospy.Service('simulator_print_graph', simulator_algorithm_result,
                      handle_print_graph)
    c = rospy.Service('simulator_stop', simulator_stop, handle_simulator_stop)
    d = rospy.Service('simulator_set_light_position',
                      simulator_set_light_position,
                      handle_simulator_set_light_position)
    e = rospy.Service('simulator_object_interaction',
                      simulator_object_interaction,
                      handle_simulator_object_interaction)

    #rospy.Subscriber('/scan',LaserScan,update_value,queue_size=1)
    #rospy.Subscriber('/odom',Odometry, turtle_odometry ,queue_size=1)

    odom_pub = rospy.Publisher("/odom_simul", Odometry, queue_size=50)
    odom_broadcaster = tf.TransformBroadcaster()

    x = 0.0
    y = 0.0
    th = 0.0

    vx = 0.1
    vy = -0.1
    vth = 0.1
    current_time = rospy.Time.now()
    last_time = rospy.Time.now()

    pub_params = rospy.Publisher('simulator_parameters_pub',
                                 Parameters,
                                 queue_size=0)
    #rospy.Subscriber("simulator_laser_pub", Laser_values, callback)

    msg_params = Parameters()

    rate = rospy.Rate(100)

    while not gui.stopped:
        parameters = gui.get_parameters()
        msg_params.robot_x = parameters[0]
        msg_params.robot_y = parameters[1]
        msg_params.robot_theta = parameters[2]
        msg_params.robot_radio = parameters[3]
        msg_params.robot_max_advance = parameters[4]
        msg_params.robot_turn_angle = parameters[5]
        msg_params.laser_num_sensors = parameters[6]
        msg_params.laser_origin = parameters[7]
        msg_params.laser_range = parameters[8]
        msg_params.laser_value = parameters[9]
        msg_params.world_name = parameters[10]
        msg_params.noise = parameters[11]
        msg_params.light_x = parameters[12]
        msg_params.light_y = parameters[13]
        msg_params.run = parameters[14]
        msg_params.behavior = parameters[15]
        msg_params.steps = parameters[16]
        msg_params.turtle = parameters[16]
        pub_params.publish(msg_params)

        x = parameters[0]
        y = parameters[1]
        th = parameters[2]
        odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)

        odom_broadcaster.sendTransform((x, y, 0.), odom_quat, current_time,
                                       "base_link_rob2w", "map")

        odom = Odometry()
        odom.header.stamp = current_time
        odom.header.frame_id = "map"
        odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))
        odom.child_frame_id = "base_link_rob2w"
        odom.twist.twist = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0))
        odom_pub.publish(odom)

        rate.sleep()

        #print(gui.stopped)

    for _ in range(20):
        msg_params.run = False
        pub_params.publish(msg_params)
        rate.sleep()
class SensorOdomManager(object):
    # ros node frequency
    main_rate = 10
    # input message definition for sensor
    odom_in_msg = Odometry()
    # output message definition
    odom_msg = Odometry()
    # tf message definition
    tf_msg = TransformStamped()

    # initialization function
    def __init__(self):
        # param fetch
        self.odom_in_topic = get_param('~odom_in_topic', "/in_odom")
        self.odom_out_topic = get_param('~odom_out_topic', "/odom")
        self.odom_out_frame = get_param('~odom_out_frame', "odom")
        self.base_frame = get_param('~base_frame', "base_link")
        self.odom_calc_hz = get_param('~odom_calc_hz', 10)
        self.sensor_offset = json.loads(
            get_param('~sensor_offset', '{"x": 0., "y": 0., "z": 0.}'))
        self.sensor_rotation = json.loads(
            get_param('~sensor_rotation', '{"x": 0., "y": 0., "z": 0.}'))
        self.ignore_x = get_param('~ignore_x', 'False')
        self.ignore_y = get_param('~ignore_y', 'False')
        self.ignore_z = get_param('~ignore_z', 'False')
        # shutdown handling
        rospy.on_shutdown(self.terminate)
        # ros publishers
        self.odom_publisher = rospy.Publisher(self.odom_out_topic,
                                              Odometry,
                                              tcp_nodelay=True,
                                              queue_size=2)
        self.odom_subscriber = rospy.Subscriber(self.odom_in_topic, Odometry,
                                                self.sensor_odom_callback)
        # setup qaternion
        self.qtAdjustedRot = np.array([0, 0, 0, 1])
        # setup input message from sensor
        self.odom_msg.header.frame_id = self.odom_out_frame
        self.odom_msg.child_frame_id = self.base_frame
        self.odom_msg.pose.pose.position.x = 0.0
        self.odom_msg.pose.pose.position.y = 0.0
        self.odom_msg.pose.pose.position.z = 0.0
        self.odom_msg.pose.pose.orientation.x = 0.0
        self.odom_msg.pose.pose.orientation.y = 0.0
        self.odom_msg.pose.pose.orientation.z = 0.0
        self.odom_msg.pose.pose.orientation.w = 1.0
        self.odom_msg.twist.twist.linear.x = 0.0
        self.odom_msg.twist.twist.linear.y = 0.0
        self.odom_msg.twist.twist.linear.z = 0.0
        self.odom_msg.twist.twist.angular.x = 0.0
        self.odom_msg.twist.twist.angular.y = 0.0
        self.odom_msg.twist.twist.angular.z = 0.0
        # setup transform
        self.tf_publisher = tf2_ros.TransformBroadcaster()
        self.tf_msg.header.frame_id = self.odom_out_frame
        self.tf_msg.child_frame_id = self.base_frame
        self.tf_msg.transform.translation.x = 0.0
        self.tf_msg.transform.translation.y = 0.0
        self.tf_msg.transform.translation.z = 0.0
        self.tf_msg.transform.rotation.x = 0.0
        self.tf_msg.transform.rotation.y = 0.0
        self.tf_msg.transform.rotation.w = 0.0
        self.tf_msg.transform.rotation.z = 1.0
        # bool for flagging the first msg received from sensor
        self.first_frame_received = False

    # calback function of the sensor input topic
    def sensor_odom_callback(self, data):
        # mark the flag that the sensor message was received
        self.first_frame_received = True
        # get data from the sensor msg
        self.odom_in_msg.header.frame_id = data.header.frame_id
        self.odom_in_msg.child_frame_id = data.child_frame_id
        self.odom_in_msg.pose.pose.position.x = data.pose.pose.position.x
        self.odom_in_msg.pose.pose.position.y = data.pose.pose.position.y
        self.odom_in_msg.pose.pose.position.z = data.pose.pose.position.z
        self.odom_in_msg.pose.pose.orientation.x = data.pose.pose.orientation.x
        self.odom_in_msg.pose.pose.orientation.y = data.pose.pose.orientation.y
        self.odom_in_msg.pose.pose.orientation.z = data.pose.pose.orientation.z
        self.odom_in_msg.pose.pose.orientation.w = data.pose.pose.orientation.w
        self.odom_in_msg.twist.twist.linear.x = data.twist.twist.linear.x
        self.odom_in_msg.twist.twist.linear.y = data.twist.twist.linear.y
        self.odom_in_msg.twist.twist.linear.z = data.twist.twist.linear.z
        self.odom_in_msg.twist.twist.angular.x = data.twist.twist.angular.x
        self.odom_in_msg.twist.twist.angular.y = data.twist.twist.angular.y
        self.odom_in_msg.twist.twist.angular.z = data.twist.twist.angular.z

    # odometry publisher function
    def pub_odometry(self, time_now):
        # in case we received at least one frame from the sensor
        if (self.first_frame_received == True):
            # update timestapm
            now = time_now
            self.odom_msg.header.stamp = now
            self.tf_msg.header.stamp = now
            # update frame ids
            self.odom_msg.header.frame_id = self.odom_out_frame
            self.odom_msg.child_frame_id = self.base_frame
            # Twist/velocity:
            self.odom_msg.twist.twist.linear.x = self.odom_in_msg.twist.twist.linear.x
            self.odom_msg.twist.twist.angular.z = self.odom_in_msg.twist.twist.linear.z
            # get sensor pose quaternion
            self.qtAdjustedRot = np.array([
                self.odom_in_msg.pose.pose.orientation.x,
                self.odom_in_msg.pose.pose.orientation.y,
                self.odom_in_msg.pose.pose.orientation.z,
                self.odom_in_msg.pose.pose.orientation.w
            ])
            # in case there is no configured rotation to the sensor, ignore the quaternion calc
            if (self.sensor_rotation["x"] != 0.
                    or self.sensor_rotation["y"] != 0.
                    or self.sensor_rotation["z"] != 0.):
                # calc quaternion rotation tool (angles are negative because we need to adjust the sensor from the config data )
                qtSensorAdjustmentTool = tf.transformations.quaternion_from_euler(
                    np.radians(-self.sensor_rotation["x"]),
                    np.radians(-self.sensor_rotation["y"]),
                    np.radians(-self.sensor_rotation["z"]))
                # rotate sensor tf
                qtRot = tf.transformations.quaternion_multiply(
                    qtSensorAdjustmentTool, self.qtAdjustedRot)
                self.qtAdjustedRot = tf.transformations.quaternion_multiply(
                    qtRot,
                    tf.transformations.quaternion_inverse(qtSensorAdjustment))
            # get the euler angles from pose quaternion
            self.rpy = tf.transformations.euler_from_quaternion(
                self.qtAdjustedRot)
            # calculate translations
            # check if we need to calculate or ignore x coordonate
            if (self.ignore_x == False):
                # calculate x coordonate
                self.transl_x = self.odom_in_msg.pose.pose.position.x - (
                    (self.sensor_offset["x"] * math.cos(self.rpy[2])) -
                    (-self.sensor_offset["y"] * math.sin(self.rpy[2])))
            else:
                # ignore x coordonate of the sensor, therefore set it to 0
                self.transl_x = 0
            # check if we need to calculate or ignore y coordonate
            if (self.ignore_y == False):
                # calculate y coordonate
                self.transl_y = self.odom_in_msg.pose.pose.position.y - (
                    (self.sensor_offset["x"] * math.sin(self.rpy[2])) +
                    (-self.sensor_offset["y"] * math.cos(self.rpy[2])))
            else:
                # ignore y coordonate of the sensor, therefore set it to 0
                self.transl_y = 0
            # check if we need to calculate or ignore z coordonate
            if (self.ignore_z == False):
                # calculate z coordonate
                self.transl_z = self.odom_in_msg.pose.pose.position.z - self.sensor_offset[
                    "z"]
            else:
                # ignore z coordonate of the sensor, therefore set it to 0
                self.transl_z = 0
            # update odom message
            self.odom_msg.pose.pose.position.x = self.transl_x
            self.odom_msg.pose.pose.position.y = self.transl_y
            self.odom_msg.pose.pose.position.z = self.transl_z
            self.odom_msg.pose.pose.orientation.x = self.qtAdjustedRot[0]
            self.odom_msg.pose.pose.orientation.y = self.qtAdjustedRot[1]
            self.odom_msg.pose.pose.orientation.z = self.qtAdjustedRot[2]
            self.odom_msg.pose.pose.orientation.w = self.qtAdjustedRot[3]
            # update tf
            self.tf_msg.transform.translation.x = self.transl_x
            self.tf_msg.transform.translation.y = self.transl_y
            self.tf_msg.transform.translation.z = self.transl_z
            self.tf_msg.transform.rotation.x = self.qtAdjustedRot[0]
            self.tf_msg.transform.rotation.y = self.qtAdjustedRot[1]
            self.tf_msg.transform.rotation.z = self.qtAdjustedRot[2]
            self.tf_msg.transform.rotation.w = self.qtAdjustedRot[3]
            # ... and publish!
            self.tf_publisher.sendTransform(self.tf_msg)
            self.odom_publisher.publish(self.odom_msg)

    # shutdown hook handler
    def terminate(self):
        # shutdown ros timer
        self.fast_timer.shutdown()

    # scheduling timer handler
    def fast_timer(self, timer_event):
        time_now = rospy.Time.now()
        self.pub_odometry(time_now)

    # main handler
    def main_loop(self):
        # Main control, handle startup and error handling
        # while a ROS timer will handle the high-rate (~50Hz) comms + odometry calcs
        self.main_rate = rospy.Rate(10)  # hz
        # Start timer to run high-rate comms
        self.fast_timer = rospy.Timer(
            rospy.Duration(1 / float(self.odom_calc_hz)), self.fast_timer)
#Locatio

ns = [
    location("Kitchen", 0.59, -2.05, 0.969, -0.247),
    location("TV", 2.06, 3.84, 0.908, -0.42),
    location("Desk", -0.22, 0.198, -0.0345, 1),
    location("Dinning", -3.8, 0.2715, 0, 1)
]

#Hardcode with location=(x,y) and rotation = (z,w)

client = actionlib.SimpleActionClient('move_base', MoveBaseAction)

current_AMCL_pose = PoseWithCovarianceStamped()
current_Odom_pose = Odometry()
current_state = CurrentState(0, 0)
#record what amcl is posting
#A estimation of the current pose

root = Tk()
#ROOT is the whole window
topFrame = Frame(root)
topFrame.pack(side=TOP)
downFrame = Frame(root)
downFrame.pack(side=BOTTOM)
#Down Row for info about navigation
leftFrame = Frame(topFrame)
leftFrame.pack(side=LEFT)
#Left Column for info about the current location
middleFrame = Frame(topFrame)
Esempio n. 45
0
    def _BroadcastOdometryInfo(self, lineParts):
        partsCount = len(lineParts)
        #rospy.logwarn(partsCount)
        if (partsCount < 6):
            pass

        try:
            x = float(lineParts[1])
            y = float(lineParts[2])
            theta = float(lineParts[3])

            vx = float(lineParts[4])
            vy = float(lineParts[1])
            omega = float(lineParts[1])
            self.batteryVoltage = float(lineParts[15]) * 0.0287
            #BatteryState = BatteryState()
            #BatteryState = batteryVoltage * 0.287
            # calculate position

            #quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
            quaternion = Quaternion()
            quaternion.x = 0.0
            quaternion.y = 0.0
            quaternion.z = sin(theta / 2.0)
            quaternion.w = cos(theta / 2.0)

            rosNow = rospy.Time.now()

            # First, we'll publish the transform from frame odom to frame base_link over tf
            # Note that sendTransform requires that 'to' is passed in before 'from' while
            # the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
            self._OdometryTransformBroadcaster.sendTransform(
                (x, y, 0),
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                rosNow, "base_link", "odom")

            # next, we'll publish the odometry message over ROS
            odometry = Odometry()
            odometry.header.frame_id = "odom"
            odometry.header.stamp = rosNow
            odometry.pose.pose.position.x = x
            odometry.pose.pose.position.y = y
            odometry.pose.pose.position.z = 0
            odometry.pose.pose.orientation = quaternion
            odometry.pose.covariance = [
                1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0,
                0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3
            ]

            odometry.child_frame_id = "base_link"
            odometry.twist.twist.linear.x = vx
            odometry.twist.twist.linear.y = vy
            odometry.twist.twist.angular.z = omega
            odometry.twist.covariance = [
                1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0,
                0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3, 0, 0, 0, 0, 0, 0, 1e3
            ]

            self._OdometryPublisher.publish(odometry)

#self._BatteryStatePublisher.publish(12)

            #rospy.loginfo(self.batteryVoltage)

        except:
            rospy.logwarn("odom Unexpected error:" + str(sys.exc_info()[0]))
Esempio n. 46
0
    robot_yaw = 0
    robot_pos_x = 0
    robot_pos_y = 0

    publishCount = 0
    printCount = 0

    # ROS subscriber, publisher and broadcaster setup:
    rospy.init_node('neato_driver_rpi')
    velocity_inputs = CallbackHandler()

    rospy.Subscriber('cmd_vel', Twist, velocity_inputs.handle_input)
    odom_broadcaster = tf.TransformBroadcaster()
    odom_publisher = rospy.Publisher('odom', Odometry, queue_size=50)
    odom = Odometry()
    odom.header.frame_id = 'odom'
    odom.child_frame_id = 'base_footprint'

    try:
        while not rospy.is_shutdown():

            # Convert from robot velocity to wheel spin velocities:
            angularRef = velocity_inputs.robotAngularRef * angular2wheel
            linearRef = velocity_inputs.robotLinearRef * linear2wheel
            right_ref = linearRef + angularRef
            left_ref = linearRef - angularRef

            # In case the combination of the linear and angular references become too fast
            # for the wheels to cope with, the following condition will reduce the wheel speed
            # to the value defined in variable MAX_WHEEL_VEL while keeping the ratio between
Esempio n. 47
0
    def odom_cb(self, odom_data):
        '''
            Converts OdometrySmall to Odometry message and publishes to odom topic
        '''
        dt = odom_data.dt / 1000000.0  # Delta time converted to seconds

        msg = Odometry()
        msg.header.stamp = odom_data.header.stamp
        msg.header.frame_id = "odom"
        msg.child_frame_id = "base_link"

        # Foward kinematics
        v_left = odom_data.rps1 / self.rot_per_m_right  # Wheel speed in m/s
        v_right = odom_data.rps2 / self.rot_per_m_left  # Wheel speed in m/s
        vx = (v_right + v_left
              ) / 2 * self.odom_pitch_correction_coef  # Speed forward in m/s
        omega = (v_right - v_left) / self.base_width  # Angular speed in rad/s

        # Clamped angle
        self.theta += omega * dt
        if self.theta > math.pi:
            self.theta -= 2 * math.pi
        elif self.theta < -math.pi:
            self.theta += 2 * math.pi

        # print("Theta: %f, Omega %f" %(self.theta, omega))
        # print("vL: %f, vR %f" %(v_left, v_right))

        # Position
        self.posx += vx * math.cos(self.theta) * dt
        self.posy += vx * math.sin(self.theta) * dt
        msg.pose.pose.position = Point(self.posx, self.posy, 0)  # x - fwd
        # Angle
        quat = quaternion_from_euler(0, 0, self.theta)
        msg.pose.pose.orientation.x = quat[0]
        msg.pose.pose.orientation.y = quat[1]
        msg.pose.pose.orientation.z = quat[2]
        msg.pose.pose.orientation.w = quat[3]

        # Linear speed
        msg.twist.twist.linear.x = vx
        # msg.twist.twist.linear.y = 0  # Default already zero

        # Angular speed
        msg.twist.twist.angular.z = omega

        # Covariance | Make diagonal covariance matrix
        default_cov = [
            0.0,
        ] * 36
        for i in range(0, 6):
            default_cov[i + i * 6] = 0.00001  # NOTE: tuning robot localization

        msg.twist.covariance = tuple(default_cov)
        msg.pose.covariance = tuple(default_cov)

        pub = rospy.Publisher("odom", Odometry, queue_size=10)
        pub.publish(msg)

        if self.debug is True:
            self.calibration_debug_data(odom_data)
Esempio n. 48
0
# ------------
rospy.Subscriber("green_robot/encoderCount/left", Int32Stamped,
                 callBackLeftEncoderCount)
rospy.Subscriber("green_robot/encoderCount/right", Int32Stamped,
                 callBackRightEncoderCount)

# main node loop
# ---------------

# -----------------------------------------------------------------------------
if __name__ == '__main__':
    # -----------------------------------------------------------------------------
    #rospy.spin()
    #global robot

    odomMsg = Odometry()
    odomMsg.header.frame_id = 'world'
    odomMsg.child_frame_id = 'robot'

    t = rospy.get_time()
    tPrec = t
    leftWheelTheta = 0.0
    leftWheelThetaPrec = 0.0
    rightWheelTheta = 0.0
    rightWheelThetaPrec = 0.0
    rL = robot.leftWheel.diameter / 2.0
    rR = robot.rightWheel.diameter / 2.0
    quaternion = Quaternion()

    #rospy.loginfo("robot x=%f y=%f theta=%f rL=%f rR=%f res=%f=%f" % (robot.x, robot.y, robot.theta, rL, rR, robot.leftWheel.encoderResolution, robot.rightWheel.encoderResolution))
Esempio n. 49
0
    def poll(self):
        now = rospy.Time.now()
        if now > self.t_next:
            try:
                r1, r2, r3, r4 = self.arduino.ping()
                self.left_ranger = r1
                self.front_ranger = r2
                self.right_ranger = r3
                self.back_ranger = r4
                rospy.loginfo("ranger: " + str(r1) + "," + str(r2) + "," +
                              str(r3) + "," + str(r4))
            except:
                rospy.logerr("ping error")
                return
            # Read the encoders
            try:
                left_enc, right_enc = self.arduino.get_encoder_counts()
                #rospy.loginfo("left_enc: " + str(left_enc)+"right_enc: " + str(right_enc))
            except:
                self.bad_encoder_count += 1
                rospy.logerr("Encoder exception count: " +
                             str(self.bad_encoder_count))
                return

            dt = now - self.then
            self.then = now
            dt = dt.to_sec()

            # Calculate odometry
            if self.enc_left == None:
                dright = 0
                dleft = 0
            else:
                if (left_enc < self.encoder_low_wrap
                        and self.enc_left > self.encoder_high_wrap):
                    self.l_wheel_mult = self.l_wheel_mult + 1
                elif (left_enc > self.encoder_high_wrap
                      and self.enc_left < self.encoder_low_wrap):
                    self.l_wheel_mult = self.l_wheel_mult - 1
                else:
                    self.l_wheel_mult = 0
                if (right_enc < self.encoder_low_wrap
                        and self.enc_right > self.encoder_high_wrap):
                    self.r_wheel_mult = self.r_wheel_mult + 1
                elif (right_enc > self.encoder_high_wrap
                      and self.enc_right < self.encoder_low_wrap):
                    self.r_wheel_mult = self.r_wheel_mult - 1
                else:
                    self.r_wheel_mult = 0
                #dright = (right_enc - self.enc_right) / self.ticks_per_meter
                #dleft = (left_enc - self.enc_left) / self.ticks_per_meter
                dleft = 1.0 * (left_enc + self.l_wheel_mult *
                               (self.encoder_max - self.encoder_min) -
                               self.enc_left) / self.ticks_per_meter
                dright = 1.0 * (right_enc + self.r_wheel_mult *
                                (self.encoder_max - self.encoder_min) -
                                self.enc_right) / self.ticks_per_meter

            self.enc_right = right_enc
            self.enc_left = left_enc

            dxy_ave = (dright + dleft) / 2.0
            dth = (dright - dleft) / self.wheel_track
            vxy = dxy_ave / dt
            vth = dth / dt

            if (dxy_ave != 0):
                dx = cos(dth) * dxy_ave
                dy = -sin(dth) * dxy_ave
                self.x += (cos(self.th) * dx - sin(self.th) * dy)
                self.y += (sin(self.th) * dx + cos(self.th) * dy)

            if (dth != 0):
                self.th += dth

            quaternion = Quaternion()
            quaternion.x = 0.0
            quaternion.y = 0.0
            quaternion.z = sin(self.th / 2.0)
            quaternion.w = cos(self.th / 2.0)

            # Create the odometry transform frame broadcaster.
            self.odomBroadcaster.sendTransform(
                (self.x, self.y, 0),
                (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
                rospy.Time.now(), self.base_frame, "odom")

            odom = Odometry()
            odom.header.frame_id = "odom"
            odom.child_frame_id = self.base_frame
            odom.header.stamp = now
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = vxy
            odom.twist.twist.linear.y = 0
            odom.twist.twist.angular.z = vth

            self.odomPub.publish(odom)

            if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
                self.v_des_left = 0
                self.v_des_right = 0

            if self.v_left < self.v_des_left:
                self.v_left += self.max_accel
                if self.v_left > self.v_des_left:
                    self.v_left = self.v_des_left
            else:
                self.v_left -= self.max_accel
                if self.v_left < self.v_des_left:
                    self.v_left = self.v_des_left

            if self.v_right < self.v_des_right:
                self.v_right += self.max_accel
                if self.v_right > self.v_des_right:
                    self.v_right = self.v_des_right
            else:
                self.v_right -= self.max_accel
                if self.v_right < self.v_des_right:
                    self.v_right = self.v_des_right
            self.lVelPub.publish(self.v_left)
            self.rVelPub.publish(self.v_right)

            # Set motor speeds in encoder ticks per PID loop
            if not self.stopped:
                self.arduino.drive(self.v_left, self.v_right)

            self.t_next = now + self.t_delta
Esempio n. 50
0
def calcodom(data):
    global forward
    global rwb_last_rotation
    global lwb_last_rotation
    global x_pos
    global y_pos
    global th
    global x_orientation
    global y_orientation
    global z_orientation
    global w_orientation
    global last_time

    current_time = rospy.Time.now()
    time_diff = (current_time - last_time).nsecs
    last_time = current_time

    odom_broadcaster = tf.TransformBroadcaster()
    #rospy.loginfo(data.transforms[0])

    if data.transforms[0].child_frame_id == "left_steering" and time_diff != 0:

        lwb = data.transforms[1]
        rwb = data.transforms[4]

        #rospy.loginfo(lwb.transform.rotation.y)
        #rospy.loginfo(rwb.transform.rotation.y)

        lwb_diff = lwb.transform.rotation.y - lwb_last_rotation
        rwb_diff = rwb.transform.rotation.y - rwb_last_rotation

        if lwb_diff < 0.01 and lwb_diff > -0.01:
            lwb_diff = 0
        if rwb_diff < 0.01 and rwb_diff > -0.01:
            rwb_diff = 0
        if lwb_diff < -0.5 and forward == 1:
            #rospy.loginfo("lwbbefore %f",lwb_diff)
            #rospy.loginfo("lwb %f",lwb_last_rotation)
            #rospy.loginfo("lwb %f",lwb.transform.rotation.y)
            lwb_diff = 2 + lwb.transform.rotation.y - lwb_last_rotation
            #rospy.loginfo("lwb %f",lwb_diff)
        if rwb_diff < -0.5 and forward == 1:
            #rospy.loginfo("rwbbefore %f",rwb_diff)
            #rospy.loginfo("rwb %f",rwb_last_rotation)
            #rospy.loginfo("rwb %f",rwb.transform.rotation.y)
            rwb_diff = 2 + rwb.transform.rotation.y - rwb_last_rotation
            #rospy.loginfo("rwb %f",rwb_diff)
        if lwb_diff > 0.5 and forward == 0:
            #rospy.loginfo("lwbbefore %f",lwb_diff)
            #rospy.loginfo("lwb %f",lwb_last_rotation)
            #rospy.loginfo("lwb %f",lwb.transform.rotation.y)
            lwb_diff = 2 - lwb.transform.rotation.y + lwb_last_rotation
            #rospy.loginfo("lwb %f",lwb_diff)
        if rwb_diff > 0.5 and forward == 0:
            #rospy.loginfo("rwbbefore %f",rwb_diff)
            #rospy.loginfo("rwb %f",rwb_last_rotation)
            #rospy.loginfo("rwb %f",rwb.transform.rotation.y)
            rwb_diff = 2 - rwb.transform.rotation.y + rwb_last_rotation
            #rospy.loginfo("rwb %f",rwb_diff)

        lwb_diff = lwb_diff * 2 * 0.05 * math.pi
        rwb_diff = rwb_diff * 2 * 0.05 * math.pi

        #rospy.loginfo(lwb_diff)
        #rospy.loginfo(rwb_diff)
        #rospy.loginfo("")

        lwb_last_rotation = lwb.transform.rotation.y
        rwb_last_rotation = rwb.transform.rotation.y

        vel_left = lwb_diff / time_diff
        vel_right = rwb_diff / time_diff

        vel_x = (vel_left + vel_right / 2)
        vel_y = 0
        vel_th = (vel_right - vel_left) / 0.325

        delta_x = (vel_x * math.cos(th)) * time_diff
        delta_y = (vel_x * math.sin(th)) * time_diff
        delta_th = vel_th * time_diff

        x_pos += delta_x
        y_pos += delta_y
        th += delta_th

        rospy.loginfo("xpos at %f", x_pos)
        rospy.loginfo("ypos at %f", y_pos)
        rospy.loginfo("th at %f", th)
        rospy.loginfo("lwbdiff at %f", lwb_diff)
        rospy.loginfo("rwbdiff at %f", rwb_diff)

        odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
        x_orientation = odom_quat[0]
        y_orientation = odom_quat[1]
        z_orientation = odom_quat[2]
        w_orientation = odom_quat[3]
        quat = Quaternion(x_orientation, y_orientation, z_orientation,
                          w_orientation)

        odom_trans = TransformStamped()
        odom_trans.header.stamp = current_time
        odom_trans.header.frame_id = "odom"
        odom_trans.child_frame_id = "base_link"

        odom_trans.transform.translation.x = x_pos
        odom_trans.transform.translation.y = y_pos
        odom_trans.transform.translation.z = 0.0
        #odom_trans.transform.rotation = data.pose[1].orientation;

        odom_broadcaster.sendTransform(
            (x_pos, y_pos, 0.0),
            (x_orientation, y_orientation, z_orientation, w_orientation),
            current_time, "base_link", "odom")

        odom = Odometry()
        odom.header.stamp = current_time
        odom.header.frame_id = "odom"

        odom.pose.pose.position.x = x_pos
        odom.pose.pose.position.y = y_pos
        odom.pose.pose.position.z = 0.0
        odom.pose.pose.orientation = quat
        odom.pose.covariance[0] = 0.2
        odom.pose.covariance[7] = 0.2
        odom.pose.covariance[35] = 0.4

        odom.child_frame_id = "base_link"
        odom.twist.twist.linear.x = vel_x
        odom.twist.twist.linear.y = vel_y
        odom.twist.twist.angular.z = vel_th

        pub.publish(odom)
Esempio n. 51
0
frame_id = "mechROS/tf/odom"
child_frame_id = "/mechROS_base_link"

# Define topics
raw_imu_topic = "/raw_imu"
publish_imu_odometry_topic = "/IMU/odom"
publish_imu_mag_topic = "imu/mag"
publish_imu_topic = "/imu/data_raw"  # For Magdwick filter convension
imu_magdwick_topic = "/imu/data"
velocity_command_topic = "/cmd_vel"

# Po mereni smazat
raw_imu_publish = "raw_data_imu"

# Define static odometry information
odom = Odometry()
odom.header.frame_id = frame_id
odom.child_frame_id = child_frame_id
odom.pose.covariance = cov_pose
odom.twist.covariance = cov_twist

# Define static IMU information
imu = Imu()
imu.header.frame_id = frame_id
imu.orientation_covariance = orientation_cov
imu.angular_velocity_covariance = ang_vel_cov

# Define static magnetic information
mag = MagneticField()
mag.header.frame_id = frame_id
mag.magnetic_field_covariance = mag_field_cov
#!/usr/bin/env python
# -*- coding: UTF-8 -*-
# license removed for brevity
import rospy
from geometry_msgs.msg import Twist, Accel
from nav_msgs.msg import Odometry
from math import atan2

#全局变量声明
flowfield_velocity_latest = Twist() #记录最后一个到来的流场数据
control_msg_latest = Accel() #记录最后一个到来的控制输入
expect_velocity = Twist() #记录当前的静场运动速度
ground_truth_latest = Odometry() #记录当前的实时状态
flowfield_direction_publisher  = rospy.Publisher('flowfield_direction', Twist, queue_size=10)

def OnFlowfieldMsg(flowfield_velocity):
    #全局变量声明
    global flowfield_velocity_latest
    global flowfield_direction_publisher
    
    flowfield_velocity_latest = flowfield_velocity;
    flowfield_direction = Twist();
    flowfield_speed_square = (flowfield_velocity.linear.x * flowfield_velocity.linear.x + flowfield_velocity.linear.y * flowfield_velocity.linear.y)
    flowfield_direction.linear.x=flowfield_velocity.linear.x / flowfield_speed_square
    flowfield_direction.linear.y=flowfield_velocity.linear.y / flowfield_speed_square
    flowfield_direction_publisher.publish(flowfield_direction)
    
def OnControlMsg(control_msg):
    #全局变量声明
    global control_msg_latest
    control_msg_latest = control_msg
Esempio n. 53
0
    def spin(self):

        # state
        s = self.sensor_state
        odom = Odometry(header=rospy.Header(frame_id=self.odom_frame), child_frame_id=self.base_frame)
        js = JointState(name = ["left_wheel_joint", "right_wheel_joint", "front_castor_joint", "back_castor_joint"],
                        position=[0,0,0,0], velocity=[0,0,0,0], effort=[0,0,0,0])

        r = rospy.Rate(self.update_rate)
        last_cmd_vel = 0, 0
        last_cmd_vel_time = rospy.get_rostime()
        last_js_time = rospy.Time(0)
        # We set the retry count to 0 initially to make sure that only 
        # if we received at least one sensor package, we are robust 
        # agains a few sensor read failures. For some strange reason, 
        # sensor read failures can occur when switching to full mode 
        # on the Roomba. 
        sensor_read_retry_count = 0 


        while not rospy.is_shutdown():
            last_time = s.header.stamp
            curr_time = rospy.get_rostime()

            # SENSE/COMPUTE STATE
            try:
                self.sense(s)
                transform = self.compute_odom(s, last_time, odom)
                # Future-date the joint states so that we don't have
                # to publish as frequently.
                js.header.stamp = curr_time + rospy.Duration(1)
            except select.error:
                # packet read can get interrupted, restart loop to
                # check for exit conditions
                continue

            except DriverError: 
                if sensor_read_retry_count > 0: 
                    rospy.logwarn('Failed to read sensor package. %d retries left.' % sensor_read_retry_count) 
                    sensor_read_retry_count -= 1 
                    continue 
                else: 
                    raise 
            sensor_read_retry_count = self._SENSOR_READ_RETRY_COUNT 

            # Reboot Create if we detect that charging is necessary.
#            if s.charging_sources_available > 0 and \
#                   s.oi_mode == 1 and \
#                   s.charging_state in [0, 5] and \
#                   s.charge < 0.93*s.capacity:
#                rospy.loginfo("going into soft-reboot and exiting driver")
#                self._robot_reboot()
#                rospy.loginfo("exiting driver")
#                break

            # Reboot Create if we detect that battery is at critical level switch to passive mode.
#            if s.charging_sources_available > 0 and \
#                   s.oi_mode == 3 and \
#                   s.charging_state in [0, 5] and \
#                   s.charge < 0.15*s.capacity:
#                rospy.loginfo("going into soft-reboot and exiting driver")
#                self._robot_reboot()
#                rospy.loginfo("exiting driver")
#                break

            # PUBLISH STATE
            if transform is not None:
                self.sensor_state_pub.publish(s)
                self.odom_pub.publish(odom)
                if self.publish_tf:
                    self.publish_odometry_transform(odom)
            # 1hz, future-dated joint state
#            if curr_time > last_js_time + rospy.Duration(1):
#                self.joint_states_pub.publish(js)
#                last_js_time = curr_time
#            self._diagnostics.publish(s, self._gyro)
#            if self._gyro:
#                self._gyro.publish(s, last_time)

            # ACT
            if self.req_cmd_vel is not None:
                # check for velocity command and set the robot into full mode if not plugged in
#                if s.oi_mode != self.operate_mode and s.charging_sources_available != 1:
#                    if self.operate_mode == 2:
#                        self._robot_run_safe()
#                    else:
#                        self._robot_run_full()

                # check for bumper contact and limit drive command
                req_cmd_vel = self.check_bumpers(s, self.req_cmd_vel)

                # Set to None so we know it's a new command
                self.req_cmd_vel = None
                # reset time for timeout
                last_cmd_vel_time = last_time

            else:
                #zero commands on timeout
#                if last_time - last_cmd_vel_time > self.cmd_vel_timeout:
#                    last_cmd_vel = 0,0
                # double check bumpers
                req_cmd_vel = self.check_bumpers(s, last_cmd_vel)

#            if(req_cmd_vel[0] != last_cmd_vel[0] or req_cmd_vel[1] != last_cmd_vel[1]): # Added if only drive command are different then issue new command
            # send command
            if self.emergency_activated:
                #self.drive_cmd(0, 0)
                self.robot.sci.getSer().write('STOP 0' + chr(13))
            else:
                self.drive_cmd(*req_cmd_vel)
            # record command
            last_cmd_vel = req_cmd_vel

            r.sleep()
Esempio n. 54
0
#!/usr/bin/env python

import rospy
from std_msgs.msg import Float32MultiArray
from nav_msgs.msg import Odometry
import numpy as np
from math import sin, cos, pi

last_data = Odometry()
started = False
pub = rospy.Publisher('/kumara/odom', Odometry, queue_size=1000)


#last_data.pose.pose.position.z      =       0.0
#last_data.pose.pose.orientation.x   =       0.0
#last_data.pose.pose.orientation.y   =       0.0
def etoq(yaw):
    yaw = (yaw * pi) / 180.00  #convert to rad
    w = cos(yaw / 2)
    z = sin(yaw / 2)
    return [w, z]


def callback(data):
    global started, last_data
    last_data.header.frame_id = "odom"
    last_data.child_frame_id = "base_footprint"
    last_data.pose.pose.position.x = data.data[0]
    last_data.pose.pose.position.y = data.data[1]
    last_data.pose.pose.orientation.w = etoq(data.data[2])[0]
    last_data.pose.pose.orientation.z = etoq(data.data[2])[1]
Esempio n. 55
0
 def __init__(self, topic_name='/odom'):
     self._topic_name = topic_name
     self._sub = rospy.Subscriber(self._topic_name, Odometry,
                                  self.topic_callback)
     self._odomdata = Odometry()
# Author : Abhishek Raj Dutta
# Date :22/11/2106
# This code merges stereo odometry with IMU and command velocities to produce an estimate of the HUSKY's pose and heading

import rospy
import tf.transformations
from geometry_msgs.msg import Twist
from geometry_msgs.msg import TwistStamped
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
import math
import numpy as np

pub = rospy.Publisher("/odometry/filtered/self", Odometry, queue_size=10)
br = tf.TransformBroadcaster()
odom = Odometry()
imu = Imu()
yawViso = 0.0
vYawViso = 0.0
vX = 0.0
yawImu = 0.0
yawImu1 = 0.0
yaw = 0.0
i = 0
vYawCov = 0.09
ImuYawCov = 0.15707963267948966
yawCov = 10
cmd = np.array([(0.0, 0.0), (0.0, 0.0)])
yaw = np.array([0.0, 0.0])
meas = np.array([(0.0, 0.0), (0.0, 0.0)])
# meas = np.array ([0.0,0.0])
Esempio n. 57
0
    def poll(self):
        self.send_debug_pid()

        time_now = rospy.Time.now()
        if time_now > self.t_next:
            #rospy.logwarn("Voltage: %f, Current: %f", float(vol), float(current))
            #vol = self.arduino.detect_voltage()
            #current = self.arduino.detect_current()
            #rospy.logwarn("Voltage: %f, Current: %f", vol, current)
            # Read the encoders
            try:
                aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.get_encoder_counts(
                )
            except:
                self.bad_encoder_count += 1
                rospy.logerr("Encoder exception count: " +
                             str(self.bad_encoder_count))
                return
            #rospy.loginfo("Encoder A:"+str(aWheel_enc)+",B:"+str(bWheel_enc)+",C:" + str(cWheel_enc))

            # Calculate odometry
            dist_A = (aWheel_enc - self.enc_A) / self.ticks_per_meter
            dist_B = (bWheel_enc - self.enc_B) / self.ticks_per_meter
            dist_C = (cWheel_enc - self.enc_C) / self.ticks_per_meter

            # save current encoder value for next calculate
            self.enc_A = aWheel_enc
            self.enc_B = bWheel_enc
            self.enc_C = cWheel_enc

            dt = time_now - self.then
            self.then = time_now
            dt = dt.to_sec()

            va = dist_A / dt
            vb = dist_B / dt
            vc = dist_C / dt

            #forward kinemation
            vx = sqrt(3) * (va - vb) / 3.0
            vy = (va + vb - 2 * vc) / 3.0
            vth = (va + vb + vc) / (3 * self.wheel_track)

            delta_x = (vx * cos(self.th) - vy * sin(self.th)) * dt
            delta_y = (vx * sin(self.th) + vy * cos(self.th)) * dt
            delta_th = vth * dt

            self.x += delta_x
            self.y += delta_y
            self.th += delta_th

            quaternion = Quaternion()
            quaternion.x = 0.0
            quaternion.y = 0.0
            quaternion.z = sin(self.th / 2.0)
            quaternion.w = cos(self.th / 2.0)

            # create the odometry transform frame broadcaster.
            #self.odomBroadcaster.sendTransform(
            #    (self.x, self.y, 0),
            #    (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            #    rospy.Time.now(),
            #    self.base_frame,
            #    self.odom_name
            #)

            odom = Odometry()
            odom.header.frame_id = self.odom_name
            odom.child_frame_id = self.base_frame
            odom.header.stamp = time_now
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.pose.covariance = [
                1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0, 0, 0, 1e6, 0, 0,
                0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e-9
            ]
            odom.twist.twist.linear.x = vx
            odom.twist.twist.linear.y = vy
            odom.twist.twist.angular.z = vth
            odom.twist.covariance = [
                1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0, 0, 0, 1e6, 0, 0,
                0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e-9
            ]
            self.odomPub.publish(odom)

            # send motor cmd
            if time_now > (self.last_cmd_time + rospy.Duration(self.timeout)):
                self.stop()
            else:
                self.send_motor_speed()

            # set next compare time
            self.t_next = time_now + self.t_delta
Esempio n. 58
0
    def controller(self):
        rate = rospy.Rate(10)  # 10hz

        # self.x = self.polar2cartesian([earth_radius, 41.104444, 29.026997])[0] # update self.x everytime
        # self.y = self.polar2cartesian([earth_radius, 41.104444, 29.026997])[1] # update self.x everytime

        while not rospy.is_shutdown():

            # for simulating
            self.vx = self.twist.linear.x
            self.vy = self.twist.linear.y
            self.vth = self.twist.angular.z

            self.current_time = rospy.Time.now()

            # We do not need this part, we are doing our own localization
            # compute odometry in a typical way given the velocities of the robot
            self.dt = (self.current_time - self.last_time).to_sec()
            self.delta_x = (self.vx * cos(self.th) -
                            self.vy * sin(self.th)) * self.dt
            self.delta_y = (self.vx * sin(self.th) +
                            self.vy * cos(self.th)) * self.dt
            self.delta_th = self.vth * self.dt

            # from GPS
            # self.target_location = [earth_radius, 41.103465, 29.027909] # publish to move_base_simple/goal
            # from magnetometer
            # self.yaw = 0

            # X, Y = current location
            # th = current yaw
            """
            self.x += self.delta_x
            self.y += self.delta_y
            self.th += self.delta_th
            """
            # since all odometry is 6DOF we'll need a quaternion created from yaw
            self.odom_quat = tf.transformations.quaternion_from_euler(
                0, 0, self.th)

            # first, we'll publish the transform over tf
            """
            self.odom_broadcaster.sendTransform(
                (self.x, self.y, 0.),
                self.odom_quat,
                self.current_time,
                "base_link",
                "odom"
            )
            """
            # next, we'll publish the odometry message over ROS
            self.odom = Odometry()
            self.odom.header.stamp = self.current_time
            self.odom.header.frame_id = "odom"

            # set the position
            self.odom.pose.pose = Pose(Point(self.x, self.y, 0.),
                                       Quaternion(*self.odom_quat))

            # Write a tranform formula for calculating linear.x linear.y and angular.z speed
            # set the velocity
            self.odom.child_frame_id = "base_link"
            self.odom.twist.twist = Twist(Vector3(self.vx, self.vy, 0),
                                          Vector3(0, 0, self.vth))

            # publish the PoseWithCovarianceStamped
            self.pwcs = PoseWithCovarianceStamped()
            self.pwcs.header.stamp = self.odom.header.stamp
            self.pwcs.header.frame_id = "odom"
            self.pwcs.pose.pose = self.odom.pose.pose
            self.last_time = self.current_time

            # publish the NavSatFix
            self.sensor_data = None
            self.gps_fix = NavSatFix()
            self.gps_fix.header.frame_id = "base_link"
            self.gps_fix.header.stamp = self.odom.header.stamp
            self.gps_fix.status.status = 0  # GPS FIXED
            self.gps_fix.status.service = 1  # GPS SERVICE = GPS
            # Buralar bizden gelecek
            self.gps_fix.latitude = 41.24600
            self.gps_fix.longitude = 29.123123
            self.gps_fix.altitude = 0
            self.gps_fix.position_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
            self.gps_fix.position_covariance_type = 0

            # publish the NavSatFix
            self.waypoint = WayPoint()
            # self.waypoint.id.uuid = [1]
            self.waypoint.position.latitude = 41.24678
            self.waypoint.position.longitude = 29.123100
            self.waypoint.position.altitude = 0
            # self.waypoint.props.key = "key"
            # self.waypoint.props.value = "1"

            # publish the NavSatFix
            self.imuMsg = Imu()
            self.imuMsg.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
            self.imuMsg.angular_velocity_covariance = [
                0, 0, 0, 0, 0, 0, 0, 0, 0
            ]
            self.imuMsg.linear_acceleration_covariance = [
                0, 0, 0, 0, 0, 0, 0, 0, 0
            ]

            self.roll = 0
            self.pitch = 0
            self.yaw = self.th  # şimdilik
            # Acceloremeter
            self.imuMsg.linear_acceleration.x = 0
            self.imuMsg.linear_acceleration.y = 0
            self.imuMsg.linear_acceleration.z = 0

            # Gyro
            self.imuMsg.angular_velocity.x = 0
            self.imuMsg.angular_velocity.y = 0
            self.imuMsg.angular_velocity.z = 0

            self.q = tf.transformations.quaternion_from_euler(
                self.roll, self.pitch, self.yaw)
            self.imuMsg.orientation.x = self.q[0]  #magnetometer
            self.imuMsg.orientation.y = self.q[1]
            self.imuMsg.orientation.z = self.q[2]
            self.imuMsg.orientation.w = self.q[3]

            self.imuMsg.header.frame_id = "base_link"
            self.imuMsg.header.stamp = self.odom.header.stamp
            # Subscriber(s)
            rospy.Subscriber('/husky_velocity_controller/cmd_vel', Twist,
                             self.callback)
            rospy.Subscriber('/odometry/gps', Odometry, self.gps_callback)
            # Publisher(s)
            self.odom_pub.publish(self.odom)
            self.odom_combined_pub.publish(self.pwcs)
            self.gps_pub.publish(self.gps_fix)
            self.waypoint_pub.publish(self.waypoint)
            self.imu_pub.publish(self.imuMsg)
            rate.sleep()
Esempio n. 59
0
    th += delta_th

    # since all odometry is 6DOF we'll need a quaternion created from yaw
    odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)

    # first, we'll publish the transform over tf
    odom_broadcaster.sendTransform(
        (x, y, 0.),
        odom_quat,
        current_time,
        "base_link",
        "odom"
    )

    # next, we'll publish the odometry message over ROS
    odom = Odometry()
    odom.header.stamp = current_time
    odom.header.frame_id = "odom"

    # set the position
    odom.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat))

    # set the velocity
    odom.child_frame_id = "base_link"
    odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))

    # publish the message
    odom_pub.publish(odom)

    last_time = current_time
    r.sleep()
Esempio n. 60
0
class CommandHandler(AbstractCommandHandler):
    def __init__(self):

        #initailizing the services and publishers/subscribers
        rospy.wait_for_service('mavros/cmd/arming')
        rospy.wait_for_service('mavros/cmd/takeoff')
        rospy.wait_for_service('mavros/set_mode')

        try:
            self._armService = rospy.ServiceProxy('mavros/cmd/arming',
                                                  mavros_msgs.srv.CommandBool)
            self._flightModeService = rospy.ServiceProxy(
                'mavros/set_mode', mavros_msgs.srv.SetMode)
        except rospy.ServiceException, e:
            rospy.logerr("Service call failed: %s" % e)

        self._sp_pub = rospy.Publisher('mavros/setpoint_raw/local',
                                       PositionTarget,
                                       queue_size=1)
        self._local_pos = Odometry()
        self._taskname = None
        self._rate = rospy.Rate(20.0)
        self._state = State()
        self._vision_pose = PoseStamped()
        self._camera_pose = None
        self.tfmessage = None

        self._vision_pose_pub = rospy.Publisher("/mavros/vision_pose/pose",
                                                PoseStamped,
                                                queue_size=1000)

        rospy.Subscriber('mavros/state', State, self.stateCb)
        rospy.Subscriber('mavros/local_position/odom', Odometry, self.odomCb)
        rospy.Subscriber("/tf", TFMessage, self.tfCb)

        ##---------COmpute the HT from imu-camera---------------#
        translation_matrix = [-0.05, 0, 0.24638]
        translation_matrix = np.array(translation_matrix)
        translation_matrix = translation_matrix.reshape(1, 3)
        rotation_angles = [
            0.5 * np.pi / 180, 5 * np.pi / 180.0, 90 * np.pi / 180
        ]
        alpha = rotation_angles[2]
        beta = rotation_angles[1]
        gamma = rotation_angles[0]
        last_row = np.array([0.0, 0.0, 0.0, 1.0]).reshape(1, 4)
        rotation_matrix = np.array(
            [[
                np.cos(alpha) * np.cos(beta),
                np.cos(alpha) * np.sin(beta) * np.sin(gamma) -
                np.sin(alpha) * np.cos(gamma),
                np.cos(alpha) * np.sin(beta) * np.cos(gamma) +
                np.sin(alpha) * np.sin(gamma)
            ],
             [
                 np.sin(alpha) * np.cos(beta),
                 np.sin(alpha) * np.sin(beta) * np.sin(gamma) +
                 np.cos(alpha) * np.cos(gamma),
                 np.sin(alpha) * np.sin(beta) * np.cos(gamma) -
                 np.cos(alpha) * np.sin(gamma)
             ],
             [
                 -np.sin(beta),
                 np.cos(beta) * np.sin(gamma),
                 np.cos(beta) * np.cos(gamma)
             ]])

        self.ht = np.concatenate(
            (rotation_matrix, translation_matrix.transpose()), axis=1)
        self.ht = np.concatenate((self.ht, last_row), axis=0)