def execute(self, userdata):
        self.time_init=rospy.get_rostime()
        self.face_data=FaceDetections()
        self.subs=rospy.Subscriber(topic_name, FaceDetections, self.callback_topic)
        

        while  (len(self.face_data.faces) == 0) and  (rospy.get_rostime().secs - self.time_init.secs) < self.topic_time_out:
            rospy.sleep(0.3)
            if self.preempt_requested():
                return 'preempted'

        self.subs.unregister()
        rospy.loginfo('Faces after Unregistering:: ' + str(self.face_data))
        if len(self.face_data.faces) > 0 :
            userdata.topic_output_msg = copy.deepcopy(self.face_data)
            userdata.standard_error = ''
            rospy.logwarn('Face Topic Reader: Faces Detected')
            rospy.logwarn('Face Detected:: ' + str(self.face_data))
            return 'succeeded'
        
        else :
            #time out
            userdata.standard_error = "Topic Reader : TimeOut Error"
            userdata.topic_output_msg = ''
            rospy.logwarn('Face Topic Reader: Time Out Error')
            return 'aborted'
Beispiel #2
0
def complete_timing_trials():
  rospy.init_node('time_trials')
  times = dict(zip(['pickup', 'place'], [[], []]))
  awesome = pick_and_place()

  for i in range(5):
    
    #raw_input("pick up object?")
    start = rospy.get_rostime().to_sec()
    result = awesome.pick_up()
    while not result:
        result = awesome.pick_up()
        continue
    end = rospy.get_rostime().to_sec()
    times['pickup'].append(int(end - start)+1) # ceiling round off
    
    #raw_input("place object?")
    rospy.sleep(1)  # wait for robot to see AR tag
    start = rospy.get_rostime().to_sec()
    result = awesome.place(use_offset=False)
    while not result:
        result = awesome.place()
    end = rospy.get_rostime().to_sec()
    times['place'].append(int(end - start)+1) # ceiling round off
    
    
    with open("time_results.csv", "wb") as f:
      for key in times:
          f.write( "%s, %s\n" % (key, ", ".join(repr(e) for e in times[key])))
Beispiel #3
0
def onMessaged(linkstates):
    global listener
    global broadcaster
    global lastupdate
    if (rospy.get_rostime() - lastupdate).to_sec() < 0.03:
        return
    lastupdate = rospy.get_rostime()
    lastUpdateTime = rospy.get_rostime()
    for (link_idx, link_name) in enumerate(linkstates.name):
        robot_name = link_name.partition('::')[0]
        part_name = link_name.partition('::')[2]
        quat = linkstates.pose[link_idx].orientation
        pos = linkstates.pose[link_idx].position
        #rospy.loginfo(part_name)
        if listener.frameExists(part_name):
            #rospy.loginfo('stuff')
            broadcaster.sendTransform((pos.x,pos.y,pos.z),
                                      (quat.x,quat.y,quat.z,quat.w),
                                      rospy.get_rostime(),
                                      '/'+part_name,
                                      'world')
        if listener.frameExists(robot_name+'/'+part_name):
            #rospy.loginfo('otherstuff')
            broadcaster.sendTransform((pos.x,pos.y,pos.z),
                                      (quat.x,quat.y,quat.z,quat.w),
                                      rospy.get_rostime(),
                                      '/'+robot_name+'/'+part_name,
                                      'world')
 def goal_pose_cb(self, data):
     rospy.loginfo('Position command: pos={}, speed={}'.format(data.pos, data.speed))
     multiplier = 1000
     move_cmd = JointState()
     move_cmd.name = [self.gripper_name]
     move_cmd.position = [data.pos / multiplier]
     move_cmd.velocity = [abs(data.speed) / multiplier]
     if move_cmd.position[0] < self.js.position[self.link_id]:
         move_cmd.velocity = [x * -1 for x in move_cmd.velocity]
     rate = rospy.Rate(10)
     start_time = rospy.get_rostime()
     while not rospy.is_shutdown() and abs(self.js.position[self.link_id] - move_cmd.position[0]) > .007:
         self.boxy_cmd.publish(move_cmd)
         rate.sleep()
         if rospy.get_rostime() - start_time > rospy.Duration(10):
             rospy.logwarn('movement took too long, stopping.')
             break
     move_cmd.velocity = [0.0 for _ in move_cmd.velocity]
     move_cmd.effort = [0.0 for _ in move_cmd.velocity]
     srv_cmd = SetJointStateRequest()
     srv_cmd.state = move_cmd
     # print('send {}'.format(srv_cmd))
     self.boxy_joint_state.call(srv_cmd)
     # self.boxy_joint_state(srv_cmd)
     rospy.loginfo('done.')
Beispiel #5
0
 def send_command(self, command):
   """Send command to the robot.
   @param command: command to send to robot (tuple)   
   returns: respons from robot (tuple)
   """
   #msg = ','.join(map(str, command))
   msg = ','.join(str(x) for x in command) + '\r'
   #rospy.loginfo("msg=%s", str(msg))
   
   if not self.fake_connection:
     done = False
     start_time = rospy.get_rostime()
     while not done:
       try:
         if self.sci is None:
           self.sci = SerialInterface(self.tty, self.baudrate)
         with self.sci.lock:
           self.sci.flush_input()
           self.sci.send(msg)
           rsp = self.sci.read()
         #rsp = msg
         #rospy.loginfo("rsp=%s", str(rsp))
         done = True
       except Exception, err:
         rospy.logerr(str(err))
         if rospy.get_rostime() - start_time > self.connection_timeout:
           raise rospy.ROSInterruptException("connection to robot seems broken")
         self.sci = None # assume serial interface is broken
         rospy.loginfo('waiting for connection')
         rospy.sleep(1.) # wait 1 seconds before retrying
    def handle_multi_range_message(self, multi_range_msg):
        """Handle a ROS multi-range message by updating and publishing the state.

        Args:
             multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message.
        """
        # Update tracker position based on time-of-flight measurements
        new_estimate = self.update_estimate(multi_range_msg)
        if new_estimate is None:
            rospy.logwarn('Could not compute initial estimate: address={}, remote_address={}'.format(
                multi_range_msg.address, multi_range_msg.remote_address))
        else:
            # Publish tracker message
            ros_msg = uwb.msg.UWBTracker()
            ros_msg.header.stamp = rospy.get_rostime()
            ros_msg.address = multi_range_msg.address
            ros_msg.remote_address = multi_range_msg.remote_address
            ros_msg.state = new_estimate.state
            ros_msg.covariance = np.ravel(new_estimate.covariance)
            self.uwb_pub.publish(ros_msg)

            # Publish target transform (rotation is identity)
            self.tf_broadcaster.sendTransform(
                (new_estimate.state[0], new_estimate.state[1], new_estimate.state[2]),
                tf.transformations.quaternion_from_euler(0, 0, 0),
                rospy.get_rostime(),
                self.target_frame,
                self.tracker_frame
            )
Beispiel #7
0
    def move_to_synchro_pos(self):
        self.init_conman()
        self.conmanSwitch([], ['Irp6ptfgSplineTrajectoryGeneratorMotor','Irp6pmSplineTrajectoryGeneratorMotor','Irp6pmSplineTrajectoryGeneratorJoint','Irp6pmPoseInt','Irp6pmForceControlLaw','Irp6pmForceTransformation'], True)
  
        self.conmanSwitch(['Irp6pmSplineTrajectoryGeneratorMotor'], [], True)
    
        self.client = actionlib.SimpleActionClient('/irp6p_arm/spline_trajectory_action_motor', FollowJointTrajectoryAction)
        self.client.wait_for_server()

        goal = FollowJointTrajectoryGoal()
        goal.trajectory.joint_names = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']
        goal.trajectory.points.append(JointTrajectoryPoint([0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [], [], rospy.Duration(10.0)))
        goal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.2)

        self.client.send_goal(goal, self.irp6pm_done_callback)
        
        self.conmanSwitch(['Irp6ptfgSplineTrajectoryGeneratorMotor'], [], True)
    
        self.client_tfg = actionlib.SimpleActionClient('/irp6p_tfg/spline_trajectory_action_motor', FollowJointTrajectoryAction)
        self.client_tfg.wait_for_server()

        goal = FollowJointTrajectoryGoal()
        goal.trajectory.joint_names = ['joint1']
        goal.trajectory.points.append(JointTrajectoryPoint([0.0], [0.0], [], [], rospy.Duration(10.0)))
        goal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.2)

        self.client_tfg.send_goal(goal, self.irp6ptfg_done_callback)

        self.status.motion_in_progress = True
        self.change_motors_widget_state()
Beispiel #8
0
 def Control12(self):
   t= 0.0
   dt= self.time_step
   #base= self.joint_positions[1]
   base= [0.0]*7
   cmd= trajectory_msgs.msg.JointTrajectory()
   cmd.header.stamp= self.last_l_arm_controller_state.header.stamp
   cmd.joint_names= self.joint_names[1]
   cmd.points.append(trajectory_msgs.msg.JointTrajectoryPoint())
   if self.over_target:
     cmd.points.append(trajectory_msgs.msg.JointTrajectoryPoint())
   while t<3.14159*4.0:
     t= t+dt
     cmd.points[0].time_from_start= rospy.Duration(dt)
     cmd.points[0].positions= base + np.array([self.Curve(t)]*7)
     #cmd.points[0].velocities= [0.0]*7
     cmd.points[0].velocities= [self.CurveD(t)]*7
     if self.over_target:
       cmd.points[1].time_from_start= rospy.Duration(dt*2)
       cmd.points[1].positions= base + np.array([self.Curve(t+dt)]*7)
       #cmd.points[1].velocities= [0.0]*7
       cmd.points[1].velocities= [self.CurveD(t+dt)]*7
     self.l_pub.publish(cmd)
     start_time= rospy.get_rostime()
     while rospy.get_rostime() < start_time + rospy.Duration(dt):
       time.sleep(dt*0.1)
Beispiel #9
0
	def new_diagnostics(self, diag):
        	for status in diag.status:
            		if(status.name == "//base_controller"):
                		if(status.level != 0):## && self.last_base_diag == 0):
                    			self.diag_err = True
                		elif(status.level == 0):## && self.last_base_diag == 1):
                    			self.diag_err = False
		if((rospy.get_rostime() - self.last_led).to_sec() > 0.5):
			self.last_led = rospy.get_rostime()
		        #Trigger LEDS
	    		if(self.diag_err):
				if(self.color != "red"):
		    			sss.set_light("red")	
					self.color = "red"
	    		else:
        			if ((rospy.get_rostime() - self.last_vel).to_sec() > 1.0):
					if(self.color != "green"):
		            			sss.set_light("green")
						self.color = "green"
	    	    		else:
        		    		if(self.on):
            		    			self.on = False
						if(self.color != "yellow"):
	            	    				sss.set_light("yellow")
							self.color = "yellow"
	 		           	else:
        	        			self.on = True
						if(self.color != "led_off"):
				                	sss.set_light("led_off")
							self.color = "led_off"
Beispiel #10
0
    def publish(self, data):
        if not self._calib and data.getImuMsgId() == PUB_ID:
            q = data.getOrientation()
            roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
            array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))

            res = Quaternion()
            res.w = array[0]
            res.x = array[1]
            res.y = array[2]
            res.z = array[3]

            msg = Imu()
            msg.header.frame_id = self._frameId
            msg.header.stamp = rospy.get_rostime()
            msg.orientation = res
            msg.linear_acceleration = data.getAcceleration()
            msg.angular_velocity = data.getVelocity()

            magMsg = MagneticField()
            magMsg.header.frame_id = self._frameId
            magMsg.header.stamp = rospy.get_rostime()
            magMsg.magnetic_field = data.getMagnetometer()

            self._pub.publish(msg)
            self._pubMag.publish(magMsg)

        elif data.getImuMsgId() == CALIB_ID:
            x, y, z = data.getValues()
            msg = imuCalib()

            if x > self._xMax:
                self._xMax = x
            if x < self._xMin:
                self._xMin = x

            if y > self._yMax:
                self._yMax = y
            if y < self._yMin:
                self._yMin = y

            if z > self._zMax:
                self._zMax = z
            if z < self._zMin:
                self._zMin = z


            msg.x.data = x
            msg.x.max = self._xMax
            msg.x.min = self._xMin

            msg.y.data = y
            msg.y.max = self._yMax
            msg.y.min = self._yMin

            msg.z.data = z
            msg.z.max = self._zMax
            msg.z.min = self._zMin

            self._pubCalib.publish(msg)
Beispiel #11
0
    def run(self):
        self._stop = False

        start = rospy.get_rostime()
        rospy.sleep(0.5)
        twist = self.twist
        max_rotate_count = self.max_rotate_count
        rotate_count = max_rotate_count
        yaw_rate = self.yaw_rate

        while not rospy.is_shutdown() and not self._stop:
            if rotate_count == max_rotate_count:
                if twist.angular.z > 0:
                    mod = -1.0
                else:
                    mod = 1.0
                update = mod*yaw_rate/10.0

                while math.fabs(twist.angular.z) <= yaw_rate:
                    twist.angular.z = twist.angular.z + update
                    self.pub_cmd.publish(twist)
                    rospy.sleep(0.04)

                 # Make sure it is exact so the inequality in the while loop doesn't mess up next time around
                twist.angular.z = mod*yaw_rate
                rotate_count = 0
            else:
                rotate_count += 1
            now = rospy.get_rostime()
            msg = "Rotate: " + str(now.secs - start.secs)
            self.log(msg)
            self.pub_cmd.publish(twist)
            self.rate.sleep()
Beispiel #12
0
	def nSteps(self, nL, nR):
		if nL == 0 and nR == 0:
			return None
		P = 150.0
		R = 0.09
		N = 500
		t_sim = (1.0*max(abs(nL),(nR)))/P
		trcs = 0.4
		aux = (1.0*min(abs(nL),abs(nR)))/(1.0*max(abs(nR),abs(nL))) 
		if abs(nL)>=abs(nR):
			VL = math.copysign(1.0,nL) * P * 2.0 * math.pi * R / N
			VR = math.copysign(1.0,nR) * abs(VL) * aux
		else:
			VR = math.copysign(1.0,nR) * P * 2.0 * math.pi * R / N
			VL = math.copysign(1.0,nL) * abs(VR) * aux
	
		v_lin = (VL + VR)/2.0
		v_ang = (VR - VL)/trcs

		tm = rospy.get_rostime()
		while tm.secs == 0 and tm.nsecs == 0:
			tm = rospy.get_rostime()

		while not rospy.is_shutdown():
			cur = rospy.get_rostime()
			self.cmd.linear.x = v_lin
			self.cmd.angular.z = v_ang
			self.cmd_pub.publish(self.cmd)	
			self.rate.sleep()
			if not (cur - tm) < rospy.Duration(t_sim):
				break
Beispiel #13
0
  def Control11(self):
    t= 0.0
    dt= self.time_step
    #base= self.joint_positions[1]
    base= np.array([0.0]*7)
    goal= pr2_controllers_msgs.msg.JointTrajectoryGoal()
    goal.trajectory.header.stamp= rospy.Time.now()
    goal.trajectory.joint_names= self.joint_names[1];
    for i in range(self.num_points):
      goal.trajectory.points.append(trajectory_msgs.msg.JointTrajectoryPoint())
    g_prev= base.copy()
    while t<3.14159*4.0:
      traj_duration= dt
      t= t+dt
      g= np.array([self.Curve(t)]*7)
      #g[4]= AngleMod1(40.0*(g[4]))
      dg= np.array([self.CurveD(t)]*7)
      #InterpolateLinearly1(goal.trajectory.points, self.joint_positions[1],self.joint_velocities[1], g,dg, dt, self.num_points)
      if False:
        InterpolateLinearly1(goal.trajectory.points, g_prev, np.array([self.CurveD(t-dt)]*7), g, np.array([self.CurveD(t)]*7), dt, self.num_points)
      else:
        traj_duration= InterpolateLinearly2(goal.trajectory.points, g_prev, g, dt, dt/float(self.num_points), True, self.vel_limits)

      #print goal.trajectory.points
      for p in goal.trajectory.points:
        self.fp_goal.write('%f %s %s\n' % ( (rospy.get_rostime()-self.ljscallback_start_time+p.time_from_start).to_sec(), ' '.join(map(str,p.positions)), ' '.join(map(str,p.velocities)) ))
      #self.fp_goal.write('%f %s %s\n' % ( (rospy.get_rostime()-self.ljscallback_start_time).to_sec(), ' '.join(map(str,g)), ' '.join(map(str,dg)) ))
      goal.trajectory.header.stamp= rospy.Time.now()
      self.traj_client.send_goal(goal)
      #self.traj_client.wait_for_result()
      start_time= rospy.get_rostime()
      while rospy.get_rostime() < start_time + rospy.Duration(traj_duration):
        time.sleep(traj_duration*0.01)
      g_prev= g
Beispiel #14
0
    def calc_triangle(self):
        # transform for the first corner of triangle
        fst_pt = PointStamped()
        fst_pt.header.frame_id = 'base_link'
        fst_pt.point.x = 1
        fst_pt.point.y = .5
        fst_pt.header.stamp = rospy.get_rostime()
        try: 
            self.tf_listener.waitForTransform('base_link', # from here
                                              'map',     # to here
                                              fst_pt.header.stamp,
                                              rospy.Duration(1.0))

            fst_pt = self.tf_listener.transformPoint('map', fst_pt)
        except tf.Exception as e:
            rospy.loginfo(e)
        
        # transform for the second corner of triangle
        snd_pt = PointStamped()
        snd_pt.header.frame_id = 'base_link'
        snd_pt.point.x = 1
        snd_pt.point.y = -.5
        snd_pt.header.stamp = rospy.get_rostime()
        try: 
            self.tf_listener.waitForTransform('base_link', # from here
                                              'map',     # to here
                                              snd_pt.header.stamp,
                                              rospy.Duration(1.0))

            snd_pt = self.tf_listener.transformPoint('map', snd_pt)
        except tf.Exception as e:
            rospy.loginfo(e)

        return [fst_pt.point.x, fst_pt.point.y, snd_pt.point.x, snd_pt.point.y]
Beispiel #15
0
    def publish(self, data):
        q = data.getOrientation()
        roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])

        # print "Before ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"

        array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))

        res = Quaternion()
        res.w = array[0]
        res.x = array[1]
        res.y = array[2]
        res.z = array[3]

        roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])

        # print "after ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"

        msg = Imu()
        msg.header.frame_id = self._frameId
        msg.header.stamp = rospy.get_rostime()
        msg.orientation = res
        msg.linear_acceleration = data.getAcceleration()
        msg.angular_velocity = data.getVelocity()

        magMsg = MagneticField()
        magMsg.header.frame_id = self._frameId
        magMsg.header.stamp = rospy.get_rostime()
        magMsg.magnetic_field = data.getMagnetometer()

        self._pub.publish(msg)
        self._pubMag.publish(magMsg)
 def increment_array_callback(msg):
     print('Received array ' + str(msg.data))
     msg = get_joint_velocity_msg(msg.data, rospy.get_rostime())
     velocity_publisher.publish(msg)
     rospy.sleep(1./rate)
     msg = get_joint_velocity_msg([0, 0, 0, 0, 0, 0, 0], rospy.get_rostime())
     velocity_publisher.publish(msg)
    def run(self):
        self._stop = False

        start = rospy.get_rostime()
        rospy.sleep(0.5)
        twist = self.twist
        freq = self.freq
        self.rate = rospy.Rate(freq)
        a = self.accl
        max_speed = self.max_speed

        msg = "Time : " + str(rospy.get_rostime().secs) + " Vel : " +str(twist.linear.x)
        finish = False
        while not rospy.is_shutdown() and not self._stop:
            if not finish: 
                twist.linear.x = twist.linear.x + ( 1.0 / freq ) * a 
            if twist.linear.x > max_speed:
                finish = True
                twist.linear.x = 0.0
            msg = "Time : " + str(rospy.get_rostime().secs) + " Vel : " +str(twist.angular.z)
            self.log(msg)
            self.pub_cmd.publish(twist)
            self.rate.sleep()
        twist.linear.x = 0
        self.pub_cmd.publish(twist)
    def __init__(self):

        rospy.init_node('seniorcar_odometry', anonymous=True)
        rospy.Subscriber("seniorcar_state", SeniorcarState, self.update_odometry)
        rospy.Subscriber('imu/data', Imu, self.update_pitch_roll)
        self.pub = rospy.Publisher('seniorcar_odometry',Odometry, queue_size=1000)

        self.current_time = rospy.get_rostime()
        self.last_time = rospy.get_rostime()

        self.odometry.header.frame_id = "odom"
        self.odometry.child_frame_id  = "base_link"
        self.odometry.pose.covariance  = COV_MATRIX
        self.odometry.twist.covariance = COV_MATRIX
        self.odometry.pose.pose.position.x = 0
        self.odometry.pose.pose.position.y = 0
        self.odometry.pose.pose.position.z = 0
        self.odometry.pose.pose.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0))

        global CALCULATE_3DIMENTION_ODOMETRY
        global STEER_ANGLE_OFFSET
        global IMU_PITCH_DEFFULT_ANGLE
        global IMU_ROLL_DEFAULT_ANGLE
        CALCULATE_3DIMENTION_ODOMETRY = rospy.get_param('~calculate_3dimention_odmetry',CALCULATE_3DIMENTION_ODOMETRY)
        STEER_ANGLE_OFFSET = rospy.get_param('~steer_angle_offset',STEER_ANGLE_OFFSET)
        IMU_PITCH_DEFFULT_ANGLE = rospy.get_param('~imu_pitch_offset',IMU_PITCH_DEFFULT_ANGLE) * math.pi / 180.0
        IMU_ROLL_DEFAULT_ANGLE= rospy.get_param('~imu_roll_offset',IMU_ROLL_DEFAULT_ANGLE) * math.pi / 180.0
def main():
    pub_plus = rospy.Publisher('~plus_rect_event', MouseEvent, queue_size=1)
    pub_minus = rospy.Publisher('~minus_rect_event', MouseEvent, queue_size=1)

    width = rospy.get_param('~image_width')
    height = rospy.get_param('~image_height')
    plus_events = [
        MouseEvent(type=3, x=width/4, y=height/4, width=width, height=height),
        MouseEvent(type=4, x=width/2, y=height/2, width=width, height=height),
        MouseEvent(type=2, x=3*width/4, y=3*height/4, width=width, height=height),
    ]
    minus_events = [
        MouseEvent(type=3, x=3*width/4, y=3*height/4, width=width, height=height),
        MouseEvent(type=4, x=width/2, y=height/2, width=width, height=height),
        MouseEvent(type=2, x=width/4, y=height/4, width=width, height=height),
    ]
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        for e in plus_events:
            e.header.stamp = rospy.get_rostime()
            pub_plus.publish(e)
            rate.sleep()
        for e in minus_events:
            e.header.stamp = rospy.get_rostime()
            pub_minus.publish(e)
            rate.sleep()
    def execute(self, userdata):
        goal = SingleJointPositionGoal()
        goal.position = userdata.position
        rospy.loginfo("Sending torso goal with position %0.3f and waiting for result"%userdata.position) 

        # send the goal
        self.torso_client.send_goal(goal)
        start_time = rospy.get_rostime()
        r = rospy.Rate(10)
        while not rospy.is_shutdown():
            now = rospy.get_rostime()
            if now - start_time > rospy.Duration(self.torso_timeout):
                rospy.loginfo("torso timed out!")
                self.torso_client.cancel_goal()
                return 'failed'
            if self.preempt_requested():
                rospy.loginfo("torso goal preempted!")
                self.torso_client.cancel_goal()
                self.service_preempt()
                return 'preempted'
            state = self.torso_client.get_state()
            if state == GoalStatus.SUCCEEDED:
                break
            r.sleep()
        rospy.loginfo("move torso succeeded")
        return 'succeeded'
def main():
    pub_plus = rospy.Publisher('~plus_rect_event', MouseEvent, queue_size=1)
    pub_minus = rospy.Publisher('~minus_rect_event', MouseEvent, queue_size=1)

    rospy.loginfo('Waiting for image_view2 launch..')
    rospy.sleep(5)

    width = rospy.get_param('~image_width')
    height = rospy.get_param('~image_height')
    plus_events = [
        MouseEvent(type=3, x=width/4, y=height/4, width=width, height=height),
        MouseEvent(type=4, x=width/2, y=height/2, width=width, height=height),
        MouseEvent(type=2, x=3*width/4, y=3*height/4, width=width, height=height),
    ]
    minus_events = [
        MouseEvent(type=3, x=3*width/4, y=3*height/4, width=width, height=height),
        MouseEvent(type=4, x=width/2, y=height/2, width=width, height=height),
        MouseEvent(type=2, x=width/4, y=height/4, width=width, height=height),
    ]
    rate = rospy.Rate(10)
    for e in plus_events:
        e.header.stamp = rospy.get_rostime()
        pub_plus.publish(e)
        rate.sleep()
    for e in minus_events:
        e.header.stamp = rospy.get_rostime()
        pub_minus.publish(e)
        rate.sleep()
Beispiel #22
0
    def run(self):
        lastCommandSent = None
        while self.checkRunning():
            cmd = None
            if lastCommandSent is None or rospy.get_rostime().to_sec()-lastCommandSent > 0.2:
                cmd = self.getCommand()

            if cmd is not None:
                self.ser.write(cmd)
                self.ser.flush()
                lastCommandSent = rospy.get_rostime().to_sec()

            data = ''
            t = ''
            while t != '\n' and self.checkRunning():
                if self.ser.inWaiting() > 0:
                    t = self.ser.read(1)
                    if (t != '\r' and t != '\n'):
                        data += t
                elif len(data) == 0:
                    break
                else:
                    time.sleep(0.01)

            if len(data) > 0 and t == '\n' and self.checkRunning():
                self.addData(data)

            time.sleep(0.001)
Beispiel #23
0
 def wait_for_data(self, duration):
     """ Waits to receive statistics data """
     start_time = rospy.get_rostime()
     while not rospy.is_shutdown() and not (rospy.get_rostime() > (start_time + rospy.Duration(duration))):
         if len(self.graph.nodes) >= len(EXPECTED_NODES) and len(self.graph.topics) >= len(EXPECTED_TOPICS):
             return
         rospy.sleep(1.0)
Beispiel #24
0
	def process_pose(self, pose_msg):
		#Get the pose from the message
		x = pose_msg.pose.pose.position.x
		y = pose_msg.pose.pose.position.y
		z = pose_msg.pose.pose.position.z
		x_dot = pose_msg.twist.twist.linear.x
		y_dot = pose_msg.twist.twist.linear.y
		r,p,ya = convert_quat_to_euler(pose_msg.pose.pose.orientation)
		vel_wf = np.array([x_dot, y_dot])
		rot_mat = np.array([[np.cos(ya), np.sin(ya)], [-np.sin(ya), np.cos(ya)]])
		vel_bf = np.dot(rot_mat, vel_wf)
		v_x = vel_bf[0]
		v_y = vel_bf[1]
		#Process the pose to get statistics
		total_v = (v_x**2 + v_y**2)**.5
		if (total_v > self.max_speed):
			self.max_speed = total_v
		slip = 0
		if (v_x > 0.1):
			slip = -np.arctan(v_y/np.abs(v_x))
		if (slip > self.max_slip):
			self.max_slip = slip
		line_eval = (y > self.line[0]*x + self.line[1])
		#Check if we've completed the last
		if ((self.last_eval is not 0) and (line_eval is not self.last_eval) and (x > self.line[2]) and (x < self.line[3])):
			if (self.start_time is None):
				self.start_time = rospy.get_rostime()
			else:
				self.end_time = rospy.get_rostime()
				self.lap_time = self.end_time.to_sec() - self.start_time.to_sec()
				#Read the launch params
				self.params = get_launch_params(self.prefix)
				self.publish_msg()
				self.reset_lap()
		self.last_eval = line_eval
Beispiel #25
0
	def run(self):
		#every 2 seconds
		rate = rospy.Rate(0.5)
		now = rospy.get_rostime()
		while self.active and not rospy.is_shutdown():
			lastEpoch = now
			rate.sleep()
			now = rospy.get_rostime()

			self.connectedLock.acquire(shared=True)
			#get a snapshot of the connectedClients
			connectedSnapshot = self.getConnectedClients()

			#calculate who the leader is
			newLeader = self.calculateLeader(now, connectedSnapshot)
			self.connectedLock.release()

			if self.leader:
				rospy.loginfo("aggregator leader: %s - %s", self.leader.topic, self.weights)
			if not newLeader == self.leader and newLeader is not None:
				self.leader = newLeader
				self.swapForwarding()
				leaderMsgs = self.leader.msgsFromEpoch(now)
				if leaderMsgs:
					self.forwardingCallback(leaderMsgs[-1][1])
Beispiel #26
0
 def Control12(self):
   t= 0.0
   dt= self.time_step
   #base= self.joint_positions[1]
   base= [0.0]*7
   goal= pr2_controllers_msgs.msg.JointTrajectoryGoal()
   goal.trajectory.header.stamp = rospy.Time.now()
   goal.trajectory.joint_names= self.joint_names[1];
   goal.trajectory.points.append(trajectory_msgs.msg.JointTrajectoryPoint())
   if self.over_target:
     goal.trajectory.points.append(trajectory_msgs.msg.JointTrajectoryPoint())
   while t<3.14159*4.0:
     t= t+dt
     goal.trajectory.points[0].time_from_start= rospy.Duration(dt)
     goal.trajectory.points[0].positions= base + np.array([self.Curve(t)]*7)
     #goal.trajectory.points[0].velocities= [0.0]*7
     goal.trajectory.points[0].velocities= [self.CurveD(t)]*7
     if self.over_target:
       goal.trajectory.points[1].time_from_start= rospy.Duration(dt*2)
       goal.trajectory.points[1].positions= base + np.array([self.Curve(t+dt)]*7)
       #goal.trajectory.points[1].velocities= [0.0]*7
       goal.trajectory.points[1].velocities= [self.CurveD(t+dt)]*7
     print goal.trajectory.points
     goal.trajectory.header.stamp= rospy.Time.now()
     self.traj_client.send_goal(goal)
     #self.traj_client.wait_for_result()
     start_time= rospy.get_rostime()
     while rospy.get_rostime() < start_time + rospy.Duration(dt):
       time.sleep(dt*0.1)
Beispiel #27
0
def computeOrientation(start, goal):
    
    global tolerance
    
    rospy.wait_for_service('move_base_node/NavfnROS/make_plan')
    
    try:
        # Initiating the connection to the make plan service from the move base node.
        make_plan_connection = rospy.ServiceProxy('move_base_node/NavfnROS/make_plan', GetPlan)
        
        start = PoseStamped()
        goal = PoseStamped()
        
        start.header.stamp  = rospy.get_rostime()
        goal.header.stamp  = rospy.get_rostime()
        
        start.header.frame_id  = '/map'
        goal.header.frame_id  = '/map'
        
        start.pose = startPose
        goal.pose = goalPose
        
        # Calculation of the path by move base
        response = make_plan_connection(start,goal,tolerance)
        
        deltaX = start.pose.pose.position.x - path[0].pose.pose.position.x
        deltaY = start.pose.pose.position.y - path[0].pose.pose.position.y

        yaw = atan2(deltaY,deltaX)
        
        return yaw
    
    except rospy.ServiceException, e:
        print "Service call failed: %s" %e
Beispiel #28
0
def update_encoders(arg):
#Fix indentation
  encoderr.update(GPIO.input("P9_23"),GPIO.input("P9_24"),rospy.get_rostime().to_sec())
  left = JointState()
#Add joint name and timestamps
  print rospy.get_rostime().to_sec()
  left.header.frame_id = "front_left_wheel_link"
  left.header.stamp  = arg.current_real
  left.position.append(encoderr.position) 
  left.velocity.append(encoderr.velocity)
  encoderl.update(GPIO.input("P8_17"),GPIO.input("P8_26"),rospy.get_rostime().to_sec())
  right = JointState()
  right.header.frame_id = "front_right_wheel_link"
  right.header.stamp = arg.current_real
  right.position.append(encoderl.position) 
  right.velocity.append(encoderl.velocity)
  rospy.loginfo(left)
  publ.publish(left)
  rospy.loginfo(right)
  pubr.publish(right)
  rearleft = JointState()
  rearleft.header.frame_id = "rear_left_wheel_link"
  rearleft.header.stamp = arg.current_real
  rearleft.position.append(encoderr.position)
  rearleft.velocity.append(encoderr.velocity)
  rospy.loginfo(rearleft)
  rearright = JointState()
  rearright.header.frame_id = "rear_right_wheel_link"
  rearright.header.stamp = arg.current_real
  rearright.position.append(encoderl.position)
  rearright.velocity.append(encoderl.velocity)
Beispiel #29
0
def test_tf():
    rospy.init_node('tf_test', anonymous=True)
    rate = rospy.Rate(100)
    init_time = rospy.get_rostime().to_sec()

    motor_publisher = rospy.Publisher('/motor_controller/command', Float64)
    broadcaster = tf.TransformBroadcaster()

    a = 0

    while not rospy.is_shutdown():
        t = rospy.get_rostime().to_sec() - init_time

        if a >= 6.28:
            a = a - 0.5
        else:
            a = a + 0.5

        radius = 0.5
        x = np.cos(a) * radius
        y = np.sin(a) * radius

        motor_publisher.publish(a)

        broadcaster.sendTransform((x, y, 0), tf.transformations.quaternion_from_euler(-3.14/2, 0, a + 3.14), rospy.Time.now(),
                                  "laser1", "world")

        broadcaster.sendTransform((0, 0, radius), tf.transformations.quaternion_from_euler(0, 3.14/2, a), rospy.Time.now(),
                                  "laser2", "world")

        print('time: ' + str(t))
        print('angle: ' + str(a))
        rate.sleep()
 def push_to_path(self, PoseStamped):  
     print 'pushing to path'
     self.path.header.seq = 1
     self.path.header.frame_id = PoseStamped.header.frame_id
     self.path.header.stamp = rospy.get_rostime()
     self.path.poses.append(deepcopy(PoseStamped))  # Deepcopy avoids pointing to the same element for all the path
     self.path.header.stamp = rospy.get_rostime()
Beispiel #31
0
#! /usr/bin/env python

import roslib
roslib.load_manifest('irp6_launch')
import rospy
import actionlib

from trajectory_msgs.msg import *
from pr2_controllers_msgs.msg import *

if __name__ == '__main__':
    rospy.init_node('simple_trajectory')
    client = actionlib.SimpleActionClient(
        'irp6p_controller/joint_trajectory_action', JointTrajectoryAction)
    client.wait_for_server()

    goal = JointTrajectoryGoal()
    goal.trajectory.joint_names = [
        'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6'
    ]
    goal.trajectory.points.append(
        JointTrajectoryPoint([0.0, -1.57, 0.0, 0.0, 4.71, -2.9],
                             [0, 0, 0, 0, 0, 0], [], rospy.Duration(10.0)))

    goal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.1)

    client.send_goal(goal)

    client.wait_for_result()
#!/usr/bin/python
PKG = 'demo'
import roslib
roslib.load_manifest(PKG)
import rospy, sys, os
 
#remove or add the message type
from nav_msgs.msg import Odometry

rospy.init_node('stamp_publisher')

pub=rospy.Publisher('/stamp', Odometry, queue_size=1, latch=False)

if len(sys.argv) < 2:
    print("usage: stamp_publisher.py rate")
    exit(1)
rate = float(sys.argv[1])
rospy.loginfo("rate : %f", rate)

#set a publishing rate. Below is a publishing rate of 10 times/second
r=rospy.Rate(rate)

msg = Odometry()

while not rospy.is_shutdown():
    msg.header.stamp = rospy.get_rostime()
    pub.publish(msg)
    r.sleep()
    
    
Beispiel #33
0
    #Get data and publish infinitly
    for line in logFile:
        msgNum = msgNum + 1
        data_t = line.split(',')

        d_dist = int(data_t[0].strip()) / 1000.0
        d_angle = int(data_t[1].strip()) / 1000.0

        range_raw = int(data_t[2].strip())

        # compute odometry
        robot_theta_i = robot_theta_i + 2 * d_angle / robot_description_r
        robot_x_i = robot_x_i + d_dist * math.cos(robot_theta_i)
        robot_y_i = robot_y_i + d_dist * math.sin(robot_theta_i)

        odoMsg.header.stamp = rospy.get_rostime()
        odoMsg.pose.pose.position.x = robot_x_i
        odoMsg.pose.pose.position.y = robot_y_i
        odoMsg.pose.pose.position.z = 0.0
        odoMsg.pose.pose.orientation.x = 0.0
        odoMsg.pose.pose.orientation.y = 0.0
        odoMsg.pose.pose.orientation.z = math.sin(robot_theta_i / 2.0)
        odoMsg.pose.pose.orientation.w = math.cos(robot_theta_i / 2.0)
        odomPub.publish(odoMsg)

        scanMsg.header.stamp = rospy.get_rostime()
        scanRange = getRangeFromRawData(range_raw)
        scanMsg.ranges = [scanRange, scanRange]
        scanPub.publish(scanMsg)
        print("publishing msg #{0}".format(msgNum))
        time.sleep(1.0)
Beispiel #34
0
        depth_path = path_name + "/depth_0/%06d.npy" % image_index
        if (not os.path.isfile(left_img_path)) or (
                not os.path.isfile(right_img_path)):
            break

        left_img = cv2.imread(left_img_path, 0)
        right_img = cv2.imread(right_img_path, 0)
        depth_np = np.load(depth_path)
        depth_np = 386.1448 / depth_np  #00-02
        # depth_np = 379.8145 / depth_np  #04-12
        depth_visual = np.clip(depth_np, 0.5, 60)
        depth_visual = depth_visual / 60.0 * 255.0
        depth_visual = depth_visual.astype(np.uint8)
        col = cv2.applyColorMap(depth_visual, cv2.COLORMAP_RAINBOW)

        pub_ros_time = rospy.get_rostime()
        left_msg = cv_bridge.cv2_to_imgmsg(left_img, "mono8")
        left_msg.header.stamp = pub_ros_time
        left_pub.publish(left_msg)

        right_msg = cv_bridge.cv2_to_imgmsg(right_img, "mono8")
        right_msg.header.stamp = pub_ros_time
        right_pub.publish(right_msg)

        depth_msg = cv_bridge.cv2_to_imgmsg(depth_np, "32FC1")
        depth_msg.header.stamp = pub_ros_time
        depth_pub.publish(depth_msg)

        depth_visual_msg = cv_bridge.cv2_to_imgmsg(col, "bgr8")
        depth_visual_msg.header.stamp = pub_ros_time
        depth_visual_pub.publish(depth_visual_msg)
def run():
    """
    Based on the received command, land,takeoff, go to a waypoint, pivot and go to waypoint or go to origin

    """
    global currentDroneData , actionState, targetInMap, latchStartTime, latched, wayHomePtr, pub_cmd_vel, pub_takeoff, pub_land, pub_reset
    latchTime = rospy.Duration(5.0)

    rospy.loginfo("Waiting for a command")

    # Loop until ros is shutdown
    while not rospy.is_shutdown():

        # Keep publish waypoints to ros
        publishWaypoints()
        # Keep publish ardrone position to ros
        publishArdronePos()

        # Reset the latch  time
        if actionState == SYS_DEFS.RESET_LATCH_TIME_ACTION_STATE:
            latched = False

        # Take off
        elif actionState == SYS_DEFS.TAKE_OFF_ACTION_STATE:
            # Check if the drone is not latched
            if not latched:
                latchStartTime = rospy.get_rostime()
                latched = True

            # Check if ros time is less than start time + latch time
            if rospy.get_rostime() < latchStartTime + latchTime:
                pub_takeoff.publish(takeoff_msg)
                rospy.loginfo("Taking off")
            else:
                # Hover and reset action state
                command(0, 0, 0, 0, 0, 0)
                actionState = 0
        # Land
        elif actionState == SYS_DEFS.LAND_ACTION_STATE:
            # Decrease the altitude of the drone, until it reaches 0.5
            if currentDroneData.z <=  0.5:
                # Check if the drone is not latched
                if not latched:
                    # assign ros time
                    latchStartTime = rospy.get_rostime()
                    latched = True
                # Check if ros time is less than start time + latch time
                if rospy.get_rostime() < latchStartTime + latchTime:
                    # Land the drone
                    pub_land.publish(land_msg)
                    rospy.loginfo("Landing...")
                else:
                    actionState = 0

            else:
                # Decrease the altitude
                command(0, 0, -1, 0, 0, 0)
                # Go trough the landing state again
                actionState = 2

        # Reset
        elif actionState == SYS_DEFS.RESET_DRONE_ACTION_STATE:
            rospy.loginfo("Resetting")
            # Publish reset message
            pub_reset.publish(reset_msg)
            # Set action state to 0
            actionState = 0

        # Go to the waypoint without looking
        elif actionState == SYS_DEFS.GO_TO_WAYPOINT_WITHOUT_LOOKING_ACTION_STATE:
            # Convert drone coordinates into drone frames
            returnTargetInDrone(targetInMap)
            # Keep going if the waypoint is not reached
            if not wayPointReached(SYS_DEFS.WAYPOINT_ACCURACY):
                xAct = (targetInDrone.position.x * SYS_DEFS.POINT_GAIN)
                yAct = (targetInDrone.position.y * SYS_DEFS.POINT_GAIN)
                zAct = (targetInDrone.position.z * SYS_DEFS.POINT_GAIN)

                rospy.loginfo("Real Pose X: " + str(realPose.pose.position.x) +
                              " Y: " + str(realPose.pose.position.y) +
                              " Z: " + str(realPose.pose.position.z))
                rospy.loginfo("Acceleration X: " + str(xAct) +
                              " Y: " + str(yAct) +
                              " Z: " + str(zAct))
                rospy.loginfo("Current DD X: " + str(currentDroneData.x) +
                              " Y: " + str(currentDroneData.y) +
                              " Z: " + str(currentDroneData.z))
                rospy.loginfo("Target DD X: " + str(targetInDrone.position.x) +
                              " Y: " + str(targetInDrone.position.y) +
                              " Z: " + str(targetInDrone.position.z))
                command(xAct, yAct, zAct, 0, 0, 0)
            else:
                rospy.loginfo("Waypoint Reached ")
                command(0, 0, 0, 0, 0, 0)
                actionState = 0


        # Look at the waypoint
        elif actionState == SYS_DEFS.LOOK_AT_WAYPOINT_ACTION_STATE:
            rospy.loginfo("Looking at the waypoint")
            # Convert drone coordinates into drone frames
            returnTargetInDrone(targetInMap)
            # Adjust z rotation
            zRotAct = targetInDrone.orientation.z * SYS_DEFS.POINT_GAIN
            command(0, 0, 0, 0, 0, zRotAct)

        # Look and Go to the waypoint
        elif actionState == SYS_DEFS.LOOK_AND_GO_TO_WAYPOINT_ACTION_STATE:
            # Convert drone coordinates into drone frames
            returnTargetInDrone(targetInMap)
            # Keep going if the waypoint is not reached
            if not wayPointReached(SYS_DEFS.WAYPOINT_ACCURACY):
                # Check if the drone is facing the waypoint, fix orientation if not
                if wayPointFaced(SYS_DEFS.ANGLE_ACCURACY):
                    # Adjust accelleration with angle gain and point gain
                    zRotAct = (targetInDrone.orientation.z  * SYS_DEFS.ANGLE_GAIN)
                    xAct    = (targetInDrone.position.x     * SYS_DEFS.POINT_GAIN)
                    yAct    = (targetInDrone.position.y     * SYS_DEFS.POINT_GAIN)
                    zAct    = (targetInDrone.position.z     * SYS_DEFS.POINT_GAIN)
                    rospy.loginfo("Real Pose X: " + str(realPose.pose.position.x) +
                                  " Y: " + str(realPose.pose.position.y) +
                                  " Z: " + str(realPose.pose.position.z))
                    rospy.loginfo("Acceleration X: " + str(xAct) +
                                  " Y: " + str(yAct) +
                                  " Z: " + str(zAct))
                    rospy.loginfo("Current DD X: " + str(currentDroneData.x) +
                                  " Y: " + str(currentDroneData.y) +
                                  " Z: " + str(currentDroneData.z))
                    rospy.loginfo("Target DD X: " + str(targetInDrone.position.x) +
                                  " Y: " + str(targetInDrone.position.y) +
                                  " Z: " + str(targetInDrone.position.z))
                    command(xAct, yAct, zAct, 0, 0, zRotAct)

                else:
                    # Fix orientation if the drone is not facing the waypoint
                    rospy.loginfo("Fixing orientation")
                    zRotAct = targetInDrone.orientation.z * SYS_DEFS.ANGLE_GAIN
                    command(0, 0, 0, 0, 0, zRotAct)
            else:
                # Waypoint is reached
                rospy.loginfo("Waypoint reached ")
                # Now hover
                command(0, 0, 0, 0, 0, 0)
                # Reset to state 0
                actionState = 0

        # Follow Flightpath
        elif actionState == SYS_DEFS.FOLLOW_FLIGHT_PATH_WAYPOINTS_ACTION_STATE:
            # Get the global counter
            global currentWaypointCounterForFlightPath
            # Get a coordinate from the xml file and assign it to targetInMap
            targetInMap = droneWaypointsFromXML.getWaypointsCoordinates()
            # Check if the anchor is reached
            if droneWaypointsFromXML.extractCoordinatesFromXML(currentWaypointCounterForFlightPath):
                # Convert drone coordinates into drone frames
                returnTargetInDrone(targetInMap)
                # Keep going if the waypoint is not reached
                if not wayPointReached(SYS_DEFS.WAYPOINT_ACCURACY):
                    # Check if the drone is facing the waypoint, fix orientation if not
                    if wayPointFaced(SYS_DEFS.ANGLE_ACCURACY):
                        # Adjust accelleration with angle gain and point gain
                        zRotAct = (targetInDrone.orientation.z  * SYS_DEFS.ANGLE_GAIN)
                        xAct    = (targetInDrone.position.x     * SYS_DEFS.POINT_GAIN)
                        yAct    = (targetInDrone.position.y     * SYS_DEFS.POINT_GAIN)
                        zAct    = (targetInDrone.position.z     * SYS_DEFS.POINT_GAIN)
                        command(xAct, yAct, zAct, 0, 0, zRotAct)

                    else:
                        # Fix orientation if the drone is not facing the waypoint
                        rospy.loginfo("Fixing orientation")
                        zRotAct = targetInDrone.orientation.z * SYS_DEFS.ANGLE_GAIN
                        command(0, 0, 0, 0, 0, zRotAct)
                else:
                    # Waypoint is reached, move to the next one
                    rospy.loginfo("Target DD X: " + str(targetInMap.position.x) +
                                  " Y: " + str(targetInMap.position.y) +
                                  " Z: " + str(targetInMap.position.z))#
                    # Increase counter, so we can move to the next waypoint
                    currentWaypointCounterForFlightPath +=  1
                    rospy.loginfo("Waypoint Reached " + str(currentWaypointCounterForFlightPath))

            else:
                # There are no more waypoints to go through
                rospy.loginfo("XML waypoints finished")
                # Reset counter to 0
                currentWaypointCounterForFlightPath = 0
                # Now hover so user can see that there are no more anchors
                command(0, 0, 0, 0, 0, 0)
                # Reset to actionState 0
                actionState = 0


        # Follow Flightpath for DWM1001
        elif actionState == SYS_DEFS.FOLLOW_FLIGHT_PATH_DWM1001_ACTION_STATE:
            # Get the global counter
            global currentWaypointCounterForFlightPathDWM1001
            # Get a coordinate from anchors and assign it to targetInMap
            targetInMap = gazeboDwm1001.getAnchorCoordinates()
            # Check if the anchor is reached
            if gazeboDwm1001.anchorsReached(currentWaypointCounterForFlightPathDWM1001):
                # Convert drone coordinates into drone frames
                returnTargetInDrone(targetInMap)
                # Keep going if the waypoint is not reached
                if not wayPointReached(SYS_DEFS.WAYPOINT_ACCURACY):
                    # Check if the drone is facing the waypoint, fix orientation if not
                    if wayPointFaced(SYS_DEFS.ANGLE_ACCURACY):
                        # Adjust accelleration with angle gain and point gain
                        zRotAct = (targetInDrone.orientation.z  * SYS_DEFS.ANGLE_GAIN)
                        xAct    = (targetInDrone.position.x     * SYS_DEFS.POINT_GAIN)
                        yAct    = (targetInDrone.position.y     * SYS_DEFS.POINT_GAIN)
                        zAct    = (targetInDrone.position.z     * SYS_DEFS.POINT_GAIN)
                        command(xAct, yAct, zAct, 0, 0, zRotAct)

                    else:
                        # Fix orientation if the drone is not facing the waypoint
                        rospy.loginfo("Fixing orientation")
                        zRotAct = targetInDrone.orientation.z * SYS_DEFS.ANGLE_GAIN
                        command(0, 0, 0, 0, 0, zRotAct)
                else:
                    # Anchor is reached, move to the second one
                    rospy.loginfo("Target DD X: " + str(targetInMap.position.x) +
                                  " Y: " + str(targetInMap.position.y) +
                                  " Z: " + str(targetInMap.position.z))
                    # Increase counter, so we can move to the next anchor
                    currentWaypointCounterForFlightPathDWM1001 +=  1
                    rospy.loginfo("Anchor Reached " + str(currentWaypointCounterForFlightPathDWM1001))

            else:
                # There are no more anchors to go through
                rospy.loginfo("Anchors finished")
                # Reset counter to 0
                currentWaypointCounterForFlightPathDWM1001 = 0
                # Now hover so user can see that there are no more anchors
                command(0, 0, 0, 0, 0, 0)
                # Reset to actionState 0
                actionState = 0

        # Publish message twist produced by action state
        pub_cmd_vel.publish(messageTwist)
        rate.sleep()
 def send_requests(self, device_id, requests=[]):
     if device_id in self.device_ids and requests:
         self.requests_msg.stamp = rospy.get_rostime()
         self.requests_msg.device_id = device_id
         self.requests_msg.requests = requests
         self.requests_pub.publish(self.requests_msg)
def position_control():
    rospy.init_node('offb_python_node', anonymous=True)
    prev_state = current_state
    rate = rospy.Rate(20.0)  # MUST be more then 2Hz

    # send a few setpoints before starting
    for i in range(100):
        local_pos_pub.publish(pose)
        rate.sleep()

    count = 0
    # wait for FCU connection
    while not current_state.connected:
        rate.sleep()

    last_request = rospy.get_rostime()
    while not rospy.is_shutdown():
        count = count + 0.05
        now = rospy.get_rostime()
        if current_state.mode != "OFFBOARD" and (now - last_request >
                                                 rospy.Duration(5.)):
            set_mode_client(base_mode=0, custom_mode="OFFBOARD")
            last_request = now
        else:
            if not current_state.armed and (now - last_request >
                                            rospy.Duration(5.)):
                arming_client(True)
                last_request = now

    # older versions of PX4 always return success==True, so better to check Status instead
        if prev_state.armed != current_state.armed:
            rospy.loginfo("Vehicle armed: %r" % current_state.armed)
        if prev_state.mode != current_state.mode:
            rospy.loginfo("Current mode: %s" % current_state.mode)
            offboard_started_time = rospy.get_rostime()
        prev_state = current_state

        #quaternion_count = quaternion_from_euler(0, 0, count)
        #pose.pose.orientation.x = quaternion_count[0]
        #pose.pose.orientation.y = quaternion_count[1]
        #pose.pose.orientation.z = quaternion_count[2]
        #pose.pose.orientation.w = quaternion_count[3]
        # Update timestamp and publish pose
        now = rospy.get_rostime()
        if (current_state.mode == "OFFBOARD"
                and now - offboard_started_time <= rospy.Duration(10.)):
            pose.header.stamp = rospy.Time.now()
            local_pos_pub.publish(pose)
            rate.sleep()
        else:
            #create and send velocity setpoints
            twist = Twist()
            xvel = (3.0 - 0.4 * (math.fabs(displacement))) * math.cos(angle)
            twist.linear.x = xvel
            yvel = 3.0 * math.sin(angle)
            twist.linear.y = yvel
            if (altitude < targetHeight):
                twist.linear.z = 0.2
            else:
                twist.linear.z = 0
            twist.angular.x = 0
            twist.angular.y = 0
            try:
                twist.angular.z = -(0.5 * displacement) * (
                    xvel * math.cos(angle) / angle -
                    yvel * math.sin(angle) / angle) - 0.5 * (angle)
            except:
                twist.angular.z = 0
            body_vel_pub.publish(twist)
Beispiel #38
0
    def __init__(self):
        self.my_name = rospy.get_param("~my_name")
        self.cuprint = CUPrint("{}/etddf".format(self.my_name))
        self.blue_agent_names = rospy.get_param("~blue_team_names")
        blue_positions = rospy.get_param("~blue_team_positions")

        self.topside_name = rospy.get_param("~topside_name")
        assert self.topside_name not in self.blue_agent_names

        red_agent_name = rospy.get_param("~red_team_name")

        self.update_times = []

        self.red_agent_exists = red_agent_name != ""
        if self.red_agent_exists:
            self.red_agent_name = red_agent_name
            self.red_agent_id = len(self.blue_agent_names)

        self.use_strapdown = rospy.get_param("~use_strapdown")
        self.do_correct_strapdown = rospy.get_param("~correct_strapdown")
        self.correct_strapdown_next_seq = False
        self.position_process_noise = rospy.get_param(
            "~position_process_noise")
        self.velocity_process_noise = rospy.get_param(
            "~velocity_process_noise")
        self.fast_ci = rospy.get_param("~fast_ci")
        self.force_modem_pose = rospy.get_param("~force_modem_pose")
        self.meas_variances = {}
        self.meas_variances["sonar_range"] = rospy.get_param(
            "~force_sonar_range_var")
        self.meas_variances["sonar_az"] = rospy.get_param(
            "~force_sonar_az_var")
        self.meas_variances["modem_range"] = rospy.get_param(
            "~force_modem_range_var")
        self.meas_variances["modem_az"] = rospy.get_param(
            "~force_modem_az_var")

        known_position_uncertainty = rospy.get_param(
            "~known_position_uncertainty")
        unknown_position_uncertainty = rospy.get_param(
            "~unknown_position_uncertainty")

        self.is_deltatier = rospy.get_param("~is_deltatier")
        if self.is_deltatier:
            self.delta_multipliers = rospy.get_param("~delta_tiers")
            self.delta_codebook_table = {
                "sonar_range": rospy.get_param("~sonar_range_start_et_delta"),
                "sonar_azimuth": rospy.get_param("~sonar_az_start_et_delta")
            }
            self.buffer_size = rospy.get_param("~buffer_space")

        if self.is_deltatier:
            rospy.Service('etddf/get_measurement_package',
                          GetMeasurementPackage, self.get_meas_pkg_callback)

        self.kf = KalmanFilter(blue_positions, [], self.red_agent_exists, self.is_deltatier, \
            known_posititon_unc=known_position_uncertainty,\
            unknown_agent_unc=unknown_position_uncertainty)

        self.network_pub = rospy.Publisher("etddf/estimate/network",
                                           NetworkEstimate,
                                           queue_size=10)
        self.asset_pub_dict = {}
        for asset in self.blue_agent_names:
            self.asset_pub_dict[asset] = rospy.Publisher("etddf/estimate/" +
                                                         asset,
                                                         Odometry,
                                                         queue_size=10)
        if self.red_agent_exists:
            self.asset_pub_dict[self.red_agent_name] = rospy.Publisher(
                "etddf/estimate/" + self.red_agent_name,
                Odometry,
                queue_size=10)

        self.last_update_time = rospy.get_rostime()

        # Modem & Measurement Packages
        rospy.Subscriber("etddf/packages_in",
                         MeasurementPackage,
                         self.meas_pkg_callback,
                         queue_size=1)

        # Strapdown configuration
        self.update_seq = 0
        self.strapdown_correction_period = rospy.get_param(
            "~strapdown_correction_period")
        strap_topic = "odometry/filtered/odom"
        rospy.Subscriber(strap_topic,
                         Odometry,
                         self.nav_filter_callback,
                         queue_size=1)
        self.intersection_pub = rospy.Publisher("set_pose",
                                                PoseWithCovarianceStamped,
                                                queue_size=1)
        self.cuprint("Waiting for strapdown")
        rospy.wait_for_message(strap_topic, Odometry)
        self.cuprint("Strapdown found")

        # Sonar Subscription
        rospy.Subscriber("sonar_processing/target_list/associated",
                         SonarTargetList, self.sonar_callback)
        self.cuprint("Loaded")
Beispiel #39
0
        return mp


if __name__ == "__main__":
    rospy.init_node("etddf_node")
    et_node = ETDDF_Node()

    debug = False
    if not debug:
        rospy.spin()
    else:
        o = Odometry()
        o.pose.pose.orientation.w = 1
        et_node.use_strapdown = False
        rospy.sleep(2)
        t = rospy.get_rostime()
        et_node.nav_filter_callback(o)

        mp = MeasurementPackage()
        m = Measurement("modem_range", t, "topside", "guppy", 6, 0, [], 0)
        mp.measurements.append(m)
        m = Measurement("modem_azimuth", t, "topside", "guppy", 45, 0, [], 0)
        mp.measurements.append(m)
        mp.src_asset = "topside"
        et_node.meas_pkg_callback(mp)

        rospy.sleep(1)
        et_node.kf._filter_artificial_depth(0.0)
        et_node.nav_filter_callback(o)

        stl = SonarTargetList()
Beispiel #40
0
def callback(data):

    timer = Timer()
    timer.tic()

    now = rospy.get_rostime()

    rpn_proposals = data.proposals

    scores = data.scores.reshape(rpn_proposals, N_CLASSES)
    boxes = data.boxes.reshape(rpn_proposals, N_CLASSES * 4)
    sc_orientation = data.sc_orientation.reshape(rpn_proposals,
                                                 N_CLASSES * N_BINS)

    toSend = ObstacleList()
    toSend.header = data.header

    object_list = []

    for cls_ind in xrange(N_CLASSES - 1):

        # if cls_ind == 2:
        #     continue

        cls_ind += 1  # skip background
        cls_boxes = boxes[:, 4 * cls_ind:4 * (cls_ind + 1)]
        cls_scores = scores[:, cls_ind]
        cls_kind = np.empty(scores.shape[0])
        cls_kind.fill(cls_ind)
        cls_sc_orient = sc_orientation[:, N_BINS * cls_ind:N_BINS * cls_ind +
                                       N_BINS]

        #TODO: Didi
        #   if cls_ind == 1:
        #     # Vans and Cars are supressed together
        #     # TODO: This should be generic
        #     cls_ind += 1
        #     cls_boxes = np.vstack((cls_boxes, boxes[:, 4*cls_ind:4*(cls_ind + 1)]))
        #     cls_scores = np.hstack((cls_scores, scores[:, cls_ind]))
        #     cls_sc_orient = np.vstack((cls_sc_orient, sc_orientation))
        #     cls_kind_2 = np.empty(scores.shape[0])
        #     cls_kind_2.fill(cls_ind)
        #     cls_kind = np.hstack((cls_kind, cls_kind_2))

        dets = np.hstack((cls_boxes, cls_scores[:, np.newaxis],
                          cls_kind[:, np.newaxis])).astype(np.float32)

        keep = nms(dets[:, :-1], NMS_THRESH)

        dets = dets[keep, :]
        orients = cls_sc_orient[keep, :]

        for ind, det in enumerate(dets):
            obj = Obstacle()

            temp_bbox = det[0:4].tolist()
            bbox = [
                coord + y_offset_list[ix] for ix, coord in enumerate(temp_bbox)
            ]

            obj.kind_name = CLASSES[cls_ind]
            obj.kind = cls_ind
            obj.bbox.x_offset = bbox[0]
            obj.bbox.y_offset = bbox[1]
            obj.bbox.height = bbox[3] - bbox[1]
            obj.bbox.width = bbox[2] - bbox[0]
            obj.score = det[4]
            obj.alpha = math.pi * (2 * np.argmax(orients[ind, :]) + 1) / N_BINS
            #np.argmax(orients[ind,:])
            #obj.yaw = math.pi * (2* np.argmax(orients[ind,:]) + 1)/N_BINS

            object_list.append(obj)

    toSend.obstacles = object_list

    pub_filter.publish(toSend)

    timer.toc()
    text = "[NMS] Filtered with stamp %.3f at %.3f: took %.3fs" \
        % (data.header.stamp.to_sec(),now.to_sec(),timer.total_time)
    rospy.loginfo(text)
    def callService(self, service_uri, service, service_type, service_args=[]):
        '''
        Calls the service and return the response.
        To call the service the ServiceProxy can't be used, because it uses
        environment variables to determine the URI of the running service. In our
        case this service can be running using another ROS master. The changes on the
        environment variables is not thread safe.
        So the source code of the rospy.SerivceProxy (tcpros_service.py) was modified.

        :param str service_uri: the URI of the service
        :param str service: full service name (with name space)
        :param service_type: service class
        :type service_type: ServiceDefinition: service class
        :param service_args: arguments
        :return: the tuple of request and response.
        :rtype: (request object, response object)
        :raise StartException: on error
        :see: rospy.SerivceProxy<http://docs.ros.org/kinetic/api/rospy/html/rospy.impl.tcpros_service.ServiceProxy-class.html>
        '''
        service = str(service)
        rospy.loginfo("Call service %s[%s]: %s, %s", utf8(service), utf8(service_uri), utf8(service_type), utf8(service_args))
        from rospy.core import parse_rosrpc_uri, is_shutdown
#    from rospy.msg import args_kwds_to_message
        from rospy.exceptions import TransportInitError, TransportException
        from rospy.impl.tcpros_base import TCPROSTransport, DEFAULT_BUFF_SIZE  # ,TCPROSTransportProtocol
        from rospy.impl.tcpros_service import TCPROSServiceClient
        from rospy.service import ServiceException
        request = service_type._request_class()
        import genpy
        try:
            now = rospy.get_rostime()
            import std_msgs.msg
            keys = {'now': now, 'auto': std_msgs.msg.Header(stamp=now)}
            genpy.message.fill_message_args(request, service_args, keys)
        except genpy.MessageException as e:
            def argsummary(args):
                if type(args) in [tuple, list]:
                    return '\n'.join([' * %s (type %s)' % (a, type(a).__name__) for a in args])
                else:
                    return ' * %s (type %s)' % (args, type(args).__name__)
            raise StartException("Incompatible arguments to call service:\n%s\nProvided arguments are:\n%s\n\nService arguments are: [%s]" % (e, argsummary(service_args), genpy.message.get_printable_message_args(request)))

#    request = args_kwds_to_message(type._request_class, args, kwds)
        protocol = TCPROSServiceClient(service, service_type, headers={})
        transport = TCPROSTransport(protocol, service)
        # initialize transport
        dest_addr, dest_port = parse_rosrpc_uri(service_uri)
        # connect to service
        transport.buff_size = DEFAULT_BUFF_SIZE
        try:
            transport.connect(dest_addr, dest_port, service_uri, timeout=5)
        except TransportInitError as e:
            # can be a connection or md5sum mismatch
            raise StartException(''.join(["unable to connect to service: ", utf8(e)]))
        transport.send_message(request, 0)
        try:
            responses = transport.receive_once()
            if len(responses) == 0:
                raise StartException("service [%s] returned no response" % service)
            elif len(responses) > 1:
                raise StartException("service [%s] returned multiple responses: %s" % (service, len(responses)))
        except TransportException as e:
            # convert lower-level exception to exposed type
            if is_shutdown():
                raise StartException("node shutdown interrupted service call")
            else:
                raise StartException("transport error completing service call: %s" % (utf8(e)))
        except ServiceException as e:
            raise StartException("Service error: %s" % (utf8(e)))
        finally:
            transport.close()
            transport = None
        return request, responses[0] if len(responses) > 0 else None
Beispiel #42
0
    def nav_filter_callback(self, odom):
        # Update at specified rate
        t_now = rospy.get_rostime()
        delta_t_ros = t_now - self.last_update_time
        if delta_t_ros < rospy.Duration(1):
            return

        self.kf.propogate(self.position_process_noise,
                          self.velocity_process_noise)
        self.update_times.append(t_now)

        # Update orientation
        last_orientation_quat = odom.pose.pose.orientation
        (r, p, y) = tf.transformations.euler_from_quaternion([last_orientation_quat.x, \
                last_orientation_quat.y, last_orientation_quat.z, last_orientation_quat.w])
        self.last_orientation_rad = y
        orientation_cov = np.array(odom.pose.covariance).reshape(6, 6)

        if self.use_strapdown:
            # last_orientation_dot = odom.twist.twist.angular
            # last_orientation_dot_cov = np.array(odom.twist.covariance).reshape(6,6)

            # Turn odom estimate into numpy
            # Note the velocities are in the base_link frame --> Transform to odom frame # Assume zero pitch/roll
            v_baselink = np.array([[
                odom.twist.twist.linear.x, odom.twist.twist.linear.y,
                odom.twist.twist.linear.z
            ]]).T
            rot_mat = np.array([  # base_link to odom frame
                [np.cos(y), -np.sin(y), 0], [np.sin(y),
                                             np.cos(y), 0], [0, 0, 1]
            ])
            v_odom = rot_mat.dot(v_baselink)

            mean = np.array([[odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z, \
                            v_odom[0,0], v_odom[1,0], v_odom[2,0]]]).T
            cov_pose = np.array(odom.pose.covariance).reshape(6, 6)
            cov_twist = np.array(odom.twist.covariance).reshape(6, 6)
            cov = np.zeros((6, 6))
            cov[:3, :3] = cov_pose[:3, :3]  #+ np.eye(3) * 4 #sim
            cov[3:, 3:] = rot_mat.dot(cov_twist[:3, :3]).dot(
                rot_mat.T)  #+ np.eye(3) * 0.03 #sim

            my_id = self.blue_agent_names.index(self.my_name)
            x_nav, P_nav = self.kf.intersect_strapdown(mean,
                                                       cov,
                                                       my_id,
                                                       fast_ci=False)

            if self.do_correct_strapdown and (
                    self.update_seq % self.strapdown_correction_period == 0):
                if x_nav is not None and P_nav is not None:
                    self.correct_strapdown(odom.header, x_nav, P_nav,
                                           last_orientation_quat,
                                           orientation_cov)
            elif self.correct_strapdown_next_seq:
                self.correct_strapdown(odom.header, x_nav, P_nav,
                                       last_orientation_quat, orientation_cov)
                self.correct_strapdown_next_seq = False

        self.publish_estimates(t_now, last_orientation_quat, orientation_cov)
        self.last_update_time = t_now
        self.update_seq += 1
Beispiel #43
0
    q = quaternion_from_euler(roll, pitch, yaw)
    imuMsg.orientation.x = q[0]
    imuMsg.orientation.y = q[1]
    imuMsg.orientation.z = q[2]
    imuMsg.orientation.w = q[3]
    imuMsg.header.stamp = rospy.Time.now()
    imuMsg.header.frame_id = 'base_imu_link'
    imuMsg.header.seq = seq
    seq = seq + 1
    pub.publish(imuMsg)

    if (diag_pub_time < rospy.get_time()):
        diag_pub_time += 1
        diag_arr = DiagnosticArray()
        diag_arr.header.stamp = rospy.get_rostime()
        diag_arr.header.frame_id = '1'
        diag_msg = DiagnosticStatus()
        diag_msg.name = 'Razor_Imu'
        diag_msg.level = DiagnosticStatus.OK
        diag_msg.message = 'Received AHRS measurement'
        diag_msg.values.append(
            KeyValue('roll (deg)', str(roll * (180.0 / math.pi))))
        diag_msg.values.append(
            KeyValue('pitch (deg)', str(pitch * (180.0 / math.pi))))
        diag_msg.values.append(
            KeyValue('yaw (deg)', str(yaw * (180.0 / math.pi))))
        diag_msg.values.append(KeyValue('sequence number', str(seq)))
        diag_arr.status.append(diag_msg)
        diag_pub.publish(diag_arr)
Beispiel #44
0
    def process_base_loop(self):
        # If the last command was over 1 sec ago, stop the base
        if (self._last_cmd_vel_time is None
                or (rospy.get_rostime() - self._last_cmd_vel_time).to_sec() >
                self._deadman_secs):
            self._x_linear_cmd = 0.0
            self._z_angular_cmd = 0.0

        # ---------------------------------
        # Calculate and send motor commands
        # ---------------------------------
        with self._cmd_vel_lock:
            x_linear_cmd = self._x_linear_cmd
            z_angular_cmd = self._z_angular_cmd

        # Clamp the velocities to the max configured for the base
        x_linear_cmd = max(-self._max_x_lin_vel,
                           min(x_linear_cmd, self._max_x_lin_vel))
        z_angular_cmd = max(-self._max_z_ang_vel,
                            min(z_angular_cmd, self._max_z_ang_vel))

        cmd = calc_create_speed_cmd(x_linear_cmd, z_angular_cmd,
                                    self._wheel_dist, self._wheel_radius,
                                    self._wheel_slip_factor,
                                    self._ticks_per_rotation,
                                    self._max_drive_secs, self._max_qpps,
                                    self._max_accel)
        self._speed_cmd_pub.publish(cmd)

        # -------------------------------
        # Calculate and publish Odometry
        # -------------------------------

        # if self._roboclaw_front_stats is None or self._roboclaw_rear_stats is None:
        if self._roboclaw_front_stats is None:
            rospy.loginfo(
                "Insufficient roboclaw stats received, skipping odometry calculation"
            )
            return

        with self._stats_lock:
            # Calculate change in encoder readings
            m1_front_enc_diff = self._roboclaw_front_stats.m1_enc_val - self._m1_front_enc_prev
            m2_front_enc_diff = self._roboclaw_front_stats.m2_enc_val - self._m2_front_enc_prev
            # m1_rear_enc_diff = self._roboclaw_rear_stats.m1_enc_val - self._m1_rear_enc_prev
            # m2_rear_enc_diff = self._roboclaw_rear_stats.m2_enc_val - self._m2_rear_enc_prev

            self._m1_front_enc_prev = self._roboclaw_front_stats.m1_enc_val
            self._m2_front_enc_prev = self._roboclaw_front_stats.m2_enc_val
            # self._m1_rear_enc_prev = self._roboclaw_rear_stats.m1_enc_val
            # self._m2_rear_enc_prev = self._roboclaw_rear_stats.m2_enc_val

            # Since we have a two Roboclaw robot, take the average of the encoder diffs
            # from each Roboclaw for each side.
            # m1_enc_diff = (m1_front_enc_diff + m1_rear_enc_diff) / 2
            # m2_enc_diff = (m2_front_enc_diff + m2_rear_enc_diff) / 2
            m1_enc_diff = m1_front_enc_diff
            m2_enc_diff = m2_front_enc_diff

            # We take the nowtime from the Stats message so it matches the encoder values.
            # Otherwise we would get timing variances based on when the loop runs compared to
            # when the stats were measured..
            # Since we have a two Roboclaw robot, take the latest stats timestamp from either
            # Roboclaw.
            front_stamp = self._roboclaw_front_stats.header.stamp
            # rear_stamp = self._roboclaw_rear_stats.header.stamp
            # nowtime = max(front_stamp, rear_stamp)
            nowtime = front_stamp

        x_linear_v, y_linear_v, z_angular_v = calc_base_frame_velocity_from_encoder_diffs(
            m1_enc_diff, m2_enc_diff, self._ticks_per_rotation,
            self._wheel_radius, self._wheel_dist, self._wheel_slip_factor,
            self._last_odom_time, nowtime)

        time_delta_secs = (nowtime - self._last_odom_time).to_sec()
        self._last_odom_time = nowtime

        odom = calc_odometry_from_base_velocity(
            x_linear_v, y_linear_v, z_angular_v, self._world_x, self._world_y,
            self._world_theta, time_delta_secs, nowtime, self._base_frame_id,
            self._world_frame_id)
        self._odom_pub.publish(odom)

        # -----------------------------------------
        # Calculate and broacast tf transformation
        # -----------------------------------------
        self._world_x = odom.pose.pose.position.x
        self._world_y = odom.pose.pose.position.y
        self._world_theta = yaw_from_odom_message(odom)
        quat = odom.pose.pose.orientation

        self._tf_broadcaster.sendTransform((self._world_x, self._world_y, 0),
                                           (quat.x, quat.y, quat.z, quat.w),
                                           nowtime, self._base_frame_id,
                                           self._world_frame_id)

        self._last_odom_time = nowtime

        rospy.logdebug("World position: [{}, {}] heading: {}".format(
            self._world_x, self._world_y, self._world_theta))
        rospy.logdebug("Forward speed: {}, Turn speed: {}".format(
            self._x_linear_cmd, self._z_angular_cmd))
Beispiel #45
0
def initPathMarkers():

    # cannot write in one equation!!!
    sourcePoint = Marker()
    goalPoint = Marker()
    neighbourPoint = Marker()
    finalPath = Marker()

    sourcePoint.header.frame_id = 'path_planner'
    goalPoint.header.frame_id = 'path_planner'
    neighbourPoint.header.frame_id = 'path_planner'
    finalPath.header.frame_id = 'path_planner'

    sourcePoint.header.stamp = rospy.get_rostime()
    goalPoint.header.stamp = rospy.get_rostime()
    neighbourPoint.header.stamp = rospy.get_rostime()
    finalPath.header.stamp = rospy.get_rostime()

    sourcePoint.ns = "path_planner"
    goalPoint.ns = "path_planner"
    neighbourPoint.ns = "path_planner"
    finalPath.ns = "path_planner"

    sourcePoint.action = 0  # add/modify an object
    goalPoint.action = 0
    neighbourPoint.action = 0
    finalPath.action = 0

    sourcePoint.id = 0
    goalPoint.id = 1
    neighbourPoint.id = 2
    finalPath.id = 3

    # sourcePoint.text      = 'sourcePoint'
    # goalPoint.text        = 'goalPoint'
    # neighbourPoint.text   = 'neighbourPoint'
    # finalPath.text        = 'finalPath'

    sourcePoint.type = 2  # Sphere
    goalPoint.type = 2
    neighbourPoint.type = 8  # Points
    finalPath.type = 4  # Line Strip

    sourcePoint.pose.orientation.w = 1.0
    goalPoint.pose.orientation.w = 1.0
    neighbourPoint.pose.orientation.w = 1.0
    finalPath.pose.orientation.w = 1.0

    sourcePoint.pose.position.x = 0.0
    sourcePoint.pose.position.y = 0.0
    sourcePoint.pose.position.z = 0.0

    goalPoint.pose.position.x = 10.0
    goalPoint.pose.position.y = 10.0
    goalPoint.pose.position.z = 0.0

    neighbourPoint.pose.position.x = 0.0
    neighbourPoint.pose.position.y = 0.0
    neighbourPoint.pose.position.z = 0.0

    sourcePoint.scale.x = sourcePoint.scale.y = sourcePoint.scale.z = 1.0
    goalPoint.scale.x = goalPoint.scale.y = goalPoint.scale.z = 1.0
    neighbourPoint.scale.x = neighbourPoint.scale.y = neighbourPoint.scale.z = 0.1
    finalPath.scale.x = 0.5  # scale.x controls the width of the line segments

    sourcePoint.color.g = 1.0
    goalPoint.color.r = 1.0
    neighbourPoint.color.r = 0.8
    neighbourPoint.color.g = 0.4

    finalPath.color.r = 0.2
    finalPath.color.g = 0.2
    finalPath.color.b = 1.0

    sourcePoint.color.a = 1.0
    goalPoint.color.a = 1.0
    neighbourPoint.color.a = 0.5
    finalPath.color.a = 1.0

    return (sourcePoint, goalPoint, neighbourPoint, finalPath)
Beispiel #46
0
 def run(self):
     msg = WheelsCmdStamped()
     msg.header.stamp = rospy.get_rostime()
     msg.vel_left = 0.1
     msg.vel_right = 0.1        
     self.pub.publish(msg)
Beispiel #47
0
    def publishTagPositions(self, serialData):
        """
        Publish anchors and tag in topics using Tag and Anchor Object
        :param networkDataArray:  Array from serial port containing all informations, tag xyz and anchor xyz
        :returns: none
        """

        # Update the wrong message counts
        for idx, er in enumerate(self.error_buffer):
            if er in serialData:
                self.error_cnt[idx] += 1

        # Output error, if request 10 times failure
        for idx, cnt in enumerate(self.error_cnt):
            if cnt > 10:
                # print self.error_buffer[idx]
                print("===>>> ERROR: Tag {} is Lost".format(
                    self.error_buffer[idx][-1]))
                # print(self.error_cnt )
                self.reset_error()

        arrayData = [x for x in serialData.strip().split(' ')]

        # Initial data checking
        if len(self.measure_matrix) < 4 and self.new_measure_received:
            # print([x  for x in serialData.strip().split(',')], '        ', len([x  for x in serialData.strip().split(',')]) )
            data_length = len([x for x in serialData.strip().split(',')])
            if (data_length is 17):
                self.measure_matrix.append(
                    [x for x in serialData.strip().split(',')])
            else:
                self.new_measure_received = False
                self.measure_matrix = []
                print("Initial Wrong , reinitial !")
        # Publish the measurements
        elif len(self.measure_matrix) is 4:
            pub = rospy.Publisher("/tags/distance", String, queue_size=100)
            now = rospy.get_rostime()
            str12 = "{} {} : 1 -> 2 : ".format(now.secs, now.nsecs) + str(
                self.to_distance(self.measure_matrix[0][5],
                                 self.measure_matrix[0][4]))
            str13 = "{} {} : 1 -> 3 : ".format(now.secs, now.nsecs) + str(
                self.to_distance(self.measure_matrix[0][9],
                                 self.measure_matrix[0][8]))
            str14 = "{} {} : 1 -> 4 : ".format(now.secs, now.nsecs) + str(
                self.to_distance(self.measure_matrix[0][13],
                                 self.measure_matrix[0][12]))
            str21 = "{} {} : 2 -> 1 : ".format(now.secs, now.nsecs) + str(
                self.to_distance(self.measure_matrix[1][1],
                                 self.measure_matrix[1][0]))
            str23 = "{} {} : 2 -> 3 : ".format(now.secs, now.nsecs) + str(
                self.to_distance(self.measure_matrix[1][9],
                                 self.measure_matrix[1][8]))
            str24 = "{} {} : 2 -> 4 : ".format(now.secs, now.nsecs) + str(
                self.to_distance(self.measure_matrix[1][13],
                                 self.measure_matrix[1][12]))
            str31 = "{} {} : 3 -> 1 : ".format(now.secs, now.nsecs) + str(
                self.to_distance(self.measure_matrix[2][1],
                                 self.measure_matrix[2][0]))
            str32 = "{} {} : 3 -> 2 : ".format(now.secs, now.nsecs) + str(
                self.to_distance(self.measure_matrix[2][5],
                                 self.measure_matrix[2][4]))
            str34 = "{} {} : 3 -> 4 : ".format(now.secs, now.nsecs) + str(
                self.to_distance(self.measure_matrix[2][13],
                                 self.measure_matrix[2][12]))
            str41 = "{} {} : 4 -> 1 : ".format(now.secs, now.nsecs) + str(
                self.to_distance(self.measure_matrix[3][1],
                                 self.measure_matrix[3][0]))
            str42 = "{} {} : 4 -> 2 : ".format(now.secs, now.nsecs) + str(
                self.to_distance(self.measure_matrix[3][5],
                                 self.measure_matrix[3][4]))
            str43 = "{} {} : 4 -> 3 : ".format(now.secs, now.nsecs) + str(
                self.to_distance(self.measure_matrix[3][9],
                                 self.measure_matrix[3][8]))

            self.reset_error()

            for i in range(4):
                for j in range(4):
                    if i != j:
                        self.publishers[i][j].publish(
                            Float64(data=self.to_distance(
                                self.measure_matrix[i][4 * j + 1],
                                self.measure_matrix[i][4 * j])))

            pub.publish(str12)
            pub.publish(str13)
            pub.publish(str14)
            pub.publish(str21)
            pub.publish(str23)
            pub.publish(str24)
            pub.publish(str31)
            pub.publish(str32)
            pub.publish(str34)
            pub.publish(str41)
            pub.publish(str42)
            pub.publish(str43)
            print("{} : message published ".format(self.msg_counter))
            self.msg_counter += 1

            # print(self.measure_matrix)
            self.measure_matrix = []
            self.new_measure_received = False

        if 'Sending' in arrayData:
            # print("New data received ...")
            self.new_measure_received = True
def LineTracing():
    global rawImage
    global angularVelocity
    global prevTime
    global edgesImage
    global error
    global totalFinalX

    now = rospy.get_rostime()
    nowTime = now.nsecs
    errorTime = 0
    if nowTime < prevTime:
        errorTime = nowTime - prevTime + 1000000000
    else:
        errorTime = nowTime - prevTime
    errorTime = errorTime / 100000000.0
    prevTime = nowTime

    img_hsv = cv2.cvtColor(rawImage, cv2.COLOR_BGR2HSV)

    lower_color = (12, 87, 0)
    upper_color = (44, 255, 255)
    img_mask = cv2.inRange(img_hsv, lower_color, upper_color)

    kernel = np.ones((3, 3), np.uint8)
    img_mask = cv2.erode(img_mask, kernel, iterations=2)
    img_mask = cv2.dilate(img_mask, kernel, iterations=2)

    edgesImage = cv2.Canny(img_mask, 50, 150, apertureSize=3)
    lines = cv2.HoughLinesP(edgesImage,
                            0.8,
                            np.pi / 180,
                            90,
                            minLineLength=50,
                            maxLineGap=100)

    if lines is None:
        return None

    x_array = []
    y_array = []

    for i in lines:

        x1 = i[0][0]
        y1 = i[0][1]
        x2 = i[0][2]
        y2 = i[0][3]

        if y1 != y2:
            length = math.sqrt(
                math.pow(int(x2 - x1), 2) + math.pow(int(y2 - y1), 2))
            if length > 100:
                x_array.append(i[0][0])
                y_array.append(i[0][1])
                x_array.append(i[0][2])
                y_array.append(i[0][3])
                cv2.line(rawImage, (i[0][0], i[0][1]), (i[0][2], i[0][3]),
                         (0, 0, 255), 2)

    if (len(x_array) > 3):
        f1 = np.polyfit(y_array, x_array, 1)

        final_y = 380
        final_x = int(f1[0] * 380 + f1[1])
        cv2.circle(rawImage, (final_x, final_y), 5, (255, 255, 0), 8)

        error = (320 - final_x) / 1000.0

        K_P = 1.1

        goal = error * K_P

        max_value = 0.45

        if goal < -max_value:
            goal = -max_value
        if goal > max_value:
            goal = max_value
        # print("angular")
        # print(goal)

        angularVelocity = goal
import numpy as np
import cv
import tf.transformations as tfm
import rospy
import tf
import sys


def loadSource(filename):
    content = cv.Load(filename)
    return np.asarray(content)


if __name__ == '__main__':
    # Prepare transformation
    filename = sys.argv[1]
    ymsrc = loadSource(filename)
    print("Loaded " + filename)
    quaternion = tfm.quaternion_from_matrix(ymsrc)
    translation = (ymsrc[0, 3], ymsrc[1, 3], ymsrc[2, 3])

    rospy.init_node("velodyne_to_camera_publisher")
    sender = tf.TransformBroadcaster(100)
    rate = rospy.Rate(25)

    while not rospy.is_shutdown():
        sender.sendTransform(translation, quaternion, rospy.get_rostime(),
                             "camera", "velodyne")
        print("Sent")
        rate.sleep()
Beispiel #50
0
def is_separate_trigger(switch):
    if rospy.get_rostime().to_sec(
    ) - switch.last_time_on >= switch.debounce_time:
        return True
    else:
        return False
	driving_log = csv.DictWriter(f, fieldnames=["RGB Image","Depth Image", "Steering Angle", "Throttle"])
	driving_log.writeheader()


# Keep publishing info until rospy is shut down
print("\n\nStart publishing data via /recorder [steering, throttle] and images is saved at /home/jjs/git/JScar/bag/imgs.\n Press 'X' to stop the recording process.\n")


while not rospy.is_shutdown():
    if image is None:
      	continue
    # Convert image to RGB color space
    #image = cv2.cvtColor(image, 4)
#     image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
    # Create image filename for current frame
    timestamp = rospy.get_rostime()
    # RGB image
    fname = str(timestamp)+"_"+str(seq) +"_"+str(round(steering, 2)) + "_"+ str(round(throttle, 2))
    fname = RGB_IMG_PATH+fname+".jpg"
    # Depth Image
    depth_fname = str(timestamp)+"_"+str(seq) +"_"+str(round(steering, 2)) + "_"+ str(round(throttle, 2))
    depth_fname = DEPTH_IMG_PATH+depth_fname+".jpg"
    # Write images into disk
    print(fname)
    cv2.imwrite(fname, image)
    cv2.imwrite(depth_fname, depth_img)
    
    # new csv line
#    driving_log.writerow({'RGB Image': fname, 'Depth Image': depth_fname, 'Steering Angle': steering,'Throttle': throttle,'Speed': speed})
    driving_log.writerow({'RGB Image': fname, 'Depth Image': depth_fname, 'Steering Angle': steering,'Throttle': throttle})
    # publish data to /car_recorder
Beispiel #52
0
    def omni_pose_cb(self, msg):
        self.publish_rviz_markers()
        if self.enabled:
            #Get the omni's tip pose in the PR2's torso frame
            tip_omni, msg_frame = tfu.posestamped_as_matrix(msg)
            self.torso_lift_link_T_omni(tip_omni, msg_frame)
            #self.torso_T_omni(tip_omni, msg_frame)

            #Publish new arm pose
            ps = PoseStamped()
            ps.header.frame_id = '/torso_lift_link'
            ps.header.stamp = rospy.get_rostime()
            ps.pose.position.x = self.tip_tt[0]
            ps.pose.position.y = self.tip_tt[1]
            ps.pose.position.z = self.tip_tt[2]
            ps.pose.orientation.x = self.tip_tq[0]
            ps.pose.orientation.y = self.tip_tq[1]
            ps.pose.orientation.z = self.tip_tq[2]
            ps.pose.orientation.w = self.tip_tq[3]

            self.set_goal_pub.publish(ps)

            if self.zero_out_forces:
                wr = OmniFeedback()
                wr.force.x = 0
                wr.force.y = 0
                wr.force.z = 0
                self.omni_fb.publish(wr)
                self.zero_out_forces = False
        else:
            #this is a zero order hold publishing the last received values until the control loop is active again
            tip_tt = self.tip_tt
            tip_tq = self.tip_tq
            ps = PointStamped()
            ps.header.frame_id = '/torso_lift_link'
            ps.header.stamp = rospy.get_rostime()
            ps.point.x = tip_tt[0]
            ps.point.y = tip_tt[1]
            ps.point.z = tip_tt[2]
            ps.pose.orientation.x = tip_tq[0]
            ps.pose.orientation.y = tip_tq[1]
            ps.pose.orientation.z = tip_tq[2]
            ps.pose.orientation.w = tip_tq[3]

            self.set_goal_pub.publish(ps)

            #this is to make the omni force well move if the arm has moved but the commanded
            #position of the arm has not changed
            tip_omni, msg_frame = tfu.posestamped_as_matrix(msg)
            m_o1 = tfu.transform(self.omni_name, msg_frame,
                                 self.tflistener) * tip_omni
            ee_point = np.matrix(tr.translation_from_matrix(m_o1)).T
            q = self.robot.get_joint_angles()
            pos, rot = self.robot.kinematics.FK(q, self.robot.kinematics.n_jts)
            # tip_torso = tfu.transform('/torso_lift_link', self.gripper_tip_frame, self.tflistener) \
            #                       * tfu.tf_as_matrix(([0.,0.,0.], tr.quaternion_from_euler(0,0,0)))

            tip_torso = hrl_tr.composeHomogeneousTransform(rot, pos)
            #tip_torso = tfu.tf_as_matrix(([pos, rot]))
            #def composeHomogeneousTransform(rot, disp):

            center_t, center_q = self.omni_T_torso(tip_torso)
            center_col_vec = np.matrix(center_t).T

            #Send force control info
            wr = OmniFeedback()
            # offsets (0, -.268, -.15) introduced by Hai in phantom driver
            # should be cleaned up at some point so that it is consistent with position returned by phantom -marc
            lock_pos = tr.translation_matrix(np.matrix(
                [0, -.268, -.150])) * tfu.transform(
                    self.omni_name + '_sensable', self.omni_name,
                    self.tflistener) * np.row_stack(
                        (center_col_vec, np.matrix([1.])))

            wr.position.x = (
                lock_pos[0, 0]
            ) * 1000.0  #multiply by 1000 mm/m to get units phantom expects
            wr.position.y = (lock_pos[1, 0]) * 1000.0
            wr.position.z = (lock_pos[2, 0]) * 1000.0

            # wr.position.x = tip_tt[0]  #multiply by 1000 mm/m to get units phantom expects
            # wr.position.y = tip_tt[1]
            # wr.position.z = tip_tt[2]
            self.omni_fb.publish(wr)
    def __init__(self):

        self.LASER_MESSAGE_DEFINED = [
            "RAWLASER1", "RAWLASER2", "RAWLASER3", "RAWLASER4"
        ]
        self.OLD_LASER_MESSAGE_DEFINED = [
            "FLASER", "RLASER", "LASER3", "LASER4"
        ]
        self.ROBOT_LASER_MESSAGE_DEFINED = ["ROBOTLASER1", "ROBOTLASER2"]
        self.GPS_MESSAGE_DEFINED = ["NMEAGGA", "NMEARMC"]
        self.ODOM_DEFINED = "ODOM"
        self.TRUEPOS_DEFINED = "TRUEPOS"
        self.PARAM_DEFINED = "PARAM"
        self.SYNC_DEFINED = "SYNC"

        self.unknown_entries = []

        self.stamp = rospy.get_rostime()

        self.pose_msg = Odometry()
        self.true_pose_msg = Odometry()

        self.laser_msg = LaserScan()

        self.tf_odom_robot_msg = TransformStamped()
        self.tf_laser_robot_msg = TransformStamped()
        self.tf2_msg = TFMessage()

        self.tf_tr = tf.Transformer(True, rospy.Duration(2.0))

        self.pause = False

        self.rate = rospy.get_param("~global_rate", 40)

        RAWLASER1_topic = rospy.get_param("~RAWLASER1_topic", "/RAWLASER1")
        RAWLASER2_topic = rospy.get_param("~RAWLASER2_topic", "/RAWLASER2")
        RAWLASER3_topic = rospy.get_param("~RAWLASER3_topic", "/RAWLASER3")
        RAWLASER4_topic = rospy.get_param("~RAWLASER4_topic", "/RAWLASER4")

        FLASER_topic = rospy.get_param("~FLASER_topic", "/FLASER")
        RLASER_topic = rospy.get_param("~RLASER_topic", "/RLASER")
        LASER3_topic = rospy.get_param("~LASER3_topic", "/LASER3")
        LASER4_topic = rospy.get_param("~LASER4_topic", "/LASER4")

        ROBOTLASER1_topic = rospy.get_param("~ROBOTLASER1_topic",
                                            "/ROBOTLASER1")
        ROBOTLASER2_topic = rospy.get_param("~ROBOTLASER2_topic",
                                            "/ROBOTLASER2")

        NMEAGGA_topic = rospy.get_param("~NMEAGGA_topic", "/NMEAGGA")
        NMEARMC_topic = rospy.get_param("~NMEARMC_topic", "/NMEARMC")

        ODOM_topic = rospy.get_param("~ODOM_topic", "/ODOM")
        TRUEPOS_topic = rospy.get_param("~TRUEPOS_topic", "/TRUEPOS")

        tf_topic = rospy.get_param("~tf_topic", "/tf")

        robot_link = rospy.get_param("~robot_link", "base_link")
        odom_link = rospy.get_param("~odom_link", "odom")
        odom_robot_link = rospy.get_param("~odom_robot_link",
                                          "odom_robot_link")
        true_odom_link = rospy.get_param("~trueodom_link", "gt_odom")
        ROBOTLASER1_link = rospy.get_param("~ROBOTLASER1_link",
                                           "ROBOTLASER1_link")
        ROBOTLASER2_link = rospy.get_param("~ROBOTLASER2_link",
                                           "ROBOTLASER2_link")

        # Hacky param to choose which one of ODOM logs or ROBOTLASER1 to publish on tf.
        # If set to False, ODOM is pub on tf
        # If set to True,  ROBOTLASER1 is pub on tf
        # Must be careful, if set to True and there are no ROBOTLASER1 but a ODOM
        # There won't be anything on tf ...
        # TODO : - less hacky way
        #		 - choose which one of ROBOTLASER1 or ROBOTLASER2 etc
        self.publish_corrected = rospy.get_param("~pub_corrected", False)

        self.topics = {
            "RAWLASER1": RAWLASER1_topic,
            "RAWLASER2": RAWLASER2_topic,
            "RAWLASER3": RAWLASER3_topic,
            "RAWLASER4": RAWLASER4_topic,
            "FLASER": FLASER_topic,
            "RLASER": RLASER_topic,
            "LASER3": LASER3_topic,
            "LASER4": LASER4_topic,
            "ROBOTLASER1": ROBOTLASER1_topic,
            "ROBOTLASER2": ROBOTLASER2_topic,
            "NMEAGGA": NMEAGGA_topic,
            "NMEARMC": NMEARMC_topic,
            "ODOM": ODOM_topic,
            "TRUEPOS": TRUEPOS_topic,
            "TF": tf_topic
        }

        self.links = {
            "ROBOT": robot_link,
            "ODOM": odom_link,
            "ROBOTODOM": odom_robot_link,
            "TRUEPOS": true_odom_link,
            "ROBOTLASER1": ROBOTLASER1_link,
            "ROBOTLASER2": ROBOTLASER2_link
        }

        self.params = {
            "laser": params.old_laser_params,
            "newlaser": params.new_laser_params,
            "robot": params.robot_params,
            "gps": params.gps_params,
            "base": params.base_params,
            "arm": params.arm_params,
            "segway": params.segway_params
        }  #TOfinish
Beispiel #54
0
    def __init__(self):

        self.ERRORS = {
            0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
            0x0001:
            (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
            0x0002:
            (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
            0x0004:
            (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
            0x0008:
            (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
            0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
                     "Temperature2"),
            0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
                     "Main batt voltage high"),
            0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
                     "Logic batt voltage high"),
            0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
                     "Logic batt voltage low"),
            0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
                     "M1 driver fault"),
            0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
                     "M2 driver fault"),
            0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
                     "Main batt voltage high"),
            0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
                     "Main batt voltage low"),
            0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
                     "Temperature1"),
            0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
                     "Temperature2"),
            0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
            0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")
        }

        rospy.init_node("roboclaw_node")
        rospy.on_shutdown(self.shutdown)
        rospy.loginfo("Connecting to roboclaw")
        dev_name = rospy.get_param("~dev", "/dev/ttyACM0")
        baud_rate = int(rospy.get_param("~baud", "115200"))

        self.address = int(rospy.get_param("~address", "128"))
        if self.address > 0x87 or self.address < 0x80:
            rospy.logfatal("Address out of range")
            rospy.signal_shutdown("Address out of range")

        # TODO need someway to check if address is correct
        try:
            roboclaw.Open(dev_name, baud_rate)
        except Exception as e:
            rospy.logfatal("Could not connect to Roboclaw")
            rospy.logdebug(e)
            rospy.signal_shutdown("Could not connect to Roboclaw")

        self.updater = diagnostic_updater.Updater()
        self.updater.setHardwareID("Roboclaw")
        self.updater.add(
            diagnostic_updater.FunctionDiagnosticTask("Vitals",
                                                      self.check_vitals))

        try:
            version = roboclaw.ReadVersion(self.address)
        except Exception as e:
            rospy.logwarn("Problem getting roboclaw version")
            rospy.logdebug(e)
            pass

        if not version[0]:
            rospy.logwarn("Could not get version from roboclaw")
        else:
            rospy.logdebug(repr(version[1]))

        roboclaw.SpeedM1M2(self.address, 0, 0)
        roboclaw.ResetEncoders(self.address)

        self.MAX_SPEED = float(rospy.get_param("~max_speed", "2.0"))
        self.TICKS_PER_METER = float(
            rospy.get_param("~ticks_per_meter", "4342.2"))
        self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315"))
        self.LEFT_MOTOR_NUMBER = int(rospy.get_param("~left_motor_number", 1))

        self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
        self.last_set_speed_time = rospy.get_rostime()
        self.stopped = True

        rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback)

        rospy.sleep(1)

        rospy.logdebug("dev %s", dev_name)
        rospy.logdebug("baud %d", baud_rate)
        rospy.logdebug("address %d", self.address)
        rospy.logdebug("max_speed %f", self.MAX_SPEED)
        rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER)
        rospy.logdebug("base_width %f", self.BASE_WIDTH)
Beispiel #55
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
            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)

            # send command
            self.drive_cmd(*req_cmd_vel)
            # record command
            last_cmd_vel = req_cmd_vel

            r.sleep()
Beispiel #56
0
def pose():

    msg_grip = JointState()
    pub3 = rospy.Publisher('/dvrk/MTML/state_gripper_current',
                           JointState,
                           queue_size=1)
    msg_grip.header.frame_id = "map"
    msg_grip.position = [0]
    msg_grip.position[0] = -0.10

    im = 0
    pub2 = rospy.Publisher('/dvrk/MTML/position_cartesian_current',
                           PoseStamped,
                           queue_size=1)
    rospy.init_node('poser', anonymous=True)

    msg = PoseStamped()
    rate = rospy.Rate(10)

    msg.header.frame_id = "map"
    msg.header.stamp = rospy.get_rostime()

    msg.pose.position.x = 0.20
    msg.pose.position.y = 0.10
    msg.pose.position.z = -0.25

    msg.pose.orientation.x = 0.70
    msg.pose.orientation.y = 0.70
    msg.pose.orientation.z = 0.70
    msg.pose.orientation.w = 0

    msg_cam = Joy()

    while not rospy.is_shutdown():

        pub_cam = rospy.Publisher('/dvrk/footpedals/coag', Joy, queue_size=1)

        pub = rospy.Publisher('/dvrk/MTML/status', String, queue_size=1)
        hello_str = "This"
        pub.publish(hello_str)

        msg.pose.position.x = msg.pose.position.x
        msg.pose.position.y = msg.pose.position.y
        msg.pose.position.z = msg.pose.position.z

        msg.pose.orientation.x = msg.pose.orientation.x
        msg.pose.orientation.y = msg.pose.orientation.y
        msg.pose.orientation.z = msg.pose.orientation.z
        msg.pose.orientation.w = msg.pose.orientation.w

        char = getch()

        if (char == "c"):
            msg_cam.buttons = [1]
            pub_cam.publish(msg_cam)

        if (char == "v"):
            msg_cam.buttons = [0]
            pub_cam.publish(msg_cam)

        if (char == "n"):
            msg_grip.position[0] = msg_grip.position[0] + 0.1
            pub3.publish(msg_grip)

        if (char == "m"):
            msg_grip.position[0] = msg_grip.position[0] - 0.1
            pub3.publish(msg_grip)

        if (char == "p"):
            print("Stop!")
            exit(0)

        if char == "a":
            msg.pose.position.x = msg.pose.position.x - 0.01
            pub2.publish(msg)

            rate.sleep()
            print("Left pressed")

        elif char == "e":
            msg.pose.position.y = msg.pose.position.y + 0.01
            pub2.publish(msg)

            rate.sleep()
            print("Right pressed")

        elif char == "d":
            msg.pose.position.y = msg.pose.position.y - 0.01
            pub2.publish(msg)

            rate.sleep()
            print("Right pressed")

        elif char == "f":
            msg.pose.position.z = msg.pose.position.z + 0.01
            pub2.publish(msg)

            rate.sleep()
            print("Right pressed")

        elif char == "r":
            msg.pose.position.z = msg.pose.position.z - 0.01
            pub2.publish(msg)

            rate.sleep()
            print("Right pressed")

        elif char == "w":
            msg.pose.position.x = msg.pose.position.x + 0.01
            pub2.publish(msg)

            rate.sleep()
            print("Up pressed")

        elif char == "l":
            getSlavePos()

        elif char == "1":
            q = [
                msg.pose.orientation.x, msg.pose.orientation.y,
                msg.pose.orientation.z, msg.pose.orientation.w
            ]
            q_new = angleUpdate(q, 0, 1)
            msg.pose.orientation.x = q_new[0]
            msg.pose.orientation.y = q_new[1]
            msg.pose.orientation.z = q_new[2]
            msg.pose.orientation.w = q_new[3]
            pub2.publish(msg)
            rate.sleep()

        elif char == "2":
            q = [
                msg.pose.orientation.x, msg.pose.orientation.y,
                msg.pose.orientation.z, msg.pose.orientation.w
            ]
            q_new = angleUpdate(q, 0, -1)
            msg.pose.orientation.x = q_new[0]
            msg.pose.orientation.y = q_new[1]
            msg.pose.orientation.z = q_new[2]
            msg.pose.orientation.w = q_new[3]
            pub2.publish(msg)
            rate.sleep()

        elif char == "3":
            q = [
                msg.pose.orientation.x, msg.pose.orientation.y,
                msg.pose.orientation.z, msg.pose.orientation.w
            ]
            q_new = angleUpdate(q, 1, 1)
            msg.pose.orientation.x = q_new[0]
            msg.pose.orientation.y = q_new[1]
            msg.pose.orientation.z = q_new[2]
            msg.pose.orientation.w = q_new[3]
            pub2.publish(msg)
            rate.sleep()

        elif char == "4":
            q = [
                msg.pose.orientation.x, msg.pose.orientation.y,
                msg.pose.orientation.z, msg.pose.orientation.w
            ]
            q_new = angleUpdate(q, 1, -1)
            msg.pose.orientation.x = q_new[0]
            msg.pose.orientation.y = q_new[1]
            msg.pose.orientation.z = q_new[2]
            msg.pose.orientation.w = q_new[3]
            pub2.publish(msg)
            rate.sleep()

        elif char == "5":
            q = [
                msg.pose.orientation.x, msg.pose.orientation.y,
                msg.pose.orientation.z, msg.pose.orientation.w
            ]
            q_new = angleUpdate(q, 2, 1)
            msg.pose.orientation.x = q_new[0]
            msg.pose.orientation.y = q_new[1]
            msg.pose.orientation.z = q_new[2]
            msg.pose.orientation.w = q_new[3]
            pub2.publish(msg)
            rate.sleep()

        elif char == "6":
            q = [
                msg.pose.orientation.x, msg.pose.orientation.y,
                msg.pose.orientation.z, msg.pose.orientation.w
            ]
            q_new = angleUpdate(q, 2, -1)
            msg.pose.orientation.x = q_new[0]
            msg.pose.orientation.y = q_new[1]
            msg.pose.orientation.z = q_new[2]
            msg.pose.orientation.w = q_new[3]
            pub2.publish(msg)
            rate.sleep()
#!/usr/bin/env python

import rospy
import dynamic_reconfigure.client

def callback(config):
    rospy.loginfo("Config set to {int_param}, {double_param}, {str_param}, {bool_param}, {size}".format(**config))

if __name__ == "__main__":
    rospy.init_node("dynamic_client")
    rospy.wait_for_service("dynamic_srv/set_parameters")
    client = dynamic_reconfigure.client.Client("dynamic_srv", timeout=10, config_callback=callback)

    r = rospy.Rate(1)
    x = 0
    b = False
    while not rospy.is_shutdown():
	x = x+1
	if x>10:
	    x = 0
	b = not b
        client.update_configuration({"int_param":x, "double_param":(1/(x+1)), "str_param":str(rospy.get_rostime()), "bool_param":b, "size": 1})

	r.sleep()

Beispiel #58
0
def _tf_handle(msg):
    _msgs.append(
        (msg, rospy.get_rostime(), msg._connection_header['callerid']))
Beispiel #59
0
    def has_crossed(self):
        """ checks if crossing maneuver is over """

        return self.mode == Mode.CROSS and \
               rospy.get_rostime() - self.cross_start > rospy.Duration.from_sec(self.params.crossing_time)
 def publish(self):
     self.marker.header.stamp = rospy.get_rostime()
     self.pub_send_marker.publish(self.marker)